From f40bd23e7df85e6321816991efe10f72e648bb35 Mon Sep 17 00:00:00 2001
From: kugurofu <tenten31569@icloud.com>
Date: Fri, 8 Aug 2025 08:31:02 +0000
Subject: [PATCH 01/25] add odom_combine

---
 .../orange_gnss/gnss_odom_publisher_ttyUSB.py |  11 +-
 orange_gnss/orange_gnss/odom_combination.py   | 203 ++++++++++++++++++
 orange_gnss/setup.py                          |   3 +-
 3 files changed, 214 insertions(+), 3 deletions(-)
 create mode 100644 orange_gnss/orange_gnss/odom_combination.py

diff --git a/orange_gnss/orange_gnss/gnss_odom_publisher_ttyUSB.py b/orange_gnss/orange_gnss/gnss_odom_publisher_ttyUSB.py
index 7e2973d..64d0429 100644
--- a/orange_gnss/orange_gnss/gnss_odom_publisher_ttyUSB.py
+++ b/orange_gnss/orange_gnss/gnss_odom_publisher_ttyUSB.py
@@ -26,6 +26,7 @@ def __init__(self):
         self.country_id = self.get_parameter('country_id').get_parameter_value().integer_value
         self.Position_magnification = self.get_parameter('Position_magnification').get_parameter_value().double_value
         #self.theta = self.get_parameter('heading').get_parameter_value().double_value
+        self.tsukuba_theta=90
 
         self.initial_coordinate = None
         self.fix_data = None
@@ -62,7 +63,11 @@ def send_request(self):
         request = Avglatlon.Request()
         request.avg_lat = self.initial_coordinate[0]  # ← average lat
         request.avg_lon = self.initial_coordinate[1]  # ← average lon
-        request.theta = self.theta
+        request.current_lat = self.current_coordinate[0]  # ← current lat
+        request.current_lon = self.current_coordinate[1]  # ← current lon
+        #request.theta = self.theta
+        request.theta = self.tsukuba_theta # tsukuba start theta
+        request.current_theta = self.theta # for tsukuba
 
         future = self.client.call_async(request)
         future.add_done_callback(self.response_callback)
@@ -104,7 +109,9 @@ def acquire_gps_data(self):
             time.sleep(0.1)  # Slight delay to avoid overwhelming the GPS device
 
         if count > 0:
-            self.initial_coordinate = [lat_sum / count, lon_sum / count]
+            #self.initial_coordinate = [lat_sum / count, lon_sum / count] # calculate average
+            self.initial_coordinate = [36.0497502, 140.0459234] # tsukuba start point
+            self.current_coordinate = [lat_sum / count, lon_sum / count] # for tsukuba
             self.theta = (heading_sum / count) - 90
             self.initialized = True
             self.get_logger().info(f"Initial coordinate set to: {self.initial_coordinate}")
diff --git a/orange_gnss/orange_gnss/odom_combination.py b/orange_gnss/orange_gnss/odom_combination.py
new file mode 100644
index 0000000..b2c2ec8
--- /dev/null
+++ b/orange_gnss/orange_gnss/odom_combination.py
@@ -0,0 +1,203 @@
+#!/usr/bin/env python3
+import rclpy
+import math
+import numpy as np
+from rclpy.qos import QoSProfile, QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy
+from nav_msgs.msg import Odometry
+from rclpy.node import Node
+from std_msgs.msg import Header
+import threading
+import time
+from my_msgs.srv import Avglatlon
+
+class Odom_Combination(Node):
+    def __init__(self):
+        super().__init__('odom_combination')
+
+        qos_profile = QoSProfile(
+            history=QoSHistoryPolicy.KEEP_LAST,
+            reliability=QoSReliabilityPolicy.RELIABLE,
+            durability=QoSDurabilityPolicy.VOLATILE,
+            depth = 1
+        )
+        
+        # subscription
+        self.odom_sub = self.create_subscription(Odometry, '/odom', self.get_odom, qos_profile)
+        
+        # publisher
+        self.odom_pub = self.create_publisher(Odometry, '/odom/combine', qos_profile)
+        
+        # service
+        self.avg_gps_service = self.create_service(Avglatlon, 'send_avg_gps', self.receive_avg_gps_callback)
+
+        self.position_x = 0.0
+        self.position_y = 0.0
+        self.theta_z = 0.0
+        self.theta = 0.0
+        self.initial_xy = (0.0, 0.0)
+        self.Position_magnification = 1.0  # 必要なら調整
+
+        self.timer = self.create_timer(0.1, self.combine)
+
+    def get_odom(self, msg):
+        '''
+        self.position_x = msg.pose.pose.position.x
+        self.position_y = msg.pose.pose.position.y
+        self.position_z = msg.pose.pose.position.z
+        x, y, z, w = msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w
+        roll, pitch, yaw = quaternion_to_euler(x, y, z, w)
+        self.theta_x, self.theta_y, self.theta_z = 0, 0, yaw * 180 / math.pi
+        '''
+        self.position_x = msg.pose.pose.position.x
+        self.position_y = msg.pose.pose.position.y
+        x, y, z, w = msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w
+        roll, pitch, yaw = quaternion_to_euler(x, y, z, w)
+        self.theta_z = yaw  # 単位はラジアンのままでOK
+    
+    def conversion(self, coordinate, origin, theta):
+        ido = coordinate[0]
+        keido = coordinate[1]
+        ido0 = origin[0]
+        keido0 = origin[1]
+
+        # self.get_logger().info(f"theta: {theta}")
+
+        a = 6378137
+        f = 35/10439
+        e1 = 734/8971
+        e2 = 127/1547
+        n = 35/20843
+        a0 = 1
+        a2 = 102/40495
+        a4 = 1/378280
+        a6 = 1/289634371
+        a8 = 1/204422462123
+        pi180 = 71/4068
+        # %math.pi/180
+        d_ido = ido - ido0
+        d_keido = keido - keido0
+        rd_ido = d_ido * pi180
+        rd_keido = d_keido * pi180
+        r_ido = ido * pi180
+        r_keido = keido * pi180
+        r_ido0 = ido0 * pi180
+        W = math.sqrt(1-(e1**2)*(math.sin(r_ido)**2))
+        N = a / W
+        t = math.tan(r_ido)
+        ai = e2*math.cos(r_ido)
+
+       # %===Y===%
+        S = a*(a0*r_ido - a2*math.sin(2*r_ido)+a4*math.sin(4*r_ido) -
+               a6*math.sin(6*r_ido)+a8*math.sin(8*r_ido))/(1+n)
+        S0 = a*(a0*r_ido0-a2*math.sin(2*r_ido0)+a4*math.sin(4*r_ido0) -
+                a6*math.sin(6*r_ido0)+a8*math.sin(8*r_ido0))/(1+n)
+        m0 = S/S0
+        B = S-S0
+        y1 = (rd_keido**2)*N*math.sin(r_ido)*math.cos(r_ido)/2
+        y2 = (rd_keido**4)*N*math.sin(r_ido) * \
+            (math.cos(r_ido)**3)*(5-(t**2)+9*(ai**2)+4*(ai**4))/24
+        y3 = (rd_keido**6)*N*math.sin(r_ido)*(math.cos(r_ido)**5) * \
+            (61-58*(t**2)+(t**4)+270*(ai**2)-330*(ai**2)*(t**2))/720
+        gps_y = self.Position_magnification * m0 * (B + y1 + y2 + y3)
+
+       # %===X===%
+        x1 = rd_keido*N*math.cos(r_ido)
+        x2 = (rd_keido**3)*N*(math.cos(r_ido)**3)*(1-(t**2)+(ai**2))/6
+        x3 = (rd_keido**5)*N*(math.cos(r_ido)**5) * \
+            (5-18*(t**2)+(t**4)+14*(ai**2)-58*(ai**2)*(t**2))/120
+        gps_x = self.Position_magnification * m0 * (x1 + x2 + x3)
+
+        # point = (gps_x, gps_y)Not match
+
+        degree_to_radian = math.pi / 180
+        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)
+
+        return point    
+    
+    def receive_avg_gps_callback(self, request, response):
+        avg_lat, avg_lon, current_lat, current_lon, theta = request.avg_lat, request.avg_lon, request.current_lat, request.current_lon, request.current_theta
+        if theta == 0.0:
+            self.get_logger().warn("GPSからのthetaがまだ取得されていません。")
+            response.success = False
+            return response
+        
+        self.theta = theta    
+        current_coordinate = [current_lat, current_lon]
+        initial_coordinate = [avg_lat, avg_lon]
+        self.initial_xy = self.conversion(current_coordinate, initial_coordinate, theta)
+        xy_np = np.array(self.initial_xy)
+
+    def yaw_to_orientation(self, yaw):
+        orientation_z = np.sin(yaw / 2.0)
+        orientation_w = np.cos(yaw / 2.0)
+        return orientation_z, orientation_w
+    
+    def combine(self):
+        pos_x = self.position_x
+        pos_y = self.position_y
+        degree_to_radian = math.pi / 180
+        pos_theta = self.theta * degree_to_radian # maybe -self.theta * degree_to_radian
+        
+        # conversionによるxy変換結果（初期座標）
+        #delta_x, delta_y = self.initial_xy
+
+        # theta_z（現在の向き）で回転
+        cos_theta = math.cos(pos_theta)
+        sin_theta = math.sin(pos_theta)
+
+        #rotated_x = cos_theta * delta_x - sin_theta * delta_y
+        #rotated_y = sin_theta * delta_x + cos_theta * delta_y
+        rotated_x = cos_theta * pos_x - sin_theta * pos_y
+        rotated_y = sin_theta * pos_x + cos_theta * pos_y
+
+        # 加算して新しい位置
+        #combined_x = self.position_x + rotated_x
+        #combined_y = self.position_y + rotated_y
+        combined_x = self.initial_xy[0] + rotated_x
+        combined_y = self.initial_xy[1] + rotated_y
+                
+        # quartanion z,w
+        odom_orientation = self.yaw_to_orientation(pos_theta)
+
+        # Odometryメッセージ作成
+        odom_msg = Odometry()
+        odom_msg.header.stamp = self.get_clock().now().to_msg()
+        odom_msg.header.frame_id = "odom"
+        odom_msg.pose.pose.position.x = combined_x
+        odom_msg.pose.pose.position.y = combined_y
+        odom_msg.pose.pose.position.z = 0.0
+        odom_msg.pose.pose.orientation.x = 0.0
+        odom_msg.pose.pose.orientation.y = 0.0
+        odom_msg.pose.pose.orientation.z = float(odom_orientation[0])
+        odom_msg.pose.pose.orientation.w = float(odom_orientation[1])
+
+        # publish
+        self.odom_pub.publish(odom_msg)        
+
+def quaternion_to_euler(x, y, z, w):
+    t0 = +2.0 * (w * x + y * z)
+    t1 = +1.0 - 2.0 * (x * x + y * y)
+    roll = math.atan2(t0, t1)
+
+    t2 = +2.0 * (w * y - z * x)
+    t2 = +1.0 if t2 > +1.0 else t2
+    t2 = -1.0 if t2 < -1.0 else t2
+    pitch = math.asin(t2)
+
+    t3 = +2.0 * (w * z + x * y)
+    t4 = +1.0 - 2.0 * (y * y + z * z)
+    yaw = math.atan2(t3, t4)
+
+    return roll, pitch, yaw
+              
+def main(args=None):
+    rclpy.init(args=args)
+    odom_combination = Odom_Combination()
+    rclpy.spin(odom_combination)
+
+if __name__ == '__main__':
+    main()          
diff --git a/orange_gnss/setup.py b/orange_gnss/setup.py
index 891b69e..3ef7c4e 100644
--- a/orange_gnss/setup.py
+++ b/orange_gnss/setup.py
@@ -36,7 +36,8 @@
             "gnss_odom_publisher_ttyUSB = orange_gnss.gnss_odom_publisher_ttyUSB:main",
             "GPSodom_correction = orange_gnss.GPSodom_correction:main",
             "lonlat_to_odom = orange_gnss.lonlat_to_odom:main",
-            "ekf_myself_noGPS = orange_gnss.ekf_myself_noGPS:main"
+            "ekf_myself_noGPS = orange_gnss.ekf_myself_noGPS:main",
+            "odom_combination = orange_gnss.odom_combination:main",
         ],
     },
 )

From 97a042d53e4a239a700bdfad4e4c631c32e96d01 Mon Sep 17 00:00:00 2001
From: kugurofu <tenten31569@icloud.com>
Date: Fri, 22 Aug 2025 08:01:51 +0000
Subject: [PATCH 02/25] change param

---
 orange_gnss/orange_gnss/GPSodom_correction.py                | 2 +-
 .../orange_gnss/gnss_odom_movingbase_fix_publisher_ttyUSB.py | 4 ++--
 orange_gnss/orange_gnss/gnss_odom_publisher_ttyUSB.py        | 5 +++--
 3 files changed, 6 insertions(+), 5 deletions(-)

diff --git a/orange_gnss/orange_gnss/GPSodom_correction.py b/orange_gnss/orange_gnss/GPSodom_correction.py
index 1ea3652..f397382 100644
--- a/orange_gnss/orange_gnss/GPSodom_correction.py
+++ b/orange_gnss/orange_gnss/GPSodom_correction.py
@@ -14,7 +14,7 @@ def __init__(self):
         super().__init__('GPSodom_correction')
 
         self.create_subscription(
-            Odometry, "/odom/gps", self.odomgps_callback, 1)
+            Odometry, "/odom/UM982", self.odomgps_callback, 1)
         self.create_subscription(
             Imu, "/livox/imu", self.imu_callback, 1)
 
diff --git a/orange_gnss/orange_gnss/gnss_odom_movingbase_fix_publisher_ttyUSB.py b/orange_gnss/orange_gnss/gnss_odom_movingbase_fix_publisher_ttyUSB.py
index 9f8810c..5582a10 100644
--- a/orange_gnss/orange_gnss/gnss_odom_movingbase_fix_publisher_ttyUSB.py
+++ b/orange_gnss/orange_gnss/gnss_odom_movingbase_fix_publisher_ttyUSB.py
@@ -250,7 +250,7 @@ def quaternion_from_euler(self, roll, pitch, yaw):
 
     def heading_to_quat(self ,real_heading):
 
-        robotheading = real_heading + 90
+        robotheading = real_heading - 90
         if robotheading >= 360:
             robotheading -= 360
 
@@ -366,7 +366,7 @@ def publish_fix(self, gps):
     
     def publish_movingbase(self, heading):
         if heading is not None and heading != 0.0:
-            robotheading = heading + 90.0
+            robotheading = heading - 90.0
             if robotheading >= 360.0:
                 robotheading -= 360.0
 
diff --git a/orange_gnss/orange_gnss/gnss_odom_publisher_ttyUSB.py b/orange_gnss/orange_gnss/gnss_odom_publisher_ttyUSB.py
index 64d0429..a141d80 100644
--- a/orange_gnss/orange_gnss/gnss_odom_publisher_ttyUSB.py
+++ b/orange_gnss/orange_gnss/gnss_odom_publisher_ttyUSB.py
@@ -26,7 +26,7 @@ def __init__(self):
         self.country_id = self.get_parameter('country_id').get_parameter_value().integer_value
         self.Position_magnification = self.get_parameter('Position_magnification').get_parameter_value().double_value
         #self.theta = self.get_parameter('heading').get_parameter_value().double_value
-        self.tsukuba_theta=90
+        self.tsukuba_theta=180.0 # nakaniwa 180
 
         self.initial_coordinate = None
         self.fix_data = None
@@ -110,11 +110,12 @@ def acquire_gps_data(self):
 
         if count > 0:
             #self.initial_coordinate = [lat_sum / count, lon_sum / count] # calculate average
-            self.initial_coordinate = [36.0497502, 140.0459234] # tsukuba start point
+            self.initial_coordinate = [35.425952230280004, 139.31380123427] # tsukuba start point 36.0497502, 140.0459234 /nakaniwa 35.4257898377487,139.313807281254
             self.current_coordinate = [lat_sum / count, lon_sum / count] # for tsukuba
             self.theta = (heading_sum / count) - 90
             self.initialized = True
             self.get_logger().info(f"Initial coordinate set to: {self.initial_coordinate}")
+            self.get_logger().info(f"current coordinate set to: {self.current_coordinate}")
             self.get_logger().info(f"Initial theta set to: {self.theta}")
             self.send_request()
         self.is_acquiring = False

From f9e1f2a5d4ba51f1cdc24dbb61bc6f774f9bf1d1 Mon Sep 17 00:00:00 2001
From: kugurofu <tenten31569@icloud.com>
Date: Mon, 22 Sep 2025 04:41:46 +0000
Subject: [PATCH 03/25] add spresense imu

---
 .../config/motor_driver_params.yaml           |   2 +-
 orange_bringup/launch/orange_robot.launch.xml |  37 ++-
 orange_description/xacro/orange_robot.xacro   |   2 +-
 ...ss_odom_movingbase_fix_publisher_ttyUSB.py |   2 +-
 .../orange_gnss/gnss_odom_publisher_ttyUSB.py |   6 +-
 .../pcd_convert/pcd_height_segmentation.py    |   4 +-
 .../pcd_convert/pcd_rotation_lidar2.py        | 232 ++++++++++++++++++
 pcd_convert/setup.py                          |   3 +-
 .../launch/try_navigation.launch.py           |  12 +-
 .../try_navigation/ekf_myself_gps.py          |   9 +-
 .../try_navigation/path_follower.py           |   6 +-
 .../try_navigation/potential_astar.py         |   2 +-
 12 files changed, 290 insertions(+), 27 deletions(-)
 create mode 100644 pcd_convert/pcd_convert/pcd_rotation_lidar2.py

diff --git a/orange_bringup/config/motor_driver_params.yaml b/orange_bringup/config/motor_driver_params.yaml
index 3796ddb..e799121 100644
--- a/orange_bringup/config/motor_driver_params.yaml
+++ b/orange_bringup/config/motor_driver_params.yaml
@@ -7,7 +7,7 @@ motor_driver_node:
     left_wheel_radius: 0.09767 # meter
     right_wheel_radius: 0.09767 # meter
     # For odometry computation
-    computation_left_wheel_radius: 0.0970 # meter
+    computation_left_wheel_radius: 0.09755 # meter
     computation_right_wheel_radius: 0.0973 # meter
     # Encoder CPR(counts per revolution)
     cpr: 16385
diff --git a/orange_bringup/launch/orange_robot.launch.xml b/orange_bringup/launch/orange_robot.launch.xml
index 4824862..c072026 100644
--- a/orange_bringup/launch/orange_robot.launch.xml
+++ b/orange_bringup/launch/orange_robot.launch.xml
@@ -65,6 +65,35 @@
     <param name="serial_interval" value="0.01"/>
     <remap from="/estop/state" to="/estop"/>
   </node>
+
+  <!-- spresense imu -->
+  <node pkg="cxd5602pwbimu" exec="cxd5602pwbimu_node">
+    <param name="port" value="/dev/sensors/spresense_imu"/>
+    <param name="frame_id" value="imu_link"/>
+    <param name="imu_topic" value="/imu/spresense"/>
+    <param name="time_out" value="10.0"/>
+    <param name="baudrate" value="460800"/>
+    <param name="gyro_bias_correction" value="true"/>
+    <param name="gyro_bias_samples" value="4800"/>
+  </node>
+  <!-- madgwick filter node -->
+  <node pkg="imu_filter_madgwick" exec="imu_filter_madgwick_node" name="madgwick_filter">
+    <param from="$(find-pkg-share cxd5602pwbimu)/config/madgwick.yaml"/>
+    <remap from="/imu/data_raw" to="/imu/spresense"/>
+    <remap from="/imu/data" to="/imu/spresense/madgwick"/>
+  </node>
+  <!-- initial_yaw_corrector -->
+  <node pkg="cxd5602pwbimu" exec="initial_yaw_corrector" name="spimu_yaw_corrector">
+    <param name="input_topic" value="/imu/spresense/madgwick"/>
+    <param name="output_topic" value="/imu/spresense/inityaw"/>
+  </node>
+  <!-- wheel_imu_odom for spresense imu -->
+  <node pkg="cxd5602pwbimu" exec="wheel_spimu_odom">
+    <param name="odom_topic" value="/odom"/>
+    <param name="imu_topic" value="/imu/spresense/inityaw"/>
+    <param name="publish_TF" value="false"/>
+  </node>
+
   <!-- imu  -->
   <include file="$(find-pkg-share icm_20948)/launch/run.launch.xml">
     <arg name="port" value="/dev/sensors/imu"/>
@@ -83,12 +112,12 @@
   <!-- GPSposition_publisher -->
   <!-- initial heading:housei nakaniwa=179.169287 tsukuba= 276.5 -->
   <!-- GPSheading_publisher -->
-  <node pkg="orange_gnss" exec="gnss_odom_publisher_ttyUSB" output="screen">
+  <node pkg="orange_gnss" exec="gnss_odom_movingbase_fix_publisher_ttyUSB" output="screen">
     <param name="port" value="/dev/sensors/GNSS_UM982"/>
     <param name="baud" value="115200"/>
     <param name="country_id" value="0"/>
     <param name="Position_magnification" value="1.675"/>
-    <param name="heading" value="0.0"/>
+    <param name="heading" value="93.6"/>
     <param name="time_out" value="1.0"/>
   </node>
   <!-- 
@@ -169,11 +198,11 @@
     <arg name="laserscan_topics" value="$(var hokuyo_scan_topic) $(var livox_scan_topic)"/>
   </include>-->
   <!-- led_node -->
-  <node pkg="orange_sensor_tools" exec="led_node">
+  <!--<node pkg="orange_sensor_tools" exec="led_node">
     <param name="use_sim_time" value="$(var use_sim_time)"/>
     <param name="serial_port" value="/dev/sensors/LED"/>
     <param name="baud_rate" value="115200"/>
     <param name="nav_topic" value="$(var nav_topic)"/>
     <param name="estop_topic" value="$(var estop_topic)"/>
-  </node>
+  </node>-->
 </launch>
diff --git a/orange_description/xacro/orange_robot.xacro b/orange_description/xacro/orange_robot.xacro
index 9e7afe4..df1006f 100644
--- a/orange_description/xacro/orange_robot.xacro
+++ b/orange_description/xacro/orange_robot.xacro
@@ -11,7 +11,7 @@
   <!--bringup-->
   <xacro:include filename="$(find orange_description)/xacro/sensors/livox.xacro"/>
   <xacro:sensor_livox name="livox_frame" parent="base_link" min_angle="-2.35619" max_angle="2.35619" samples="720">
-    <origin xyz="-0.0185 0.0 0.8985" rpy="${pi} 0.0 0.0"/>
+    <origin xyz="-0.0185 0.0 0.94" rpy="${pi} 0.0 0.0"/>   
   </xacro:sensor_livox>
   <!--simulation-->
   <!--<xacro:include filename="$(find ros2_livox_simulation)/urdf/mid360.xacro"/>
diff --git a/orange_gnss/orange_gnss/gnss_odom_movingbase_fix_publisher_ttyUSB.py b/orange_gnss/orange_gnss/gnss_odom_movingbase_fix_publisher_ttyUSB.py
index 5582a10..20c0048 100644
--- a/orange_gnss/orange_gnss/gnss_odom_movingbase_fix_publisher_ttyUSB.py
+++ b/orange_gnss/orange_gnss/gnss_odom_movingbase_fix_publisher_ttyUSB.py
@@ -21,7 +21,7 @@ def __init__(self):
         self.declare_parameter('baud', 115200)
         self.declare_parameter('country_id', 0)
         self.declare_parameter('Position_magnification', 1.675)
-        self.declare_parameter('heading', 90.0)
+        self.declare_parameter('heading', 180.0)
 
         self.dev_name = self.get_parameter('port').get_parameter_value().string_value
         self.serial_baud = self.get_parameter('baud').get_parameter_value().integer_value
diff --git a/orange_gnss/orange_gnss/gnss_odom_publisher_ttyUSB.py b/orange_gnss/orange_gnss/gnss_odom_publisher_ttyUSB.py
index a141d80..3aae221 100644
--- a/orange_gnss/orange_gnss/gnss_odom_publisher_ttyUSB.py
+++ b/orange_gnss/orange_gnss/gnss_odom_publisher_ttyUSB.py
@@ -19,14 +19,14 @@ def __init__(self):
         self.declare_parameter('baud', 115200)
         self.declare_parameter('country_id', 0)
         self.declare_parameter('Position_magnification', 1.675)
-        self.declare_parameter('heading', 180.0)
+        self.declare_parameter('heading', 90.0)
 
         self.dev_name = self.get_parameter('port').get_parameter_value().string_value
         self.serial_baud = self.get_parameter('baud').get_parameter_value().integer_value
         self.country_id = self.get_parameter('country_id').get_parameter_value().integer_value
         self.Position_magnification = self.get_parameter('Position_magnification').get_parameter_value().double_value
         #self.theta = self.get_parameter('heading').get_parameter_value().double_value
-        self.tsukuba_theta=180.0 # nakaniwa 180
+        self.tsukuba_theta=90.0 # nakaniwa 180
 
         self.initial_coordinate = None
         self.fix_data = None
@@ -110,7 +110,7 @@ def acquire_gps_data(self):
 
         if count > 0:
             #self.initial_coordinate = [lat_sum / count, lon_sum / count] # calculate average
-            self.initial_coordinate = [35.425952230280004, 139.31380123427] # tsukuba start point 36.0497502, 140.0459234 /nakaniwa 35.4257898377487,139.313807281254
+            self.initial_coordinate = [36.0497502, 140.0459234] # tsukuba start point 36.0497502, 140.0459234 /nakaniwa 35.4257898377487,139.313807281254 /35.425952230280004, 139.31380123427
             self.current_coordinate = [lat_sum / count, lon_sum / count] # for tsukuba
             self.theta = (heading_sum / count) - 90
             self.initialized = True
diff --git a/pcd_convert/pcd_convert/pcd_height_segmentation.py b/pcd_convert/pcd_convert/pcd_height_segmentation.py
index 28d2610..c7bccc6 100644
--- a/pcd_convert/pcd_convert/pcd_height_segmentation.py
+++ b/pcd_convert/pcd_convert/pcd_height_segmentation.py
@@ -41,7 +41,7 @@ def __init__(self):
         
         #パラメータ
         #set obs range
-        self.OBS_HIGHT_MIN =   555/1000; #hight range[m]
+        self.OBS_HIGHT_MIN =   200/1000; #hight range[m]
         self.OBS_HIGHT_MAX =  4000/1000; #hight range[m]
         self.OBS_MASK_X_MIN = -550/1000; #x mask range[m]
         self.OBS_MASK_X_MAX =  400/1000; #x mask range[m]
@@ -51,7 +51,7 @@ def __init__(self):
         self.GROUND_HIGHT_MIN = -10/1000; #hight range[m] # IGVC20250601 -150 -> -10
         self.GROUND_HIGHT_MAX =  150/1000; #hight range[m]
         #set step range
-        self.STEP_HIGHT_MIN =   550/1000; #hight range[m]
+        self.STEP_HIGHT_MIN =   200/1000; #hight range[m]
         self.STEP_HIGHT_MAX =   self.OBS_HIGHT_MIN# 200/1000; #hight range[m]
         self.STEP_X_MIN     = -1500/1000; #x mask range[m]
         self.STEP_X_MAX     =  2000/1000; #x mask range[rosbag2_2024_10_26-03_14_14_20241026_kakunin_bag1m]
diff --git a/pcd_convert/pcd_convert/pcd_rotation_lidar2.py b/pcd_convert/pcd_convert/pcd_rotation_lidar2.py
new file mode 100644
index 0000000..dd5f887
--- /dev/null
+++ b/pcd_convert/pcd_convert/pcd_rotation_lidar2.py
@@ -0,0 +1,232 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+
+# rclpy (ROS 2のpythonクライアント)の機能を使えるようにします。
+import rclpy
+# rclpy (ROS 2のpythonクライアント)の機能のうちNodeを簡単に使えるようにします。こう書いていない場合、Nodeではなくrclpy.node.Nodeと書く必要があります。
+from rclpy.node import Node
+import std_msgs.msg as std_msgs
+import sensor_msgs.msg as sensor_msgs
+import numpy as np
+import math
+from rclpy.qos import QoSProfile, QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy
+import time
+
+
+# C++と同じく、Node型を継承します。
+class PcdRotation(Node):
+    # コンストラクタです、PcdRotationクラスのインスタンスを作成する際に呼び出されます。
+    def __init__(self):
+        # 継承元のクラスを初期化します。
+        super().__init__('pcd_rotation_lidar2_node')
+        
+        qos_profile = QoSProfile(
+            history=QoSHistoryPolicy.KEEP_LAST,
+            reliability=QoSReliabilityPolicy.RELIABLE,
+            durability=QoSDurabilityPolicy.VOLATILE,
+            depth = 1
+        )
+        
+        qos_profile_sub = QoSProfile(
+            history=QoSHistoryPolicy.KEEP_LAST,
+            reliability=QoSReliabilityPolicy.RELIABLE,
+            durability=QoSDurabilityPolicy.VOLATILE,
+            depth = 10
+        )
+        
+        # Subscriptionを作成。 <-- ここを最小限変更して /livox/lidar2 を購読するようにしました
+        self.subscription = self.create_subscription(sensor_msgs.PointCloud2, '/converted_pointcloud2_lidar2', self.pcd_rotation, qos_profile_sub) #set subscribe pcd topic name
+        self.subscription  # 警告を回避するために設置されているだけです。削除しても挙動はかわりません。
+        
+        # Publisherを作成
+        self.pcd_rotation_publisher = self.create_publisher(sensor_msgs.PointCloud2, 'pcd_rotation_lidar2', qos_profile) #set publish pcd topic name
+        
+        #パラメータ
+        #set LiDAR position
+        self.MID360_HIGHT = 980/1000; #hight position[m]
+            
+        #上下反転  LiDAR init
+        self.THETA_INIT_X = -180 #[deg]
+        self.THETA_INIT_Y = 0 #[deg]
+        self.THETA_INIT_Z = 0 #[deg]
+        
+        #initialize calibration
+        self.initialize_calibration = 0
+        self.pcd_buff = np.array([[],[],[],[]]);
+        self.x1_init_point = 1.5
+        self.x2_init_point = 2.5
+        self.y_init_point = 0.0
+        self.x_range = 0.1
+        self.y_range = 0.3
+        
+        
+        
+    def pointcloud2_to_array(self, cloud_msg):
+        # Extract point cloud data
+        points = np.frombuffer(cloud_msg.data, dtype=np.uint8).reshape(-1, cloud_msg.point_step)
+        x = np.frombuffer(points[:, 0:4].tobytes(), dtype=np.float32)
+        y = np.frombuffer(points[:, 4:8].tobytes(), dtype=np.float32)
+        z = np.frombuffer(points[:, 8:12].tobytes(), dtype=np.float32)
+        intensity = np.frombuffer(points[:, 12:16].tobytes(), dtype=np.float32)
+
+        # Combine into a 4xN matrix
+        point_cloud_matrix = np.vstack((x, y, z, intensity))
+        #print(point_cloud_matrix)
+        #print(f"point_cloud_matrix ={point_cloud_matrix.shape}")
+        #print(f"x ={x.dtype, x.shape}")
+        
+        return point_cloud_matrix
+        
+    def pcd_rotation(self, msg):
+        
+        #print stamp message
+        t_stamp = msg.header.stamp
+        #print(f"t_stamp ={t_stamp}")
+        
+        #get pcd data
+        points = self.pointcloud2_to_array(msg)
+        #print(f"points ={points.shape}")
+        #for pcd rotation
+        xyz_point = np.vstack([points[0,:],points[1,:],points[2,:]])
+        #print(f"xyz_point ={xyz_point.shape}")
+        pointcloud, rot_matrix = rotation_xyz(xyz_point, self.THETA_INIT_X, self.THETA_INIT_Y, self.THETA_INIT_Z)
+        #add intensity
+        pointcloud_intensity = np.insert(pointcloud, 3, points[3,:], axis=0)
+        
+        if self.initialize_calibration == 0:
+            pcd_buff_len = 3* 2*10*4
+            self.pcd_buff = np.insert(self.pcd_buff, len(self.pcd_buff[0,:]), pointcloud_intensity.T, axis=1)
+            print(f"len(self.pcd_buff), {self.pcd_buff.shape}")
+            if (len(self.pcd_buff[0,:]) > pcd_buff_len):
+                #self.pcd_buff = self.pcd_buff[:,(len(self.pcd_buff[0,:])-pcd_buff_len):len(self.pcd_buff[0,:])]
+                x1_min = self.x1_init_point - self.x_range;
+                x1_max = self.x1_init_point + self.x_range;
+                x2_min = self.x2_init_point - self.x_range;
+                x2_max = self.x2_init_point + self.x_range;
+                y_min = self.y_init_point  - self.y_range;
+                y_max = self.y_init_point  + self.y_range;
+                #print(f"x1_min,x1_max,x2_min,x2_max,y_min,y_max ={ x1_min, x1_max, x2_min, x2_max, y_min, y_max}")
+                pcd_x1_ind = self.pcd_serch(self.pcd_buff, x1_min, x1_max, y_min, y_max)
+                pcd_x2_ind = self.pcd_serch(self.pcd_buff, x2_min, x2_max, y_min, y_max)
+                #print(f"len(self.pcd_buff[0,pcd_x1_ind]), {len(self.pcd_buff[0,pcd_x1_ind])}")
+                #print(f"len(self.pcd_buff[0,pcd_x2_ind]), {len(self.pcd_buff[0,pcd_x2_ind])}")
+                if len(self.pcd_buff[0,pcd_x1_ind]) & len(self.pcd_buff[0,pcd_x2_ind]) > 0:
+                    pcd_x1 = self.pcd_buff[:,pcd_x1_ind]
+                    pcd_x2 = self.pcd_buff[:,pcd_x2_ind]
+                    #pcd_x1_closest_ind = np.abs(pcd_x1[0,:] - self.x1_init_point).argmin()
+                    z1_median = np.median(pcd_x1[2,:])
+                    pcd_x1_closest_ind = np.abs(pcd_x1[2,:]-z1_median).argmin()
+                    #pcd_x2_closest_ind = np.abs(pcd_x2[0,:] - self.x2_init_point).argmin()
+                    z2_median = np.median(pcd_x2[2,:])
+                    pcd_x2_closest_ind = np.abs(pcd_x2[2,:]-z2_median).argmin()
+                    #z= {slope}x + intercept
+                    slope = (pcd_x2[2,pcd_x2_closest_ind] - pcd_x1[2,pcd_x1_closest_ind]) / (pcd_x2[0,pcd_x2_closest_ind] - pcd_x1[0,pcd_x1_closest_ind])
+                    intercept = pcd_x1[2,pcd_x1_closest_ind] - slope*pcd_x1[0,pcd_x1_closest_ind]
+                    
+                    
+                    print(f"pcd_x1 x,y,z ={pcd_x1[0,pcd_x1_closest_ind], pcd_x1[1,pcd_x1_closest_ind], pcd_x1[2,pcd_x1_closest_ind]}")
+                    print(f"pcd_x2 x,y,z ={pcd_x2[0,pcd_x2_closest_ind], pcd_x2[1,pcd_x2_closest_ind], pcd_x2[2,pcd_x2_closest_ind]}")
+                    delta_x = pcd_x2[0,pcd_x2_closest_ind] - 0
+                    delta_z = - (intercept - pcd_x2[2,pcd_x2_closest_ind]) 
+                    theta = np.arctan2(delta_z, delta_x) /math.pi*180
+                    print(f"delta_x ={delta_x}")
+                    print(f"delta_z ={delta_z}")
+                    print(f"slope ={slope}")
+                    print(f"intercept ={intercept}")
+                    print(f"theta ={theta}")
+                    self.MID360_HIGHT = - intercept; #hight position[m]
+                    self.THETA_INIT_Y = self.THETA_INIT_Y + theta #[deg]
+                    self.initialize_calibration = 1
+                    print(f"self.MID360_HIGHT ={self.MID360_HIGHT}")
+                    
+        
+        #add mid height position
+        pointcloud_intensity[2,:] += self.MID360_HIGHT
+        #print(f"pointcloud_intensity ={pointcloud_intensity.shape}")
+        
+        #publish for rviz2
+        if self.initialize_calibration == 1:
+            self.pcd_rotation = point_cloud_intensity_msg(pointcloud_intensity.T, t_stamp, 'map')
+            self.pcd_rotation_publisher.publish(self.pcd_rotation ) 
+        
+    def pcd_serch(self, pointcloud, x_min, x_max, y_min, y_max):
+        pcd_ind_x = ((x_min <= pointcloud[0,:]) * (pointcloud[0,:] <= x_max )) 
+        pcd_ind_y = ((y_min <= pointcloud[1,:]) * (pointcloud[1,:] <= y_max ))
+        pcd_ind = pcd_ind_x * pcd_ind_y
+        return pcd_ind
+        
+def rotation_xyz(pointcloud, theta_x, theta_y, theta_z):
+    rad_x = math.radians(theta_x)
+    rad_y = math.radians(theta_y)
+    rad_z = math.radians(theta_z)
+    rot_x = np.array([[ 1,               0,                0],
+                      [ 0, math.cos(rad_x), -math.sin(rad_x)],
+                      [ 0, math.sin(rad_x),  math.cos(rad_x)]])
+    
+    rot_y = np.array([[ math.cos(rad_y), 0,  math.sin(rad_y)],
+                      [               0, 1,                0],
+                      [-math.sin(rad_y), 0,  math.cos(rad_y)]])
+    
+    rot_z = np.array([[ math.cos(rad_z), -math.sin(rad_z), 0],
+                      [ math.sin(rad_z),  math.cos(rad_z), 0],
+                      [               0,                0, 1]])
+    rot_matrix = rot_z.dot(rot_y.dot(rot_x))
+    #print(f"rot_matrix ={rot_matrix}")
+    #print(f"pointcloud ={pointcloud.shape}")
+    rot_pointcloud = rot_matrix.dot(pointcloud)
+    return rot_pointcloud, rot_matrix
+
+def point_cloud_intensity_msg(points, t_stamp, parent_frame):
+    # In a PointCloud2 message, the point cloud is stored as an byte 
+    # array. In order to unpack it, we also include some parameters 
+    # which desribes the size of each individual point.
+    ros_dtype = sensor_msgs.PointField.FLOAT32
+    dtype = np.float32
+    itemsize = np.dtype(dtype).itemsize # A 32-bit float takes 4 bytes.
+    data = points.astype(dtype).tobytes() 
+
+    # The fields specify what the bytes represents. The first 4 bytes 
+    # represents the x-coordinate, the next 4 the y-coordinate, etc.
+    fields = [
+            sensor_msgs.PointField(name='x', offset=0, datatype=ros_dtype, count=1),
+            sensor_msgs.PointField(name='y', offset=4, datatype=ros_dtype, count=1),
+            sensor_msgs.PointField(name='z', offset=8, datatype=ros_dtype, count=1),
+            sensor_msgs.PointField(name='intensity', offset=12, datatype=ros_dtype, count=1),
+        ]
+
+    # The PointCloud2 message also has a header which specifies which 
+    # coordinate frame it is represented in. 
+    header = std_msgs.Header(frame_id=parent_frame, stamp=t_stamp)
+    
+
+    return sensor_msgs.PointCloud2(
+        header=header,
+        height=1, 
+        width=points.shape[0],
+        is_dense=True,
+        is_bigendian=False,
+        fields=fields,
+        point_step=(itemsize * 4), # Every point consists of three float32s.
+        row_step=(itemsize * 4 * points.shape[0]), 
+        data=data
+    )
+
+# mainという名前の関数です。C++のmain関数とは異なり、これは処理の開始地点ではありません。
+def main(args=None):
+    # rclpyの初期化処理です。ノードを立ち上げる前に実装する必要があります。
+    rclpy.init(args=args)
+    # クラスのインスタンスを作成
+    pcd_rotation = PcdRotation()
+    # spin処理を実行、spinをしていないとROS 2のノードはデータを入出力することが出来ません。
+    rclpy.spin(pcd_rotation)
+    # 明示的にノードの終了処理を行います。
+    pcd_rotation.destroy_node()
+    # rclpyの終了処理、これがないと適切にノードが破棄されないため様々な不具合が起こります。
+    rclpy.shutdown()
+
+# 本スクリプト(publish.py)の処理の開始地点です。
+if __name__ == '__main__':
+    # 関数`main`を実行する。
+    main()
+
+
diff --git a/pcd_convert/setup.py b/pcd_convert/setup.py
index b9be1a8..f721c98 100644
--- a/pcd_convert/setup.py
+++ b/pcd_convert/setup.py
@@ -45,7 +45,8 @@
     entry_points={
         'console_scripts': [
             'pcd_rotation = pcd_convert.pcd_rotation:main',
-            'pcd_height_segmentation = pcd_convert.pcd_height_segmentation:main'
+            'pcd_height_segmentation = pcd_convert.pcd_height_segmentation:main',
+            'pcd_rotation_lidar2 = pcd_convert.pcd_rotation_lidar2:main'
         ],
     },
 )
diff --git a/try_navigation/launch/try_navigation.launch.py b/try_navigation/launch/try_navigation.launch.py
index e4591ed..62d5821 100644
--- a/try_navigation/launch/try_navigation.launch.py
+++ b/try_navigation/launch/try_navigation.launch.py
@@ -103,12 +103,12 @@ def generate_launch_description():
             arguments=[],
         ),
         #takamori Autonav
-        Node(package='try_navigation',
-            executable='reflection_to_pcd',
-            name='reflection_to_pcd',
-            output='screen',
-            arguments=[],
-        ),
+        #Node(package='try_navigation',
+        #    executable='reflection_to_pcd',
+        #    name='reflection_to_pcd',
+        #    output='screen',
+        #    arguments=[],
+        #),
         #takamori Selfdrive
         #Node(package='try_navigation',
         #    executable='self_drive_line',
diff --git a/try_navigation/try_navigation/ekf_myself_gps.py b/try_navigation/try_navigation/ekf_myself_gps.py
index c7b2699..9f411ab 100644
--- a/try_navigation/try_navigation/ekf_myself_gps.py
+++ b/try_navigation/try_navigation/ekf_myself_gps.py
@@ -67,7 +67,7 @@ def __init__(self):
         
 
         self.sub_a = self.create_subscription(
-            Odometry, '/odom', self.sensor_a_callback, 10)
+            Odometry, '/odom/wheel_spimu', self.sensor_a_callback, 10) # /odom/wheel_spimu
         self.sub_b = self.create_subscription(
             Odometry, '/odom/UM982', self.sensor_b_callback, 10)
         #self.sub_b = self.create_subscription(
@@ -163,10 +163,11 @@ def sensor_b_callback(self, data):
 
     def determination_of_R(self):
         if self.GpsXY is not None:
-            if (-30<self.GpsXY[0]) and (self.GpsXY[0]<90) and (-120 < self.GpsXY[1]) and (self.GpsXY[1]<-40):
-                self.gps_rr_flag = 1
-                #self.gps_rr_flag = 0
+            if (-38<self.GpsXY[0]) and (self.GpsXY[0]<60) and (25 < self.GpsXY[1]) and (self.GpsXY[1]<110):
+                #self.gps_rr_flag = 1
+                self.gps_rr_flag = 0
                 #self.offsetyaw_bad_gps = -10/180*math.pi
+                self.GPS_angle_conut = 0; ## tukuba 20250920 zantei
             else:
                 self.gps_rr_flag = 1
                 self.offsetyaw_bad_gps = 0
diff --git a/try_navigation/try_navigation/path_follower.py b/try_navigation/try_navigation/path_follower.py
index f07f857..abd6b5e 100644
--- a/try_navigation/try_navigation/path_follower.py
+++ b/try_navigation/try_navigation/path_follower.py
@@ -42,11 +42,11 @@ def __init__(self):
         
         # Subscriptionを作成。
         self.subscription = self.create_subscription(nav_msgs.Path, '/potential_astar_path', self.get_path, qos_profile) #set subscribe pcd topic name
-        #self.subscription = self.create_subscription(nav_msgs.Odometry,'/odom/wheel_imu', self.get_odom, qos_profile_sub)
+        #self.subscription = self.create_subscription(nav_msgs.Odometry,'/odom/wheel_imu', self.get_odom, qos_profile_sub) # /odom/wheel_spimu
         self.subscription = self.create_subscription(nav_msgs.Odometry,'/fusion/odom', self.get_odom, qos_profile_sub)
         #self.subscription = self.create_subscription(nav_msgs.Odometry,'/odom_ekf_match', self.get_odom, qos_profile_sub)
         #self.subscription = self.create_subscription(nav_msgs.Odometry,'/odom_ref_slam', self.get_odom_ref, qos_profile_sub)
-        self.subscription = self.create_subscription(nav_msgs.Odometry,'/fusion/odom', self.get_odom_ref, qos_profile_sub)
+        self.subscription = self.create_subscription(nav_msgs.Odometry,'/fusion/odom', self.get_odom_ref, qos_profile_sub) #/fusion/odom
         self.subscription = self.create_subscription(sensor_msgs.PointCloud2, '/pcd_segment_obs', self.obs_steer, qos_profile)
         self.goal_sub = self.create_subscription(PoseStamped, '/goal_pose', self.goal_pose_callback, qos_profile)
         self.stop_sub = self.create_subscription(String, '/stop_sign_status', self.stop_sign_callback, 10)
@@ -366,7 +366,7 @@ def robot_ctrl(self):
         if abs(target_theta) < 10:
             if 0.0 < speed:
                 if speed < 0.5:
-                    speed = 1.1 # autonav 0.8
+                    speed = 0.5 # autonav 0.8
         #elif target_theta  < -lim_steer:
         if target_theta  < -lim_steer:
             speed = 0.10
diff --git a/try_navigation/try_navigation/potential_astar.py b/try_navigation/try_navigation/potential_astar.py
index d55fbfb..b78d173 100644
--- a/try_navigation/try_navigation/potential_astar.py
+++ b/try_navigation/try_navigation/potential_astar.py
@@ -61,7 +61,7 @@ def __init__(self):
         # Subscriptionを作成。CustomMsg型,'/livox/lidar'という名前のtopicをsubscribe。
         self.subscription = self.create_subscription(sensor_msgs.PointCloud2, '/pcd_segment_obs', self.potential_astar, qos_profile)
         #self.subscription = self.create_subscription(nav_msgs.Odometry,'/odom/wheel_imu', self.get_odom, qos_profile_sub)
-        self.subscription = self.create_subscription(nav_msgs.Odometry,'/fusion/odom', self.get_odom, qos_profile_sub)
+        self.subscription = self.create_subscription(nav_msgs.Odometry,'/fusion/odom', self.get_odom, qos_profile_sub) # /odom/wheel_spimu
         #self.subscription = self.create_subscription(nav_msgs.Odometry,'/odom_fast', self.get_odom, qos_profile_sub)
         #self.subscription = self.create_subscription(nav_msgs.Odometry,'/odom_ekf_match', self.get_odom, qos_profile_sub)
         self.subscription = self.create_subscription(geometry_msgs.PoseArray,'/current_waypoint', self.get_waypoint, qos_profile_sub)

From 1cc0abf409a24165973d24ac40aadf319ea3cbd3 Mon Sep 17 00:00:00 2001
From: kugurofu <tenten31569@icloud.com>
Date: Mon, 22 Sep 2025 05:00:16 +0000
Subject: [PATCH 04/25] change gps_odom fixed value

---
 ...ss_odom_movingbase_fix_publisher_ttyUSB.py | 19 +++++++++++++++----
 1 file changed, 15 insertions(+), 4 deletions(-)

diff --git a/orange_gnss/orange_gnss/gnss_odom_movingbase_fix_publisher_ttyUSB.py b/orange_gnss/orange_gnss/gnss_odom_movingbase_fix_publisher_ttyUSB.py
index 20c0048..1d4ccdf 100644
--- a/orange_gnss/orange_gnss/gnss_odom_movingbase_fix_publisher_ttyUSB.py
+++ b/orange_gnss/orange_gnss/gnss_odom_movingbase_fix_publisher_ttyUSB.py
@@ -21,13 +21,14 @@ def __init__(self):
         self.declare_parameter('baud', 115200)
         self.declare_parameter('country_id', 0)
         self.declare_parameter('Position_magnification', 1.675)
-        self.declare_parameter('heading', 180.0)
+        self.declare_parameter('heading', 90.0)
 
         self.dev_name = self.get_parameter('port').get_parameter_value().string_value
         self.serial_baud = self.get_parameter('baud').get_parameter_value().integer_value
         self.country_id = self.get_parameter('country_id').get_parameter_value().integer_value
         self.Position_magnification = self.get_parameter('Position_magnification').get_parameter_value().double_value
-        self.theta = self.get_parameter('heading').get_parameter_value().double_value
+        #self.theta = self.get_parameter('heading').get_parameter_value().double_value
+        self.tsukuba_theta = 90.0 # tsukuba 90.0 / nakaniwa 180.0
         
         #self.theta = 275.6 # tukuba param
         #self.theta = 180 #nakaniwa param
@@ -73,7 +74,11 @@ def send_request(self):
         request = Avglatlon.Request()
         request.avg_lat = self.initial_coordinate[0]  # ← average lat
         request.avg_lon = self.initial_coordinate[1]  # ← average lon
-        request.theta = self.theta
+        request.current_lat = self.currennt_coordinate[0]  # ← currennt lat
+        request.current_lon = self.currennt_coordinate[1]  # ← currennt lon
+        #request.theta = self.theta
+        request.theta = self.tsukuba_theta # tsukuba start theta
+        request.current_theta = self.theta # for tsukuba
 
         future = self.client.call_async(request)
         future.add_done_callback(self.response_callback)
@@ -124,13 +129,19 @@ def acquire_gps_data(self):
             if GPS_data and GPS_data[1] != 0 and GPS_data[2] != 0:
                 lat_sum += GPS_data[1]
                 lon_sum += GPS_data[2]
+                heading_sum += floatGPS_data[5]
                 count += 1
             time.sleep(0.1)  # Slight delay to avoid overwhelming the GPS device
 
         if count > 0:
-            self.initial_coordinate = [lat_sum / count, lon_sum / count]
+            #self.initial_coordinate = [lat_sum / count, lon_sum / count] # calculate average
+            self.initial_coordinate = [36.0497399536, 140.04593714523637] # tsukuba start point 36.0497502, 140.0459234 / nakaniwa 35.425952230280004, 139.31380123427
+            self.current_coordinate = [lat_sum / count, lon_sum / count] # for tsukuba
+            self.theta = (heading_sum / count) - 90
             self.initialized = True
             self.get_logger().info(f"Initial coordinate set to: {self.initial_coordinate}")
+            self.get_logger().info(f"current coordinate set to: {self.current_coordinate}")
+            self.get_logger().info(f"Initial theta set to: {self.theta}")
             self.send_request()
         self.is_acquiring = False
 

From 553688e5ec5117a3f3ff16f062967483d08f6551 Mon Sep 17 00:00:00 2001
From: kugurofu <tenten31569@icloud.com>
Date: Mon, 29 Sep 2025 11:16:09 +0000
Subject: [PATCH 05/25] add comment

---
 .../try_navigation/ekf_myself_gps.py          | 36 ++++++++++++++-----
 1 file changed, 28 insertions(+), 8 deletions(-)

diff --git a/try_navigation/try_navigation/ekf_myself_gps.py b/try_navigation/try_navigation/ekf_myself_gps.py
index 9f411ab..180fbc4 100644
--- a/try_navigation/try_navigation/ekf_myself_gps.py
+++ b/try_navigation/try_navigation/ekf_myself_gps.py
@@ -163,14 +163,16 @@ def sensor_b_callback(self, data):
 
     def determination_of_R(self):
         if self.GpsXY is not None:
+            #カルマンフィルタのデッドレコニングよりになる範囲を指定
             if (-38<self.GpsXY[0]) and (self.GpsXY[0]<60) and (25 < self.GpsXY[1]) and (self.GpsXY[1]<110):
                 #self.gps_rr_flag = 1
-                self.gps_rr_flag = 0
+                self.gps_rr_flag = 0 #self.gps_rr_flag =1はGPS受信精度よく、0でGPS受信精度低い範囲に入ったフラグ
                 #self.offsetyaw_bad_gps = -10/180*math.pi
-                self.GPS_angle_conut = 0; ## tukuba 20250920 zantei
+                #self.GPS_angle_conut = 0; ## tukuba 20250920 zantei
             else:
                 self.gps_rr_flag = 1
                 self.offsetyaw_bad_gps = 0
+        #GPS受信精度が悪い場合のR指定 
         if self.gps_rr_flag == 0:  # Bad
             #self.R1 = 0.17**2  # FAST-LIO
             #self.R2 = 0.17**2  # CLAS-movingbase
@@ -180,7 +182,9 @@ def determination_of_R(self):
             self.R4 = 1     # GPStheta
             #######edit#########
             self.RR_count_bad = 50
+        #GPS受信精度が良い場合のR指定 
         else:
+            #GPS受信精度が良くなってもすぐには切り替えず、一定回数（self.RR_caount_badが0以下になるまで）になるまでの暫定R指定とよくなった場合のR指定 
             self.RR_count_bad += -1
             if self.RR_count_bad <= 0:
                 #self.R1 = 0.05**2  # FAST-LIO
@@ -400,6 +404,7 @@ def calculate_offset(self, combyaw, GTheta, GPStheta):
         return self.GOffset
 
     def publish_fused_value(self):
+        #速度情報（Odomなど）＋サンプリングタイム（同ファイル別関数で設定）＋角度情報が揃ったらスタート
         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]
@@ -408,21 +413,24 @@ def publish_fused_value(self):
             self.R4 = R[3]
             self.get_logger().info(f"++ RR_count_bad: {self.RR_count_bad}++")
             
-            #if self.Number_of_satellites >= 28: #################### IGVC20250529 use  #######################
-            if self.Number_of_satellites >= self.set_yaw_satellites_no: #################### IGVC20250530 change  #######################
+            #if self.Number_of_satellites >= 28: 
+            #GPSの受信衛星整数が多い（≒精度が良い？としている）場合に角度補正を行う
+            #現状はカルマンフィルタとは別に、受信精度がいい時のGPS方位とオドメトリの角度の値をストックし、
+            #そのストックした値の差分の中央値（GPS方位-オドメトリ方位　の10秒分（100回/10Hz））のデータの中央値）を
+            #オフセット角度として保存・更新し、足して補正してる状態。
+            if self.Number_of_satellites >= self.set_yaw_satellites_no and self.gps_rr_flag == 1:
                 self.GPS_angle_conut += 1
                 self.GPS_angle_reset_count = 0
                 yaw_offset1 = self.GPSYaw % (360/180*math.pi)
                 yaw_offset2 = self.GTheta % (360/180*math.pi)
                 yaw_offset_val = yaw_offset1 - yaw_offset2
                 self.diff_yaw_buff.append(yaw_offset_val)
-                 
             else:
                 self.GPS_angle_reset_count += 1
                 if self.GPS_angle_reset_count > 20:
                     self.GPS_angle_conut = 0
                     self.GPS_angle_reset_count = 0
-
+            #一定回数（self.set_yaw_count）以上になったらカウントリセットし、角度差分の中央値をオフセットとして補正
             if self.GPS_angle_conut > self.set_yaw_count:
                 yaw_offset_list = list(self.diff_yaw_buff)
                 yaw_offset_median = sorted(yaw_offset_list)[len(yaw_offset_list) // 2]
@@ -435,6 +443,7 @@ def publish_fused_value(self):
                 self.diff_yaw_buff = []
                 self.get_logger().info(f"!!!!!!!!!!!!!!!!!!!!!! offsetyaw: {self.offsetyaw}!!!!!!!!!!!!!!!!!!!!!!!!!!")
                     
+            #ここからカルマンフィルタ　:GPS情報が受信できている（Noneではない）＆受信精度が良い区間（sefl.gps_rr_flag=1）時に処理を行う
             if self.GpsXY is not None and self.gps_rr_flag:#self.Number_of_satellites > #22 :
                 self.get_logger().info(f"!!!!++++++++++ Number_of_satellites: {self.Number_of_satellites}++++++++++!!!!!!!!!")
                 ########## Change the written line #######
@@ -451,7 +460,7 @@ def publish_fused_value(self):
                 #        self.combyaw, self.GTheta, self.GPStheta)
                 #########################
                 
-                        
+                #角度はodom等の入力角度に補正。(カルマンフィルタで角度の計算は行っていない :カルマンフィルタの計算では入力と出力で同じ値）
                 ######### edit ###########
                 #self.robot_yaw = self.GTheta + self.offsetyaw
                 yaw1 = self.GTheta % (360/180*math.pi)
@@ -465,7 +474,9 @@ def publish_fused_value(self):
                     self.robot_yaw -= 2 * np.pi
                 
                 ########### Change the written line #######
+                #kalf_speed_paramは速度の神の手調整。速度が正しければ補正はいらないはず。
                 kalf_speed = self.Speed * self.kalf_speed_param
+                #カルマンフィルタ関数実行
                 fused_value = self.KalfGPSXY(
                     kalf_speed, self.SmpTime, self.robot_yaw, self.GpsXY, self.R1, self.R2)    
                 ###########################################
@@ -475,6 +486,7 @@ def publish_fused_value(self):
                 #    kalf_speed, self.SmpTime, self.robot_yaw, self.R1, self.R2)
                 ########################################## 
                 
+                #カルマンフィルタ結果(fused_value)のｘｙ座標反映。カルマンフィルタで角度の計算は行っていないので、角度オフセット補正した値のまま。
                 robot_orientation = self.yaw_to_orientation(self.robot_yaw)
                 self.robot_orientationz = robot_orientation[0]
                 self.robot_orientationw = robot_orientation[1]
@@ -489,13 +501,15 @@ def publish_fused_value(self):
                 ######## gps reset #########
                 #self.GpsXY = None
                 ############################
-                    
+                
+            #GPS情報が受信できていない時の処理   
             else:
                 ########## Change the written line #######
                 #fused_value = self.KalfXY(
                 #    self.Speed, self.SmpTime, self.GTheta, self.R1, self.R2)
                 ##########################################
                 
+                #角度はodom等の入力角度に補正。(カルマンフィルタで角度の計算は行っていない :カルマンフィルタの計算では入力と出力で同じ値）
                 ######### edit ###########
                 #self.robot_yaw = self.GTheta + self.offsetyaw
                 yaw1 = self.GTheta % (360/180*math.pi)
@@ -508,11 +522,14 @@ def publish_fused_value(self):
                     self.robot_yaw -= 2 * np.pi
                 
                 ########## Change the written line #######
+                #kalf_speed_paramは速度の神の手調整。速度が正しければ補正はいらないはず。
                 kalf_speed = self.Speed * self.kalf_speed_param
+                #GPSなし持カルマンフィルタ関数実行（GPSデータなしの場合のため、デッドレコニングのみ）
                 fused_value = self.KalfXY(
                     kalf_speed, self.SmpTime, self.robot_yaw, self.R1, self.R2)
                 ##########################################    
                 
+                #カルマンフィルタ結果(fused_value)のｘｙ座標反映。カルマンフィルタで角度の計算は行っていないので、角度オフセット補正した値のまま。
                 robot_orientation = self.yaw_to_orientation(self.robot_yaw)
                 self.robot_orientationz = robot_orientation[0]
                 self.robot_orientationw = robot_orientation[1]
@@ -524,6 +541,9 @@ def publish_fused_value(self):
                 self.fused_msg.pose.pose.orientation.w = float(
                     self.robot_orientationw)
                 
+                
+                #GPS受信精度が悪い場合は角度オフセットカウントリセット（一定回数以上GPS受信ができている場合のみ角度オフセット実施）
+                #2秒以上(self.GPS_angle_reset_count > 20 : 20/10Hz)GPSがいいの来なかったらリセット
                 self.GPS_angle_reset_count += 1
                 if self.GPS_angle_reset_count > 20:
                     self.GPS_angle_conut = 0

From a2e9d70db8e766cf74408e505a0123edf5a9dfb0 Mon Sep 17 00:00:00 2001
From: kugurofu <tenten31569@icloud.com>
Date: Tue, 30 Sep 2025 06:13:26 +0000
Subject: [PATCH 06/25] add satelite count information

---
 .../orange_gnss/gnss_odom_movingbase_fix_publisher_ttyUSB.py  | 4 +++-
 try_navigation/try_navigation/path_follower.py                | 2 +-
 2 files changed, 4 insertions(+), 2 deletions(-)

diff --git a/orange_gnss/orange_gnss/gnss_odom_movingbase_fix_publisher_ttyUSB.py b/orange_gnss/orange_gnss/gnss_odom_movingbase_fix_publisher_ttyUSB.py
index 1d4ccdf..df366f5 100644
--- a/orange_gnss/orange_gnss/gnss_odom_movingbase_fix_publisher_ttyUSB.py
+++ b/orange_gnss/orange_gnss/gnss_odom_movingbase_fix_publisher_ttyUSB.py
@@ -101,7 +101,7 @@ def timer_callback(self):
         self.gps_data_cache = self.get_gps_quat(self.dev_name, self.country_id)
 
         if self.gps_data_cache:
-            fix_type, lat, lon, alt, _, heading = self.gps_data_cache
+            fix_type, lat, lon, alt, satelitecount_data, heading = self.gps_data_cache
             if fix_type != 0:
                 self.publish_fix(self.gps_data_cache)
                 self.publish_odom(lat, lon, alt)
@@ -369,6 +369,8 @@ def publish_fix(self, gps):
         self.lonlat_msg.latitude = gps[1]
         self.lonlat_msg.longitude = gps[2]
         self.lonlat_msg.altitude = gps[3]
+        
+        self.lonlat_msg.status.service = gps[4] # satelitecount
 
         self.lonlat_pub.publish(self.lonlat_msg)
             # self.get_logger().info(f"Published GPS data: {lonlat}")
diff --git a/try_navigation/try_navigation/path_follower.py b/try_navigation/try_navigation/path_follower.py
index abd6b5e..87bfcce 100644
--- a/try_navigation/try_navigation/path_follower.py
+++ b/try_navigation/try_navigation/path_follower.py
@@ -131,7 +131,7 @@ def __init__(self):
             #[-69.5,	-62,	-47.5,	-27.5, 1.0], #tyokusen4 ||| [-67.5,	-62,	-47.5,	-27.5, 1.0]before:[-67.0,	-62,	-47.5,	-27.5, 1.0], 
             #[-55,	-35,	41,	46,    1.0], #Goal
             
-            
+            [63.0, 65.0, 19.0, 39.0, 1.0], #shiyakusyo
             [ 999,  999, 999, 999, 0.0] ]) #
         self.stop_num = 0;
         

From 8edecba43fec2524ae1140499faf9e4484e935e2 Mon Sep 17 00:00:00 2001
From: kugurofu <tenten31569@icloud.com>
Date: Tue, 30 Sep 2025 07:33:53 +0000
Subject: [PATCH 07/25] delete heading sum

---
 ...ss_odom_movingbase_fix_publisher_ttyUSB.py | 28 +++++++++----------
 1 file changed, 14 insertions(+), 14 deletions(-)

diff --git a/orange_gnss/orange_gnss/gnss_odom_movingbase_fix_publisher_ttyUSB.py b/orange_gnss/orange_gnss/gnss_odom_movingbase_fix_publisher_ttyUSB.py
index df366f5..37d8f07 100644
--- a/orange_gnss/orange_gnss/gnss_odom_movingbase_fix_publisher_ttyUSB.py
+++ b/orange_gnss/orange_gnss/gnss_odom_movingbase_fix_publisher_ttyUSB.py
@@ -27,8 +27,8 @@ def __init__(self):
         self.serial_baud = self.get_parameter('baud').get_parameter_value().integer_value
         self.country_id = self.get_parameter('country_id').get_parameter_value().integer_value
         self.Position_magnification = self.get_parameter('Position_magnification').get_parameter_value().double_value
-        #self.theta = self.get_parameter('heading').get_parameter_value().double_value
-        self.tsukuba_theta = 90.0 # tsukuba 90.0 / nakaniwa 180.0
+        self.theta = self.get_parameter('heading').get_parameter_value().double_value
+        #self.tsukuba_theta = 90.0 # tsukuba 90.0 / nakaniwa 180.0
         
         #self.theta = 275.6 # tukuba param
         #self.theta = 180 #nakaniwa param
@@ -74,11 +74,11 @@ def send_request(self):
         request = Avglatlon.Request()
         request.avg_lat = self.initial_coordinate[0]  # ← average lat
         request.avg_lon = self.initial_coordinate[1]  # ← average lon
-        request.current_lat = self.currennt_coordinate[0]  # ← currennt lat
-        request.current_lon = self.currennt_coordinate[1]  # ← currennt lon
-        #request.theta = self.theta
-        request.theta = self.tsukuba_theta # tsukuba start theta
-        request.current_theta = self.theta # for tsukuba
+        #request.current_lat = self.currennt_coordinate[0]  # ← currennt lat
+        #request.current_lon = self.currennt_coordinate[1]  # ← currennt lon
+        request.theta = self.theta
+        #request.theta = self.tsukuba_theta # tsukuba start theta
+        #request.current_theta = self.theta # for tsukuba
 
         future = self.client.call_async(request)
         future.add_done_callback(self.response_callback)
@@ -129,19 +129,19 @@ def acquire_gps_data(self):
             if GPS_data and GPS_data[1] != 0 and GPS_data[2] != 0:
                 lat_sum += GPS_data[1]
                 lon_sum += GPS_data[2]
-                heading_sum += floatGPS_data[5]
+                #heading_sum += float(GPS_data[5])
                 count += 1
             time.sleep(0.1)  # Slight delay to avoid overwhelming the GPS device
 
         if count > 0:
-            #self.initial_coordinate = [lat_sum / count, lon_sum / count] # calculate average
-            self.initial_coordinate = [36.0497399536, 140.04593714523637] # tsukuba start point 36.0497502, 140.0459234 / nakaniwa 35.425952230280004, 139.31380123427
-            self.current_coordinate = [lat_sum / count, lon_sum / count] # for tsukuba
-            self.theta = (heading_sum / count) - 90
+            self.initial_coordinate = [lat_sum / count, lon_sum / count] # calculate average
+            #self.initial_coordinate = [36.0497399536, 140.04593714523637] # tsukuba start point 36.0497502, 140.0459234 / nakaniwa 35.425952230280004, 139.31380123427
+            #self.current_coordinate = [lat_sum / count, lon_sum / count] # for tsukuba
+            #self.theta = (heading_sum / count) - 90
             self.initialized = True
             self.get_logger().info(f"Initial coordinate set to: {self.initial_coordinate}")
-            self.get_logger().info(f"current coordinate set to: {self.current_coordinate}")
-            self.get_logger().info(f"Initial theta set to: {self.theta}")
+            #self.get_logger().info(f"current coordinate set to: {self.current_coordinate}")
+            #self.get_logger().info(f"Initial theta set to: {self.theta}")
             self.send_request()
         self.is_acquiring = False
 

From eeb2f2c2e88f4a501c77df6e959477c99f32aac7 Mon Sep 17 00:00:00 2001
From: kugurofu <tenten31569@icloud.com>
Date: Tue, 30 Sep 2025 07:59:03 +0000
Subject: [PATCH 08/25] add lidar 2 description in xacro file

---
 orange_description/xacro/orange_robot.xacro | 7 +++++++
 1 file changed, 7 insertions(+)

diff --git a/orange_description/xacro/orange_robot.xacro b/orange_description/xacro/orange_robot.xacro
index df1006f..ba8393f 100644
--- a/orange_description/xacro/orange_robot.xacro
+++ b/orange_description/xacro/orange_robot.xacro
@@ -7,17 +7,24 @@
   <xacro:sensor_hokuyo name="hokuyo" parent="base_link" min_angle="-1.22" max_angle="1.22" samples="720">
     <origin xyz="0.1075 0.0 0.1085" rpy="0.0 0.0 0.0"/>
   </xacro:sensor_hokuyo>
+  
   <!--import 3D LiDAR model-->
   <!--bringup-->
   <xacro:include filename="$(find orange_description)/xacro/sensors/livox.xacro"/>
   <xacro:sensor_livox name="livox_frame" parent="base_link" min_angle="-2.35619" max_angle="2.35619" samples="720">
     <origin xyz="-0.0185 0.0 0.94" rpy="${pi} 0.0 0.0"/>   
   </xacro:sensor_livox>
+  
+  <xacro:sensor_livox name="livox_under" parent="base_link" min_angle="-2.35619" max_angle="2.35619" samples="720">
+    <origin xyz="-0.0185 0.0 0.67" rpy="${pi} 0.0 0.0"/>   
+  </xacro:sensor_livox>
+  
   <!--simulation-->
   <!--<xacro:include filename="$(find ros2_livox_simulation)/urdf/mid360.xacro"/>
   <xacro:mid360 name="livox" parent="base_link" topic="mid360">
     <origin xyz="-0.0185 0.0 0.8985" rpy="${pi} 0.0 0.0"/>
   </xacro:mid360>-->
+  
   <!--import imu model-->
   <xacro:include filename="$(find orange_description)/xacro/sensors/imu.xacro"/>
   <xacro:sensor_imu name="imu" parent="base_link" size="0.05 0.05 0.05">

From 4bd815fd0359d9fe0eae5348859501ea075a233d Mon Sep 17 00:00:00 2001
From: kugurofu <tenten31569@icloud.com>
Date: Tue, 7 Oct 2025 06:00:51 +0000
Subject: [PATCH 09/25] add map obs

---
 orange_bringup/launch/orange_robot.launch.xml  |  2 +-
 .../orange_gnss/gnss_odom_publisher_ttyUSB.py  |  2 +-
 .../try_navigation/potential_astar.py          | 18 ++++++++++++++++--
 3 files changed, 18 insertions(+), 4 deletions(-)

diff --git a/orange_bringup/launch/orange_robot.launch.xml b/orange_bringup/launch/orange_robot.launch.xml
index c072026..341a8cc 100644
--- a/orange_bringup/launch/orange_robot.launch.xml
+++ b/orange_bringup/launch/orange_robot.launch.xml
@@ -117,7 +117,7 @@
     <param name="baud" value="115200"/>
     <param name="country_id" value="0"/>
     <param name="Position_magnification" value="1.675"/>
-    <param name="heading" value="93.6"/>
+    <param name="heading" value="93.0"/>
     <param name="time_out" value="1.0"/>
   </node>
   <!-- 
diff --git a/orange_gnss/orange_gnss/gnss_odom_publisher_ttyUSB.py b/orange_gnss/orange_gnss/gnss_odom_publisher_ttyUSB.py
index 3aae221..a6d4926 100644
--- a/orange_gnss/orange_gnss/gnss_odom_publisher_ttyUSB.py
+++ b/orange_gnss/orange_gnss/gnss_odom_publisher_ttyUSB.py
@@ -110,7 +110,7 @@ def acquire_gps_data(self):
 
         if count > 0:
             #self.initial_coordinate = [lat_sum / count, lon_sum / count] # calculate average
-            self.initial_coordinate = [36.0497502, 140.0459234] # tsukuba start point 36.0497502, 140.0459234 /nakaniwa 35.4257898377487,139.313807281254 /35.425952230280004, 139.31380123427
+            self.initial_coordinate = [36.0497440979, 140.045935093] # tsukuba start point 36.0497502, 140.0459234 /nakaniwa 35.4257898377487,139.313807281254 /35.425952230280004, 139.31380123427
             self.current_coordinate = [lat_sum / count, lon_sum / count] # for tsukuba
             self.theta = (heading_sum / count) - 90
             self.initialized = True
diff --git a/try_navigation/try_navigation/potential_astar.py b/try_navigation/try_navigation/potential_astar.py
index b78d173..f95772d 100644
--- a/try_navigation/try_navigation/potential_astar.py
+++ b/try_navigation/try_navigation/potential_astar.py
@@ -108,7 +108,7 @@ def __init__(self):
         self.cg2nd=20 #ポテンシャルの引力パラメータ
         self.lg2nd=20 #ポテンシャルの引力パラメータ
         self.co2nd=11 #ポテンシャルの斥力パラメータ SICKパラ目：co=11;lo=0.55;
-        self.lo2nd=0.22#55 #0.5#0.9#ポテンシャルの斥力パラメータ  24/11/29 ok IGVC20250601 0.25 -> 0.22
+        self.lo2nd=0.21#55 #0.5#0.9#ポテンシャルの斥力パラメータ  24/11/29 ok IGVC20250601 0.25 -> 0.22
         #self.lo2nd=0.30#55 #0.5#0.9#ポテンシャルの斥力パラメータ
         
         
@@ -176,6 +176,11 @@ def __init__(self):
             [   0,      0,    0,       0,        1,        0,       0,      0]  # waypoint 21 GOAL!!!!!!
         ]
         
+        tukuba_obs_x =np.linspace(-45,25,860);
+        tukuba_obs_y =np.linspace(97,92,860);
+        tukuba_obs_z =np.linspace(0,0,860);
+        self.tsukuba_obs = np.array([tukuba_obs_x, tukuba_obs_y, tukuba_obs_z]);
+        
         
         ################# IGVC SelfDrive Quolification line stop test #20250530# #################
         self.sd_line_stop_test = 0
@@ -424,6 +429,14 @@ def potential_astar(self, msg):
         else:
             relative_point_rot = np.array([[],[],[]])
         """    
+        #map_obs add
+        if len(self.tsukuba_obs[0,:])>0:
+            relative_point_x = self.tsukuba_obs[0,:] - self.position_x
+            relative_point_y = self.tsukuba_obs[1,:] - self.position_y
+            relative_point = np.array((relative_point_x, relative_point_y, self.tsukuba_obs[2,:]))
+            relative_point_rot, t_point_rot_matrix = rotation_xyz(relative_point, self.theta_x, self.theta_y, -self.theta_z)
+        else:
+            relative_point_rot = np.array([[],[],[]])
         ###################################
                 
         #obs round&duplicated  :grid_size before:28239 after100:24592 after50:8894 after10:3879
@@ -456,7 +469,8 @@ def potential_astar(self, msg):
         if dot_line_local.shape[1] > 0:
             obs_points = np.insert(obs_points, len(obs_points[0,:]), dot_line_local.T, axis=1)
         
-        #obs_points = np.insert(obs_points, len(obs_points[0,:]), relative_point_rot.T, axis=1)
+        obs_points = np.insert(obs_points, len(obs_points[0,:]), relative_point_rot.T, axis=1)
+        #obs_points = np.insert(obs_points, len(obs_points[0,:]), self.tsukuba_obs.T, axis=1)
         points_round = np.round(obs_points * self.obs_pixel) / self.obs_pixel
         obs_xy_local = points_round[:,~pd.DataFrame({"x":points_round[0,:], "y":points_round[1,:]}).duplicated()]
         obs_xy = np.vstack((obs_xy_local[0,:], obs_xy_local[1,:]))

From 30ebe31d53d91da7bb3b2428e0b6937d9318da94 Mon Sep 17 00:00:00 2001
From: kugurofu <tenten31569@icloud.com>
Date: Thu, 16 Oct 2025 03:53:51 +0000
Subject: [PATCH 10/25] since here change branch to description from gpo odom

---
 orange_bringup/launch/orange_robot.launch.xml |   4 +-
 .../orange_gnss/gnss_odom_publisher_ttyUSB.py |   4 +-
 orange_gnss/orange_gnss/odom_combination.py   |  54 +++--
 .../launch/try_navigation.launch.py           |  26 +++
 .../try_navigation/ekf_myself_gps.py          |   4 +-
 .../try_navigation/path_follower.py           | 207 +++++++++++++++++-
 .../try_navigation/potential_astar.py         |  12 +-
 7 files changed, 274 insertions(+), 37 deletions(-)

diff --git a/orange_bringup/launch/orange_robot.launch.xml b/orange_bringup/launch/orange_robot.launch.xml
index 341a8cc..c7038ca 100644
--- a/orange_bringup/launch/orange_robot.launch.xml
+++ b/orange_bringup/launch/orange_robot.launch.xml
@@ -112,12 +112,12 @@
   <!-- GPSposition_publisher -->
   <!-- initial heading:housei nakaniwa=179.169287 tsukuba= 276.5 -->
   <!-- GPSheading_publisher -->
-  <node pkg="orange_gnss" exec="gnss_odom_movingbase_fix_publisher_ttyUSB" output="screen">
+  <node pkg="orange_gnss" exec="gnss_odom_publisher_ttyUSB" output="screen">
     <param name="port" value="/dev/sensors/GNSS_UM982"/>
     <param name="baud" value="115200"/>
     <param name="country_id" value="0"/>
     <param name="Position_magnification" value="1.675"/>
-    <param name="heading" value="93.0"/>
+    <param name="heading" value="180.0"/>
     <param name="time_out" value="1.0"/>
   </node>
   <!-- 
diff --git a/orange_gnss/orange_gnss/gnss_odom_publisher_ttyUSB.py b/orange_gnss/orange_gnss/gnss_odom_publisher_ttyUSB.py
index a6d4926..2d31724 100644
--- a/orange_gnss/orange_gnss/gnss_odom_publisher_ttyUSB.py
+++ b/orange_gnss/orange_gnss/gnss_odom_publisher_ttyUSB.py
@@ -26,7 +26,7 @@ def __init__(self):
         self.country_id = self.get_parameter('country_id').get_parameter_value().integer_value
         self.Position_magnification = self.get_parameter('Position_magnification').get_parameter_value().double_value
         #self.theta = self.get_parameter('heading').get_parameter_value().double_value
-        self.tsukuba_theta=90.0 # nakaniwa 180
+        self.tsukuba_theta=180.0 # nakaniwa 180 tsukuba 93
 
         self.initial_coordinate = None
         self.fix_data = None
@@ -110,7 +110,7 @@ def acquire_gps_data(self):
 
         if count > 0:
             #self.initial_coordinate = [lat_sum / count, lon_sum / count] # calculate average
-            self.initial_coordinate = [36.0497440979, 140.045935093] # tsukuba start point 36.0497502, 140.0459234 /nakaniwa 35.4257898377487,139.313807281254 /35.425952230280004, 139.31380123427
+            self.initial_coordinate = [35.4257898377487,139.313807281254] # tsukuba start point 36.0497502, 140.0459234 /nakaniwa 35.4257898377487,139.313807281254 /35.425952230280004, 139.31380123427
             self.current_coordinate = [lat_sum / count, lon_sum / count] # for tsukuba
             self.theta = (heading_sum / count) - 90
             self.initialized = True
diff --git a/orange_gnss/orange_gnss/odom_combination.py b/orange_gnss/orange_gnss/odom_combination.py
index b2c2ec8..d73e3c9 100644
--- a/orange_gnss/orange_gnss/odom_combination.py
+++ b/orange_gnss/orange_gnss/odom_combination.py
@@ -22,23 +22,25 @@ def __init__(self):
         )
         
         # subscription
-        self.odom_sub = self.create_subscription(Odometry, '/odom', self.get_odom, qos_profile)
+        self.odom_sub = self.create_subscription(Odometry, '/odom/wheel_spimu', self.get_odom, qos_profile)
+        self.gps_odom_sub = self.create_subscription(Odometry, '/odom/UM982', self.get_gps_odom, qos_profile)
         
         # publisher
         self.odom_pub = self.create_publisher(Odometry, '/odom/combine', qos_profile)
         
         # service
-        self.avg_gps_service = self.create_service(Avglatlon, 'send_avg_gps', self.receive_avg_gps_callback)
+        #self.avg_gps_service = self.create_service(Avglatlon, 'send_avg_gps', self.receive_avg_gps_callback)
 
         self.position_x = 0.0
         self.position_y = 0.0
         self.theta_z = 0.0
         self.theta = 0.0
-        self.initial_xy = (0.0, 0.0)
-        self.Position_magnification = 1.0  # 必要なら調整
+        self.initial_xy = None #(0.0, 0.0)
+        #self.Position_magnification = 1.0  # 必要なら調整
 
         self.timer = self.create_timer(0.1, self.combine)
-
+    
+    # /odom callback
     def get_odom(self, msg):
         '''
         self.position_x = msg.pose.pose.position.x
@@ -54,6 +56,15 @@ def get_odom(self, msg):
         roll, pitch, yaw = quaternion_to_euler(x, y, z, w)
         self.theta_z = yaw  # 単位はラジアンのままでOK
     
+    # /odom/UM982 callback
+    def get_gps_odom(self, msg):
+        if self.initial_xy is None:
+            init_x = msg.pose.pose.position.x
+            init_y = msg.pose.pose.position.y
+            self.initial_xy = (init_x, init_y)
+            self.get_logger().info(f"Initial /odom/UM982 position set to: x={init_x:.3f}, y={init_y:.3f}")   
+    
+    '''
     def conversion(self, coordinate, origin, theta):
         ido = coordinate[0]
         keido = coordinate[1]
@@ -117,7 +128,8 @@ def conversion(self, coordinate, origin, theta):
         point = (h_y, -h_x)
 
         return point    
-    
+    '''
+    '''
     def receive_avg_gps_callback(self, request, response):
         avg_lat, avg_lon, current_lat, current_lon, theta = request.avg_lat, request.avg_lon, request.current_lat, request.current_lon, request.current_theta
         if theta == 0.0:
@@ -130,35 +142,37 @@ def receive_avg_gps_callback(self, request, response):
         initial_coordinate = [avg_lat, avg_lon]
         self.initial_xy = self.conversion(current_coordinate, initial_coordinate, theta)
         xy_np = np.array(self.initial_xy)
-
+    '''
     def yaw_to_orientation(self, yaw):
         orientation_z = np.sin(yaw / 2.0)
         orientation_w = np.cos(yaw / 2.0)
         return orientation_z, orientation_w
     
     def combine(self):
+        if self.initial_xy is None:
+            self.get_logger().warn("Waiting for initial /odom/UM982 position...")
+            return
+            
         pos_x = self.position_x
         pos_y = self.position_y
-        degree_to_radian = math.pi / 180
-        pos_theta = self.theta * degree_to_radian # maybe -self.theta * degree_to_radian
-        
+        #degree_to_radian = math.pi / 180
+        #pos_theta = self.theta * degree_to_radian # maybe -self.theta * degree_to_radian
+        pos_theta = self.theta_z
         # conversionによるxy変換結果（初期座標）
         #delta_x, delta_y = self.initial_xy
 
         # theta_z（現在の向き）で回転
-        cos_theta = math.cos(pos_theta)
-        sin_theta = math.sin(pos_theta)
+        #cos_theta = math.cos(pos_theta)
+        #sin_theta = math.sin(pos_theta)
 
-        #rotated_x = cos_theta * delta_x - sin_theta * delta_y
-        #rotated_y = sin_theta * delta_x + cos_theta * delta_y
-        rotated_x = cos_theta * pos_x - sin_theta * pos_y
-        rotated_y = sin_theta * pos_x + cos_theta * pos_y
+        #rotated_x = cos_theta * pos_x - sin_theta * pos_y
+        #rotated_y = sin_theta * pos_x + cos_theta * pos_y
 
         # 加算して新しい位置
-        #combined_x = self.position_x + rotated_x
-        #combined_y = self.position_y + rotated_y
-        combined_x = self.initial_xy[0] + rotated_x
-        combined_y = self.initial_xy[1] + rotated_y
+        #combined_x = self.initial_xy[0] + rotated_x
+        #combined_y = self.initial_xy[1] + rotated_y
+        combined_x = self.initial_xy[0] + pos_x
+        combined_y = self.initial_xy[1] + pos_y
                 
         # quartanion z,w
         odom_orientation = self.yaw_to_orientation(pos_theta)
diff --git a/try_navigation/launch/try_navigation.launch.py b/try_navigation/launch/try_navigation.launch.py
index 62d5821..fa25304 100644
--- a/try_navigation/launch/try_navigation.launch.py
+++ b/try_navigation/launch/try_navigation.launch.py
@@ -5,6 +5,9 @@
 from launch_ros.actions import Node
 from launch.actions import IncludeLaunchDescription
 from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
 
 def generate_launch_description():
     rviz_config_dir = os.path.join(
@@ -18,7 +21,26 @@ def generate_launch_description():
         'livox_to_pointcloud2.launch.py'
     )
     
+    # define config
+    odom = LaunchConfiguration('odom')
+    declare_odom_arg = DeclareLaunchArgument(
+        'odom',
+        default_value='/fusion/odom',
+        description='Odometry topic name'
+    )
+    
+    waypoint_path = LaunchConfiguration('waypoint_path')
+    declare_waypoint_arg = DeclareLaunchArgument(
+        'waypoint_path',
+        default_value='kbkn_maps/waypoints/hosei/2025/nakaniwa_tsukuba.yaml',
+        description='waypoint name'
+    )
+    
+    
+    
     return LaunchDescription([
+        declare_odom_arg, declare_waypoint_arg,
+        
         #rviz2
         Node(package='rviz2',
             executable='rviz2',
@@ -68,6 +90,8 @@ def generate_launch_description():
             executable='gps_waypoint',
             name='gps_waypoint',
             output='screen',
+            parameters=[{'odom': odom},
+                        {'waypoint_path': waypoint_path}],
             arguments=[],
         ),
         # $ ros2 run navigation_control gps_waypoint
@@ -85,6 +109,7 @@ def generate_launch_description():
             executable='potential_astar',
             name='potential_astar_node',
             output='screen',
+            parameters=[{'odom': odom}],
             arguments=[],
         ),
         #robot ctrl
@@ -92,6 +117,7 @@ def generate_launch_description():
             executable='path_follower',
             name='path_follower_node',
             output='screen',
+            parameters=[{'odom': odom}],
             arguments=[],
         ),
         
diff --git a/try_navigation/try_navigation/ekf_myself_gps.py b/try_navigation/try_navigation/ekf_myself_gps.py
index 180fbc4..af17719 100644
--- a/try_navigation/try_navigation/ekf_myself_gps.py
+++ b/try_navigation/try_navigation/ekf_myself_gps.py
@@ -67,7 +67,7 @@ def __init__(self):
         
 
         self.sub_a = self.create_subscription(
-            Odometry, '/odom/wheel_spimu', self.sensor_a_callback, 10) # /odom/wheel_spimu
+            Odometry, '/odom/combine', self.sensor_a_callback, 10) # /odom/wheel_spimu
         self.sub_b = self.create_subscription(
             Odometry, '/odom/UM982', self.sensor_b_callback, 10)
         #self.sub_b = self.create_subscription(
@@ -164,7 +164,7 @@ def sensor_b_callback(self, data):
     def determination_of_R(self):
         if self.GpsXY is not None:
             #カルマンフィルタのデッドレコニングよりになる範囲を指定
-            if (-38<self.GpsXY[0]) and (self.GpsXY[0]<60) and (25 < self.GpsXY[1]) and (self.GpsXY[1]<110):
+            if (-60<self.GpsXY[0]) and (self.GpsXY[0]<60) and (25 < self.GpsXY[1]) and (self.GpsXY[1]<110):
                 #self.gps_rr_flag = 1
                 self.gps_rr_flag = 0 #self.gps_rr_flag =1はGPS受信精度よく、0でGPS受信精度低い範囲に入ったフラグ
                 #self.offsetyaw_bad_gps = -10/180*math.pi
diff --git a/try_navigation/try_navigation/path_follower.py b/try_navigation/try_navigation/path_follower.py
index 87bfcce..f78718b 100644
--- a/try_navigation/try_navigation/path_follower.py
+++ b/try_navigation/try_navigation/path_follower.py
@@ -15,6 +15,9 @@
 from geometry_msgs.msg import PoseStamped
 from std_msgs.msg import Int32
 from std_msgs.msg import String
+from visualization_msgs.msg import Marker, MarkerArray
+from builtin_interfaces.msg import Duration  
+
 
 # C++と同じく、Node型を継承します。
 class PathFollower(Node):
@@ -60,7 +63,8 @@ def __init__(self):
         
         # Publisherを作成
         self.cmd_vel_publisher = self.create_publisher(geometry_msgs.Twist, 'cmd_vel', qos_profile) #set publish pcd topic name
-        
+        self.marker_pub = self.create_publisher(MarkerArray, 'wall_follow_markers', 10)
+
         #パラメータ init
         self.path_plan = np.array([[0],[0],[0]])
         
@@ -181,6 +185,27 @@ def __init__(self):
         self.previous_status = None    
         self.human_status = None    
         ##################################################################
+
+        ####################   Wall-Follower Specific Params    #########################
+
+        self.wall_follow_intervals = [(5, 12)]   # (start_waypoint, end_waypoint)
+        self.wall_follow_side = 'right'          # 'right' or 'left' depends on which wall u follow
+        # ロボット座標系で見た検出ボックス（前方 x 範囲, 右側 y 範囲）
+        self.wf_x_min = 0.0
+        self.wf_x_max = 3.0
+        # 右側の y は負（例: -1.5 〜 -0.2）
+        self.wf_y_min = -1.5
+        self.wf_y_max = -0.2
+        # 壁の高さフィルタ（m）
+        self.wf_z_min = 0.3
+        self.wf_z_max = 1.0
+        # 壁からの標準距離とゲイン
+        self.wf_desired_dist = 0.4    # 目標距離[m]
+        self.wf_k_dist = 1.0          # m -> rad の換算ゲイン（調整）
+        self.wf_min_points = 25       # フィットに必要な最低点数
+        self.wf_speed = 0.25          # 基本速度[m/s]（狭路）
+
+        #################################################################################
         
     # actionリクエストの受信時に呼ばれる(tuika)
     def listener_callback(self, goal_handle):
@@ -379,7 +404,17 @@ def robot_ctrl(self):
             speed = -0.10
         if np.any(c_obs_back) :
             speed = 0.10
-        
+
+        ####wall follow  overwrite####
+        if self.in_wall_follow_interval():
+            wf_res = self.wall_follow_control()
+            if wf_res is not None:
+                speed, target_rad = wf_res
+                target_theta = target_rad * 180.0 / math.pi
+                # ここで即座に PD 平滑化（ラジアンで扱う）
+                target_rad = self.sensim0(target_rad)
+        #self.get_logger().info(f"WF active: speed={speed:.2f}, target_rad={target_rad:.3f} (deg={target_theta:.1f}), pts={pts_side.shape[1]}")
+
         
         ################# IGVC SelfDrive Full #20250601# #################
         if self.sd_full_flag == 1:
@@ -498,7 +533,7 @@ def robot_ctrl(self):
         #else:
         #    speed = 0.2
         #self.get_logger().info('speed = %f' % (speed))
-        target_theta = target_theta +90/180*math.pi
+        target_theta = target_theta +90     ######/180*math.pi
         target_rad_pd = self.sensim0(target_rad)
         #target_rad_pd = target_rad
         
@@ -619,10 +654,166 @@ def obs_steer(self, msg):
         else:
             self.lh_obs = 0
 
-    def pcd_serch(self, pointcloud, x_min, x_max, y_min, y_max):
-        pcd_ind = (( (x_min <= pointcloud[0,:]) * (pointcloud[0,:] <= x_max)) * ((y_min <= pointcloud[1,:]) * (pointcloud[1,:]) <= y_max ) )
-        pcd_mask = pointcloud[:, pcd_ind]
-        return pcd_mask
+    # pcd_serch を z も考慮する形で置換（点群抽出用ユーティリティ）
+    def pcd_serch(self, pointcloud, x_min, x_max, y_min, y_max, z_min=None, z_max=None):
+        """
+        pointcloud: 4 x N array (x,y,z,intensity)
+        returns: filtered 4 x M array
+        """
+        mask = ((pointcloud[0, :] >= x_min) & (pointcloud[0, :] <= x_max) &
+                (pointcloud[1, :] >= y_min) & (pointcloud[1, :] <= y_max))
+        if z_min is not None and z_max is not None:
+            mask = mask & ((pointcloud[2, :] >= z_min) & (pointcloud[2, :] <= z_max))
+        return pointcloud[:, mask]
+
+    def fit_line_pca(self, pts_xy):
+        """
+        pts_xy: 2 x N numpy array (x; y) in robot frame
+        returns: (centroid (2,), tangent_unit (2,)) or None if insufficient points / invalid input
+        - Uses PCA (covariance -> eigenvector) to get principal direction (tangent).
+        - Expects pts_xy.shape == (2, N).
+        """
+        # --- 基本的な入力チェック ---
+        if pts_xy is None:
+            return None
+        # 形が 2 x N でないなら None
+        if pts_xy.ndim != 2 or pts_xy.shape[0] != 2:
+            return None
+
+        # 最低点数チェック（クラスのパラメータ名と一致させる）
+        min_pts = getattr(self, 'wf_min_points', 10)
+        if pts_xy.shape[1] < min_pts:
+            return None
+
+        # --- centroid と共分散行列計算 ---
+        cx = np.mean(pts_xy[0, :])
+        cy = np.mean(pts_xy[1, :])
+        X = pts_xy - np.array([[cx], [cy]])  # 2 x N
+
+        cov = np.cov(X)  # 2 x 2
+        # 対称行列なので eigh を使う（数値安定）
+        w, v = np.linalg.eigh(cov)  # w: eigenvalues (ascending), v: eigenvectors (columns)
+
+        # 最大固有値に対応する固有ベクトルが接線（tangent）
+        tangent = v[:, np.argmax(w)]
+        norm = np.linalg.norm(tangent)
+        if norm == 0 or not np.isfinite(norm):
+            return None
+        tangent = tangent / norm
+
+        return np.array([cx, cy]), tangent
+
+    def in_wall_follow_interval(self):
+        # waypoint_number が interval のどれかに入っているか
+        for (s, e) in self.wall_follow_intervals:
+            if s <= getattr(self, 'waypoint_number', 0) <= e:
+                return True
+        return False
+
+    def wall_follow_control(self):
+        """
+        returns (speed, target_rad) in robot frame (rad). 
+        If cannot compute (e.g. not enough points), return None.
+        """
+        pts = self.obs_points  # 4 x N in robot frame
+        if pts.size == 0 or pts.shape[1] == 0:
+            return None
+
+        # apply z-height + right-side box (use params defined in __init__)
+        pts_side = self.pcd_serch(pts, self.wf_x_min, self.wf_x_max,
+                                  self.wf_y_min, self.wf_y_max,
+                                  z_min=self.wf_z_min, z_max=self.wf_z_max)
+        if pts_side.shape[1] < self.wf_min_points:
+            return None
+
+        res = self.fit_line_pca(pts_side[:2, :])
+        if res is None:
+            return None
+        centroid, tangent = res
+
+        normal = np.array([-tangent[1], tangent[0]])
+        signed_dist = float(np.dot(centroid, normal))
+
+        desired_signed = -abs(self.wf_desired_dist)  # right-side -> negative
+        lat_error = desired_signed - signed_dist
+
+        # ensure tangent points to global -x (robot frame conversion)
+        robot_yaw = math.radians(self.theta_z)
+        R = np.array([[math.cos(robot_yaw), -math.sin(robot_yaw)],
+                      [math.sin(robot_yaw),  math.cos(robot_yaw)]])
+        tangent_global = R.dot(tangent)
+        if tangent_global[0] > 0:
+            tangent = -tangent
+            normal = -normal
+            signed_dist = -signed_dist
+            lat_error = desired_signed - signed_dist
+
+        target_tangent_angle = math.atan2(tangent[1], tangent[0])
+        correction_rad = self.wf_k_dist * lat_error
+        target_rad = target_tangent_angle + correction_rad
+
+        pts_forward = self.pcd_serch(pts, 0.0, 0.6, -0.3, 0.3, z_min=self.wf_z_min, z_max=self.wf_z_max)
+        speed = self.wf_speed
+        if pts_forward.shape[1] > 8:
+            speed = 0.0
+
+        return speed, target_rad
+        # 2) Marker を作るユーティリティ（PathFollower クラスに追加）
+    def publish_wall_follow_markers(self, centroid=None, tangent=None, pts_side=None,active=False, signed_dist=None, detection_box=None, front_obstacle=False):
+        """
+        centroid: np.array([cx,cy]) in robot frame or None
+        tangent: np.array([tx,ty]) unit vector in robot frame or None
+        pts_side: 4 x N points (subset) or None
+        active: Boolean (壁追従モードか)
+        signed_dist: float or None
+        detection_box: dict with keys x_min,x_max,y_min,y_max,z_min,z_max (optional) to show box
+        front_obstacle: bool -> show red warning
+        """
+        ma = MarkerArray()
+        now = self.get_clock().now().to_msg()
+        ns = "wall_follow"
+
+        # 0) Mode box / big indicator - left-top like
+        m0 = Marker()
+        m0.header.frame_id = "odom"   # or the frame you use for robot-frame markers
+        m0.header.stamp = now
+        m0.ns = ns
+        m0.id = 0
+        m0.type = Marker.CUBE
+        m0.action = Marker.ADD
+        # place at robot left-front corner in RViz view (slightly above)
+        m0.pose.position.x = 0.0
+        m0.pose.position.y = 0.0
+        m0.pose.position.z = 1.6
+        m0.pose.orientation.w = 1.0
+        m0.scale.x = 0.25
+        m0.scale.y = 0.25
+        m0.scale.z = 0.05
+        if active:
+            m0.color.r = 0.0; m0.color.g = 1.0; m0.color.b = 0.0; m0.color.a = 0.9
+        else:
+            m0.color.r = 1.0; m0.color.g = 0.0; m0.color.b = 0.0; m0.color.a = 0.9
+        m0.lifetime = Duration(sec=0, nanosec=200000000)  # 0.2s
+        ma.markers.append(m0)
+
+        if centroid is not None:
+            m1 = Marker()
+            m1.header.frame_id = "base_link"
+            m1.header.stamp = now
+            m1.ns = ns
+            m1.id = 1
+            m1.type = Marker.SPHERE
+            m1.action = Marker.ADD
+            m1.pose.position.x = float(centroid[0])
+            m1.pose.position.y = float(centroid[1])
+            m1.pose.position.z = 1.0  # show at wall height
+            m1.pose.orientation.w = 1.0
+            m1.scale.x = 0.12
+            m1.scale.y = 0.12
+            m1.scale.z = 0.12
+            m1.color.r = 0.0; m1.color.g = 0.8; m1.color.b = 0.8; m1.color.a = 0.9
+            m1.lifetime = std_msgs.Duration(sec=0, nanosec=200000000)
+            ma.markers.append(m1)
 
 def rotation_xyz(pointcloud, theta_x, theta_y, theta_z):
     theta_x = math.radians(theta_x)
@@ -675,4 +866,4 @@ def main(args=None):
 # 本スクリプト(publish.py)の処理の開始地点です。
 if __name__ == '__main__':
     # 関数`main`を実行する。
-    main()
+    main()
\ No newline at end of file
diff --git a/try_navigation/try_navigation/potential_astar.py b/try_navigation/try_navigation/potential_astar.py
index f95772d..f39bb70 100644
--- a/try_navigation/try_navigation/potential_astar.py
+++ b/try_navigation/try_navigation/potential_astar.py
@@ -58,10 +58,16 @@ def __init__(self):
             depth = 1
         )
         
+        # set parameter (launch can change this parameter)
+        self.declare_parameter('odom', '/fusion/odom')
+        
+        # define parameter
+        odom_topic = self.get_parameter('odom').get_parameter_value().string_value
+        
         # Subscriptionを作成。CustomMsg型,'/livox/lidar'という名前のtopicをsubscribe。
         self.subscription = self.create_subscription(sensor_msgs.PointCloud2, '/pcd_segment_obs', self.potential_astar, qos_profile)
         #self.subscription = self.create_subscription(nav_msgs.Odometry,'/odom/wheel_imu', self.get_odom, qos_profile_sub)
-        self.subscription = self.create_subscription(nav_msgs.Odometry,'/fusion/odom', self.get_odom, qos_profile_sub) # /odom/wheel_spimu
+        self.subscription = self.create_subscription(nav_msgs.Odometry, odom_topic, self.get_odom, qos_profile_sub) # /odom/wheel_spimu
         #self.subscription = self.create_subscription(nav_msgs.Odometry,'/odom_fast', self.get_odom, qos_profile_sub)
         #self.subscription = self.create_subscription(nav_msgs.Odometry,'/odom_ekf_match', self.get_odom, qos_profile_sub)
         self.subscription = self.create_subscription(geometry_msgs.PoseArray,'/current_waypoint', self.get_waypoint, qos_profile_sub)
@@ -108,7 +114,7 @@ def __init__(self):
         self.cg2nd=20 #ポテンシャルの引力パラメータ
         self.lg2nd=20 #ポテンシャルの引力パラメータ
         self.co2nd=11 #ポテンシャルの斥力パラメータ SICKパラ目：co=11;lo=0.55;
-        self.lo2nd=0.21#55 #0.5#0.9#ポテンシャルの斥力パラメータ  24/11/29 ok IGVC20250601 0.25 -> 0.22
+        self.lo2nd=0.22#55 #0.5#0.9#ポテンシャルの斥力パラメータ  24/11/29 ok IGVC20250601 0.25 -> 0.22
         #self.lo2nd=0.30#55 #0.5#0.9#ポテンシャルの斥力パラメータ
         
         
@@ -177,7 +183,7 @@ def __init__(self):
         ]
         
         tukuba_obs_x =np.linspace(-45,25,860);
-        tukuba_obs_y =np.linspace(97,92,860);
+        tukuba_obs_y =np.linspace(97,94,860);
         tukuba_obs_z =np.linspace(0,0,860);
         self.tsukuba_obs = np.array([tukuba_obs_x, tukuba_obs_y, tukuba_obs_z]);
         

From 4e9f4a2f5054d1f2b80169b02a112cde545fae07 Mon Sep 17 00:00:00 2001
From: kugurofu <tenten31569@icloud.com>
Date: Thu, 16 Oct 2025 10:56:56 +0000
Subject: [PATCH 11/25] delete wall follow in this branch since it desnt work
 so far

---
 .../try_navigation/path_follower.py           | 184 +-----------------
 1 file changed, 1 insertion(+), 183 deletions(-)

diff --git a/try_navigation/try_navigation/path_follower.py b/try_navigation/try_navigation/path_follower.py
index f78718b..4b810f1 100644
--- a/try_navigation/try_navigation/path_follower.py
+++ b/try_navigation/try_navigation/path_follower.py
@@ -15,8 +15,7 @@
 from geometry_msgs.msg import PoseStamped
 from std_msgs.msg import Int32
 from std_msgs.msg import String
-from visualization_msgs.msg import Marker, MarkerArray
-from builtin_interfaces.msg import Duration  
+
 
 
 # C++と同じく、Node型を継承します。
@@ -54,7 +53,6 @@ def __init__(self):
         self.goal_sub = self.create_subscription(PoseStamped, '/goal_pose', self.goal_pose_callback, qos_profile)
         self.stop_sub = self.create_subscription(String, '/stop_sign_status', self.stop_sign_callback, 10)
         self.human_sub = self.create_subscription(String, '/human_status', self.human_callback, 10)
-        self.waypoint_number_subscription = self.create_subscription(Int32,'/waypoint_number', self.get_waypoint_number, qos_profile_sub)
         self.subscription  # 警告を回避するために設置されているだけです。削除しても挙動はかわりません。
         
         # タイマーを0.05秒（50ミリ秒）ごとに呼び出す
@@ -186,27 +184,6 @@ def __init__(self):
         self.human_status = None    
         ##################################################################
 
-        ####################   Wall-Follower Specific Params    #########################
-
-        self.wall_follow_intervals = [(5, 12)]   # (start_waypoint, end_waypoint)
-        self.wall_follow_side = 'right'          # 'right' or 'left' depends on which wall u follow
-        # ロボット座標系で見た検出ボックス（前方 x 範囲, 右側 y 範囲）
-        self.wf_x_min = 0.0
-        self.wf_x_max = 3.0
-        # 右側の y は負（例: -1.5 〜 -0.2）
-        self.wf_y_min = -1.5
-        self.wf_y_max = -0.2
-        # 壁の高さフィルタ（m）
-        self.wf_z_min = 0.3
-        self.wf_z_max = 1.0
-        # 壁からの標準距離とゲイン
-        self.wf_desired_dist = 0.4    # 目標距離[m]
-        self.wf_k_dist = 1.0          # m -> rad の換算ゲイン（調整）
-        self.wf_min_points = 25       # フィットに必要な最低点数
-        self.wf_speed = 0.25          # 基本速度[m/s]（狭路）
-
-        #################################################################################
-        
     # actionリクエストの受信時に呼ばれる(tuika)
     def listener_callback(self, goal_handle):
         self.get_logger().info(f"Received goal with a: {goal_handle.request.a}, b: {goal_handle.request.b}")
@@ -405,16 +382,6 @@ def robot_ctrl(self):
         if np.any(c_obs_back) :
             speed = 0.10
 
-        ####wall follow  overwrite####
-        if self.in_wall_follow_interval():
-            wf_res = self.wall_follow_control()
-            if wf_res is not None:
-                speed, target_rad = wf_res
-                target_theta = target_rad * 180.0 / math.pi
-                # ここで即座に PD 平滑化（ラジアンで扱う）
-                target_rad = self.sensim0(target_rad)
-        #self.get_logger().info(f"WF active: speed={speed:.2f}, target_rad={target_rad:.3f} (deg={target_theta:.1f}), pts={pts_side.shape[1]}")
-
         
         ################# IGVC SelfDrive Full #20250601# #################
         if self.sd_full_flag == 1:
@@ -666,155 +633,6 @@ def pcd_serch(self, pointcloud, x_min, x_max, y_min, y_max, z_min=None, z_max=No
             mask = mask & ((pointcloud[2, :] >= z_min) & (pointcloud[2, :] <= z_max))
         return pointcloud[:, mask]
 
-    def fit_line_pca(self, pts_xy):
-        """
-        pts_xy: 2 x N numpy array (x; y) in robot frame
-        returns: (centroid (2,), tangent_unit (2,)) or None if insufficient points / invalid input
-        - Uses PCA (covariance -> eigenvector) to get principal direction (tangent).
-        - Expects pts_xy.shape == (2, N).
-        """
-        # --- 基本的な入力チェック ---
-        if pts_xy is None:
-            return None
-        # 形が 2 x N でないなら None
-        if pts_xy.ndim != 2 or pts_xy.shape[0] != 2:
-            return None
-
-        # 最低点数チェック（クラスのパラメータ名と一致させる）
-        min_pts = getattr(self, 'wf_min_points', 10)
-        if pts_xy.shape[1] < min_pts:
-            return None
-
-        # --- centroid と共分散行列計算 ---
-        cx = np.mean(pts_xy[0, :])
-        cy = np.mean(pts_xy[1, :])
-        X = pts_xy - np.array([[cx], [cy]])  # 2 x N
-
-        cov = np.cov(X)  # 2 x 2
-        # 対称行列なので eigh を使う（数値安定）
-        w, v = np.linalg.eigh(cov)  # w: eigenvalues (ascending), v: eigenvectors (columns)
-
-        # 最大固有値に対応する固有ベクトルが接線（tangent）
-        tangent = v[:, np.argmax(w)]
-        norm = np.linalg.norm(tangent)
-        if norm == 0 or not np.isfinite(norm):
-            return None
-        tangent = tangent / norm
-
-        return np.array([cx, cy]), tangent
-
-    def in_wall_follow_interval(self):
-        # waypoint_number が interval のどれかに入っているか
-        for (s, e) in self.wall_follow_intervals:
-            if s <= getattr(self, 'waypoint_number', 0) <= e:
-                return True
-        return False
-
-    def wall_follow_control(self):
-        """
-        returns (speed, target_rad) in robot frame (rad). 
-        If cannot compute (e.g. not enough points), return None.
-        """
-        pts = self.obs_points  # 4 x N in robot frame
-        if pts.size == 0 or pts.shape[1] == 0:
-            return None
-
-        # apply z-height + right-side box (use params defined in __init__)
-        pts_side = self.pcd_serch(pts, self.wf_x_min, self.wf_x_max,
-                                  self.wf_y_min, self.wf_y_max,
-                                  z_min=self.wf_z_min, z_max=self.wf_z_max)
-        if pts_side.shape[1] < self.wf_min_points:
-            return None
-
-        res = self.fit_line_pca(pts_side[:2, :])
-        if res is None:
-            return None
-        centroid, tangent = res
-
-        normal = np.array([-tangent[1], tangent[0]])
-        signed_dist = float(np.dot(centroid, normal))
-
-        desired_signed = -abs(self.wf_desired_dist)  # right-side -> negative
-        lat_error = desired_signed - signed_dist
-
-        # ensure tangent points to global -x (robot frame conversion)
-        robot_yaw = math.radians(self.theta_z)
-        R = np.array([[math.cos(robot_yaw), -math.sin(robot_yaw)],
-                      [math.sin(robot_yaw),  math.cos(robot_yaw)]])
-        tangent_global = R.dot(tangent)
-        if tangent_global[0] > 0:
-            tangent = -tangent
-            normal = -normal
-            signed_dist = -signed_dist
-            lat_error = desired_signed - signed_dist
-
-        target_tangent_angle = math.atan2(tangent[1], tangent[0])
-        correction_rad = self.wf_k_dist * lat_error
-        target_rad = target_tangent_angle + correction_rad
-
-        pts_forward = self.pcd_serch(pts, 0.0, 0.6, -0.3, 0.3, z_min=self.wf_z_min, z_max=self.wf_z_max)
-        speed = self.wf_speed
-        if pts_forward.shape[1] > 8:
-            speed = 0.0
-
-        return speed, target_rad
-        # 2) Marker を作るユーティリティ（PathFollower クラスに追加）
-    def publish_wall_follow_markers(self, centroid=None, tangent=None, pts_side=None,active=False, signed_dist=None, detection_box=None, front_obstacle=False):
-        """
-        centroid: np.array([cx,cy]) in robot frame or None
-        tangent: np.array([tx,ty]) unit vector in robot frame or None
-        pts_side: 4 x N points (subset) or None
-        active: Boolean (壁追従モードか)
-        signed_dist: float or None
-        detection_box: dict with keys x_min,x_max,y_min,y_max,z_min,z_max (optional) to show box
-        front_obstacle: bool -> show red warning
-        """
-        ma = MarkerArray()
-        now = self.get_clock().now().to_msg()
-        ns = "wall_follow"
-
-        # 0) Mode box / big indicator - left-top like
-        m0 = Marker()
-        m0.header.frame_id = "odom"   # or the frame you use for robot-frame markers
-        m0.header.stamp = now
-        m0.ns = ns
-        m0.id = 0
-        m0.type = Marker.CUBE
-        m0.action = Marker.ADD
-        # place at robot left-front corner in RViz view (slightly above)
-        m0.pose.position.x = 0.0
-        m0.pose.position.y = 0.0
-        m0.pose.position.z = 1.6
-        m0.pose.orientation.w = 1.0
-        m0.scale.x = 0.25
-        m0.scale.y = 0.25
-        m0.scale.z = 0.05
-        if active:
-            m0.color.r = 0.0; m0.color.g = 1.0; m0.color.b = 0.0; m0.color.a = 0.9
-        else:
-            m0.color.r = 1.0; m0.color.g = 0.0; m0.color.b = 0.0; m0.color.a = 0.9
-        m0.lifetime = Duration(sec=0, nanosec=200000000)  # 0.2s
-        ma.markers.append(m0)
-
-        if centroid is not None:
-            m1 = Marker()
-            m1.header.frame_id = "base_link"
-            m1.header.stamp = now
-            m1.ns = ns
-            m1.id = 1
-            m1.type = Marker.SPHERE
-            m1.action = Marker.ADD
-            m1.pose.position.x = float(centroid[0])
-            m1.pose.position.y = float(centroid[1])
-            m1.pose.position.z = 1.0  # show at wall height
-            m1.pose.orientation.w = 1.0
-            m1.scale.x = 0.12
-            m1.scale.y = 0.12
-            m1.scale.z = 0.12
-            m1.color.r = 0.0; m1.color.g = 0.8; m1.color.b = 0.8; m1.color.a = 0.9
-            m1.lifetime = std_msgs.Duration(sec=0, nanosec=200000000)
-            ma.markers.append(m1)
-
 def rotation_xyz(pointcloud, theta_x, theta_y, theta_z):
     theta_x = math.radians(theta_x)
     theta_y = math.radians(theta_y)

From 18d4429201e62bf3b24a1f30dcd094a49c9c39fb Mon Sep 17 00:00:00 2001
From: kugurofu <tenten31569@icloud.com>
Date: Thu, 16 Oct 2025 11:01:08 +0000
Subject: [PATCH 12/25] PCL_ROOT=/usr is ignored since Cmake was old, fixed

---
 orange_sensor_tools/CMakeLists.txt | 1 +
 1 file changed, 1 insertion(+)

diff --git a/orange_sensor_tools/CMakeLists.txt b/orange_sensor_tools/CMakeLists.txt
index a107f1b..7487201 100644
--- a/orange_sensor_tools/CMakeLists.txt
+++ b/orange_sensor_tools/CMakeLists.txt
@@ -1,4 +1,5 @@
 cmake_minimum_required(VERSION 3.8)
+cmake_policy(SET CMP0074 NEW)    # ← これを追加
 project(orange_sensor_tools)
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")

From 89b7912438047300b731c0fbaffbc468df27311f Mon Sep 17 00:00:00 2001
From: kugurofu <tenten31569@icloud.com>
Date: Fri, 17 Oct 2025 08:29:22 +0000
Subject: [PATCH 13/25] path follower pub comment out

---
 try_navigation/try_navigation/path_follower.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/try_navigation/try_navigation/path_follower.py b/try_navigation/try_navigation/path_follower.py
index 4b810f1..794ff24 100644
--- a/try_navigation/try_navigation/path_follower.py
+++ b/try_navigation/try_navigation/path_follower.py
@@ -61,7 +61,7 @@ def __init__(self):
         
         # Publisherを作成
         self.cmd_vel_publisher = self.create_publisher(geometry_msgs.Twist, 'cmd_vel', qos_profile) #set publish pcd topic name
-        self.marker_pub = self.create_publisher(MarkerArray, 'wall_follow_markers', 10)
+        #self.marker_pub = self.create_publisher(MarkerArray, 'wall_follow_markers', 10)
 
         #パラメータ init
         self.path_plan = np.array([[0],[0],[0]])
@@ -684,4 +684,4 @@ def main(args=None):
 # 本スクリプト(publish.py)の処理の開始地点です。
 if __name__ == '__main__':
     # 関数`main`を実行する。
-    main()
\ No newline at end of file
+    main()

From a66ba8919c5f5232673c9648d90d265e4d512841 Mon Sep 17 00:00:00 2001
From: kugurofu <tenten31569@icloud.com>
Date: Mon, 20 Oct 2025 07:15:23 +0000
Subject: [PATCH 14/25] change 20251018 tsukuba run

---
 orange_bringup/launch/orange_robot.launch.xml |   2 +-
 .../orange_gnss/gnss_odom_publisher_ttyUSB.py |   4 +-
 .../launch/try_navigation.launch.py           |  16 +--
 .../try_navigation/ekf_myself_gps.py          |   4 +-
 .../try_navigation/path_follower.py           | 102 +++++++++++++++++-
 .../try_navigation/potential_astar.py         |  10 ++
 6 files changed, 123 insertions(+), 15 deletions(-)

diff --git a/orange_bringup/launch/orange_robot.launch.xml b/orange_bringup/launch/orange_robot.launch.xml
index c7038ca..37f5ec7 100644
--- a/orange_bringup/launch/orange_robot.launch.xml
+++ b/orange_bringup/launch/orange_robot.launch.xml
@@ -117,7 +117,7 @@
     <param name="baud" value="115200"/>
     <param name="country_id" value="0"/>
     <param name="Position_magnification" value="1.675"/>
-    <param name="heading" value="180.0"/>
+    <param name="heading" value="93.0"/>
     <param name="time_out" value="1.0"/>
   </node>
   <!-- 
diff --git a/orange_gnss/orange_gnss/gnss_odom_publisher_ttyUSB.py b/orange_gnss/orange_gnss/gnss_odom_publisher_ttyUSB.py
index 2d31724..af1e6bc 100644
--- a/orange_gnss/orange_gnss/gnss_odom_publisher_ttyUSB.py
+++ b/orange_gnss/orange_gnss/gnss_odom_publisher_ttyUSB.py
@@ -26,7 +26,7 @@ def __init__(self):
         self.country_id = self.get_parameter('country_id').get_parameter_value().integer_value
         self.Position_magnification = self.get_parameter('Position_magnification').get_parameter_value().double_value
         #self.theta = self.get_parameter('heading').get_parameter_value().double_value
-        self.tsukuba_theta=180.0 # nakaniwa 180 tsukuba 93
+        self.tsukuba_theta=93.0 # nakaniwa 180 tsukuba 93
 
         self.initial_coordinate = None
         self.fix_data = None
@@ -110,7 +110,7 @@ def acquire_gps_data(self):
 
         if count > 0:
             #self.initial_coordinate = [lat_sum / count, lon_sum / count] # calculate average
-            self.initial_coordinate = [35.4257898377487,139.313807281254] # tsukuba start point 36.0497502, 140.0459234 /nakaniwa 35.4257898377487,139.313807281254 /35.425952230280004, 139.31380123427
+            self.initial_coordinate = [36.04974095972727, 140.04593633886364 ] # tsukuba start point 36.0497502, 140.0459234 36.04972947264545, 140.04590687627274 36.04974095972727, 140.04593633886364 , left 36.04976195993636, 140.04593755179093/nakaniwa 35.4257898377487,139.313807281254 /35.425952230280004, 139.31380123427
             self.current_coordinate = [lat_sum / count, lon_sum / count] # for tsukuba
             self.theta = (heading_sum / count) - 90
             self.initialized = True
diff --git a/try_navigation/launch/try_navigation.launch.py b/try_navigation/launch/try_navigation.launch.py
index fa25304..5b275fe 100644
--- a/try_navigation/launch/try_navigation.launch.py
+++ b/try_navigation/launch/try_navigation.launch.py
@@ -32,7 +32,7 @@ def generate_launch_description():
     waypoint_path = LaunchConfiguration('waypoint_path')
     declare_waypoint_arg = DeclareLaunchArgument(
         'waypoint_path',
-        default_value='kbkn_maps/waypoints/hosei/2025/nakaniwa_tsukuba.yaml',
+        default_value='kbkn_maps/waypoints/tsukuba/2025/papa/tsukuba_waypoint.yaml',
         description='waypoint name'
     )
     
@@ -113,13 +113,13 @@ def generate_launch_description():
             arguments=[],
         ),
         #robot ctrl
-        Node(package='try_navigation',
-            executable='path_follower',
-            name='path_follower_node',
-            output='screen',
-            parameters=[{'odom': odom}],
-            arguments=[],
-        ),
+        #Node(package='try_navigation',
+        #    executable='path_follower',
+        #    name='path_follower_node',
+        #    output='screen',
+        #    parameters=[{'odom': odom}],
+        #    arguments=[],
+        #),
         
         #navigation start
         Node(package='navigation_control',
diff --git a/try_navigation/try_navigation/ekf_myself_gps.py b/try_navigation/try_navigation/ekf_myself_gps.py
index af17719..cc5555f 100644
--- a/try_navigation/try_navigation/ekf_myself_gps.py
+++ b/try_navigation/try_navigation/ekf_myself_gps.py
@@ -67,7 +67,7 @@ def __init__(self):
         
 
         self.sub_a = self.create_subscription(
-            Odometry, '/odom/combine', self.sensor_a_callback, 10) # /odom/wheel_spimu
+            Odometry, '/odom/wheel_spimu', self.sensor_a_callback, 10) # /odom/wheel_spimu
         self.sub_b = self.create_subscription(
             Odometry, '/odom/UM982', self.sensor_b_callback, 10)
         #self.sub_b = self.create_subscription(
@@ -168,7 +168,7 @@ def determination_of_R(self):
                 #self.gps_rr_flag = 1
                 self.gps_rr_flag = 0 #self.gps_rr_flag =1はGPS受信精度よく、0でGPS受信精度低い範囲に入ったフラグ
                 #self.offsetyaw_bad_gps = -10/180*math.pi
-                #self.GPS_angle_conut = 0; ## tukuba 20250920 zantei
+                self.GPS_angle_conut = 0; ## tukuba 20250920 zantei
             else:
                 self.gps_rr_flag = 1
                 self.offsetyaw_bad_gps = 0
diff --git a/try_navigation/try_navigation/path_follower.py b/try_navigation/try_navigation/path_follower.py
index 794ff24..4344c39 100644
--- a/try_navigation/try_navigation/path_follower.py
+++ b/try_navigation/try_navigation/path_follower.py
@@ -61,6 +61,7 @@ def __init__(self):
         
         # Publisherを作成
         self.cmd_vel_publisher = self.create_publisher(geometry_msgs.Twist, 'cmd_vel', qos_profile) #set publish pcd topic name
+        self.pcd_test_publisher = self.create_publisher(sensor_msgs.PointCloud2, 'pcd_test_global', qos_profile) 
         #self.marker_pub = self.create_publisher(MarkerArray, 'wall_follow_markers', 10)
 
         #パラメータ init
@@ -141,6 +142,8 @@ def __init__(self):
         self.obs_points = np.array([[],[],[],[]])
         self.rh_obs = 0
         self.lh_obs = 0
+        self.ch_obs = 0
+        self.t_stamp = 0
         
         
         ################# IGVC SelfDrive Quolification line stop test #20250530# #################
@@ -362,6 +365,47 @@ def robot_ctrl(self):
             target_rad = -lim_steer/180*math.pi
         '''
         
+        rh_obs = self.rh_obs
+        lh_obs = self.lh_obs
+        ch_obs = self.ch_obs
+        #c_obs_near = ( -50<obs_theta) * (obs_theta<  50) * (obs_dist<0.5)
+        #c_obs_back = ( -50<obs_theta) * (obs_theta<  50) * (obs_dist<0.4)
+        cf = 1.15
+                
+        if ~np.any(ch_obs) :
+            if np.any(rh_obs) and np.any(lh_obs):
+                target_theta = (target_rad) * (180 / math.pi)
+                print("--- Center --- Befor target_theta[deg]:",target_theta)
+                speed = 0.25
+                lh_obs_close = min(lh_obs[1,:]) # y0 lh min
+                rh_obs_close = max(rh_obs[1,:]) # y0 rh min
+                cy = cf
+                cx = (lh_obs_close + rh_obs_close) / 2
+                #c_point = (cx, cy)
+                target_rad = math.atan2(cx, cf)
+                target_theta = (target_rad) * (180 / math.pi)
+                print("--- Center --- After target_theta[deg]:",target_theta)
+                            
+            elif np.any(rh_obs) and ~np.any(lh_obs):
+                speed = 0.25
+                if 0 <= self.waypoint_number and self.waypoint_number <= 9:
+                    target_theta = (target_rad) * (180 / math.pi)
+                    print("!!!RH!!!! Befor target_theta[deg]:",target_theta)
+                    rh_obs_close = max(rh_obs[1,:]) # y0 rh min
+                    rh_dist = 0.65
+                    cy = cf
+                    cx = rh_obs_close + rh_dist
+                    target_rad = math.atan2(cx, cf)
+                    target_theta = (target_rad) * (180 / math.pi)
+                    print("!!!RH!!!! After target_theta[deg]:",target_theta)
+                    
+            
+            elif ~np.any(rh_obs) and np.any(lh_obs):
+                speed = 0.25
+            
+            #elif ~np.any(rh_obs) and ~np.any(lh_obs):
+            #    speed = 0.25
+                           
         lim_steer = 20
         #lim_steer = 30 24/11/29 ok
         #if abs(target_theta) < 10 and 0.0 < speed and speed < 0.5:
@@ -602,6 +646,7 @@ def obs_steer(self, msg):
         #print stamp message
         t_stamp = msg.header.stamp
         #print(f"t_stamp ={t_stamp}")
+        self.t_stamp = t_stamp
         
         #get pcd data
         points = self.pointcloud2_to_array(msg)
@@ -610,16 +655,34 @@ def obs_steer(self, msg):
         #map_obs
         self.obs_points = points
         
-        rh_obs = self.pcd_serch(points, -0.2,1,0,0.7)
+        rh_obs = self.pcd_serch(points, 1.0,1.3,-0.7,0)
         if len(rh_obs[0,:]) > 10:
             self.rh_obs = 1
         else:
             self.rh_obs = 0
-        lh_obs = self.pcd_serch(points, -0.2,1,-0.7,0)
+        lh_obs = self.pcd_serch(points, 1.0,1.3,0,0.7)
         if len(lh_obs[0,:]) > 10:
             self.lh_obs = 1
         else:
             self.lh_obs = 0
+        
+        ch_obs = self.pcd_serch(points, 1.0,1.3,-0.4,0.4)
+            
+        self.rh_obs = rh_obs
+        self.lh_obs = lh_obs
+        self.ch_obs = ch_obs
+        
+        #print(lh_obs.shape)
+        #print("x0 min",min(lh_obs[0,:]))
+        #print("x0 max",max(lh_obs[0,:]))
+        #print("y0 min",min(lh_obs[1,:])) ###ok!!!!!!!!!
+        #print("y0 max",max(lh_obs[1,:]))
+        
+        #print("y0 rh min",max(rh_obs[1,:]))
+        
+        #global test obs rviz2 kesu
+        obs_test_msg = point_cloud_intensity_msg(ch_obs.T, t_stamp, 'odom')
+        self.pcd_test_publisher.publish(obs_test_msg) 
 
     # pcd_serch を z も考慮する形で置換（点群抽出用ユーティリティ）
     def pcd_serch(self, pointcloud, x_min, x_max, y_min, y_max, z_min=None, z_max=None):
@@ -667,6 +730,41 @@ def quaternion_to_euler(x, y, z, w):
     pitch = np.arctan2(-rot_matrix[2, 0], np.sqrt(rot_matrix[2, 1]**2 + rot_matrix[2, 2]**2))
     yaw = np.arctan2(rot_matrix[1, 0], rot_matrix[0, 0])
     return roll, pitch, yaw
+
+def point_cloud_intensity_msg(points, t_stamp, parent_frame):
+    # In a PointCloud2 message, the point cloud is stored as an byte 
+    # array. In order to unpack it, we also include some parameters 
+    # which desribes the size of each individual point.
+    ros_dtype = sensor_msgs.PointField.FLOAT32
+    dtype = np.float32
+    itemsize = np.dtype(dtype).itemsize # A 32-bit float takes 4 bytes.
+    data = points.astype(dtype).tobytes() 
+
+    # The fields specify what the bytes represents. The first 4 bytes 
+    # represents the x-coordinate, the next 4 the y-coordinate, etc.
+    fields = [
+            sensor_msgs.PointField(name='x', offset=0, datatype=ros_dtype, count=1),
+            sensor_msgs.PointField(name='y', offset=4, datatype=ros_dtype, count=1),
+            sensor_msgs.PointField(name='z', offset=8, datatype=ros_dtype, count=1),
+            sensor_msgs.PointField(name='intensity', offset=12, datatype=ros_dtype, count=1),
+        ]
+
+    # The PointCloud2 message also has a header which specifies which 
+    # coordinate frame it is represented in. 
+    header = std_msgs.Header(frame_id=parent_frame, stamp=t_stamp)
+    
+
+    return sensor_msgs.PointCloud2(
+        header=header,
+        height=1, 
+        width=points.shape[0],
+        is_dense=True,
+        is_bigendian=False,
+        fields=fields,
+        point_step=(itemsize * 4), # Every point consists of three float32s.
+        row_step=(itemsize * 4 * points.shape[0]), 
+        data=data
+    )
         
 # mainという名前の関数です。C++のmain関数とは異なり、これは処理の開始地点ではありません。
 def main(args=None):
diff --git a/try_navigation/try_navigation/potential_astar.py b/try_navigation/try_navigation/potential_astar.py
index f39bb70..70370c4 100644
--- a/try_navigation/try_navigation/potential_astar.py
+++ b/try_navigation/try_navigation/potential_astar.py
@@ -425,6 +425,15 @@ def potential_astar(self, msg):
             elif self.obs_info[self.waypoint_number][self.dotline_info] == 1:
                 dot_line_local = localization_xyz(self.dot_obs_points, position_x, position_y, theta_x, theta_y, theta_z)
         
+        self_radius = 0.8
+        angle_min = -60 + 180
+        angle_max = 60 + 180
+        angles = np.linspace(np.radians(angle_min),np.radians(angle_max),100)
+        x_radius = self_radius * np.cos(angles)
+        y_radius = self_radius * np.sin(angles)
+        z_radius = 0 * np.cos(angles)
+        self_radius_points = np.vstack((x_radius, y_radius, z_radius))
+        
         """
         #map_obs add
         if len(self.map_obs_points[0,:])>0:
@@ -476,6 +485,7 @@ def potential_astar(self, msg):
             obs_points = np.insert(obs_points, len(obs_points[0,:]), dot_line_local.T, axis=1)
         
         obs_points = np.insert(obs_points, len(obs_points[0,:]), relative_point_rot.T, axis=1)
+        obs_points = np.insert(obs_points, len(obs_points[0,:]), self_radius_points.T, axis=1)
         #obs_points = np.insert(obs_points, len(obs_points[0,:]), self.tsukuba_obs.T, axis=1)
         points_round = np.round(obs_points * self.obs_pixel) / self.obs_pixel
         obs_xy_local = points_round[:,~pd.DataFrame({"x":points_round[0,:], "y":points_round[1,:]}).duplicated()]

From c9f957c16d61f66594b72dd2de37a513cf2f96ae Mon Sep 17 00:00:00 2001
From: kugurofu <tenten31569@icloud.com>
Date: Mon, 20 Oct 2025 09:05:45 +0000
Subject: [PATCH 15/25] fix stop point and add launch imu

---
 orange_bringup/launch/imu_data.launch.xml     | 52 +++++++++++++++++++
 .../orange_gnss/gnss_odom_publisher_ttyUSB.py |  2 +-
 .../try_navigation/path_follower.py           | 18 +++----
 .../try_navigation/potential_astar.py         | 11 ++--
 4 files changed, 69 insertions(+), 14 deletions(-)
 create mode 100644 orange_bringup/launch/imu_data.launch.xml

diff --git a/orange_bringup/launch/imu_data.launch.xml b/orange_bringup/launch/imu_data.launch.xml
new file mode 100644
index 0000000..2879952
--- /dev/null
+++ b/orange_bringup/launch/imu_data.launch.xml
@@ -0,0 +1,52 @@
+<?xml version="1.0"?>
+<launch>
+  <!--# Control mode
+      # 1: relative position control mode
+      # Default: Subscribe to "/zlac8015d/pos/cmd_deg" and "/zlac8015d/pos/cmd_dist"
+      # 3: Speed rpm control mode
+      # Default: Subscribe to "/zlac8015d/twist/cmd_vel", "/zlac8015d/vel/cmd_vel" and "/zlac8015d/vel/cmd_rpm"-->
+  <arg name="control_mode" default="3"/>
+  <arg name="debug_motor" default="false"/>
+  <arg name="debug_imu" default="false"/>
+  <arg name="publish_TF" default="false"/>
+  <arg name="publish_odom" default="true"/>
+  <arg name="use_sim_time" default="false"/>
+  <arg name="twist_cmd_vel_topic" default="/cmd_vel"/>
+  <arg name="cmd_vel_topic" default="/vel/cmd_vel"/>
+  <arg name="cmd_rpm_topic" default="/vel/cmd_rpm"/>
+  <arg name="cmd_deg_topic" default="/pos/cmd_deg"/>
+  <arg name="cmd_dist_topic" default="/pos/cmd_dist"/>
+  <arg name="odom_topic" default="/odom"/>
+  <arg name="imu_topic" default="/imu"/>
+  <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"/>
+  <arg name="velodyne_points_topic" default="/velodyne_points"/>
+  <arg name="livox_scan_topic" default="/livox_scan"/>
+  <arg name="livox_points_topic" default="/livox_points"/>
+  <arg name="merged_cloud_topic" default="/merged_cloud"/>
+  <arg name="merged_scan_topic" default="/merged_scan"/>
+  <arg name="nav_topic" default="/nav_state"/>
+  <arg name="estop_topic" default="/estop"/>
+  <arg name="xacro_file_path" default="$(find-pkg-share orange_description)/xacro/orange_robot.xacro"/>
+  <arg name="motor_driver_config_file_path" default="$(find-pkg-share orange_bringup)/config/motor_driver_params.yaml"/>
+  
+  <!-- madgwick filter node -->
+  <node pkg="imu_filter_madgwick" exec="imu_filter_madgwick_node" name="madgwick_filter">
+    <param from="$(find-pkg-share cxd5602pwbimu)/config/madgwick.yaml"/>
+    <remap from="/imu/data_raw" to="/imu/spresense"/>
+    <remap from="/imu/data" to="/imu/spresense/madgwick"/>
+  </node>
+  <!-- initial_yaw_corrector -->
+  <node pkg="cxd5602pwbimu" exec="initial_yaw_corrector" name="spimu_yaw_corrector">
+    <param name="input_topic" value="/imu/spresense/madgwick"/>
+    <param name="output_topic" value="/imu/spresense/inityaw"/>
+  </node>
+  <!-- wheel_imu_odom for spresense imu -->
+  <node pkg="cxd5602pwbimu" exec="wheel_spimu_odom">
+    <param name="odom_topic" value="/odom"/>
+    <param name="imu_topic" value="/imu/spresense/inityaw"/>
+    <param name="publish_TF" value="false"/>
+  </node>
+</launch>
diff --git a/orange_gnss/orange_gnss/gnss_odom_publisher_ttyUSB.py b/orange_gnss/orange_gnss/gnss_odom_publisher_ttyUSB.py
index af1e6bc..e204fe6 100644
--- a/orange_gnss/orange_gnss/gnss_odom_publisher_ttyUSB.py
+++ b/orange_gnss/orange_gnss/gnss_odom_publisher_ttyUSB.py
@@ -110,7 +110,7 @@ def acquire_gps_data(self):
 
         if count > 0:
             #self.initial_coordinate = [lat_sum / count, lon_sum / count] # calculate average
-            self.initial_coordinate = [36.04974095972727, 140.04593633886364 ] # tsukuba start point 36.0497502, 140.0459234 36.04972947264545, 140.04590687627274 36.04974095972727, 140.04593633886364 , left 36.04976195993636, 140.04593755179093/nakaniwa 35.4257898377487,139.313807281254 /35.425952230280004, 139.31380123427
+            self.initial_coordinate = [36.04974095972727, 140.04593633886364 ] # tsukuba start point right 36.04974095972727, 140.04593633886364 , left 36.04976195993636, 140.04593755179093/nakaniwa 35.4257898377487,139.313807281254 /35.425952230280004, 139.31380123427
             self.current_coordinate = [lat_sum / count, lon_sum / count] # for tsukuba
             self.theta = (heading_sum / count) - 90
             self.initialized = True
diff --git a/try_navigation/try_navigation/path_follower.py b/try_navigation/try_navigation/path_follower.py
index 4344c39..1445612 100644
--- a/try_navigation/try_navigation/path_follower.py
+++ b/try_navigation/try_navigation/path_follower.py
@@ -134,7 +134,7 @@ def __init__(self):
             #[-69.5,	-62,	-47.5,	-27.5, 1.0], #tyokusen4 ||| [-67.5,	-62,	-47.5,	-27.5, 1.0]before:[-67.0,	-62,	-47.5,	-27.5, 1.0], 
             #[-55,	-35,	41,	46,    1.0], #Goal
             
-            [63.0, 65.0, 19.0, 39.0, 1.0], #shiyakusyo
+            [63.5, 65.0, 19.0, 39.0, 1.0], #shiyakusyo
             [ 999,  999, 999, 999, 0.0] ]) #
         self.stop_num = 0;
         
@@ -170,12 +170,12 @@ def __init__(self):
         #################################################################################
         
         ################# IGVC SelfDrive Full #20250601# #################
-        self.sd_full_flag = 1 #root flag
+        self.sd_full_flag = 0 #root flag
         self.waypoint_number = 0
-        self.sd_full_human_stop = 1  #sub flag
+        self.sd_full_human_stop = 0  #sub flag
         if self.sd_full_human_stop == 1:
             self.sd_c_obs_stop_dist = self.sd_human_stop_dist
-        self.sd_full_sign_stop = 1 #sub flag
+        self.sd_full_sign_stop = 0 #sub flag
         if self.sd_full_sign_stop == 1:
             dist = 0.5 + 0.4 + 0.5# eria +top +delay
             sd_full_stop_xy = [-32.37441428909107, -16.465277566213718, 0.0]
@@ -375,7 +375,7 @@ def robot_ctrl(self):
         if ~np.any(ch_obs) :
             if np.any(rh_obs) and np.any(lh_obs):
                 target_theta = (target_rad) * (180 / math.pi)
-                print("--- Center --- Befor target_theta[deg]:",target_theta)
+                #print("--- Center --- Befor target_theta[deg]:",target_theta)
                 speed = 0.25
                 lh_obs_close = min(lh_obs[1,:]) # y0 lh min
                 rh_obs_close = max(rh_obs[1,:]) # y0 rh min
@@ -384,20 +384,20 @@ def robot_ctrl(self):
                 #c_point = (cx, cy)
                 target_rad = math.atan2(cx, cf)
                 target_theta = (target_rad) * (180 / math.pi)
-                print("--- Center --- After target_theta[deg]:",target_theta)
+                #print("--- Center --- After target_theta[deg]:",target_theta)
                             
             elif np.any(rh_obs) and ~np.any(lh_obs):
                 speed = 0.25
                 if 0 <= self.waypoint_number and self.waypoint_number <= 9:
                     target_theta = (target_rad) * (180 / math.pi)
-                    print("!!!RH!!!! Befor target_theta[deg]:",target_theta)
+                    #print("!!!RH!!!! Befor target_theta[deg]:",target_theta)
                     rh_obs_close = max(rh_obs[1,:]) # y0 rh min
                     rh_dist = 0.65
                     cy = cf
                     cx = rh_obs_close + rh_dist
                     target_rad = math.atan2(cx, cf)
                     target_theta = (target_rad) * (180 / math.pi)
-                    print("!!!RH!!!! After target_theta[deg]:",target_theta)
+                    #print("!!!RH!!!! After target_theta[deg]:",target_theta)
                     
             
             elif ~np.any(rh_obs) and np.any(lh_obs):
@@ -618,7 +618,6 @@ def get_odom_ref(self, msg):
         self.ref_theta_y = 0 #pitch /math.pi*180
         self.ref_theta_z = yaw /math.pi*180
         
-        
         if ((self.stop_xy[self.stop_num,0] < self.ref_position_x) and (self.ref_position_x < self.stop_xy[self.stop_num,1]) and (self.stop_xy[self.stop_num,2] < self.ref_position_y) and (self.ref_position_y < self.stop_xy[self.stop_num,3]) ) or ((self.stop_xy[self.stop_num,0] < self.position_x) and (self.position_x < self.stop_xy[self.stop_num,1]) and (self.stop_xy[self.stop_num,2] < self.position_y) and (self.position_y < self.stop_xy[self.stop_num,3]) ):
             if self.stop_xy[self.stop_num,4] > 0:
                 self.get_logger().info('####### stop flag on %f #######' % (self.stop_num))
@@ -626,7 +625,6 @@ def get_odom_ref(self, msg):
             else:
                 self.get_logger().info('####### through flag on %f #######' % (self.stop_num))
             self.stop_num = self.stop_num + 1;
-            
         
     def pointcloud2_to_array(self, cloud_msg):
         # Extract point cloud data
diff --git a/try_navigation/try_navigation/potential_astar.py b/try_navigation/try_navigation/potential_astar.py
index 70370c4..61ce66d 100644
--- a/try_navigation/try_navigation/potential_astar.py
+++ b/try_navigation/try_navigation/potential_astar.py
@@ -143,6 +143,11 @@ def __init__(self):
         self.left_obs_points = np.array([[],[],[]])
         self.dot_obs_points = np.array([[],[],[]])
         
+        # behind robot obs
+        self.self_radius = 0.8
+        self.angle_min = -60 + 180
+        self.angle_max = 60 + 180
+        
         #DRIVE MODE
         self.functions_test = 0 #autonav:1 selfdrive:0
         
@@ -425,9 +430,9 @@ def potential_astar(self, msg):
             elif self.obs_info[self.waypoint_number][self.dotline_info] == 1:
                 dot_line_local = localization_xyz(self.dot_obs_points, position_x, position_y, theta_x, theta_y, theta_z)
         
-        self_radius = 0.8
-        angle_min = -60 + 180
-        angle_max = 60 + 180
+        self_radius = self.self_radius
+        angle_min = self.angle_min
+        angle_max = self.angle_max
         angles = np.linspace(np.radians(angle_min),np.radians(angle_max),100)
         x_radius = self_radius * np.cos(angles)
         y_radius = self_radius * np.sin(angles)

From 1d79f096b610b8aa13a73302b8370cbff11ba019 Mon Sep 17 00:00:00 2001
From: kugurofu <tenten31569@icloud.com>
Date: Tue, 21 Oct 2025 06:47:10 +0000
Subject: [PATCH 16/25] add stop point in pathfollower and add some map_obs

---
 try_navigation/config/config.rviz             | 588 +++++++++++++++++-
 .../try_navigation/path_follower.py           |  19 +-
 .../try_navigation/potential_astar.py         |  37 +-
 3 files changed, 612 insertions(+), 32 deletions(-)

diff --git a/try_navigation/config/config.rviz b/try_navigation/config/config.rviz
index 0079548..d1c2869 100644
--- a/try_navigation/config/config.rviz
+++ b/try_navigation/config/config.rviz
@@ -6,8 +6,16 @@ Panels:
       Expanded:
         - /Global Options1
         - /Status1
+        - /fusion/odom1/Shape1
+        - /fusion/odom1/Covariance1
+        - /GPSodom1/Shape1
+        - /imu_odom1/Shape1
+        - /PointCloud21
+        - /Odometry2/Shape1
+        - /Odometry3/Shape1
+        - /PointCloud22
       Splitter Ratio: 0.5
-    Tree Height: 549
+    Tree Height: 580
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -25,7 +33,7 @@ Panels:
     Experimental: false
     Name: Time
     SyncMode: 0
-    SyncSource: ""
+    SyncSource: pcd_obs_global
 Visualization Manager:
   Class: ""
   Displays:
@@ -47,10 +55,558 @@ Visualization Manager:
       Plane Cell Count: 10
       Reference Frame: <Fixed Frame>
       Value: true
+    - Angle Tolerance: 0.10000000149011612
+      Class: rviz_default_plugins/Odometry
+      Covariance:
+        Orientation:
+          Alpha: 0.5
+          Color: 255; 255; 127
+          Color Style: Unique
+          Frame: Local
+          Offset: 1
+          Scale: 1
+          Value: true
+        Position:
+          Alpha: 0.30000001192092896
+          Color: 204; 51; 204
+          Scale: 1
+          Value: true
+        Value: true
+      Enabled: true
+      Keep: 0
+      Name: Odometry
+      Position Tolerance: 0.10000000149011612
+      Shape:
+        Alpha: 1
+        Axes Length: 1
+        Axes Radius: 0.10000000149011612
+        Color: 255; 25; 0
+        Head Length: 0.30000001192092896
+        Head Radius: 0.10000000149011612
+        Shaft Length: 1
+        Shaft Radius: 0.05000000074505806
+        Value: Arrow
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /odom
+      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/PointCloud2
+      Color: 249; 240; 107
+      Color Transformer: FlatColor
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: pcd_obs_global
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.05000000074505806
+      Style: Flat Squares
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /pcd_obs_global
+      Use Fixed Frame: true
+      Use rainbow: true
+      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/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 0
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: white_buff
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.10000000149011612
+      Style: Flat Squares
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /white_buff
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz_default_plugins/Path
+      Color: 25; 255; 0
+      Enabled: true
+      Head Diameter: 0.30000001192092896
+      Head Length: 0.20000000298023224
+      Length: 0.30000001192092896
+      Line Style: Lines
+      Line Width: 0.029999999329447746
+      Name: Path
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Color: 255; 85; 255
+      Pose Style: None
+      Radius: 0.029999999329447746
+      Shaft Diameter: 0.10000000149011612
+      Shaft Length: 0.10000000149011612
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /potential_astar_path
+      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/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: ""
+      Decay Time: 0
+      Enabled: false
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: pothole
+      Position Transformer: ""
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /pothole_points
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: false
+    - Alpha: 0.699999988079071
+      Class: rviz_default_plugins/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: true
+      Name: globalmap
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /reflect_map_global
+      Update Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /reflect_map_global_updates
+      Use Timestamp: false
+      Value: true
+    - Alpha: 0.699999988079071
+      Class: rviz_default_plugins/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: true
+      Name: localmap
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /reflect_map_local
+      Update Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /reflect_map_local_updates
+      Use Timestamp: false
+      Value: true
+    - Angle Tolerance: 0.10000000149011612
+      Class: rviz_default_plugins/Odometry
+      Covariance:
+        Orientation:
+          Alpha: 0.5
+          Color: 255; 255; 127
+          Color Style: Unique
+          Frame: Local
+          Offset: 1
+          Scale: 1
+          Value: true
+        Position:
+          Alpha: 0.30000001192092896
+          Color: 204; 51; 204
+          Scale: 1
+          Value: true
+        Value: true
+      Enabled: true
+      Keep: 0
+      Name: fusion/odom
+      Position Tolerance: 0.10000000149011612
+      Shape:
+        Alpha: 1
+        Axes Length: 1
+        Axes Radius: 0.10000000149011612
+        Color: 220; 138; 221
+        Head Length: 0.30000001192092896
+        Head Radius: 0.10000000149011612
+        Shaft Length: 1
+        Shaft Radius: 0.05000000074505806
+        Value: Arrow
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /fusion/odom
+      Value: true
+    - Angle Tolerance: 0.10000000149011612
+      Class: rviz_default_plugins/Odometry
+      Covariance:
+        Orientation:
+          Alpha: 0.5
+          Color: 255; 255; 127
+          Color Style: Unique
+          Frame: Local
+          Offset: 1
+          Scale: 1
+          Value: true
+        Position:
+          Alpha: 0.30000001192092896
+          Color: 204; 51; 204
+          Scale: 1
+          Value: true
+        Value: true
+      Enabled: true
+      Keep: 0
+      Name: GPSodom
+      Position Tolerance: 0.10000000149011612
+      Shape:
+        Alpha: 1
+        Axes Length: 1
+        Axes Radius: 0.10000000149011612
+        Color: 98; 160; 234
+        Head Length: 0.30000001192092896
+        Head Radius: 0.10000000149011612
+        Shaft Length: 1
+        Shaft Radius: 0.05000000074505806
+        Value: Arrow
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /odom/UM982
+      Value: true
+    - Angle Tolerance: 0.10000000149011612
+      Class: rviz_default_plugins/Odometry
+      Covariance:
+        Orientation:
+          Alpha: 0.5
+          Color: 255; 255; 127
+          Color Style: Unique
+          Frame: Local
+          Offset: 1
+          Scale: 1
+          Value: true
+        Position:
+          Alpha: 0.30000001192092896
+          Color: 204; 51; 204
+          Scale: 1
+          Value: true
+        Value: true
+      Enabled: true
+      Keep: 0
+      Name: imu_odom
+      Position Tolerance: 0.10000000149011612
+      Shape:
+        Alpha: 1
+        Axes Length: 1
+        Axes Radius: 0.10000000149011612
+        Color: 249; 240; 107
+        Head Length: 0.30000001192092896
+        Head Radius: 0.10000000149011612
+        Shaft Length: 1
+        Shaft Radius: 0.05000000074505806
+        Value: Arrow
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /odom/wheel_imu
+      Value: true
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz_default_plugins/Path
+      Color: 153; 193; 241
+      Enabled: true
+      Head Diameter: 0.30000001192092896
+      Head Length: 0.20000000298023224
+      Length: 0.30000001192092896
+      Line Style: Lines
+      Line Width: 0.029999999329447746
+      Name: waypoint_path
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Color: 255; 85; 255
+      Pose Style: None
+      Radius: 0.029999999329447746
+      Shaft Diameter: 0.10000000149011612
+      Shaft Length: 0.10000000149011612
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /waypoint_path
+      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/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: ""
+      Decay Time: 0
+      Enabled: false
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: tire
+      Position Transformer: ""
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /tire_points
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: false
+    - 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/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 3.704611301422119
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /pcd_obs_global
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Angle Tolerance: 0.10000000149011612
+      Class: rviz_default_plugins/Odometry
+      Covariance:
+        Orientation:
+          Alpha: 0.5
+          Color: 255; 255; 127
+          Color Style: Unique
+          Frame: Local
+          Offset: 1
+          Scale: 1
+          Value: true
+        Position:
+          Alpha: 0.30000001192092896
+          Color: 204; 51; 204
+          Scale: 1
+          Value: true
+        Value: true
+      Enabled: true
+      Keep: 0
+      Name: Odometry
+      Position Tolerance: 0.10000000149011612
+      Shape:
+        Alpha: 1
+        Axes Length: 1
+        Axes Radius: 0.10000000149011612
+        Color: 255; 190; 111
+        Head Length: 0.30000001192092896
+        Head Radius: 0.10000000149011612
+        Shaft Length: 1
+        Shaft Radius: 0.05000000074505806
+        Value: Arrow
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /odom/wheel_spimu
+      Value: true
+    - Angle Tolerance: 0.10000000149011612
+      Class: rviz_default_plugins/Odometry
+      Covariance:
+        Orientation:
+          Alpha: 0.5
+          Color: 255; 255; 127
+          Color Style: Unique
+          Frame: Local
+          Offset: 1
+          Scale: 1
+          Value: true
+        Position:
+          Alpha: 0.30000001192092896
+          Color: 204; 51; 204
+          Scale: 1
+          Value: true
+        Value: true
+      Enabled: true
+      Keep: 0
+      Name: Odometry
+      Position Tolerance: 0.10000000149011612
+      Shape:
+        Alpha: 1
+        Axes Length: 1
+        Axes Radius: 0.10000000149011612
+        Color: 51; 209; 122
+        Head Length: 0.30000001192092896
+        Head Radius: 0.10000000149011612
+        Shaft Length: 1
+        Shaft Radius: 0.05000000074505806
+        Value: Arrow
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /odom/combine
+      Value: true
+    - Class: rviz_default_plugins/MarkerArray
+      Enabled: true
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /wall_follow_markers
+      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/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: FlatColor
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: -999999
+      Min Color: 0; 0; 0
+      Min Intensity: 999999
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.20000000298023224
+      Style: Flat Squares
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /pcd_test_global
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
-    Fixed Frame: map
+    Fixed Frame: odom
     Frame Rate: 30
   Name: root
   Tools:
@@ -93,33 +649,33 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/Orbit
-      Distance: 10
+      Distance: 21.29517364501953
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 0
-        Y: 0
-        Z: 0
+        X: 5.204740047454834
+        Y: 6.645839214324951
+        Z: 0.015708111226558685
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.785398006439209
+      Pitch: 1.5647963285446167
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 0.785398006439209
+      Yaw: 3.1453959941864014
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 846
+  Height: 877
   Hide Left Dock: false
-  Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002fb00fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000002cffc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002cf000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000251fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000251000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d0065010000000000000738000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005dc000002cf00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -127,7 +683,7 @@ Window Geometry:
   Tool Properties:
     collapsed: false
   Views:
-    collapsed: false
-  Width: 1200
-  X: 60
-  Y: 57
+    collapsed: true
+  Width: 1848
+  X: 0
+  Y: 28
diff --git a/try_navigation/try_navigation/path_follower.py b/try_navigation/try_navigation/path_follower.py
index 1445612..cd2625b 100644
--- a/try_navigation/try_navigation/path_follower.py
+++ b/try_navigation/try_navigation/path_follower.py
@@ -134,8 +134,23 @@ def __init__(self):
             #[-69.5,	-62,	-47.5,	-27.5, 1.0], #tyokusen4 ||| [-67.5,	-62,	-47.5,	-27.5, 1.0]before:[-67.0,	-62,	-47.5,	-27.5, 1.0], 
             #[-55,	-35,	41,	46,    1.0], #Goal
             
-            [63.5, 65.0, 19.0, 39.0, 1.0], #shiyakusyo
-            [ 999,  999, 999, 999, 0.0] ]) #
+            [ 63.5,  65.0,  19.0,  39.0, 1.0], #shiyakusyo
+            [ 99.5, 101.0,  25.0,  45.0, 1.0], #dourotan1
+            [177.0, 178.5,  25.0,  45.0, 1.0], #dourotan2
+            [257.5, 277.5, -60.0, -58.5, 1.0], #singoumaeteisisen1
+            [257.5, 277.5, -67.0, -65.5, 1.0], #singoumae1
+            [256.5, 276.5, -87.5, -86.0, 1.0], #singoumaeteisisen2
+            [270.0, 271.0, -99.0, -79.5, 1.0], #singoumae2
+            [405.0, 425.0, -80.5, -79.5, 1.0], #ekimae oudanhodou1
+            [405.0, 425.0, -71.0, -70.0, 1.0], #ekimae oudanhodou2
+            [289.0, 290.0, -98.0, -78.0, 1.0], #singoumaeteisisen3
+            [284.0, 285.0, -98.0, -78.0, 1.0], #singoumae3
+            [259.5, 279.5, -83.0, -82.0, 1.0], #singoumaeteisisen4
+            [259.5, 279.5, -80.0, -79.0, 1.0], #singoumae4
+            [184.5, 186.0,  25.0,  45.0, 1.0], #dourotan3
+            [107.0, 108.5,  25.0,  45.0, 1.0], #dourotan4
+            [ 74.0,  94.0, -27.0, -26.0, 1.0], #GOAL!!!!
+            [  999,   999,   999,   999, 0.0] ]) #
         self.stop_num = 0;
         
         #obs
diff --git a/try_navigation/try_navigation/potential_astar.py b/try_navigation/try_navigation/potential_astar.py
index 61ce66d..ace4fde 100644
--- a/try_navigation/try_navigation/potential_astar.py
+++ b/try_navigation/try_navigation/potential_astar.py
@@ -143,11 +143,6 @@ def __init__(self):
         self.left_obs_points = np.array([[],[],[]])
         self.dot_obs_points = np.array([[],[],[]])
         
-        # behind robot obs
-        self.self_radius = 0.8
-        self.angle_min = -60 + 180
-        self.angle_max = 60 + 180
-        
         #DRIVE MODE
         self.functions_test = 0 #autonav:1 selfdrive:0
         
@@ -187,11 +182,12 @@ def __init__(self):
             [   0,      0,    0,       0,        1,        0,       0,      0]  # waypoint 21 GOAL!!!!!!
         ]
         
-        tukuba_obs_x =np.linspace(-45,25,860);
-        tukuba_obs_y =np.linspace(97,94,860);
-        tukuba_obs_z =np.linspace(0,0,860);
-        self.tsukuba_obs = np.array([tukuba_obs_x, tukuba_obs_y, tukuba_obs_z]);
-        
+        #tukuba_obs_x =np.linspace(-45,25,860);
+        #tukuba_obs_y =np.linspace(97,94,860);
+        #tukuba_obs_z =np.linspace(0,0,860);
+        #self.tsukuba_obs = np.array([tukuba_obs_x, tukuba_obs_y, tukuba_obs_z]);
+        self.tsukuba_obs = np.array([[],[],[]])
+               
         
         ################# IGVC SelfDrive Quolification line stop test #20250530# #################
         self.sd_line_stop_test = 0
@@ -362,7 +358,7 @@ def get_dot_obs(self, msg):
         #print(f"points ={points.shape}")
         
         self.dot_obs_points = np.vstack((points[0,:], points[1,:], points[2,:]))
-        
+            
     def potential_astar(self, msg):
         
         #print stamp message
@@ -430,9 +426,9 @@ def potential_astar(self, msg):
             elif self.obs_info[self.waypoint_number][self.dotline_info] == 1:
                 dot_line_local = localization_xyz(self.dot_obs_points, position_x, position_y, theta_x, theta_y, theta_z)
         
-        self_radius = self.self_radius
-        angle_min = self.angle_min
-        angle_max = self.angle_max
+        self_radius = 0.8
+        angle_min = -60 + 180
+        angle_max = 60 + 180
         angles = np.linspace(np.radians(angle_min),np.radians(angle_max),100)
         x_radius = self_radius * np.cos(angles)
         y_radius = self_radius * np.sin(angles)
@@ -449,6 +445,14 @@ def potential_astar(self, msg):
         else:
             relative_point_rot = np.array([[],[],[]])
         """    
+        
+        # make map_obs   x1  x2  y1  y2
+        obs1 = make_obs(-45, 25, 97, 94) # siyakusyoura minami
+        obs2 = make_obs(  5, 21, 100, 101) # siyakusyoura kita
+        obs3 = make_obs(-73,-75, 105, 29) # siyakusyo nisi
+
+        self.tsukuba_obs = np.hstack((obs1, obs2, obs3))
+        
         #map_obs add
         if len(self.tsukuba_obs[0,:])>0:
             relative_point_x = self.tsukuba_obs[0,:] - self.position_x
@@ -748,6 +752,11 @@ def path_msg(waypoints, stamp, parent_frame):
         wp_msg.poses.append(waypoint)
     return wp_msg
 
+def make_obs(x1, x2, y1, y2, n=860, z=0):
+    x = np.linspace(x1, x2, n)
+    y = np.linspace(y1, y2, n)
+    z = np.full(n, z)
+    return np.array([x, y, z])
 
 # mainという名前の関数です。C++のmain関数とは異なり、これは処理の開始地点ではありません。
 def main(args=None):

From 3e460abb0d6b305d076c5ab8aca5e21ea5c78dc6 Mon Sep 17 00:00:00 2001
From: kugurofu <tenten31569@icloud.com>
Date: Fri, 24 Oct 2025 08:41:41 +0000
Subject: [PATCH 17/25] add launch parameter

---
 orange_bringup/launch/orange_robot.launch.xml |  6 ++-
 .../orange_gnss/gnss_odom_publisher_ttyUSB.py |  9 ++++-
 pcd_convert/pcd_convert/pcd_rotation.py       |  9 +++--
 .../launch/try_navigation.launch.py           | 20 ++++++++--
 .../try_navigation/ekf_myself_gps.py          | 40 +++++++++++++++++--
 5 files changed, 71 insertions(+), 13 deletions(-)

diff --git a/orange_bringup/launch/orange_robot.launch.xml b/orange_bringup/launch/orange_robot.launch.xml
index 37f5ec7..0c66fcc 100644
--- a/orange_bringup/launch/orange_robot.launch.xml
+++ b/orange_bringup/launch/orange_robot.launch.xml
@@ -110,14 +110,18 @@
     <arg name="fusion_odom_out" value="$(var fusion_odom_topic)"/>
   </include>-->
   <!-- GPSposition_publisher -->
-  <!-- initial heading:housei nakaniwa=179.169287 tsukuba= 276.5 -->
   <!-- GPSheading_publisher -->
+  <!-- initial heading:housei nakaniwa=179.169287 tsukuba= 93.0 -->
+  <!-- initial start_lat:housei nakaniwa=35.425952230280004 tsukuba right=36.04974095972727 tsukuba_left=36.04976195993636 -->
+  <!-- initial start_lon:housei nakaniwa=139.31380123427 tsukuba right=140.04593633886364 tsukuba_left=140.04593755179093 -->
   <node pkg="orange_gnss" exec="gnss_odom_publisher_ttyUSB" output="screen">
     <param name="port" value="/dev/sensors/GNSS_UM982"/>
     <param name="baud" value="115200"/>
     <param name="country_id" value="0"/>
     <param name="Position_magnification" value="1.675"/>
     <param name="heading" value="93.0"/>
+    <param name="start_lat" value="35.425952230280004"/>
+    <param name="start_lon" value="139.31380123427"/>
     <param name="time_out" value="1.0"/>
   </node>
   <!-- 
diff --git a/orange_gnss/orange_gnss/gnss_odom_publisher_ttyUSB.py b/orange_gnss/orange_gnss/gnss_odom_publisher_ttyUSB.py
index e204fe6..755da51 100644
--- a/orange_gnss/orange_gnss/gnss_odom_publisher_ttyUSB.py
+++ b/orange_gnss/orange_gnss/gnss_odom_publisher_ttyUSB.py
@@ -20,15 +20,20 @@ def __init__(self):
         self.declare_parameter('country_id', 0)
         self.declare_parameter('Position_magnification', 1.675)
         self.declare_parameter('heading', 90.0)
+        self.declare_parameter('start_lat', 35.425952230280004) # tsukuba start point right 36.04974095972727, 140.04593633886364 , left 36.04976195993636, 140.04593755179093/nakaniwa 35.4257898377487,139.313807281254 /35.425952230280004, 139.31380123427
+        self.declare_parameter('start_lon', 139.31380123427)
 
         self.dev_name = self.get_parameter('port').get_parameter_value().string_value
         self.serial_baud = self.get_parameter('baud').get_parameter_value().integer_value
         self.country_id = self.get_parameter('country_id').get_parameter_value().integer_value
         self.Position_magnification = self.get_parameter('Position_magnification').get_parameter_value().double_value
         #self.theta = self.get_parameter('heading').get_parameter_value().double_value
-        self.tsukuba_theta=93.0 # nakaniwa 180 tsukuba 93
+        self.tsukuba_theta= self.get_parameter('heading').get_parameter_value().double_value # nakaniwa 180 tsukuba 93
 
         self.initial_coordinate = None
+        self.start_lat = self.get_parameter('start_lat').get_parameter_value().double_value
+        self.start_lon = self.get_parameter('start_lon').get_parameter_value().double_value
+        self.start_GPS_coordinate = [self.start_lat, self.start_lon]
         self.fix_data = None
         self.count = 0
         
@@ -110,7 +115,7 @@ def acquire_gps_data(self):
 
         if count > 0:
             #self.initial_coordinate = [lat_sum / count, lon_sum / count] # calculate average
-            self.initial_coordinate = [36.04974095972727, 140.04593633886364 ] # tsukuba start point right 36.04974095972727, 140.04593633886364 , left 36.04976195993636, 140.04593755179093/nakaniwa 35.4257898377487,139.313807281254 /35.425952230280004, 139.31380123427
+            self.initial_coordinate = self.start_GPS_coordinate
             self.current_coordinate = [lat_sum / count, lon_sum / count] # for tsukuba
             self.theta = (heading_sum / count) - 90
             self.initialized = True
diff --git a/pcd_convert/pcd_convert/pcd_rotation.py b/pcd_convert/pcd_convert/pcd_rotation.py
index bbc1679..60ed85e 100644
--- a/pcd_convert/pcd_convert/pcd_rotation.py
+++ b/pcd_convert/pcd_convert/pcd_rotation.py
@@ -40,15 +40,15 @@ def __init__(self):
         
         #パラメータ
         #set LiDAR position
-        self.MID360_HIGHT = 980/1000; #hight position[m]
+        self.MID360_HIGHT = 950.8/1000; #hight position[m] 0.9508604675798957
             
         #上下反転  LiDAR init
         self.THETA_INIT_X = 180 #[deg]
-        self.THETA_INIT_Y = 0 #[deg]
+        self.THETA_INIT_Y = 2.663 #[deg] 2.663001576990896
         self.THETA_INIT_Z = 0 #[deg]
         
         #initialize calibration
-        self.initialize_calibration = 0
+        self.initialize_calibration = 1
         self.pcd_buff = np.array([[],[],[],[]]);
         self.x1_init_point = 1.5
         self.x2_init_point = 2.5
@@ -102,7 +102,7 @@ def pcd_rotation(self, msg):
                 x2_max = self.x2_init_point + self.x_range;
                 y_min = self.y_init_point  - self.y_range;
                 y_max = self.y_init_point  + self.y_range;
-                #print(f"x1_min,x1_max,x2_min,x2_max,y_min,y_max ={ x1_min, x1_max, x2_min, x2_max, y_min, y_max}")
+                print(f"x1_min,x1_max,x2_min,x2_max,y_min,y_max ={ x1_min, x1_max, x2_min, x2_max, y_min, y_max}")
                 pcd_x1_ind = self.pcd_serch(self.pcd_buff, x1_min, x1_max, y_min, y_max)
                 pcd_x2_ind = self.pcd_serch(self.pcd_buff, x2_min, x2_max, y_min, y_max)
                 #print(f"len(self.pcd_buff[0,pcd_x1_ind]), {len(self.pcd_buff[0,pcd_x1_ind])}")
@@ -135,6 +135,7 @@ def pcd_rotation(self, msg):
                     self.THETA_INIT_Y = self.THETA_INIT_Y + theta #[deg]
                     self.initialize_calibration = 1
                     print(f"self.MID360_HIGHT ={self.MID360_HIGHT}")
+                    print(f"self.THETA_INIT_Y ={self.THETA_INIT_Y}")
                     
         
         #add mid height position
diff --git a/try_navigation/launch/try_navigation.launch.py b/try_navigation/launch/try_navigation.launch.py
index 5b275fe..90901ab 100644
--- a/try_navigation/launch/try_navigation.launch.py
+++ b/try_navigation/launch/try_navigation.launch.py
@@ -36,10 +36,15 @@ def generate_launch_description():
         description='waypoint name'
     )
     
-    
+    waypoint_start_index = LaunchConfiguration('waypoint_start_index')
+    declare_waypoint_start_arg = DeclareLaunchArgument(
+        'waypoint_start_index',
+        default_value= '0',
+        description='waypoint start index'
+    )
     
     return LaunchDescription([
-        declare_odom_arg, declare_waypoint_arg,
+        declare_odom_arg, declare_waypoint_arg, declare_waypoint_start_arg,
         
         #rviz2
         Node(package='rviz2',
@@ -60,6 +65,14 @@ def generate_launch_description():
             output='screen',
             arguments=[]
         ),
+        
+        #odom combination
+        Node(package='orange_gnss',
+            executable='odom_combination',
+            name='odom_combination',
+            output='screen',
+            arguments=[],
+        ),
    
         #gps ekf edit
         Node(package='try_navigation',
@@ -91,7 +104,8 @@ def generate_launch_description():
             name='gps_waypoint',
             output='screen',
             parameters=[{'odom': odom},
-                        {'waypoint_path': waypoint_path}],
+                        {'waypoint_path': waypoint_path},
+                        {'waypoint_start_index': waypoint_start_index}],
             arguments=[],
         ),
         # $ ros2 run navigation_control gps_waypoint
diff --git a/try_navigation/try_navigation/ekf_myself_gps.py b/try_navigation/try_navigation/ekf_myself_gps.py
index cc5555f..3dcf895 100644
--- a/try_navigation/try_navigation/ekf_myself_gps.py
+++ b/try_navigation/try_navigation/ekf_myself_gps.py
@@ -67,7 +67,7 @@ def __init__(self):
         
 
         self.sub_a = self.create_subscription(
-            Odometry, '/odom/wheel_spimu', self.sensor_a_callback, 10) # /odom/wheel_spimu
+            Odometry, '/odom/combine', self.sensor_a_callback, 10) # /odom/wheel_spimu /odom/combine
         self.sub_b = self.create_subscription(
             Odometry, '/odom/UM982', self.sensor_b_callback, 10)
         #self.sub_b = self.create_subscription(
@@ -88,6 +88,8 @@ def __init__(self):
         self.get_logger().info("Start ekf_myself node")
         self.get_logger().info("-------------------------")
         
+        self.is_initialized = False
+        
     def reset_tf_buffer(self): 
         # キャッシュのクリアとして、Bufferのインスタンスを再作成 
         self.br = tf2_ros.TransformBroadcaster(self) 
@@ -131,6 +133,15 @@ def sensor_a_callback(self, data):
 
         self.GTheta = self.orientation_to_yaw(
             data.pose.pose.orientation.z, data.pose.pose.orientation.w)
+            
+        # initialize odom
+        if not self.is_initialized and self.SmpTime is not None and self.SmpTime > 0: 
+            self.initialize_odomA(
+                data.pose.pose.position.x, 
+                data.pose.pose.position.y, 
+                self.GTheta, 
+                self.SmpTime
+            )
 
     def sensor_b_callback(self, data):
         self.GpsXY = np.array(
@@ -261,6 +272,20 @@ def initialize(self, GTheta, SmpTime):
             [[(1.379e-3)**2, 0], [0, (0.03 * np.pi / 180 * SmpTime)**2]])
         G0 = np.array([[1, 0], [0, 0], [0, 0], [0, 1]])
         self.P = G0 @ self.Q @ G0.T
+    
+    # 20251021 tuika
+    def initialize_odomA(self, x, y, GTheta, SmpTime):
+        self.GTheta0 = GTheta
+        # odomAの座標 (x, y) でXXを初期化
+        self.XX = np.array([x, y, np.cos(GTheta), np.sin(GTheta)]) 
+        self.w = np.array([(1.379e-3)**2, (0.03 * np.pi / 180 * SmpTime)**2])
+        self.H = np.array([[1, 0, 0, 0], [0, 1, 0, 0]])
+        self.Q = np.array(
+            [[(1.379e-3)**2, 0], [0, (0.03 * np.pi / 180 * SmpTime)**2]])
+        G0 = np.array([[1, 0], [0, 0], [0, 0], [0, 1]])
+        self.P = G0 @ self.Q @ G0.T
+        self.is_initialized = True # 初期化フラグを立てる
+        self.get_logger().info(f"EKF initialized with OdomA at: ({x}, {y})") # ログ出力
 
     def initializeGPS(self, GpsXY, GTheta, SmpTime):
         self.GTheta0 = GTheta
@@ -274,8 +299,11 @@ def initializeGPS(self, GpsXY, GTheta, SmpTime):
         self.P = G0 @ self.Q @ G0.T
 
     def KalfXY(self, Speed, SmpTime, GTheta, R1, R2):
-        if self.H is None:
-            self.initialize(GTheta, SmpTime)
+        #if self.H is None:
+        #    self.initialize(GTheta, SmpTime)
+            
+        if not self.is_initialized:
+            return np.array([0, 0])
 
         self.R = np.array([[R1, 0], [0, R2]])
 
@@ -303,6 +331,12 @@ def KalfXY(self, Speed, SmpTime, GTheta, R1, R2):
         return self.XX[:2]
 
     def KalfGPSXY(self, Speed, SmpTime, GTheta, GpsXY, R1, R2):
+        #if self.H is None:
+        #    self.initializeGPS(GpsXY, GTheta, SmpTime)
+        
+        if not self.is_initialized:
+            return np.array([0, 0])
+        
         if self.H is None:
             self.initializeGPS(GpsXY, GTheta, SmpTime)
 

From 1028a14335390ac09d4fd493aa0f5b91a925bc7b Mon Sep 17 00:00:00 2001
From: kugurofu <tenten31569@icloud.com>
Date: Mon, 27 Oct 2025 10:58:02 +0000
Subject: [PATCH 18/25] change motor error processing and add tsukuba obs

---
 orange_bringup/launch/orange_robot.launch.xml |  2 +-
 .../orange_bringup/motor_driver_node.py       | 29 +++++++++++++++++++
 .../orange_gnss/gnss_odom_publisher_ttyUSB.py |  8 ++---
 pcd_convert/pcd_convert/pcd_rotation.py       |  4 +--
 .../pcd_convert/pcd_rotation_lidar2.py        | 13 +++++----
 .../launch/try_navigation.launch.py           |  2 +-
 .../try_navigation/ekf_myself_gps.py          |  4 ++-
 .../try_navigation/path_follower.py           | 23 +++++++++++----
 .../try_navigation/potential_astar.py         | 13 +++++++--
 9 files changed, 75 insertions(+), 23 deletions(-)

diff --git a/orange_bringup/launch/orange_robot.launch.xml b/orange_bringup/launch/orange_robot.launch.xml
index 0c66fcc..85aaf06 100644
--- a/orange_bringup/launch/orange_robot.launch.xml
+++ b/orange_bringup/launch/orange_robot.launch.xml
@@ -119,7 +119,7 @@
     <param name="baud" value="115200"/>
     <param name="country_id" value="0"/>
     <param name="Position_magnification" value="1.675"/>
-    <param name="heading" value="93.0"/>
+    <param name="heading" value="180.0"/>
     <param name="start_lat" value="35.425952230280004"/>
     <param name="start_lon" value="139.31380123427"/>
     <param name="time_out" value="1.0"/>
diff --git a/orange_bringup/orange_bringup/motor_driver_node.py b/orange_bringup/orange_bringup/motor_driver_node.py
index 4f4faea..1f6bdc3 100644
--- a/orange_bringup/orange_bringup/motor_driver_node.py
+++ b/orange_bringup/orange_bringup/motor_driver_node.py
@@ -492,6 +492,7 @@ def get_wheels_travelled(self):
         )  # unit in meter
         return l_travelled, r_travelled
 
+    '''
     def modbus_fail_read_handler(self, ADDR, WORD):
         read_success = False
         reg = [None] * WORD
@@ -506,6 +507,34 @@ def modbus_fail_read_handler(self, ADDR, WORD):
                 self.get_logger().error(e)
                 pass
         return reg
+        '''
+    def modbus_fail_read_handler(self, ADDR, WORD, max_retry=5):
+        read_success = False
+        reg = [0] * WORD
+        retry_count = 0
+
+        while not read_success and retry_count < max_retry:
+            result = self.client.read_holding_registers(ADDR, WORD, slave=self.ID)
+            if result is None or not hasattr(result, "registers"):
+                self.get_logger().warn(f"Modbus read failed (try {retry_count+1}/{max_retry})")
+                time.sleep(0.2)
+                retry_count += 1
+                continue
+
+            try:
+                for i in range(WORD):
+                    reg[i] = result.registers[i]
+                read_success = True
+            except Exception as e:
+                self.get_logger().error(f"Read error: {e}")
+                retry_count += 1
+                time.sleep(0.2)
+
+        if not read_success:
+            self.get_logger().error(f"Modbus read failed after {max_retry} retries. Stopping.")
+            raise RuntimeError("Modbus communication failed.")
+    
+        return reg  
 
     def set_rpm_with_limit(self, left_rpm, right_rpm):
         if self.left_rpm_lim < left_rpm:
diff --git a/orange_gnss/orange_gnss/gnss_odom_publisher_ttyUSB.py b/orange_gnss/orange_gnss/gnss_odom_publisher_ttyUSB.py
index 755da51..746e26c 100644
--- a/orange_gnss/orange_gnss/gnss_odom_publisher_ttyUSB.py
+++ b/orange_gnss/orange_gnss/gnss_odom_publisher_ttyUSB.py
@@ -48,8 +48,8 @@ def __init__(self):
         
         # service client
         self.client = self.create_client(Avglatlon, 'send_avg_gps')
-        while not self.client.wait_for_service(timeout_sec=1.0):
-            self.get_logger().info("service not available...")
+        #while not self.client.wait_for_service(timeout_sec=1.0):
+        #    self.get_logger().info("service not available...")
 
         self.get_logger().info("Start get_lonlat quat node")
         self.get_logger().info("-------------------------")
@@ -246,8 +246,8 @@ def heading_to_quat(self ,real_heading):
         if robotheading >= 360:
             robotheading -= 360
 
-        self.get_logger().info(f"real_heading: {real_heading}")
-        self.get_logger().info(f"robotheading: {robotheading}")
+        #self.get_logger().info(f"real_heading: {real_heading}")
+        #self.get_logger().info(f"robotheading: {robotheading}")
 
         if self.count == 0:
             self.get_logger().info(f"!!!----------robotheading: {robotheading} deg----------!!!")
diff --git a/pcd_convert/pcd_convert/pcd_rotation.py b/pcd_convert/pcd_convert/pcd_rotation.py
index 60ed85e..5d2e928 100644
--- a/pcd_convert/pcd_convert/pcd_rotation.py
+++ b/pcd_convert/pcd_convert/pcd_rotation.py
@@ -134,8 +134,8 @@ def pcd_rotation(self, msg):
                     self.MID360_HIGHT = - intercept; #hight position[m]
                     self.THETA_INIT_Y = self.THETA_INIT_Y + theta #[deg]
                     self.initialize_calibration = 1
-                    print(f"self.MID360_HIGHT ={self.MID360_HIGHT}")
-                    print(f"self.THETA_INIT_Y ={self.THETA_INIT_Y}")
+                    #print(f"self.MID360_HIGHT ={self.MID360_HIGHT}")
+                    #print(f"self.THETA_INIT_Y ={self.THETA_INIT_Y}")
                     
         
         #add mid height position
diff --git a/pcd_convert/pcd_convert/pcd_rotation_lidar2.py b/pcd_convert/pcd_convert/pcd_rotation_lidar2.py
index dd5f887..071d957 100644
--- a/pcd_convert/pcd_convert/pcd_rotation_lidar2.py
+++ b/pcd_convert/pcd_convert/pcd_rotation_lidar2.py
@@ -43,15 +43,15 @@ def __init__(self):
         
         #パラメータ
         #set LiDAR position
-        self.MID360_HIGHT = 980/1000; #hight position[m]
+        self.MID360_HIGHT = 750/1000; #self.MID360_HIGHT =1.7483812830163088
             
         #上下反転  LiDAR init
-        self.THETA_INIT_X = -180 #[deg]
-        self.THETA_INIT_Y = 0 #[deg]
-        self.THETA_INIT_Z = 0 #[deg]
+        self.THETA_INIT_X = 0 #[deg]-180
+        self.THETA_INIT_Y = 0.05435636478316808 #self.THETA_INIT_Y =0.05435636478316808#[deg]
+        self.THETA_INIT_Z = 0 #[deg]  
         
         #initialize calibration
-        self.initialize_calibration = 0
+        self.initialize_calibration = 1
         self.pcd_buff = np.array([[],[],[],[]]);
         self.x1_init_point = 1.5
         self.x2_init_point = 2.5
@@ -137,7 +137,8 @@ def pcd_rotation(self, msg):
                     self.MID360_HIGHT = - intercept; #hight position[m]
                     self.THETA_INIT_Y = self.THETA_INIT_Y + theta #[deg]
                     self.initialize_calibration = 1
-                    print(f"self.MID360_HIGHT ={self.MID360_HIGHT}")
+                    #print(f"self.MID360_HIGHT ={self.MID360_HIGHT}")
+                    #print(f"self.THETA_INIT_Y ={self.THETA_INIT_Y}")
                     
         
         #add mid height position
diff --git a/try_navigation/launch/try_navigation.launch.py b/try_navigation/launch/try_navigation.launch.py
index 90901ab..818f82c 100644
--- a/try_navigation/launch/try_navigation.launch.py
+++ b/try_navigation/launch/try_navigation.launch.py
@@ -32,7 +32,7 @@ def generate_launch_description():
     waypoint_path = LaunchConfiguration('waypoint_path')
     declare_waypoint_arg = DeclareLaunchArgument(
         'waypoint_path',
-        default_value='kbkn_maps/waypoints/tsukuba/2025/papa/tsukuba_waypoint.yaml',
+        default_value='kbkn_maps/waypoints/hosei/2025/nakaniwa_tsukuba.yaml',
         description='waypoint name'
     )
     
diff --git a/try_navigation/try_navigation/ekf_myself_gps.py b/try_navigation/try_navigation/ekf_myself_gps.py
index 3dcf895..cbfdf5c 100644
--- a/try_navigation/try_navigation/ekf_myself_gps.py
+++ b/try_navigation/try_navigation/ekf_myself_gps.py
@@ -262,7 +262,8 @@ def sensim0(self, data):
         data = (self.k_p * self.e_n + self.k_d*(self.e_n - self.e_n1))
         self.e_n1 = self.e_n
         return data
-
+    
+    '''
     def initialize(self, GTheta, SmpTime):
         self.GTheta0 = GTheta
         self.XX = np.array([0, 0, np.cos(GTheta), np.sin(GTheta)])
@@ -272,6 +273,7 @@ def initialize(self, GTheta, SmpTime):
             [[(1.379e-3)**2, 0], [0, (0.03 * np.pi / 180 * SmpTime)**2]])
         G0 = np.array([[1, 0], [0, 0], [0, 0], [0, 1]])
         self.P = G0 @ self.Q @ G0.T
+    '''
     
     # 20251021 tuika
     def initialize_odomA(self, x, y, GTheta, SmpTime):
diff --git a/try_navigation/try_navigation/path_follower.py b/try_navigation/try_navigation/path_follower.py
index cd2625b..45bc1cb 100644
--- a/try_navigation/try_navigation/path_follower.py
+++ b/try_navigation/try_navigation/path_follower.py
@@ -53,6 +53,7 @@ def __init__(self):
         self.goal_sub = self.create_subscription(PoseStamped, '/goal_pose', self.goal_pose_callback, qos_profile)
         self.stop_sub = self.create_subscription(String, '/stop_sign_status', self.stop_sign_callback, 10)
         self.human_sub = self.create_subscription(String, '/human_status', self.human_callback, 10)
+        self.waypoint_number_sub = self.create_subscription(Int32,'/waypoint_number', self.get_waypoint_number, qos_profile_sub)
         self.subscription  # 警告を回避するために設置されているだけです。削除しても挙動はかわりません。
         
         # タイマーを0.05秒（50ミリ秒）ごとに呼び出す
@@ -386,11 +387,13 @@ def robot_ctrl(self):
         #c_obs_near = ( -50<obs_theta) * (obs_theta<  50) * (obs_dist<0.5)
         #c_obs_back = ( -50<obs_theta) * (obs_theta<  50) * (obs_dist<0.4)
         cf = 1.15
+        
+        #self.get_logger().info(f"self.waypoint_number: {self.waypoint_number}")
                 
         if ~np.any(ch_obs) :
             if np.any(rh_obs) and np.any(lh_obs):
                 target_theta = (target_rad) * (180 / math.pi)
-                #print("--- Center --- Befor target_theta[deg]:",target_theta)
+                print("--- Center --- Befor target_theta[deg]:",target_theta)
                 speed = 0.25
                 lh_obs_close = min(lh_obs[1,:]) # y0 lh min
                 rh_obs_close = max(rh_obs[1,:]) # y0 rh min
@@ -399,24 +402,34 @@ def robot_ctrl(self):
                 #c_point = (cx, cy)
                 target_rad = math.atan2(cx, cf)
                 target_theta = (target_rad) * (180 / math.pi)
-                #print("--- Center --- After target_theta[deg]:",target_theta)
+                print("--- Center --- After target_theta[deg]:",target_theta)
                             
             elif np.any(rh_obs) and ~np.any(lh_obs):
                 speed = 0.25
-                if 0 <= self.waypoint_number and self.waypoint_number <= 9:
+                if (6 <= self.waypoint_number <= 8) or (self.waypoint_number==15) or (self.waypoint_number==18):
                     target_theta = (target_rad) * (180 / math.pi)
-                    #print("!!!RH!!!! Befor target_theta[deg]:",target_theta)
+                    print("!!!RH!!!! Befor target_theta[deg]:",target_theta)
                     rh_obs_close = max(rh_obs[1,:]) # y0 rh min
                     rh_dist = 0.65
                     cy = cf
                     cx = rh_obs_close + rh_dist
                     target_rad = math.atan2(cx, cf)
                     target_theta = (target_rad) * (180 / math.pi)
-                    #print("!!!RH!!!! After target_theta[deg]:",target_theta)
+                    print("!!!RH!!!! After target_theta[deg]:",target_theta)
                     
             
             elif ~np.any(rh_obs) and np.any(lh_obs):
                 speed = 0.25
+                if 11 <= self.waypoint_number <= 12:
+                    target_theta = (target_rad) * (180 / math.pi)
+                    print("!!!LH!!!! Befor target_theta[deg]:",target_theta)
+                    lh_obs_close = min(lh_obs[1,:]) # y0 lh min
+                    lh_dist = 0.65
+                    cy = cf
+                    cx = lh_obs_close + lh_dist
+                    target_rad = math.atan2(cx, cf)
+                    target_theta = (target_rad) * (180 / math.pi)
+                    print("!!!LH!!!! After target_theta[deg]:",target_theta)
             
             #elif ~np.any(rh_obs) and ~np.any(lh_obs):
             #    speed = 0.25
diff --git a/try_navigation/try_navigation/potential_astar.py b/try_navigation/try_navigation/potential_astar.py
index ace4fde..0fa2791 100644
--- a/try_navigation/try_navigation/potential_astar.py
+++ b/try_navigation/try_navigation/potential_astar.py
@@ -448,10 +448,17 @@ def potential_astar(self, msg):
         
         # make map_obs   x1  x2  y1  y2
         obs1 = make_obs(-45, 25, 97, 94) # siyakusyoura minami
-        obs2 = make_obs(  5, 21, 100, 101) # siyakusyoura kita
-        obs3 = make_obs(-73,-75, 105, 29) # siyakusyo nisi
+        obs2 = make_obs(  5, 20, 100, 102) # siyakusyoura kita
+        obs3 = make_obs( 28, 20, 108, 102) # siyakusyoura kita2
+        obs4 = make_obs(-73,-75, 105, 29) # siyakusyo nisi
+        obs5 = make_obs( 61,100, 32, 41) # siyakusyo oudanhodou
+        obs6 = make_obs( 61,44, 32, 35) # siyakusyo oudanhodou
+        obs7 = make_obs(276,270, 28,-58) # 7-Eleven mae
+        obs8 = make_obs(-21,-39, 24, 27) # siyakusyo sibahu1
+        obs9 = make_obs(-51,-39, 37, 27) # siyakusyo sibahu2
+        
 
-        self.tsukuba_obs = np.hstack((obs1, obs2, obs3))
+        self.tsukuba_obs = np.hstack((obs1, obs2, obs3, obs4, obs5, obs6, obs7, obs8, obs9))
         
         #map_obs add
         if len(self.tsukuba_obs[0,:])>0:

From ccd7f2d8a9c00f05b23ddc33597738dfda6cd9a6 Mon Sep 17 00:00:00 2001
From: kugurofu <tenten31569@icloud.com>
Date: Wed, 5 Nov 2025 10:19:48 +0000
Subject: [PATCH 19/25] enable to stop at the waypoint after start running in
 the middle way

---
 .../try_navigation/path_follower.py           | 20 +++++++++++++++++++
 1 file changed, 20 insertions(+)

diff --git a/try_navigation/try_navigation/path_follower.py b/try_navigation/try_navigation/path_follower.py
index 45bc1cb..c8be277 100644
--- a/try_navigation/try_navigation/path_follower.py
+++ b/try_navigation/try_navigation/path_follower.py
@@ -646,10 +646,30 @@ def get_odom_ref(self, msg):
         self.ref_theta_y = 0 #pitch /math.pi*180
         self.ref_theta_z = yaw /math.pi*180
         
+        if self.waypoint_number >= 79:
+            if self.stop_num <= 14:
+                self.stop_num = 15
+        elif self.waypoint_number >= 71:
+            if self.stop_num <= 12:
+                self.stop_num = 13
+        elif self.waypoint_number >= 62:
+            if self.stop_num <= 8:
+                self.stop_num = 9
+        elif self.waypoint_number >= 51:
+            if self.stop_num <= 7:
+                self.stop_num = 8
+        elif self.waypoint_number >= 33:
+            if self.stop_num <= 6:
+                self.stop_num = 7
+        elif self.waypoint_number >= 25:
+            if self.stop_num <= 2:
+                self.stop_num = 3
+        
         if ((self.stop_xy[self.stop_num,0] < self.ref_position_x) and (self.ref_position_x < self.stop_xy[self.stop_num,1]) and (self.stop_xy[self.stop_num,2] < self.ref_position_y) and (self.ref_position_y < self.stop_xy[self.stop_num,3]) ) or ((self.stop_xy[self.stop_num,0] < self.position_x) and (self.position_x < self.stop_xy[self.stop_num,1]) and (self.stop_xy[self.stop_num,2] < self.position_y) and (self.position_y < self.stop_xy[self.stop_num,3]) ):
             if self.stop_xy[self.stop_num,4] > 0:
                 self.get_logger().info('####### stop flag on %f #######' % (self.stop_num))
                 self.stop_flag = 1;
+                #print(self.stop_num)
             else:
                 self.get_logger().info('####### through flag on %f #######' % (self.stop_num))
             self.stop_num = self.stop_num + 1;

From 8e7d9b0edd3efb87935536f32de38efc782952bd Mon Sep 17 00:00:00 2001
From: kugurofu <tenten31569@icloud.com>
Date: Thu, 6 Nov 2025 09:11:21 +0000
Subject: [PATCH 20/25] change from -180 to 180

---
 .../try_navigation/path_follower.py           | 28 ++++++++++++-------
 1 file changed, 18 insertions(+), 10 deletions(-)

diff --git a/try_navigation/try_navigation/path_follower.py b/try_navigation/try_navigation/path_follower.py
index c8be277..bea90c7 100644
--- a/try_navigation/try_navigation/path_follower.py
+++ b/try_navigation/try_navigation/path_follower.py
@@ -160,6 +160,9 @@ def __init__(self):
         self.lh_obs = 0
         self.ch_obs = 0
         self.t_stamp = 0
+
+        # angle megin for gps heading 
+        self.angle_diff = 30
         
         
         ################# IGVC SelfDrive Quolification line stop test #20250530# #################
@@ -389,9 +392,9 @@ def robot_ctrl(self):
         cf = 1.15
         
         #self.get_logger().info(f"self.waypoint_number: {self.waypoint_number}")
-                
+        print(theta_z)
         if ~np.any(ch_obs) :
-            if np.any(rh_obs) and np.any(lh_obs):
+            if np.any(lh_obs) and np.any(rh_obs) :  #真ん中　
                 target_theta = (target_rad) * (180 / math.pi)
                 print("--- Center --- Befor target_theta[deg]:",target_theta)
                 speed = 0.25
@@ -404,9 +407,12 @@ def robot_ctrl(self):
                 target_theta = (target_rad) * (180 / math.pi)
                 print("--- Center --- After target_theta[deg]:",target_theta)
                             
-            elif np.any(rh_obs) and ~np.any(lh_obs):
+            #or ((0 <= self.waypoint_number <= 3) and (0 - self.angle_diff <= theta_z <= 0 + self.angle_diff)) \
+            elif ~np.any(lh_obs) and np.any(rh_obs):   #右寄り　
                 speed = 0.25
-                if (6 <= self.waypoint_number <= 8) or (self.waypoint_number==15) or (self.waypoint_number==18):
+                if (6 <= self.waypoint_number <= 8) or (self.waypoint_number==15) or (self.waypoint_number==18) \
+                or ((50 <= self.waypoint_number <= 54) and (0 - self.angle_diff <= theta_z <= 0 + self.angle_diff)) \
+                or ((70 <= self.waypoint_number <= 73) and (-150 <= theta_z <= -150 - self.angle_diff) and (150  <= theta_z <= 150 + self.angle_diff)) :
                     target_theta = (target_rad) * (180 / math.pi)
                     print("!!!RH!!!! Befor target_theta[deg]:",target_theta)
                     rh_obs_close = max(rh_obs[1,:]) # y0 rh min
@@ -417,10 +423,11 @@ def robot_ctrl(self):
                     target_theta = (target_rad) * (180 / math.pi)
                     print("!!!RH!!!! After target_theta[deg]:",target_theta)
                     
-            
-            elif ~np.any(rh_obs) and np.any(lh_obs):
+            elif np.any(lh_obs) and ~np.any(rh_obs):  #左寄り
                 speed = 0.25
-                if 11 <= self.waypoint_number <= 12:
+                if 11 <= self.waypoint_number <= 12 \
+                or ((25 <= self.waypoint_number <= 28) and (-90 - self.angle_diff <= theta_z <= -90 + self.angle_diff)) \
+                or ((56 <= self.waypoint_number <= 60) and (-150 <= theta_z <= -150 - self.angle_diff) and (150  <= theta_z <= 150 + self.angle_diff)) :
                     target_theta = (target_rad) * (180 / math.pi)
                     print("!!!LH!!!! Befor target_theta[deg]:",target_theta)
                     lh_obs_close = min(lh_obs[1,:]) # y0 lh min
@@ -445,10 +452,11 @@ def robot_ctrl(self):
         if target_theta  < -lim_steer:
             speed = 0.10
             target_rad = -lim_steer/180*math.pi
+            #print("over write1")
         elif lim_steer < target_theta:
             speed = 0.10
             target_rad = lim_steer/180*math.pi
-        
+            #print("over write1")
         if abs(target_theta)  > 90:
             speed = -0.10
         if np.any(c_obs_back) :
@@ -701,12 +709,12 @@ def obs_steer(self, msg):
         #map_obs
         self.obs_points = points
         
-        rh_obs = self.pcd_serch(points, 1.0,1.3,-0.7,0)
+        rh_obs = self.pcd_serch(points, 1.0,1.3,-1.0,0)#1.0,1.3,-0.7,0
         if len(rh_obs[0,:]) > 10:
             self.rh_obs = 1
         else:
             self.rh_obs = 0
-        lh_obs = self.pcd_serch(points, 1.0,1.3,0,0.7)
+        lh_obs = self.pcd_serch(points, 1.0,1.3,0,1.0)#1.0,1.3,0,0.7
         if len(lh_obs[0,:]) > 10:
             self.lh_obs = 1
         else:

From 972f17fad7793956e034917cef7cda85d9e7a2f3 Mon Sep 17 00:00:00 2001
From: kugurofu <tenten31569@icloud.com>
Date: Tue, 11 Nov 2025 10:49:40 +0000
Subject: [PATCH 21/25] some update?

---
 orange_bringup/launch/orange_robot.launch.xml    | 6 +++---
 try_navigation/launch/try_navigation.launch.py   | 2 +-
 try_navigation/try_navigation/ekf_myself_gps.py  | 2 +-
 try_navigation/try_navigation/path_follower.py   | 7 +++----
 try_navigation/try_navigation/potential_astar.py | 3 ++-
 5 files changed, 10 insertions(+), 10 deletions(-)

diff --git a/orange_bringup/launch/orange_robot.launch.xml b/orange_bringup/launch/orange_robot.launch.xml
index 85aaf06..77b9937 100644
--- a/orange_bringup/launch/orange_robot.launch.xml
+++ b/orange_bringup/launch/orange_robot.launch.xml
@@ -119,9 +119,9 @@
     <param name="baud" value="115200"/>
     <param name="country_id" value="0"/>
     <param name="Position_magnification" value="1.675"/>
-    <param name="heading" value="180.0"/>
-    <param name="start_lat" value="35.425952230280004"/>
-    <param name="start_lon" value="139.31380123427"/>
+    <param name="heading" value="93.0"/>
+    <param name="start_lat" value="36.04973847576364"/>
+    <param name="start_lon" value="140.0459354109091"/>
     <param name="time_out" value="1.0"/>
   </node>
   <!-- 
diff --git a/try_navigation/launch/try_navigation.launch.py b/try_navigation/launch/try_navigation.launch.py
index 818f82c..8be3660 100644
--- a/try_navigation/launch/try_navigation.launch.py
+++ b/try_navigation/launch/try_navigation.launch.py
@@ -32,7 +32,7 @@ def generate_launch_description():
     waypoint_path = LaunchConfiguration('waypoint_path')
     declare_waypoint_arg = DeclareLaunchArgument(
         'waypoint_path',
-        default_value='kbkn_maps/waypoints/hosei/2025/nakaniwa_tsukuba.yaml',
+        default_value='kbkn_maps/waypoints/tsukuba/2025/papa/tsukuba_sim.yaml',
         description='waypoint name'
     )
     
diff --git a/try_navigation/try_navigation/ekf_myself_gps.py b/try_navigation/try_navigation/ekf_myself_gps.py
index cbfdf5c..c8cefc4 100644
--- a/try_navigation/try_navigation/ekf_myself_gps.py
+++ b/try_navigation/try_navigation/ekf_myself_gps.py
@@ -175,7 +175,7 @@ def sensor_b_callback(self, data):
     def determination_of_R(self):
         if self.GpsXY is not None:
             #カルマンフィルタのデッドレコニングよりになる範囲を指定
-            if (-60<self.GpsXY[0]) and (self.GpsXY[0]<60) and (25 < self.GpsXY[1]) and (self.GpsXY[1]<110):
+            if (-60<self.GpsXY[0]) and (self.GpsXY[0]<60) and (20 < self.GpsXY[1]) and (self.GpsXY[1]<110):
                 #self.gps_rr_flag = 1
                 self.gps_rr_flag = 0 #self.gps_rr_flag =1はGPS受信精度よく、0でGPS受信精度低い範囲に入ったフラグ
                 #self.offsetyaw_bad_gps = -10/180*math.pi
diff --git a/try_navigation/try_navigation/path_follower.py b/try_navigation/try_navigation/path_follower.py
index bea90c7..eab6ddc 100644
--- a/try_navigation/try_navigation/path_follower.py
+++ b/try_navigation/try_navigation/path_follower.py
@@ -135,7 +135,7 @@ def __init__(self):
             #[-69.5,	-62,	-47.5,	-27.5, 1.0], #tyokusen4 ||| [-67.5,	-62,	-47.5,	-27.5, 1.0]before:[-67.0,	-62,	-47.5,	-27.5, 1.0], 
             #[-55,	-35,	41,	46,    1.0], #Goal
             
-            [ 63.5,  65.0,  19.0,  39.0, 1.0], #shiyakusyo
+            [ 63.0,  64.5,  19.0,  39.0, 1.0], #shiyakusyo
             [ 99.5, 101.0,  25.0,  45.0, 1.0], #dourotan1
             [177.0, 178.5,  25.0,  45.0, 1.0], #dourotan2
             [257.5, 277.5, -60.0, -58.5, 1.0], #singoumaeteisisen1
@@ -423,10 +423,9 @@ def robot_ctrl(self):
                     target_theta = (target_rad) * (180 / math.pi)
                     print("!!!RH!!!! After target_theta[deg]:",target_theta)
                     
-            elif np.any(lh_obs) and ~np.any(rh_obs):  #左寄り
+            elif np.any(lh_obs) and ~np.any(rh_obs):  #左寄り 11 <= self.waypoint_number <= 12 \ 
                 speed = 0.25
-                if 11 <= self.waypoint_number <= 12 \
-                or ((25 <= self.waypoint_number <= 28) and (-90 - self.angle_diff <= theta_z <= -90 + self.angle_diff)) \
+                if ((25 <= self.waypoint_number <= 28) and (-90 - self.angle_diff <= theta_z <= -90 + self.angle_diff)) \
                 or ((56 <= self.waypoint_number <= 60) and (-150 <= theta_z <= -150 - self.angle_diff) and (150  <= theta_z <= 150 + self.angle_diff)) :
                     target_theta = (target_rad) * (180 / math.pi)
                     print("!!!LH!!!! Befor target_theta[deg]:",target_theta)
diff --git a/try_navigation/try_navigation/potential_astar.py b/try_navigation/try_navigation/potential_astar.py
index 0fa2791..2b8d81d 100644
--- a/try_navigation/try_navigation/potential_astar.py
+++ b/try_navigation/try_navigation/potential_astar.py
@@ -456,9 +456,10 @@ def potential_astar(self, msg):
         obs7 = make_obs(276,270, 28,-58) # 7-Eleven mae
         obs8 = make_obs(-21,-39, 24, 27) # siyakusyo sibahu1
         obs9 = make_obs(-51,-39, 37, 27) # siyakusyo sibahu2
+        obs10 = make_obs(36.5,36.5, 62, 96.5) # siyakusyo higasi
         
 
-        self.tsukuba_obs = np.hstack((obs1, obs2, obs3, obs4, obs5, obs6, obs7, obs8, obs9))
+        self.tsukuba_obs = np.hstack((obs1, obs4, obs5, obs6, obs7, obs8, obs9, obs10))
         
         #map_obs add
         if len(self.tsukuba_obs[0,:])>0:

From 1f29d13f30dbaf5b920592aa8b29a13a157f3958 Mon Sep 17 00:00:00 2001
From: kugurofu <tenten31569@icloud.com>
Date: Mon, 17 Nov 2025 05:25:20 +0000
Subject: [PATCH 22/25] add pcd buffer node

---
 try_navigation/setup.py                     |   4 +-
 try_navigation/try_navigation/pcd_buffer.py | 211 ++++++++++++++++++++
 2 files changed, 214 insertions(+), 1 deletion(-)
 create mode 100644 try_navigation/try_navigation/pcd_buffer.py

diff --git a/try_navigation/setup.py b/try_navigation/setup.py
index 4a20ab2..84e85a5 100644
--- a/try_navigation/setup.py
+++ b/try_navigation/setup.py
@@ -58,7 +58,9 @@
             'ekf_myself_match = try_navigation.ekf_myself_match:main',
             'ekf_myself_gps = try_navigation.ekf_myself_gps:main',
             'ekf_myself_odom = try_navigation.ekf_myself_odom:main',
-            'map_check = try_navigation.map_check:main'
+            'map_check = try_navigation.map_check:main',
+            'pcd_buffer = try_navigation.pcd_buffer:main'
+
         ],
     },
 )
diff --git a/try_navigation/try_navigation/pcd_buffer.py b/try_navigation/try_navigation/pcd_buffer.py
new file mode 100644
index 0000000..88cebd5
--- /dev/null
+++ b/try_navigation/try_navigation/pcd_buffer.py
@@ -0,0 +1,211 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+
+# rclpy (ROS 2のpythonクライアント)の機能を使えるようにします。
+import rclpy
+# rclpy の Node を使いやすくインポート
+from rclpy.node import Node
+import std_msgs.msg as std_msgs
+import sensor_msgs.msg as sensor_msgs
+import numpy as np
+import math
+from rclpy.qos import QoSProfile, QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy
+import time
+import pandas as pd
+from sensor_msgs.msg import PointCloud2, PointField
+from std_msgs.msg import Header
+
+# sensor_msgs_py の point_cloud2 が使えれば利用（環境依存）
+try:
+    from sensor_msgs_py import point_cloud2 as pc2
+except Exception:
+    pc2 = None
+
+
+class PcdBufferNode(Node):
+    """
+    指定の PointCloud2 トピックを購読して点群をバッファ（グリッド丸め -> 重複削除 -> MAX_POINTSに切り詰め）し、
+    新しいトピックに publish するノード。
+    """
+
+    def __init__(self):
+        super().__init__('pcd_buffer')
+
+        # QoS 設定
+        qos_profile = QoSProfile(
+            history=QoSHistoryPolicy.KEEP_LAST,
+            reliability=QoSReliabilityPolicy.RELIABLE,
+            durability=QoSDurabilityPolicy.VOLATILE,
+            depth=1
+        )
+
+        qos_profile_sub = QoSProfile(
+            reliability=QoSReliabilityPolicy.RELIABLE,
+            durability=QoSDurabilityPolicy.VOLATILE,
+            depth=1
+        )
+
+        # パラメータ
+        # 入出力トピック
+        self.INPUT_TOPIC   = '/pcd_obs_global'          
+        self.OUTPUT_TOPIC  = 'pcd_obs_global_buff'  
+
+        # 丸め精度
+        self.GROUND_PIXEL  = 1000.0 / 50.0      
+
+        self.MAX_POINTS    = 50000
+        self.FRAME_ID      = 'odom'
+        # 内部バッファ（4 x N: x, y, z, intensity）
+        self.pcd_obs_global_buff = np.array([[], [], [], []], dtype=np.float32)
+
+        # Subscriptionを作成
+        self.subscription = self.create_subscription(
+            sensor_msgs.PointCloud2,
+            self.INPUT_TOPIC,
+            self.pcd_buffer_callback,
+            qos_profile_sub
+        )
+        self.subscription  # 警告回避用
+
+        # Publisherを作成
+        self.pcd_obs_global_buff_publisher = self.create_publisher(sensor_msgs.PointCloud2, self.OUTPUT_TOPIC, qos_profile)
+
+        # ログ
+        self.get_logger().info(f'PcdBufferNode ready. sub:{self.INPUT_TOPIC} pub:{self.OUTPUT_TOPIC} ground_pixel:{self.GROUND_PIXEL} MAX_POINTS:{self.MAX_POINTS}')
+
+    def pointcloud2_to_array(self, cloud_msg):
+        # Extract point cloud data
+        points = np.frombuffer(cloud_msg.data, dtype=np.uint8).reshape(-1, cloud_msg.point_step)
+        x = np.frombuffer(points[:, 0:4].tobytes(), dtype=np.float32)
+        y = np.frombuffer(points[:, 4:8].tobytes(), dtype=np.float32)
+        z = np.frombuffer(points[:, 8:12].tobytes(), dtype=np.float32)
+        intensity = np.frombuffer(points[:, 12:16].tobytes(), dtype=np.float32)
+
+        # Combine into a 4xN matrix
+        point_cloud_matrix = np.vstack((x, y, z, intensity))
+        print(point_cloud_matrix)
+        print(f"point_cloud_matrix ={point_cloud_matrix.shape}")
+        print(f"x ={x.dtype, x.shape}")
+        
+        return point_cloud_matrix
+
+    def pcd_buffer_callback(self, msg):
+        """
+        PointCloud2 を受け取りバッファに追加 -> グリッド丸め -> 重複削除 -> 切り詰め -> publish
+        """
+        #print stamp message
+        t_stamp = msg.header.stamp
+        print(f"t_stamp ={t_stamp}")
+
+        # PointCloud2 -> numpy 
+        points = self.pointcloud2_to_array(msg)  
+        print(f"points ={points.shape}")
+
+        # add buffer----------------------------------------------------------------
+        if hasattr(self, 'pcd_obs_global_buff') and self.pcd_obs_global_buff is not None and self.pcd_obs_global_buff.size != 0:
+            try:
+                self.pcd_obs_global_buff = np.hstack((self.pcd_obs_global_buff, points))
+            except Exception as e:
+                self.get_logger().warning(f'hstack failed: {e}')
+                # フォールバック（ concatenate ）
+                self.pcd_obs_global_buff = np.concatenate((self.pcd_obs_global_buff, points), axis=1)
+        else:
+            self.pcd_obs_global_buff = points.copy()
+
+        # 3) グリッド丸め（位置を quantize して重複を削除）
+        points_xyz = self.pcd_obs_global_buff[:3, :]  # (3, M)
+        points_i = self.pcd_obs_global_buff[3, :]    # (M,)
+        points_round = np.round(points_xyz * self.GROUND_PIXEL) / self.GROUND_PIXEL
+
+        # pandas を使って重複削除（丸め後の x,y,z が同じものを 1 つにする）
+        df = pd.DataFrame({
+            'x': points_round[0, :],
+            'y': points_round[1, :],
+            'z': points_round[2, :],
+        })
+        mask_unique = ~df.duplicated()
+        unique_idx = np.where(mask_unique)[0]
+
+        if unique_idx.size == 0:
+            # 重複で何も残らない場合は空バッファ
+            self.pcd_obs_global_buff = np.array([[], [], [], []], dtype=np.float32)
+        else:
+            new_xyz = points_round[:, unique_idx]
+            new_i = points_i[unique_idx]
+            self.pcd_obs_global_buff = np.vstack((new_xyz, new_i))
+
+        # 4) 最大点数を超える場合は末尾（最新）を残す
+        if self.pcd_obs_global_buff.shape[1] > self.MAX_POINTS:
+            self.pcd_obs_global_buff = self.pcd_obs_global_buff[:, -self.MAX_POINTS:].copy()
+
+        # 5) Publish（PointCloud2 へ変換して publish）
+        try:
+            n_by_4 = self.pcd_obs_global_buff.T  # (N,4)
+            cloud_msg = point_cloud_intensity_msg(n_by_4, t_stamp, self.FRAME_ID)
+            self.pcd_obs_global_buff_publisher.publish(cloud_msg)
+        except Exception as e:
+            self.get_logger().error(f'publish failed: {e}')
+
+
+def point_cloud_intensity_msg(points, t_stamp, parent_frame):
+    """
+    points: (N,4) numpy array (x,y,z,intensity)
+    t_stamp: builtin_interfaces.msg.Time (そのまま header.stamp に代入)
+    parent_frame: frame id string
+    """
+    if points is None or len(points) == 0:
+        header = std_msgs.Header(frame_id=parent_frame, stamp=t_stamp)
+        return sensor_msgs.PointCloud2(header=header, height=0, width=0, is_dense=True, is_bigendian=False, fields=[], point_step=0, row_step=0, data=b'')
+
+    ros_dtype = sensor_msgs.PointField.FLOAT32
+    dtype = np.float32
+    itemsize = np.dtype(dtype).itemsize  # 4 bytes
+    arr = np.asarray(points, dtype=dtype)
+    if arr.ndim == 1:
+        arr = arr.reshape(1, -1)
+    if arr.shape[1] == 3:
+        zeros = np.zeros((arr.shape[0], 1), dtype=dtype)
+        arr = np.hstack((arr, zeros))
+    elif arr.shape[1] > 4:
+        arr = arr[:, :4]
+
+    data = arr.astype(dtype).tobytes()
+
+    fields = [
+        sensor_msgs.PointField(name='x', offset=0, datatype=ros_dtype, count=1),
+        sensor_msgs.PointField(name='y', offset=4, datatype=ros_dtype, count=1),
+        sensor_msgs.PointField(name='z', offset=8, datatype=ros_dtype, count=1),
+        sensor_msgs.PointField(name='intensity', offset=12, datatype=ros_dtype, count=1),
+    ]
+
+    header = std_msgs.Header(frame_id=parent_frame, stamp=t_stamp)
+
+    cloud = sensor_msgs.PointCloud2(
+        header=header,
+        height=1,
+        width=arr.shape[0],
+        is_dense=True,
+        is_bigendian=False,
+        fields=fields,
+        point_step=itemsize * 4,
+        row_step=itemsize * 4 * arr.shape[0],
+        data=data
+    )
+    return cloud
+
+
+# main 関数
+def main(args=None):
+    rclpy.init(args=args)
+    node = PcdBufferNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

From a4869793f4677750fe14177f0cc2da2f284ba32f Mon Sep 17 00:00:00 2001
From: kugurofu <tenten31569@icloud.com>
Date: Tue, 18 Nov 2025 07:03:34 +0000
Subject: [PATCH 23/25] fix odom combination bag and change gps first
 heading=self.tsukubatheta

---
 orange_bringup/launch/orange_robot.launch.xml |  10 +-
 .../orange_gnss/gnss_odom_publisher_ttyUSB.py |   9 +-
 orange_gnss/orange_gnss/odom_combination.py   | 119 +++---------------
 .../launch/try_navigation.launch.py           |   2 +-
 4 files changed, 26 insertions(+), 114 deletions(-)

diff --git a/orange_bringup/launch/orange_robot.launch.xml b/orange_bringup/launch/orange_robot.launch.xml
index 77b9937..e37f49a 100644
--- a/orange_bringup/launch/orange_robot.launch.xml
+++ b/orange_bringup/launch/orange_robot.launch.xml
@@ -58,13 +58,13 @@
     <param name="odom_child_frame" value="base_footprint"/>
   </node>
   <!-- estop  -->
-  <node pkg="estop_ros" exec="cmd_vel_override_node" output="screen">
+  <!--<node pkg="estop_ros" exec="cmd_vel_override_node" output="screen">
     <param name="port" value="/dev/sensors/estop"/>
     <param name="baudrate" value="115200"/>
     <param name="time_out" value="500"/>
     <param name="serial_interval" value="0.01"/>
     <remap from="/estop/state" to="/estop"/>
-  </node>
+  </node>-->
 
   <!-- spresense imu -->
   <node pkg="cxd5602pwbimu" exec="cxd5602pwbimu_node">
@@ -119,9 +119,9 @@
     <param name="baud" value="115200"/>
     <param name="country_id" value="0"/>
     <param name="Position_magnification" value="1.675"/>
-    <param name="heading" value="93.0"/>
-    <param name="start_lat" value="36.04973847576364"/>
-    <param name="start_lon" value="140.0459354109091"/>
+    <param name="heading" value="180.0"/>
+    <param name="start_lat" value="35.425952230280004"/>
+    <param name="start_lon" value="139.31380123427"/>
     <param name="time_out" value="1.0"/>
   </node>
   <!-- 
diff --git a/orange_gnss/orange_gnss/gnss_odom_publisher_ttyUSB.py b/orange_gnss/orange_gnss/gnss_odom_publisher_ttyUSB.py
index 746e26c..9993e41 100644
--- a/orange_gnss/orange_gnss/gnss_odom_publisher_ttyUSB.py
+++ b/orange_gnss/orange_gnss/gnss_odom_publisher_ttyUSB.py
@@ -251,7 +251,8 @@ def heading_to_quat(self ,real_heading):
 
         if self.count == 0:
             self.get_logger().info(f"!!!----------robotheading: {robotheading} deg----------!!!")
-            self.first_heading = robotheading
+            #self.first_heading = robotheading
+            self.first_heading = self.tsukuba_theta
             self.count = 1
 
         relative_heading = robotheading - self.first_heading
@@ -360,9 +361,9 @@ def publish_GPS_lonlat_quat(self):
             #self.lonlat_pub.publish(self.lonlat_msg)
             # self.get_logger().info(f"Published GPS data: {lonlat}")           
             
-            if self.initial_coordinate is None:
-                self.initial_coordinate = [GPS_data[1], GPS_data[2]]        
-            GPSxy = self.conversion(lonlat, self.initial_coordinate, self.theta)
+            #if self.initial_coordinate is None:
+            #    self.initial_coordinate = [GPS_data[1], GPS_data[2]]        
+            GPSxy = self.conversion(lonlat, self.start_GPS_coordinate, self.tsukuba_theta)
             GPSquat = self.heading_to_quat(GPS_data[5])       
 
             self.odom_msg.header.stamp = self.get_clock().now().to_msg()
diff --git a/orange_gnss/orange_gnss/odom_combination.py b/orange_gnss/orange_gnss/odom_combination.py
index d73e3c9..bab7625 100644
--- a/orange_gnss/orange_gnss/odom_combination.py
+++ b/orange_gnss/orange_gnss/odom_combination.py
@@ -28,9 +28,6 @@ def __init__(self):
         # publisher
         self.odom_pub = self.create_publisher(Odometry, '/odom/combine', qos_profile)
         
-        # service
-        #self.avg_gps_service = self.create_service(Avglatlon, 'send_avg_gps', self.receive_avg_gps_callback)
-
         self.position_x = 0.0
         self.position_y = 0.0
         self.theta_z = 0.0
@@ -42,107 +39,24 @@ def __init__(self):
     
     # /odom callback
     def get_odom(self, msg):
-        '''
         self.position_x = msg.pose.pose.position.x
         self.position_y = msg.pose.pose.position.y
-        self.position_z = msg.pose.pose.position.z
         x, y, z, w = msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w
         roll, pitch, yaw = quaternion_to_euler(x, y, z, w)
-        self.theta_x, self.theta_y, self.theta_z = 0, 0, yaw * 180 / math.pi
-        '''
-        self.position_x = msg.pose.pose.position.x
-        self.position_y = msg.pose.pose.position.y
-        x, y, z, w = msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w
-        roll, pitch, yaw = quaternion_to_euler(x, y, z, w)
-        self.theta_z = yaw  # 単位はラジアンのままでOK
+        self.theta_z = yaw  # radian
     
     # /odom/UM982 callback
     def get_gps_odom(self, msg):
         if self.initial_xy is None:
             init_x = msg.pose.pose.position.x
             init_y = msg.pose.pose.position.y
+            x, y, z, w = msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w
+            roll, pitch, yaw = quaternion_to_euler(x, y, z, w)
             self.initial_xy = (init_x, init_y)
+            self.init_theta = yaw
+            #self.yaw_offset = self.init_theta - self.theta_z
             self.get_logger().info(f"Initial /odom/UM982 position set to: x={init_x:.3f}, y={init_y:.3f}")   
     
-    '''
-    def conversion(self, coordinate, origin, theta):
-        ido = coordinate[0]
-        keido = coordinate[1]
-        ido0 = origin[0]
-        keido0 = origin[1]
-
-        # self.get_logger().info(f"theta: {theta}")
-
-        a = 6378137
-        f = 35/10439
-        e1 = 734/8971
-        e2 = 127/1547
-        n = 35/20843
-        a0 = 1
-        a2 = 102/40495
-        a4 = 1/378280
-        a6 = 1/289634371
-        a8 = 1/204422462123
-        pi180 = 71/4068
-        # %math.pi/180
-        d_ido = ido - ido0
-        d_keido = keido - keido0
-        rd_ido = d_ido * pi180
-        rd_keido = d_keido * pi180
-        r_ido = ido * pi180
-        r_keido = keido * pi180
-        r_ido0 = ido0 * pi180
-        W = math.sqrt(1-(e1**2)*(math.sin(r_ido)**2))
-        N = a / W
-        t = math.tan(r_ido)
-        ai = e2*math.cos(r_ido)
-
-       # %===Y===%
-        S = a*(a0*r_ido - a2*math.sin(2*r_ido)+a4*math.sin(4*r_ido) -
-               a6*math.sin(6*r_ido)+a8*math.sin(8*r_ido))/(1+n)
-        S0 = a*(a0*r_ido0-a2*math.sin(2*r_ido0)+a4*math.sin(4*r_ido0) -
-                a6*math.sin(6*r_ido0)+a8*math.sin(8*r_ido0))/(1+n)
-        m0 = S/S0
-        B = S-S0
-        y1 = (rd_keido**2)*N*math.sin(r_ido)*math.cos(r_ido)/2
-        y2 = (rd_keido**4)*N*math.sin(r_ido) * \
-            (math.cos(r_ido)**3)*(5-(t**2)+9*(ai**2)+4*(ai**4))/24
-        y3 = (rd_keido**6)*N*math.sin(r_ido)*(math.cos(r_ido)**5) * \
-            (61-58*(t**2)+(t**4)+270*(ai**2)-330*(ai**2)*(t**2))/720
-        gps_y = self.Position_magnification * m0 * (B + y1 + y2 + y3)
-
-       # %===X===%
-        x1 = rd_keido*N*math.cos(r_ido)
-        x2 = (rd_keido**3)*N*(math.cos(r_ido)**3)*(1-(t**2)+(ai**2))/6
-        x3 = (rd_keido**5)*N*(math.cos(r_ido)**5) * \
-            (5-18*(t**2)+(t**4)+14*(ai**2)-58*(ai**2)*(t**2))/120
-        gps_x = self.Position_magnification * m0 * (x1 + x2 + x3)
-
-        # point = (gps_x, gps_y)Not match
-
-        degree_to_radian = math.pi / 180
-        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)
-
-        return point    
-    '''
-    '''
-    def receive_avg_gps_callback(self, request, response):
-        avg_lat, avg_lon, current_lat, current_lon, theta = request.avg_lat, request.avg_lon, request.current_lat, request.current_lon, request.current_theta
-        if theta == 0.0:
-            self.get_logger().warn("GPSからのthetaがまだ取得されていません。")
-            response.success = False
-            return response
-        
-        self.theta = theta    
-        current_coordinate = [current_lat, current_lon]
-        initial_coordinate = [avg_lat, avg_lon]
-        self.initial_xy = self.conversion(current_coordinate, initial_coordinate, theta)
-        xy_np = np.array(self.initial_xy)
-    '''
     def yaw_to_orientation(self, yaw):
         orientation_z = np.sin(yaw / 2.0)
         orientation_w = np.cos(yaw / 2.0)
@@ -157,22 +71,18 @@ def combine(self):
         pos_y = self.position_y
         #degree_to_radian = math.pi / 180
         #pos_theta = self.theta * degree_to_radian # maybe -self.theta * degree_to_radian
-        pos_theta = self.theta_z
-        # conversionによるxy変換結果（初期座標）
-        #delta_x, delta_y = self.initial_xy
+        pos_theta = self.theta_z + self.init_theta
 
-        # theta_z（現在の向き）で回転
-        #cos_theta = math.cos(pos_theta)
-        #sin_theta = math.sin(pos_theta)
+        # rotate init_theta for xy 
+        cos_theta = math.cos(self.init_theta)
+        sin_theta = math.sin(self.init_theta)
 
-        #rotated_x = cos_theta * pos_x - sin_theta * pos_y
-        #rotated_y = sin_theta * pos_x + cos_theta * pos_y
+        rotated_x = cos_theta * pos_x - sin_theta * pos_y
+        rotated_y = sin_theta * pos_x + cos_theta * pos_y
 
-        # 加算して新しい位置
-        #combined_x = self.initial_xy[0] + rotated_x
-        #combined_y = self.initial_xy[1] + rotated_y
-        combined_x = self.initial_xy[0] + pos_x
-        combined_y = self.initial_xy[1] + pos_y
+        # init xy + rotate xy
+        combined_x = self.initial_xy[0] + rotated_x
+        combined_y = self.initial_xy[1] + rotated_y
                 
         # quartanion z,w
         odom_orientation = self.yaw_to_orientation(pos_theta)
@@ -215,3 +125,4 @@ def main(args=None):
 
 if __name__ == '__main__':
     main()          
+
diff --git a/try_navigation/launch/try_navigation.launch.py b/try_navigation/launch/try_navigation.launch.py
index 8be3660..818f82c 100644
--- a/try_navigation/launch/try_navigation.launch.py
+++ b/try_navigation/launch/try_navigation.launch.py
@@ -32,7 +32,7 @@ def generate_launch_description():
     waypoint_path = LaunchConfiguration('waypoint_path')
     declare_waypoint_arg = DeclareLaunchArgument(
         'waypoint_path',
-        default_value='kbkn_maps/waypoints/tsukuba/2025/papa/tsukuba_sim.yaml',
+        default_value='kbkn_maps/waypoints/hosei/2025/nakaniwa_tsukuba.yaml',
         description='waypoint name'
     )
     

From 5c4213ed74ae4a833c33e9f8a0957a9ebb840ef8 Mon Sep 17 00:00:00 2001
From: kugurofu <tenten31569@icloud.com>
Date: Fri, 21 Nov 2025 10:05:59 +0000
Subject: [PATCH 24/25] ummmmm

---
 ...ss_odom_movingbase_fix_publisher_ttyUSB.py | 59 +++++++++++--------
 .../pcd_convert/pcd_height_segmentation.py    |  6 +-
 pcd_convert/pcd_convert/pcd_rotation.py       |  2 +-
 .../try_navigation/ekf_myself_gps.py          | 27 ++++-----
 .../try_navigation/path_follower.py           | 49 +++++++--------
 .../try_navigation/potential_astar.py         |  3 +-
 6 files changed, 78 insertions(+), 68 deletions(-)

diff --git a/orange_gnss/orange_gnss/gnss_odom_movingbase_fix_publisher_ttyUSB.py b/orange_gnss/orange_gnss/gnss_odom_movingbase_fix_publisher_ttyUSB.py
index 37d8f07..5cc3f0b 100644
--- a/orange_gnss/orange_gnss/gnss_odom_movingbase_fix_publisher_ttyUSB.py
+++ b/orange_gnss/orange_gnss/gnss_odom_movingbase_fix_publisher_ttyUSB.py
@@ -16,25 +16,29 @@ class GPSData(Node):
     def __init__(self):
         super().__init__('gps_data_acquisition')
 
-        # Parameters
         self.declare_parameter('port', '/dev/sensors/GNSS_UM982')
         self.declare_parameter('baud', 115200)
         self.declare_parameter('country_id', 0)
         self.declare_parameter('Position_magnification', 1.675)
         self.declare_parameter('heading', 90.0)
+        self.declare_parameter('start_lat', 35.425952230280004) # tsukuba start point right 36.04974095972727, 140.04593633886364 , left 36.04976195993636, 140.04593755179093/nakaniwa 35.4257898377487,139.313807281254 /35.425952230280004, 139.31380123427
+        self.declare_parameter('start_lon', 139.31380123427)
 
         self.dev_name = self.get_parameter('port').get_parameter_value().string_value
         self.serial_baud = self.get_parameter('baud').get_parameter_value().integer_value
         self.country_id = self.get_parameter('country_id').get_parameter_value().integer_value
         self.Position_magnification = self.get_parameter('Position_magnification').get_parameter_value().double_value
-        self.theta = self.get_parameter('heading').get_parameter_value().double_value
-        #self.tsukuba_theta = 90.0 # tsukuba 90.0 / nakaniwa 180.0
-        
-        #self.theta = 275.6 # tukuba param
-        #self.theta = 180 #nakaniwa param
+        #self.theta = self.get_parameter('heading').get_parameter_value().double_value
+        self.tsukuba_theta= self.get_parameter('heading').get_parameter_value().double_value # nakaniwa 180 tsukuba 93
+        self.theta = self.tsukuba_theta
+
         self.initial_coordinate = None
+        self.start_lat = self.get_parameter('start_lat').get_parameter_value().double_value
+        self.start_lon = self.get_parameter('start_lon').get_parameter_value().double_value
+        self.start_GPS_coordinate = [self.start_lat, self.start_lon]
         self.fix_data = None
         self.count = 0
+        
         self.initialized = False  # 平均初期座標が取得できたかどうか
 
         # Publishers
@@ -47,9 +51,12 @@ def __init__(self):
         
         # service client
         self.client = self.create_client(Avglatlon, 'send_avg_gps')
-        while not self.client.wait_for_service(timeout_sec=1.0):
-            self.get_logger().info("service not available...")
+        #while not self.client.wait_for_service(timeout_sec=1.0):
+        #    self.get_logger().info("service not available...")
 
+        self.get_logger().info("Start get_lonlat quat node")
+        self.get_logger().info("-------------------------")
+        
         # Timers
         self.timer = self.create_timer(1.0, self.timer_callback)
 
@@ -63,7 +70,7 @@ def __init__(self):
         # tkinter GUI setup
         self.root = tk.Tk()
         self.root.title("GPS Data Acquisition")
-        self.start_button = tk.Button(self.root, text="Start", command=self.start_gps_acquisition, width=20, height = 5)
+        self.start_button = tk.Button(self.root, text="Start GPS Acquisition", command=self.start_gps_acquisition, width=20, height = 5)
         self.start_button.pack()
 
         self.gps_acquisition_thread = None
@@ -74,11 +81,11 @@ def send_request(self):
         request = Avglatlon.Request()
         request.avg_lat = self.initial_coordinate[0]  # ← average lat
         request.avg_lon = self.initial_coordinate[1]  # ← average lon
-        #request.current_lat = self.currennt_coordinate[0]  # ← currennt lat
-        #request.current_lon = self.currennt_coordinate[1]  # ← currennt lon
-        request.theta = self.theta
-        #request.theta = self.tsukuba_theta # tsukuba start theta
-        #request.current_theta = self.theta # for tsukuba
+        request.current_lat = self.current_coordinate[0]  # ← currennt lat
+        request.current_lon = self.current_coordinate[1]  # ← currennt lon
+        #request.theta = self.theta
+        request.theta = self.tsukuba_theta # tsukuba start theta
+        request.current_theta = self.theta # for tsukuba
 
         future = self.client.call_async(request)
         future.add_done_callback(self.response_callback)
@@ -134,14 +141,14 @@ def acquire_gps_data(self):
             time.sleep(0.1)  # Slight delay to avoid overwhelming the GPS device
 
         if count > 0:
-            self.initial_coordinate = [lat_sum / count, lon_sum / count] # calculate average
-            #self.initial_coordinate = [36.0497399536, 140.04593714523637] # tsukuba start point 36.0497502, 140.0459234 / nakaniwa 35.425952230280004, 139.31380123427
-            #self.current_coordinate = [lat_sum / count, lon_sum / count] # for tsukuba
+            #self.initial_coordinate = [lat_sum / count, lon_sum / count] # calculate average
+            self.initial_coordinate = self.start_GPS_coordinate
+            self.current_coordinate = [lat_sum / count, lon_sum / count] # for tsukuba
             #self.theta = (heading_sum / count) - 90
             self.initialized = True
             self.get_logger().info(f"Initial coordinate set to: {self.initial_coordinate}")
-            #self.get_logger().info(f"current coordinate set to: {self.current_coordinate}")
-            #self.get_logger().info(f"Initial theta set to: {self.theta}")
+            self.get_logger().info(f"current coordinate set to: {self.current_coordinate}")
+            self.get_logger().info(f"Initial theta set to: {self.tsukuba_theta}")
             self.send_request()
         self.is_acquiring = False
 
@@ -270,7 +277,8 @@ def heading_to_quat(self ,real_heading):
 
         if self.count == 0:
             self.get_logger().info(f"!!!----------robotheading: {robotheading} deg----------!!!")
-            self.first_heading = robotheading
+            #self.first_heading = robotheading
+            self.first_heading = self.tsukuba_theta
             self.count = 1
 
         relative_heading = robotheading - self.first_heading
@@ -385,7 +393,7 @@ def publish_movingbase(self, heading):
 
             if self.count == 0:
                 self.get_logger().info(f"!!!----------robotheading: {robotheading} deg----------!!!")
-                self.first_heading = robotheading
+                self.first_heading = self.tsukuba_theta
                 self.count = 1
 
             relative_heading = robotheading - self.first_heading
@@ -422,8 +430,7 @@ def publish_movingbase(self, heading):
             self.get_logger().error("!!!!-not movingbase data-!!!!")
 
             
-    def publish_odom(self, lat, lon, alt):
-        
+    def publish_odom(self, lat, lon, alt):        
         #GPS_data = self.get_gps_quat(self.dev_name, self.country_id)
         #gnggadata = (Fixtype_data,latitude_data,longitude_data,altitude_data,satelitecount_data,heading)
         #if GPS_data and GPS_data[1] != 0 and GPS_data[2] != 0:
@@ -432,9 +439,9 @@ def publish_odom(self, lat, lon, alt):
             self.satelite = GPS_data[4]
             lonlat = [GPS_data[1], GPS_data[2]]
             
-            if self.initial_coordinate is None:
-                self.initial_coordinate = [GPS_data[1], GPS_data[2]]        
-            GPSxy = self.conversion(lonlat, self.initial_coordinate, self.theta)
+            #if self.initial_coordinate is None:
+            #    self.initial_coordinate = [GPS_data[1], GPS_data[2]]        
+            GPSxy = self.conversion(lonlat, self.start_GPS_coordinate, self.tsukuba_theta)
             GPSquat = self.heading_to_quat(GPS_data[5])       
 
             self.odom_msg.header.stamp = self.get_clock().now().to_msg()
diff --git a/pcd_convert/pcd_convert/pcd_height_segmentation.py b/pcd_convert/pcd_convert/pcd_height_segmentation.py
index c7bccc6..142017d 100644
--- a/pcd_convert/pcd_convert/pcd_height_segmentation.py
+++ b/pcd_convert/pcd_convert/pcd_height_segmentation.py
@@ -51,10 +51,10 @@ def __init__(self):
         self.GROUND_HIGHT_MIN = -10/1000; #hight range[m] # IGVC20250601 -150 -> -10
         self.GROUND_HIGHT_MAX =  150/1000; #hight range[m]
         #set step range
-        self.STEP_HIGHT_MIN =   200/1000; #hight range[m]
+        self.STEP_HIGHT_MIN =   130/1000; #hight range[m]
         self.STEP_HIGHT_MAX =   self.OBS_HIGHT_MIN# 200/1000; #hight range[m]
         self.STEP_X_MIN     = -1500/1000; #x mask range[m]
-        self.STEP_X_MAX     =  2000/1000; #x mask range[rosbag2_2024_10_26-03_14_14_20241026_kakunin_bag1m]
+        self.STEP_X_MAX     =  1500/1000; #x mask range[rosbag2_2024_10_26-03_14_14_20241026_kakunin_bag1m]
         self.STEP_Y_MIN     = -5000/1000; #y mask range[m]
         self.STEP_Y_MAX     =  5000/1000; #y mask range[m]
         
@@ -98,7 +98,7 @@ def pcd_heigth_segmentation(self, msg):
         pcd_step = self.height_segment(points, self.STEP_HIGHT_MIN, self.STEP_HIGHT_MAX)
         #pcd_step_height_under = self.height_segment(points, -self.STEP_HIGHT_MAX, -self.STEP_HIGHT_MIN)
         #pcd_step = np.insert(pcd_step, len(pcd_step[0,:]), pcd_step_height_under.T, axis=1)
-        #pcd_step = self.pcd_serch(pcd_step9, self.STEP_X_MIN, self.STEP_X_MAX, self.STEP_Y_MIN, self.STEP_Y_MAX)
+        pcd_step = self.pcd_serch(pcd_step, self.STEP_X_MIN, self.STEP_X_MAX, self.STEP_Y_MIN, self.STEP_Y_MAX)
         pcd_obs = np.insert(pcd_obs, len(pcd_obs[0,:]), pcd_step.T, axis=1)
         
         #publish for rviz2
diff --git a/pcd_convert/pcd_convert/pcd_rotation.py b/pcd_convert/pcd_convert/pcd_rotation.py
index 5d2e928..9f65ad7 100644
--- a/pcd_convert/pcd_convert/pcd_rotation.py
+++ b/pcd_convert/pcd_convert/pcd_rotation.py
@@ -43,7 +43,7 @@ def __init__(self):
         self.MID360_HIGHT = 950.8/1000; #hight position[m] 0.9508604675798957
             
         #上下反転  LiDAR init
-        self.THETA_INIT_X = 180 #[deg]
+        self.THETA_INIT_X = 181 #[deg]
         self.THETA_INIT_Y = 2.663 #[deg] 2.663001576990896
         self.THETA_INIT_Z = 0 #[deg]
         
diff --git a/try_navigation/try_navigation/ekf_myself_gps.py b/try_navigation/try_navigation/ekf_myself_gps.py
index c8cefc4..058a519 100644
--- a/try_navigation/try_navigation/ekf_myself_gps.py
+++ b/try_navigation/try_navigation/ekf_myself_gps.py
@@ -52,6 +52,7 @@ def __init__(self):
         self.RR_count_bad = 0
         self.RR_count_so = 0
         self.kalf_speed_param = 1.05
+        self.kalf_speed_param_noGPS = 1.03
         self.gps_rr_flag = 1
         self.offsetyaw_bad_gps = 0
 
@@ -67,7 +68,7 @@ def __init__(self):
         
 
         self.sub_a = self.create_subscription(
-            Odometry, '/odom/combine', self.sensor_a_callback, 10) # /odom/wheel_spimu /odom/combine
+            Odometry, '/odom/combine', self.sensor_a_callback, 10) # /odom/wheel_spimu
         self.sub_b = self.create_subscription(
             Odometry, '/odom/UM982', self.sensor_b_callback, 10)
         #self.sub_b = self.create_subscription(
@@ -133,7 +134,7 @@ def sensor_a_callback(self, data):
 
         self.GTheta = self.orientation_to_yaw(
             data.pose.pose.orientation.z, data.pose.pose.orientation.w)
-            
+        
         # initialize odom
         if not self.is_initialized and self.SmpTime is not None and self.SmpTime > 0: 
             self.initialize_odomA(
@@ -175,7 +176,7 @@ def sensor_b_callback(self, data):
     def determination_of_R(self):
         if self.GpsXY is not None:
             #カルマンフィルタのデッドレコニングよりになる範囲を指定
-            if (-60<self.GpsXY[0]) and (self.GpsXY[0]<60) and (20 < self.GpsXY[1]) and (self.GpsXY[1]<110):
+            if ((-60<self.GpsXY[0]) and (self.GpsXY[0]<60) and (25 < self.GpsXY[1]) and (self.GpsXY[1]<110)) or ((476<self.GpsXY[0]) and (self.GpsXY[0]<600) and (-84 < self.GpsXY[1]) and (self.GpsXY[1]<-25)) or ((-41<self.GpsXY[0]) and (self.GpsXY[0]<-7) and (-52 < self.GpsXY[1]) and (self.GpsXY[1]<71)): # last is for nakaniwa test
                 #self.gps_rr_flag = 1
                 self.gps_rr_flag = 0 #self.gps_rr_flag =1はGPS受信精度よく、0でGPS受信精度低い範囲に入ったフラグ
                 #self.offsetyaw_bad_gps = -10/180*math.pi
@@ -262,8 +263,7 @@ def sensim0(self, data):
         data = (self.k_p * self.e_n + self.k_d*(self.e_n - self.e_n1))
         self.e_n1 = self.e_n
         return data
-    
-    '''
+
     def initialize(self, GTheta, SmpTime):
         self.GTheta0 = GTheta
         self.XX = np.array([0, 0, np.cos(GTheta), np.sin(GTheta)])
@@ -273,9 +273,8 @@ def initialize(self, GTheta, SmpTime):
             [[(1.379e-3)**2, 0], [0, (0.03 * np.pi / 180 * SmpTime)**2]])
         G0 = np.array([[1, 0], [0, 0], [0, 0], [0, 1]])
         self.P = G0 @ self.Q @ G0.T
-    '''
-    
-    # 20251021 tuika
+        
+    # 既存の initialize メソッドの代わりに、座標を受け取る initialize_odomA を定義
     def initialize_odomA(self, x, y, GTheta, SmpTime):
         self.GTheta0 = GTheta
         # odomAの座標 (x, y) でXXを初期化
@@ -306,6 +305,9 @@ def KalfXY(self, Speed, SmpTime, GTheta, R1, R2):
             
         if not self.is_initialized:
             return np.array([0, 0])
+        
+        if self.H is None:
+            self.initialize(GTheta, SmpTime)
 
         self.R = np.array([[R1, 0], [0, R2]])
 
@@ -335,13 +337,10 @@ def KalfXY(self, Speed, SmpTime, GTheta, R1, R2):
     def KalfGPSXY(self, Speed, SmpTime, GTheta, GpsXY, R1, R2):
         #if self.H is None:
         #    self.initializeGPS(GpsXY, GTheta, SmpTime)
-        
+
         if not self.is_initialized:
             return np.array([0, 0])
         
-        if self.H is None:
-            self.initializeGPS(GpsXY, GTheta, SmpTime)
-
         self.R = np.array([[R1, 0], [0, R2]])
 
         DTheta = GTheta - self.GTheta0
@@ -480,7 +479,7 @@ def publish_fused_value(self):
                 self.get_logger().info(f"!!!!!!!!!!!!!!!!!!!!!! offsetyaw: {self.offsetyaw}!!!!!!!!!!!!!!!!!!!!!!!!!!")
                     
             #ここからカルマンフィルタ　:GPS情報が受信できている（Noneではない）＆受信精度が良い区間（sefl.gps_rr_flag=1）時に処理を行う
-            if self.GpsXY is not None and self.gps_rr_flag:#self.Number_of_satellites > #22 :
+            if self.GpsXY is not None and self.gps_rr_flag==1:#self.Number_of_satellites > #22 :
                 self.get_logger().info(f"!!!!++++++++++ Number_of_satellites: {self.Number_of_satellites}++++++++++!!!!!!!!!")
                 ########## Change the written line #######
                 #fused_value = self.KalfGPSXY(
@@ -559,7 +558,7 @@ def publish_fused_value(self):
                 
                 ########## Change the written line #######
                 #kalf_speed_paramは速度の神の手調整。速度が正しければ補正はいらないはず。
-                kalf_speed = self.Speed * self.kalf_speed_param
+                kalf_speed = self.Speed * self.kalf_speed_param_noGPS
                 #GPSなし持カルマンフィルタ関数実行（GPSデータなしの場合のため、デッドレコニングのみ）
                 fused_value = self.KalfXY(
                     kalf_speed, self.SmpTime, self.robot_yaw, self.R1, self.R2)
diff --git a/try_navigation/try_navigation/path_follower.py b/try_navigation/try_navigation/path_follower.py
index eab6ddc..deb73d9 100644
--- a/try_navigation/try_navigation/path_follower.py
+++ b/try_navigation/try_navigation/path_follower.py
@@ -135,22 +135,25 @@ def __init__(self):
             #[-69.5,	-62,	-47.5,	-27.5, 1.0], #tyokusen4 ||| [-67.5,	-62,	-47.5,	-27.5, 1.0]before:[-67.0,	-62,	-47.5,	-27.5, 1.0], 
             #[-55,	-35,	41,	46,    1.0], #Goal
             
+            [ 10.0,  11.5, -19.0,  39.0, 1.0], #nakaniwa_test
+            [ 30.0,  31.5, -19.0,  39.0, 1.0], #nakaniwa_test
             [ 63.0,  64.5,  19.0,  39.0, 1.0], #shiyakusyo
-            [ 99.5, 101.0,  25.0,  45.0, 1.0], #dourotan1
-            [177.0, 178.5,  25.0,  45.0, 1.0], #dourotan2
+            [100.5, 102.0,  25.0,  45.0, 1.0], #dourotan1
+            [178.5, 180.0,  25.0,  45.0, 1.0], #dourotan2
             [257.5, 277.5, -60.0, -58.5, 1.0], #singoumaeteisisen1
-            [257.5, 277.5, -67.0, -65.5, 1.0], #singoumae1
-            [256.5, 276.5, -87.5, -86.0, 1.0], #singoumaeteisisen2
-            [270.0, 271.0, -99.0, -79.5, 1.0], #singoumae2
-            [405.0, 425.0, -80.5, -79.5, 1.0], #ekimae oudanhodou1
-            [405.0, 425.0, -71.0, -70.0, 1.0], #ekimae oudanhodou2
-            [289.0, 290.0, -98.0, -78.0, 1.0], #singoumaeteisisen3
+            [257.5, 277.5, -68.0, -66.5, 1.0], #singoumae1
+            [256.5, 276.5, -89.0, -87.5, 1.0], #singoumaeteisisen2
+            [269.5, 270.5, -99.0, -79.5, 1.0], #singoumae2
+            [405.0, 425.0, -82.5, -81.5, 1.0], #ekimae oudanhodou1 y-1
+            [555.0, 568.0, -85.0, -71.0, 0.0], #ekimae not stop
+            [405.0, 425.0, -73.0, -72.0, 1.0], #ekimae oudanhodou2 y-2
+            [289.5, 290.5, -98.0, -78.0, 1.0], #singoumaeteisisen3
             [284.0, 285.0, -98.0, -78.0, 1.0], #singoumae3
-            [259.5, 279.5, -83.0, -82.0, 1.0], #singoumaeteisisen4
-            [259.5, 279.5, -80.0, -79.0, 1.0], #singoumae4
+            [259.5, 279.5, -83.5, -80.5, 1.0], #singoumaeteisisen4 12
+            [259.5, 279.5, -81.0, -79.0, 1.0], #singoumae4 13
             [184.5, 186.0,  25.0,  45.0, 1.0], #dourotan3
             [107.0, 108.5,  25.0,  45.0, 1.0], #dourotan4
-            [ 74.0,  94.0, -27.0, -26.0, 1.0], #GOAL!!!!
+            [ 64.0, 104.0, -30.0, -24.0, 1.0], #GOAL!!!!
             [  999,   999,   999,   999, 0.0] ]) #
         self.stop_num = 0;
         
@@ -653,22 +656,22 @@ def get_odom_ref(self, msg):
         self.ref_theta_y = 0 #pitch /math.pi*180
         self.ref_theta_z = yaw /math.pi*180
         
-        if self.waypoint_number >= 79:
-            if self.stop_num <= 14:
-                self.stop_num = 15
-        elif self.waypoint_number >= 71:
-            if self.stop_num <= 12:
-                self.stop_num = 13
-        elif self.waypoint_number >= 62:
-            if self.stop_num <= 8:
-                self.stop_num = 9
-        elif self.waypoint_number >= 51:
+        if self.waypoint_number >= 225: # after dourotan4
+            if self.stop_num <= 15:
+                self.stop_num = 16
+        elif self.waypoint_number >= 198: # after singou
+            if self.stop_num <= 13:
+                self.stop_num = 14
+        elif self.waypoint_number >= 178: # after ekimae oudanhodou2
+            if self.stop_num <= 9:
+                self.stop_num = 10
+        elif self.waypoint_number >= 138: # after ekimae oudanhodou1
             if self.stop_num <= 7:
                 self.stop_num = 8
-        elif self.waypoint_number >= 33:
+        elif self.waypoint_number >= 81: # after singou
             if self.stop_num <= 6:
                 self.stop_num = 7
-        elif self.waypoint_number >= 25:
+        elif self.waypoint_number >= 57: # after dourotan2
             if self.stop_num <= 2:
                 self.stop_num = 3
         
diff --git a/try_navigation/try_navigation/potential_astar.py b/try_navigation/try_navigation/potential_astar.py
index 2b8d81d..36c3374 100644
--- a/try_navigation/try_navigation/potential_astar.py
+++ b/try_navigation/try_navigation/potential_astar.py
@@ -459,7 +459,8 @@ def potential_astar(self, msg):
         obs10 = make_obs(36.5,36.5, 62, 96.5) # siyakusyo higasi
         
 
-        self.tsukuba_obs = np.hstack((obs1, obs4, obs5, obs6, obs7, obs8, obs9, obs10))
+        #self.tsukuba_obs = np.hstack((obs1, obs4, obs5, obs6, obs7, obs8, obs9, obs10))
+        self.tsukuba_obs = np.hstack((obs7,obs1))
         
         #map_obs add
         if len(self.tsukuba_obs[0,:])>0:

From e419d9a80425347cb5251a66aab2e8a597db7701 Mon Sep 17 00:00:00 2001
From: kugurofu <tenten31569@icloud.com>
Date: Mon, 8 Dec 2025 07:14:18 +0000
Subject: [PATCH 25/25] actual performance tsukuba2025

---
 orange_bringup/launch/orange_robot.launch.xml |   8 +-
 .../pcd_convert/pcd_height_segmentation.py    |  18 +-
 .../pcd_convert/pcd_reflect_segmentation.py   | 222 +++++++++++++++++
 pcd_convert/pcd_convert/pcd_rotation.py       |   2 +-
 pcd_convert/setup.py                          |   1 +
 .../launch/try_navigation.launch.py           |  14 +-
 try_navigation/setup.py                       |   1 +
 .../try_navigation/ekf_myself_gps.py          |   8 +-
 try_navigation/try_navigation/marker_utils.py | 227 ++++++++++++++++++
 .../try_navigation/path_follower.py           | 206 +++++++++++++---
 .../try_navigation/potential_astar.py         | 144 ++++++++---
 11 files changed, 782 insertions(+), 69 deletions(-)
 create mode 100644 pcd_convert/pcd_convert/pcd_reflect_segmentation.py
 create mode 100644 try_navigation/try_navigation/marker_utils.py

diff --git a/orange_bringup/launch/orange_robot.launch.xml b/orange_bringup/launch/orange_robot.launch.xml
index e37f49a..841e94d 100644
--- a/orange_bringup/launch/orange_robot.launch.xml
+++ b/orange_bringup/launch/orange_robot.launch.xml
@@ -114,14 +114,14 @@
   <!-- initial heading:housei nakaniwa=179.169287 tsukuba= 93.0 -->
   <!-- initial start_lat:housei nakaniwa=35.425952230280004 tsukuba right=36.04974095972727 tsukuba_left=36.04976195993636 -->
   <!-- initial start_lon:housei nakaniwa=139.31380123427 tsukuba right=140.04593633886364 tsukuba_left=140.04593755179093 -->
-  <node pkg="orange_gnss" exec="gnss_odom_publisher_ttyUSB" output="screen">
+  <node pkg="orange_gnss" exec="gnss_odom_movingbase_fix_publisher_ttyUSB" output="screen">
     <param name="port" value="/dev/sensors/GNSS_UM982"/>
     <param name="baud" value="115200"/>
     <param name="country_id" value="0"/>
     <param name="Position_magnification" value="1.675"/>
-    <param name="heading" value="180.0"/>
-    <param name="start_lat" value="35.425952230280004"/>
-    <param name="start_lon" value="139.31380123427"/>
+    <param name="heading" value="93.0"/>
+    <param name="start_lat" value="36.04974095972727"/>
+    <param name="start_lon" value="140.04593633886364"/>
     <param name="time_out" value="1.0"/>
   </node>
   <!-- 
diff --git a/pcd_convert/pcd_convert/pcd_height_segmentation.py b/pcd_convert/pcd_convert/pcd_height_segmentation.py
index 142017d..1fe5e3d 100644
--- a/pcd_convert/pcd_convert/pcd_height_segmentation.py
+++ b/pcd_convert/pcd_convert/pcd_height_segmentation.py
@@ -38,6 +38,7 @@ def __init__(self):
         self.pcd_segment_obs_publisher = self.create_publisher(sensor_msgs.PointCloud2, 'pcd_segment_obs', qos_profile) #set publish pcd topic name
         self.pcd_segment_ground_publisher = self.create_publisher(sensor_msgs.PointCloud2, 'pcd_segment_ground', qos_profile) #set publish pcd topic name
         self.pcd_segment_step_publisher = self.create_publisher(sensor_msgs.PointCloud2, 'pcd_segment_step', qos_profile) #set publish pcd topic name
+        self.pcd_segment_low_step_publisher = self.create_publisher(sensor_msgs.PointCloud2, 'pcd_segment_low_step', qos_profile) #set publish pcd topic name
         
         #パラメータ
         #set obs range
@@ -51,12 +52,19 @@ def __init__(self):
         self.GROUND_HIGHT_MIN = -10/1000; #hight range[m] # IGVC20250601 -150 -> -10
         self.GROUND_HIGHT_MAX =  150/1000; #hight range[m]
         #set step range
-        self.STEP_HIGHT_MIN =   130/1000; #hight range[m]
+        self.STEP_HIGHT_MIN =   140/1000; #hight range[m]
         self.STEP_HIGHT_MAX =   self.OBS_HIGHT_MIN# 200/1000; #hight range[m]
         self.STEP_X_MIN     = -1500/1000; #x mask range[m]
         self.STEP_X_MAX     =  1500/1000; #x mask range[rosbag2_2024_10_26-03_14_14_20241026_kakunin_bag1m]
         self.STEP_Y_MIN     = -5000/1000; #y mask range[m]
         self.STEP_Y_MAX     =  5000/1000; #y mask range[m]
+        #set low step range
+        self.LOW_STEP_HIGHT_MIN =   70/1000; #hight range[m]
+        self.LOW_STEP_HIGHT_MAX =   self.OBS_HIGHT_MIN# 200/1000; #hight range[m]
+        self.LOW_STEP_X_MIN     = -1500/1000; #x mask range[m]
+        self.LOW_STEP_X_MAX     =  1500/1000; #x mask range[rosbag2_2024_10_26-03_14_14_20241026_kakunin_bag1m]
+        self.LOW_STEP_Y_MIN     = -5000/1000; #y mask range[m]
+        self.LOW_STEP_Y_MAX     =  5000/1000; #y mask range[m]
         
     def pointcloud2_to_array(self, cloud_msg):
         # Extract point cloud data
@@ -101,6 +109,12 @@ def pcd_heigth_segmentation(self, msg):
         pcd_step = self.pcd_serch(pcd_step, self.STEP_X_MIN, self.STEP_X_MAX, self.STEP_Y_MIN, self.STEP_Y_MAX)
         pcd_obs = np.insert(pcd_obs, len(pcd_obs[0,:]), pcd_step.T, axis=1)
         
+        #low step segment
+        pcd_low_step = self.height_segment(points, self.LOW_STEP_HIGHT_MIN, self.LOW_STEP_HIGHT_MAX)
+        #pcd_step_height_under = self.height_segment(points, -self.STEP_HIGHT_MAX, -self.STEP_HIGHT_MIN)
+        #pcd_step = np.insert(pcd_step, len(pcd_step[0,:]), pcd_step_height_under.T, axis=1)
+        pcd_low_step = self.pcd_serch(pcd_low_step, self.LOW_STEP_X_MIN, self.LOW_STEP_X_MAX, self.LOW_STEP_Y_MIN, self.LOW_STEP_Y_MAX)
+        
         #publish for rviz2
         self.pcd_segment_obs = point_cloud_intensity_msg(pcd_obs.T, t_stamp, 'odom')
         self.pcd_segment_obs_publisher.publish(self.pcd_segment_obs ) 
@@ -108,6 +122,8 @@ def pcd_heigth_segmentation(self, msg):
         self.pcd_segment_ground_publisher.publish(self.pcd_segment_ground ) 
         self.pcd_segment_step = point_cloud_intensity_msg(pcd_step.T, t_stamp, 'odom')
         self.pcd_segment_step_publisher.publish(self.pcd_segment_step ) 
+        self.pcd_segment_low_step = point_cloud_intensity_msg(pcd_low_step.T, t_stamp, 'odom')
+        self.pcd_segment_low_step_publisher.publish(self.pcd_segment_low_step ) 
         
     def height_segment(self, pointcloud, height_min, height_max):
         pcd_ind = ((height_min <= pointcloud[2,:]) * (pointcloud[2,:] <= height_max ))
diff --git a/pcd_convert/pcd_convert/pcd_reflect_segmentation.py b/pcd_convert/pcd_convert/pcd_reflect_segmentation.py
new file mode 100644
index 0000000..b1bf24b
--- /dev/null
+++ b/pcd_convert/pcd_convert/pcd_reflect_segmentation.py
@@ -0,0 +1,222 @@
+# rclpy (ROS 2のpythonクライアント)の機能を使えるようにします。
+import rclpy
+# rclpy (ROS 2のpythonクライアント)の機能のうちNodeを簡単に使えるようにします。こう書いていない場合、Nodeではなくrclpy.node.Nodeと書く必要があります。
+from rclpy.node import Node
+import std_msgs.msg as std_msgs
+import sensor_msgs.msg as sensor_msgs
+import numpy as np
+import math
+from rclpy.qos import QoSProfile, QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy
+import time
+
+
+# C++と同じく、Node型を継承します。
+class PcdReflectSegmentation(Node):
+    # コンストラクタです、PcdReflectSegmentationクラスのインスタンスを作成する際に呼び出されます。
+    def __init__(self):
+        # 継承元のクラスを初期化します。
+        super().__init__('pcd_reflect_segmentation_node')
+        
+        qos_profile = QoSProfile(
+            history=QoSHistoryPolicy.KEEP_LAST,
+            reliability=QoSReliabilityPolicy.RELIABLE,
+            durability=QoSDurabilityPolicy.VOLATILE,
+            depth = 1
+        )
+        
+        qos_profile_sub = QoSProfile(
+            reliability=QoSReliabilityPolicy.RELIABLE,
+            durability=QoSDurabilityPolicy.VOLATILE,
+            depth = 1
+        )
+        
+        # Subscriptionを作成。
+        self.subscription = self.create_subscription(sensor_msgs.PointCloud2, '/pcd_rotation', self.pcd_reflect_segmentation, qos_profile) #set subscribe pcd topic name
+        self.subscription  # 警告を回避するために設置されているだけです。削除しても挙動はかわりません。
+        
+        # Publisherを作成
+        #self.pcd_segment_paper_publisher = self.create_publisher(sensor_msgs.PointCloud2, 'pcd_segment_paper', qos_profile) #set publish pcd topic name
+        #self.pcd_segment_cardboard_publisher = self.create_publisher(sensor_msgs.PointCloud2, 'pcd_segment_cardboard', qos_profile) #set publish pcd topic name
+        #self.pcd_segment_asphalt_publisher = self.create_publisher(sensor_msgs.PointCloud2, 'pcd_segment_asphalt', qos_profile) #set publish pcd topic name
+        #self.pcd_segment_whiteline_publisher = self.create_publisher(sensor_msgs.PointCloud2, 'pcd_segment_whiteline', qos_profile) #set publish pcd topic name
+        #self.pcd_segment_gravel_publisher = self.create_publisher(sensor_msgs.PointCloud2, 'pcd_segment_gravel', qos_profile) #set publish pcd topic name
+        #self.pcd_segment_grass_publisher = self.create_publisher(sensor_msgs.PointCloud2, 'pcd_segment_grass', qos_profile) #set publish pcd topic name
+        #self.pcd_segment_high_ref_publisher = self.create_publisher(sensor_msgs.PointCloud2, 'pcd_segment_high_ref', qos_profile) #set publish pcd topic name
+        #self.pcd_segment_low_ref_publisher = self.create_publisher(sensor_msgs.PointCloud2, 'pcd_segment_low_ref', qos_profile) #set publish pcd topic name
+        self.pcd_segment_shibafu_publisher = self.create_publisher(sensor_msgs.PointCloud2, 'pcd_segment_shibafu', qos_profile) #set publish pcd topic name
+        #self.pcd_segment_otiba_publisher = self.create_publisher(sensor_msgs.PointCloud2, 'pcd_segment_otiba', qos_profile) #set publish pcd topic name
+        
+        #パラメータ
+        #set obs range
+        self.OBS_MASK_X_MIN = -550/1000; #x mask range[m]
+        self.OBS_MASK_X_MAX =  200/1000; #x mask range[m]
+        self.OBS_MASK_Y_MIN = -350/1000; #y mask range[m]
+        self.OBS_MASK_Y_MAX =  350/1000; #y mask range[m]
+        
+        self.obj_reflect =[
+            #     0       1
+            # median  sigma
+            [  47.00,  6.26], # 0:paper ポットホール（紙）
+            [  26.00,  2.33], # 1:cardboard 段ボール
+            [  10.00,  0.92], # 2:asphalt アスファルト
+            [ 155.00, 25.33], # 3:whiteline 白線
+            [  15.00,  3.29], # 4:gravel 砂利
+            [  64.00,  8.03], # 5:grass 
+            [ 119.05, 10.32], # 6:high_ref 
+            [   3.62,  1.21], # 7:low_ref 
+            [  52.00,  5.25], # 8:shibafu
+            [  72.00, 16.81], # 9:otiba 
+            [   0,      0]    # 
+        ]
+        self.sigma = 2
+        
+        
+    def pointcloud2_to_array(self, cloud_msg):
+        # Extract point cloud data
+        points = np.frombuffer(cloud_msg.data, dtype=np.uint8).reshape(-1, cloud_msg.point_step)
+        x = np.frombuffer(points[:, 0:4].tobytes(), dtype=np.float32)
+        y = np.frombuffer(points[:, 4:8].tobytes(), dtype=np.float32)
+        z = np.frombuffer(points[:, 8:12].tobytes(), dtype=np.float32)
+        intensity = np.frombuffer(points[:, 12:16].tobytes(), dtype=np.float32)
+
+        # Combine into a 4xN matrix
+        point_cloud_matrix = np.vstack((x, y, z, intensity))
+        print(point_cloud_matrix)
+        print(f"point_cloud_matrix ={point_cloud_matrix.shape}")
+        print(f"x ={x.dtype, x.shape}")
+        
+        return point_cloud_matrix
+        
+    def pcd_reflect_segmentation(self, msg):
+        
+        #print stamp message
+        t_stamp = msg.header.stamp
+        print(f"t_stamp ={t_stamp}")
+        
+        #get pcd data
+        points = self.pointcloud2_to_array(msg)
+        print(f"points ={points.shape}")
+        
+        #obs segment
+        pcd_mask_cut = self.pcd_mask(points, self.OBS_MASK_X_MIN, self.OBS_MASK_X_MAX, self.OBS_MASK_Y_MIN, self.OBS_MASK_Y_MAX)
+        print(f"pcd_mask_cut ={pcd_mask_cut.shape}")
+        print(f"self.obj_reflect ={len(self.obj_reflect)}")
+        
+        
+        pcd_obj_reflect_ind_list = []  # 列数に基づいて初期化
+        for obj_ind in range(len(self.obj_reflect)-1):
+             if self.obj_reflect[obj_ind][0] < 151:
+                 obj_reflect_min = self.obj_reflect[obj_ind][0] - self.sigma * self.obj_reflect[obj_ind][1]
+                 obj_reflect_max = self.obj_reflect[obj_ind][0] + self.sigma * self.obj_reflect[obj_ind][1]
+                 if obj_reflect_min < 0:
+                     obj_reflect_min = 0
+                 if obj_reflect_max > 150:
+                     obj_reflect_max = 150
+             else:
+                 obj_reflect_min = 151
+                 obj_reflect_max = 255
+             pcd_obj_reflect_ind =  np.array(self.reflect_segment(pcd_mask_cut, obj_reflect_min, obj_reflect_max))
+             pcd_obj_reflect_ind_list.append(pcd_obj_reflect_ind) 
+        print(f"pcd_obj_reflect_ind_list ={pcd_obj_reflect_ind_list[0]}")
+        
+        #publish for rviz2
+        pcd_segment_paper = pcd_mask_cut[:, pcd_obj_reflect_ind_list[0]]
+        
+        #self.pcd_segment_paper     = point_cloud_intensity_msg(pcd_mask_cut[:, pcd_obj_reflect_ind_list[0]].T, t_stamp, 'odom')
+        #self.pcd_segment_paper_publisher.publish(self.pcd_segment_paper ) 
+        #self.pcd_segment_cardboard = point_cloud_intensity_msg(pcd_mask_cut[:, pcd_obj_reflect_ind_list[1]].T, t_stamp, 'odom')
+        #self.pcd_segment_cardboard_publisher.publish(self.pcd_segment_cardboard ) 
+        #self.pcd_segment_asphalt   = point_cloud_intensity_msg(pcd_mask_cut[:, pcd_obj_reflect_ind_list[2]].T, t_stamp, 'odom')
+        #self.pcd_segment_asphalt_publisher.publish(self.pcd_segment_asphalt ) 
+        #self.pcd_segment_whiteline = point_cloud_intensity_msg(pcd_mask_cut[:, pcd_obj_reflect_ind_list[3]].T, t_stamp, 'odom')
+        #self.pcd_segment_whiteline_publisher.publish(self.pcd_segment_whiteline ) 
+        #self.pcd_segment_gravel    = point_cloud_intensity_msg(pcd_mask_cut[:, pcd_obj_reflect_ind_list[4]].T, t_stamp, 'odom')
+        #self.pcd_segment_gravel_publisher.publish(self.pcd_segment_gravel ) 
+        #self.pcd_segment_grass    = point_cloud_intensity_msg(pcd_mask_cut[:, pcd_obj_reflect_ind_list[5]].T, t_stamp, 'odom')
+        #self.pcd_segment_grass_publisher.publish(self.pcd_segment_grass ) 
+        #self.pcd_segment_high_ref    = point_cloud_intensity_msg(pcd_mask_cut[:, pcd_obj_reflect_ind_list[6]].T, t_stamp, 'odom')
+        #self.pcd_segment_high_ref_publisher.publish(self.pcd_segment_high_ref ) 
+        #self.pcd_segment_low_ref    = point_cloud_intensity_msg(pcd_mask_cut[:, pcd_obj_reflect_ind_list[7]].T, t_stamp, 'odom')
+        #self.pcd_segment_low_ref_publisher.publish(self.pcd_segment_low_ref ) 
+        
+        self.pcd_segment_shibafu    = point_cloud_intensity_msg(pcd_mask_cut[:, pcd_obj_reflect_ind_list[8]].T, t_stamp, 'odom')
+        self.pcd_segment_shibafu_publisher.publish(self.pcd_segment_shibafu ) 
+        #self.pcd_segment_otiba    = point_cloud_intensity_msg(pcd_mask_cut[:, pcd_obj_reflect_ind_list[9]].T, t_stamp, 'odom')
+        #self.pcd_segment_otiba_publisher.publish(self.pcd_segment_otiba ) 
+        
+        
+        
+        
+        
+    def height_segment(self, pointcloud, height_min, height_max):
+        pcd_ind = ((height_min <= pointcloud[2,:]) * (pointcloud[2,:] <= height_max ))
+        pcd_segment = pointcloud[:, pcd_ind]
+        return pcd_segment
+        
+    def reflect_segment(self, pointcloud, reflect_min, reflect_max):
+        pcd_ind = ((reflect_min <= pointcloud[3,:]) * (pointcloud[3,:] <= reflect_max ))
+        return pcd_ind
+        
+    def pcd_mask(self, pointcloud, x_min, x_max, y_min, y_max):
+        pcd_ind = (( (x_min <= pointcloud[0,:]) * (pointcloud[0,:] <= x_max)) * ((y_min <= pointcloud[1,:]) * (pointcloud[1,:]) <= y_max ) )
+        pcd_mask = pointcloud[:, ~pcd_ind]
+        return pcd_mask
+        
+    def pcd_serch(self, pointcloud, x_min, x_max, y_min, y_max):
+        pcd_ind = (( (x_min <= pointcloud[0,:]) * (pointcloud[0,:] <= x_max)) * ((y_min <= pointcloud[1,:]) * (pointcloud[1,:]) <= y_max ) )
+        pcd_mask = pointcloud[:, pcd_ind]
+        return pcd_mask
+        
+
+def point_cloud_intensity_msg(points, t_stamp, parent_frame):
+    # In a PointCloud2 message, the point cloud is stored as an byte 
+    # array. In order to unpack it, we also include some parameters 
+    # which desribes the size of each individual point.
+    ros_dtype = sensor_msgs.PointField.FLOAT32
+    dtype = np.float32
+    itemsize = np.dtype(dtype).itemsize # A 32-bit float takes 4 bytes.
+    data = points.astype(dtype).tobytes() 
+
+    # The fields specify what the bytes represents. The first 4 bytes 
+    # represents the x-coordinate, the next 4 the y-coordinate, etc.
+    fields = [
+            sensor_msgs.PointField(name='x', offset=0, datatype=ros_dtype, count=1),
+            sensor_msgs.PointField(name='y', offset=4, datatype=ros_dtype, count=1),
+            sensor_msgs.PointField(name='z', offset=8, datatype=ros_dtype, count=1),
+            sensor_msgs.PointField(name='intensity', offset=12, datatype=ros_dtype, count=1),
+        ]
+
+    # The PointCloud2 message also has a header which specifies which 
+    # coordinate frame it is represented in. 
+    header = std_msgs.Header(frame_id=parent_frame, stamp=t_stamp)
+    
+
+    return sensor_msgs.PointCloud2(
+        header=header,
+        height=1, 
+        width=points.shape[0],
+        is_dense=True,
+        is_bigendian=False,
+        fields=fields,
+        point_step=(itemsize * 4), # Every point consists of three float32s.
+        row_step=(itemsize * 4 * points.shape[0]), 
+        data=data
+    )
+
+# mainという名前の関数です。C++のmain関数とは異なり、これは処理の開始地点ではありません。
+def main(args=None):
+    # rclpyの初期化処理です。ノードを立ち上げる前に実装する必要があります。
+    rclpy.init(args=args)
+    # クラスのインスタンスを作成
+    pcd_reflect_segmentation = PcdReflectSegmentation()
+    # spin処理を実行、spinをしていないとROS 2のノードはデータを入出力することが出来ません。
+    rclpy.spin(pcd_reflect_segmentation)
+    # 明示的にノードの終了処理を行います。
+    pcd_reflect_segmentation.destroy_node()
+    # rclpyの終了処理、これがないと適切にノードが破棄されないため様々な不具合が起こります。
+    rclpy.shutdown()
+
+# 本スクリプト(publish.py)の処理の開始地点です。
+if __name__ == '__main__':
+    # 関数`main`を実行する。
+    main()
diff --git a/pcd_convert/pcd_convert/pcd_rotation.py b/pcd_convert/pcd_convert/pcd_rotation.py
index 9f65ad7..0a42f81 100644
--- a/pcd_convert/pcd_convert/pcd_rotation.py
+++ b/pcd_convert/pcd_convert/pcd_rotation.py
@@ -43,7 +43,7 @@ def __init__(self):
         self.MID360_HIGHT = 950.8/1000; #hight position[m] 0.9508604675798957
             
         #上下反転  LiDAR init
-        self.THETA_INIT_X = 181 #[deg]
+        self.THETA_INIT_X = 180.5 #[deg]
         self.THETA_INIT_Y = 2.663 #[deg] 2.663001576990896
         self.THETA_INIT_Z = 0 #[deg]
         
diff --git a/pcd_convert/setup.py b/pcd_convert/setup.py
index f721c98..3d9e743 100644
--- a/pcd_convert/setup.py
+++ b/pcd_convert/setup.py
@@ -46,6 +46,7 @@
         'console_scripts': [
             'pcd_rotation = pcd_convert.pcd_rotation:main',
             'pcd_height_segmentation = pcd_convert.pcd_height_segmentation:main',
+            'pcd_reflect_segmentation = pcd_convert.pcd_reflect_segmentation:main',
             'pcd_rotation_lidar2 = pcd_convert.pcd_rotation_lidar2:main'
         ],
     },
diff --git a/try_navigation/launch/try_navigation.launch.py b/try_navigation/launch/try_navigation.launch.py
index 818f82c..ce8cbfb 100644
--- a/try_navigation/launch/try_navigation.launch.py
+++ b/try_navigation/launch/try_navigation.launch.py
@@ -32,10 +32,11 @@ def generate_launch_description():
     waypoint_path = LaunchConfiguration('waypoint_path')
     declare_waypoint_arg = DeclareLaunchArgument(
         'waypoint_path',
-        default_value='kbkn_maps/waypoints/hosei/2025/nakaniwa_tsukuba.yaml',
+        default_value='kbkn_maps/waypoints/tsukuba/2025/papa/tsukuba_waypoint.yaml',
         description='waypoint name'
     )
-    
+    #kbkn_maps/waypoints/tsukuba/2025/papa/tsukuba_waypoint.yaml
+    #kbkn_maps/waypoints/hosei/2025/nakaniwa.yaml
     waypoint_start_index = LaunchConfiguration('waypoint_start_index')
     declare_waypoint_start_arg = DeclareLaunchArgument(
         'waypoint_start_index',
@@ -89,6 +90,15 @@ def generate_launch_description():
             output='screen',
             arguments=[]
         ),
+        
+        #pcd segmentation
+        Node(package='pcd_convert',
+            executable='pcd_reflect_segmentation',
+            name='pcd_reflect_segmentation_node',
+            output='screen',
+            arguments=[]
+        ),
+        
         #odom wheel
         #Node(package='try_navigation',
         #    executable='odom_wheel',
diff --git a/try_navigation/setup.py b/try_navigation/setup.py
index 84e85a5..0226f3c 100644
--- a/try_navigation/setup.py
+++ b/try_navigation/setup.py
@@ -59,6 +59,7 @@
             'ekf_myself_gps = try_navigation.ekf_myself_gps:main',
             'ekf_myself_odom = try_navigation.ekf_myself_odom:main',
             'map_check = try_navigation.map_check:main',
+            'marker_utils = try_navigation.marker_utils:main',
             'pcd_buffer = try_navigation.pcd_buffer:main'
 
         ],
diff --git a/try_navigation/try_navigation/ekf_myself_gps.py b/try_navigation/try_navigation/ekf_myself_gps.py
index 058a519..8cd10c2 100644
--- a/try_navigation/try_navigation/ekf_myself_gps.py
+++ b/try_navigation/try_navigation/ekf_myself_gps.py
@@ -52,7 +52,7 @@ def __init__(self):
         self.RR_count_bad = 0
         self.RR_count_so = 0
         self.kalf_speed_param = 1.05
-        self.kalf_speed_param_noGPS = 1.03
+        self.kalf_speed_param_noGPS = 1.00
         self.gps_rr_flag = 1
         self.offsetyaw_bad_gps = 0
 
@@ -176,11 +176,15 @@ def sensor_b_callback(self, data):
     def determination_of_R(self):
         if self.GpsXY is not None:
             #カルマンフィルタのデッドレコニングよりになる範囲を指定
-            if ((-60<self.GpsXY[0]) and (self.GpsXY[0]<60) and (25 < self.GpsXY[1]) and (self.GpsXY[1]<110)) or ((476<self.GpsXY[0]) and (self.GpsXY[0]<600) and (-84 < self.GpsXY[1]) and (self.GpsXY[1]<-25)) or ((-41<self.GpsXY[0]) and (self.GpsXY[0]<-7) and (-52 < self.GpsXY[1]) and (self.GpsXY[1]<71)): # last is for nakaniwa test
+            if ((-60<self.GpsXY[0]) and (self.GpsXY[0]<60) and (35 < self.GpsXY[1]) and (self.GpsXY[1]<110)) or ((539<self.GpsXY[0]) and (self.GpsXY[0]<600) and (-84 < self.GpsXY[1]) and (self.GpsXY[1]<-25)) or ((260<self.GpsXY[0]) and (self.GpsXY[0]<275) and (-66 < self.GpsXY[1]) and (self.GpsXY[1]<-40)): # last is for nakaniwa test or ((-41<self.GpsXY[0]) and (self.GpsXY[0]<-7) and (-52 < self.GpsXY[1]) and (self.GpsXY[1]<71)) eki x 476<self.GpsXY[0]) and (self.GpsXY[0]<600
                 #self.gps_rr_flag = 1
                 self.gps_rr_flag = 0 #self.gps_rr_flag =1はGPS受信精度よく、0でGPS受信精度低い範囲に入ったフラグ
                 #self.offsetyaw_bad_gps = -10/180*math.pi
                 self.GPS_angle_conut = 0; ## tukuba 20250920 zantei
+                if ((476<self.GpsXY[0]) and (self.GpsXY[0]<600) and (-84 < self.GpsXY[1]) and (self.GpsXY[1]<-25)):
+                   self.kalf_speed_param_noGPS = 1.00
+                else:
+                   self.kalf_speed_param_noGPS = 1.00
             else:
                 self.gps_rr_flag = 1
                 self.offsetyaw_bad_gps = 0
diff --git a/try_navigation/try_navigation/marker_utils.py b/try_navigation/try_navigation/marker_utils.py
new file mode 100644
index 0000000..3bd2ee6
--- /dev/null
+++ b/try_navigation/try_navigation/marker_utils.py
@@ -0,0 +1,227 @@
+
+import rclpy
+from rclpy.node import Node
+import numpy as np
+
+from visualization_msgs.msg import Marker, MarkerArray
+from geometry_msgs.msg import Point
+
+from nav_msgs.msg import Odometry
+
+class MarkerVisualizer(Node):
+
+    def __init__(self):
+        super().__init__('marker_visualizer')
+
+        # ---------- publisher ----------
+        self.marker_pub = self.create_publisher(MarkerArray, '/stop_zones_marker_array', 10)
+
+        #-----------subscriber-----------
+        self.oodm_sub = self.create_subscription(Odometry, '/odom', self.odom_callback,10)
+        self.oodm_sub = self.create_subscription(Odometry, '/fusion/odom', self.fusion_odom_callback,10)
+
+
+        self.pub_rate_hz = 1.0
+        self.timer = self.create_timer(1.0 / self.pub_rate_hz, self.timer_callback)
+
+        self.frame_id = 'odom'
+        self.stop_xy = np.array([
+            [ 64.2,  65.2,  19.0,  39.0, 1.0], #shiyakusyo
+            [100.0, 101.0,  25.0,  45.0, 1.0], #dourotan1
+            [177.7, 178.7,  25.0,  45.0, 1.0], #dourotan2
+            [257.5, 277.5, -60.0, -59.0, 1.0], #singoumaeteisisen1
+            [257.5, 277.5, -66.5, -65.5, 1.0], #singoumae1
+            [256.5, 276.5, -86.2, -85.2, 1.0], #singoumaeteisisen2
+            [270.3, 271.3, -99.0, -79.5, 1.0], #singoumae2
+            [405.0, 425.0, -80.2, -79.2, 1.0], #ekimae oudanhodou1 y-1
+            [545.0, 568.0, -85.0, -60.0, 0.0], #ekimae not stop
+            [410.0, 417.0, -71.5, -70.5, 1.0], #ekimae oudanhodou2 y-2
+            [290.6, 291.6, -98.0, -78.0, 1.0], #singoumaeteisisen3
+            [284.3, 285.3, -98.0, -78.0, 1.0], #singoumae3
+            [259.5, 279.5, -83.7, -82.7, 1.0], #singoumaeteisisen4 12
+            [259.5, 279.5, -80.3, -79.3, 1.0], #singoumae4 13
+            [185.0, 186.0,  25.0,  45.0, 1.0], #dourotan3
+            [107.5, 108.5,  25.0,  45.0, 1.0], #dourotan4
+            [ 64.0, 104.0, -30.0, -25.0, 1.0], #GOAL!!!!
+            [ 999,  999,  999,  999, 0.0]
+        ])
+        self.gps_dr_xy_tsukuba = np.array([
+            [ -60.0,  60.0,  35.0,  110.0, 0],
+            [ 476.0, 600.0,  -84.0,  -25.0, 1.0],
+            [ 999,  999,  999,  999, 0.0]
+        ])
+        self.gps_dr_xy_nakaniwa = np.array([
+            [ -41.0,  7.0,  -52.0,  71.0, 0],
+            [ 999,  999,  999,  999, 0.0]
+        ])
+
+        self.ekf_noGPS_area = np.array([
+            [-60.0,  60.0, 200.0, 210.0, 1.0], #shiyakusyo
+            [260.0, 275.0, -66.0, -40.0, 1.0], #nakaniwa
+            [539.0, 600.0, -84.0, -25.0, 1.0] ]) #
+
+        #------stop marker state------ 
+        self.stop_markers = []
+        self.next_stop_id = 1000
+        self.prev_moving = True
+        self.current_pose = None
+        self.current_twist = None
+        self.fusion_current_pose = None
+
+        #------ threshold for stop place-------
+        self.linear_vel_threshold = 0.10  #m/s
+        self.angular_vel_threshold = 0.10 #rad / s
+
+        self.get_logger().info('MarkerVisualizer started')
+
+    def timer_callback(self):
+        now = self.get_clock().now().to_msg()
+        ma_stop = create_zone_markers(self.stop_xy, self.frame_id, now, ns='stop_zone', start_id=0)
+        ma_dr = create_zone_markers(self.ekf_noGPS_area, self.frame_id, now, ns='ma_dr', start_id=1)
+
+        ma = MarkerArray()
+        ma.markers.extend(ma_stop.markers)
+        ma.markers.extend(ma_dr.markers)
+        # 永続的な停止マーカーを追加（最新 timestamp に更新）
+        for m in self.stop_markers:
+            m.header.stamp = now
+            ma.markers.append(m)
+
+        if ma.markers:
+            self.marker_pub.publish(ma)
+    
+    def fusion_odom_callback(self, msg:Odometry):
+        self.fusion_current_pose = msg.pose.pose
+
+    def odom_callback(self, msg: Odometry):
+        #self.fusion_current_pose = msg.pose.pose
+        self.current_twist = msg.twist.twist
+
+        lin = self.current_twist.linear
+        ang = self.current_twist.angular
+        #lin_speed = (lin.x**2 + lin.y**2 + lin.z**2) ** 0.5
+        #ang_speed = (ang.x**2 + ang.y**2 + ang.z**2) ** 0.5
+
+        #is_moving = not (lin_speed < self.linear_vel_threshold and ang_speed < self.angular_vel_threshold)
+        is_moving = not (abs(lin.x) < self.linear_vel_threshold and abs(ang.z) < self.angular_vel_threshold)
+
+        if self.fusion_current_pose is None:
+            return
+        if not is_moving:
+            print(f"lin : {lin.x:.5f}, ang : {ang.z:.5f}")
+
+        # 動いていた -> 停止へ遷移した瞬間を検出してマーカー追加
+        if self.prev_moving and (not is_moving) and (self.fusion_current_pose is not None):
+            # 停止地点マーカーを作成して保持
+            now = self.get_clock().now().to_msg()
+            m = create_stop_marker(self.fusion_current_pose, self.frame_id, now, self.next_stop_id)
+            self.next_stop_id += 1
+            self.stop_markers.append(m)
+            self.get_logger().info(f'Stop detected, marker added id={m.id} x={m.pose.position.x:.2f} y={m.pose.position.y:.2f}')
+
+        self.prev_moving = is_moving
+
+
+def create_zone_markers(range_xy, frame_id, now, ns , start_id = 0) -> MarkerArray:
+    ma = MarkerArray()
+
+    for idx, row in enumerate(range_xy):
+        xmin, xmax, ymin, ymax, flag = row.tolist()
+
+        # sentinel 行 を skip
+        if xmin >= 999 or ymin >= 999:
+            continue
+
+        base_id = start_id + idx * 10
+        cx = (xmin + xmax) / 2.0
+        cy = (ymin + ymax) / 2.0
+
+        # outline 
+        outline = Marker()
+        outline.header.frame_id = frame_id
+        outline.header.stamp = now
+        outline.ns = ns
+        outline.id = base_id + 0
+        outline.type = Marker.LINE_STRIP
+        outline.action = Marker.ADD
+        outline.scale.x = 0.1
+        if flag == 1.0:
+            outline.color.r, outline.color.g, outline.color.b, outline.color.a = 1.0, 0.0, 0.0, 1.0
+        else:
+            outline.color.r, outline.color.g, outline.color.b, outline.color.a = 1.0, 1.0, 0.0, 1.0
+        p1 = Point(x=xmin, y=ymin, z=0.0)
+        p2 = Point(x=xmax, y=ymin, z=0.0)
+        p3 = Point(x=xmax, y=ymax, z=0.0)
+        p4 = Point(x=xmin, y=ymax, z=0.0)
+        outline.points = [p1, p2, p3, p4, p1]
+        ma.markers.append(outline)
+
+        # fill 
+        fill = Marker()
+        fill.header.frame_id = frame_id
+        fill.header.stamp = now
+        fill.ns = ns
+        fill.id = base_id + 1
+        fill.type = Marker.CUBE
+        fill.action = Marker.ADD
+        fill.pose.position.x = float(cx)
+        fill.pose.position.y = float(cy)
+        fill.pose.position.z = 0.1
+        fill.pose.orientation.w = 1.0
+        fill.scale.x = float(max(0.001, (xmax - xmin)))
+        fill.scale.y = float(max(0.001, (ymax - ymin)))
+        fill.scale.z = 0.2
+        if flag == 1.0:
+            fill.color.r, fill.color.g, fill.color.b, fill.color.a = 1.0, 0.0, 0.0, 0.15
+        else:
+            fill.color.r, fill.color.g, fill.color.b, fill.color.a = 0.0, 1.0, 0.0, 0.15
+        #ma.markers.append(fill)
+
+        # 3) label
+        text = Marker()
+        text.header.frame_id = frame_id
+        text.header.stamp = now
+        text.ns = ns
+        text.id = base_id + 2
+        text.type = Marker.TEXT_VIEW_FACING
+        text.action = Marker.ADD
+        text.pose.position.x = float(cx)
+        text.pose.position.y = float(cy)
+        text.pose.position.z = 0.4
+        text.pose.orientation.w = 1.0
+        text.scale.z = 0.4
+        text.text = f'zone_{idx} flag={int(flag)}'
+        text.color.r, text.color.g, text.color.b, text.color.a = 1.0, 1.0, 1.0, 1.0
+        ma.markers.append(text)
+    return ma
+
+def create_stop_marker(pose, frame_id, now, mid) -> Marker:
+    m = Marker()
+    m.header.frame_id = frame_id
+    m.header.stamp = now
+    m.ns = 'stop_points'
+    m.id = mid
+    m.type = Marker.SPHERE
+    m.action = Marker.ADD
+    m.pose = pose
+
+    # 少し浮かせる
+    m.pose.position.z = 0.2
+    m.scale.x = 0.4
+    m.scale.y = 0.4
+    m.scale.z = 0.4
+
+    # 色（半透明赤）
+    m.color.r, m.color.g, m.color.b, m.color.a = 1.0, 0.0, 0.0, 0.8
+    return m
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = MarkerVisualizer()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    node.destroy_node()
+    rclpy.shutdown()
+
diff --git a/try_navigation/try_navigation/path_follower.py b/try_navigation/try_navigation/path_follower.py
index deb73d9..c9e911a 100644
--- a/try_navigation/try_navigation/path_follower.py
+++ b/try_navigation/try_navigation/path_follower.py
@@ -15,9 +15,26 @@
 from geometry_msgs.msg import PoseStamped
 from std_msgs.msg import Int32
 from std_msgs.msg import String
+import tkinter as tk
+from threading import Thread
 
+navigation_status = "Initializing..."
 
+# GUI
+def start_gui():
+    global navigation_status
+    root = tk.Tk()
+    root.title("Navigation Status")
+    label = tk.Label(root, text=navigation_status, font=("Helvetica", 32))
+    label.pack(padx=20, pady=20)
 
+    def update_label():
+        label.config(text=navigation_status)
+        root.after(200, update_label)
+
+    update_label()
+    root.mainloop()
+    
 # C++と同じく、Node型を継承します。
 class PathFollower(Node):
     # コンストラクタです、PcdRotationクラスのインスタンスを作成する際に呼び出されます。
@@ -50,6 +67,7 @@ def __init__(self):
         #self.subscription = self.create_subscription(nav_msgs.Odometry,'/odom_ref_slam', self.get_odom_ref, qos_profile_sub)
         self.subscription = self.create_subscription(nav_msgs.Odometry,'/fusion/odom', self.get_odom_ref, qos_profile_sub) #/fusion/odom
         self.subscription = self.create_subscription(sensor_msgs.PointCloud2, '/pcd_segment_obs', self.obs_steer, qos_profile)
+        self.step_sub = self.create_subscription(sensor_msgs.PointCloud2, '/pcd_segment_low_step', self.low_obs_steer, qos_profile)
         self.goal_sub = self.create_subscription(PoseStamped, '/goal_pose', self.goal_pose_callback, qos_profile)
         self.stop_sub = self.create_subscription(String, '/stop_sign_status', self.stop_sign_callback, 10)
         self.human_sub = self.create_subscription(String, '/human_status', self.human_callback, 10)
@@ -63,6 +81,7 @@ def __init__(self):
         # Publisherを作成
         self.cmd_vel_publisher = self.create_publisher(geometry_msgs.Twist, 'cmd_vel', qos_profile) #set publish pcd topic name
         self.pcd_test_publisher = self.create_publisher(sensor_msgs.PointCloud2, 'pcd_test_global', qos_profile) 
+        self.pcd_jam_publisher = self.create_publisher(sensor_msgs.PointCloud2, 'pcd_jam', qos_profile) 
         #self.marker_pub = self.create_publisher(MarkerArray, 'wall_follow_markers', 10)
 
         #パラメータ init
@@ -135,30 +154,39 @@ def __init__(self):
             #[-69.5,	-62,	-47.5,	-27.5, 1.0], #tyokusen4 ||| [-67.5,	-62,	-47.5,	-27.5, 1.0]before:[-67.0,	-62,	-47.5,	-27.5, 1.0], 
             #[-55,	-35,	41,	46,    1.0], #Goal
             
-            [ 10.0,  11.5, -19.0,  39.0, 1.0], #nakaniwa_test
-            [ 30.0,  31.5, -19.0,  39.0, 1.0], #nakaniwa_test
-            [ 63.0,  64.5,  19.0,  39.0, 1.0], #shiyakusyo
-            [100.5, 102.0,  25.0,  45.0, 1.0], #dourotan1
-            [178.5, 180.0,  25.0,  45.0, 1.0], #dourotan2
-            [257.5, 277.5, -60.0, -58.5, 1.0], #singoumaeteisisen1
-            [257.5, 277.5, -68.0, -66.5, 1.0], #singoumae1
-            [256.5, 276.5, -89.0, -87.5, 1.0], #singoumaeteisisen2
-            [269.5, 270.5, -99.0, -79.5, 1.0], #singoumae2
-            [405.0, 425.0, -82.5, -81.5, 1.0], #ekimae oudanhodou1 y-1
-            [555.0, 568.0, -85.0, -71.0, 0.0], #ekimae not stop
-            [405.0, 425.0, -73.0, -72.0, 1.0], #ekimae oudanhodou2 y-2
-            [289.5, 290.5, -98.0, -78.0, 1.0], #singoumaeteisisen3
-            [284.0, 285.0, -98.0, -78.0, 1.0], #singoumae3
-            [259.5, 279.5, -83.5, -80.5, 1.0], #singoumaeteisisen4 12
-            [259.5, 279.5, -81.0, -79.0, 1.0], #singoumae4 13
-            [184.5, 186.0,  25.0,  45.0, 1.0], #dourotan3
-            [107.0, 108.5,  25.0,  45.0, 1.0], #dourotan4
-            [ 64.0, 104.0, -30.0, -24.0, 1.0], #GOAL!!!!
+            #[ 10.0,  11.5, -19.0,  39.0, 1.0], #nakaniwa_test normal
+            #[ 30.0,  31.5, -19.0,  39.0, 1.0], #nakaniwa_test normal
+            #[ 10.0,  11.5, -19.0,  39.0, 1.0], #nakaniwa_test normal
+            #[ 0.0,  1.5,  30.0,  65.0, 1.0], #nakaniwa_test omawari 
+            #[ 10.0,  11.5, -19.0,  39.0, 1.0], #nakaniwa_test
+            #[ 30.0,  31.5, -19.0,  39.0, 1.0], #nakaniwa_test
+            #[ 0.0,  1.5,  30.0,  65.0, 1.0], #nakaniwa_test
+            #[ 10.0,  11.5, -19.0,  39.0, 1.0], #nakaniwa_test
+            #[ 30.0,  31.5, -19.0,  39.0, 1.0], #nakaniwa_test
+            #[ 0.0,  1.5,  30.0,  65.0, 1.0], #nakaniwa_test
+            [ 64.2,  65.2,  19.0,  39.0, 1.0], #shiyakusyo
+            [100.0, 101.0,  25.0,  45.0, 1.0], #dourotan1
+            [177.7, 178.7,  25.0,  45.0, 1.0], #dourotan2
+            [257.5, 277.5, -60.0, -59.0, 1.0], #singoumaeteisisen1
+            [257.5, 277.5, -66.5, -65.5, 1.0], #singoumae1
+            [256.5, 276.5, -86.2, -85.2, 1.0], #singoumaeteisisen2
+            [270.3, 271.3, -99.0, -79.5, 1.0], #singoumae2
+            [405.0, 425.0, -80.2, -79.2, 1.0], #ekimae oudanhodou1 y-1
+            [545.0, 568.0, -85.0, -60.0, 0.0], #ekimae not stop
+            [410.0, 417.0, -71.5, -70.5, 1.0], #ekimae oudanhodou2 y-2
+            [290.6, 291.6, -98.0, -78.0, 1.0], #singoumaeteisisen3
+            [284.3, 285.3, -98.0, -78.0, 1.0], #singoumae3
+            [259.5, 279.5, -83.7, -82.7, 1.0], #singoumaeteisisen4 12
+            [259.5, 279.5, -80.3, -79.3, 1.0], #singoumae4 13
+            [185.0, 186.0,  25.0,  45.0, 1.0], #dourotan3
+            [107.5, 108.5,  25.0,  45.0, 1.0], #dourotan4
+            [ 64.0, 104.0, -30.0, -25.0, 1.0], #GOAL!!!!
             [  999,   999,   999,   999, 0.0] ]) #
         self.stop_num = 0;
         
         #obs
         self.obs_points = np.array([[],[],[],[]])
+        self.low_step_obs_points = np.array([[],[],[],[]])
         self.rh_obs = 0
         self.lh_obs = 0
         self.ch_obs = 0
@@ -167,6 +195,14 @@ def __init__(self):
         # angle megin for gps heading 
         self.angle_diff = 30
         
+        # jam process
+        self.jam_timer = self.get_clock().now()
+        self.none_jam_timer = self.get_clock().now()
+        self.jam_active = False
+        self.jam_last_print = -1
+        self.c_jam_obs = 0
+        
+        
         
         ################# IGVC SelfDrive Quolification line stop test #20250530# #################
         self.sd_quolification_line_stop = 0 #root flag
@@ -211,12 +247,13 @@ def __init__(self):
 
     # actionリクエストの受信時に呼ばれる(tuika)
     def listener_callback(self, goal_handle):
+        global navigation_status
         self.get_logger().info(f"Received goal with a: {goal_handle.request.a}, b: {goal_handle.request.b}")
         
         # クライアントから送られたaをstop_flagに代入
         self.stop_flag = goal_handle.request.a
         print(f"stop_flag set to: {self.stop_flag}")
-        
+        navigation_status = "GO"
         
         # フィードバックの返信
         for i in range(1):
@@ -310,11 +347,23 @@ def robot_ctrl(self):
         target_rad = math.atan2(relative_point_rot[1], relative_point_rot[0])
         target_theta = (target_rad) * (180 / math.pi)
         
+        ################### Straight Waypoint ############################
+        if 191 <= self.waypoint_number <= 192:# or 0 <= self.waypoint_number <= 5:
+            target_waypoint = path[:,-1]
+            relative_point_x = target_waypoint[0] - position_x
+            relative_point_y = target_waypoint[1] - position_y
+            relative_point = np.vstack((relative_point_x, relative_point_y, target_waypoint[2]))
+            relative_point_rot, t_point_rot_matrix = rotation_xyz(relative_point, theta_x, theta_y, -theta_z)
+            #relative_point_rot, t_point_rot_matrix = rotation_xyz(relative_point, theta_x, theta_y, -reverse_theta_z)
+            target_rad = math.atan2(relative_point_rot[1], relative_point_rot[0])
+            target_theta = (target_rad) * (180 / math.pi)
+            
+        ##################################################################
         
         
         #set speed
         
-        speed_set = 0.55#55 AutoNav 1.10
+        speed_set = 0.75#55 AutoNav 1.10
         speed = speed_set
         
         ################# IGVC SelfDrive Full #20250601# #################
@@ -323,15 +372,17 @@ def robot_ctrl(self):
                 speed = speed_set#0.35
         ##################################################################
         
-        points = self.obs_points 
+        #points = self.obs_points
+        points = np.concatenate([self.obs_points, self.low_step_obs_points], axis=1)
         obs_theta = np.arctan2(points[1,:],points[0,:]) * 180/math.pi #arctan2(y,x)
         obs_dist = np.sqrt(points[0,:]**2 + points[1,:]**2)
         
         r_obs = (-120<obs_theta) * (obs_theta< -60) * (obs_dist<0.9)
         l_obs = (  60<obs_theta) * (obs_theta< 120) * (obs_dist<0.9)
-        c_obs = ( -60<obs_theta) * (obs_theta<  60) * (obs_dist<1.2)
+        c_obs = ( -60<obs_theta) * (obs_theta<  60) * (obs_dist<1.2) #1.2
         c_obs_near = ( -50<obs_theta) * (obs_theta<  50) * (obs_dist<0.5)
         c_obs_back = ( -50<obs_theta) * (obs_theta<  50) * (obs_dist<0.4)
+
         
         if np.any(r_obs) and np.any(l_obs) and ~np.any(c_obs) :
             speed = 0.25
@@ -390,12 +441,57 @@ def robot_ctrl(self):
         rh_obs = self.rh_obs
         lh_obs = self.lh_obs
         ch_obs = self.ch_obs
+        c_jam_obs = self.c_jam_obs
         #c_obs_near = ( -50<obs_theta) * (obs_theta<  50) * (obs_dist<0.5)
         #c_obs_back = ( -50<obs_theta) * (obs_theta<  50) * (obs_dist<0.4)
         cf = 1.15
         
         #self.get_logger().info(f"self.waypoint_number: {self.waypoint_number}")
-        print(theta_z)
+        #print(theta_z)
+        
+
+        #-----------------------jam process-----------------
+        now = self.get_clock().now()
+        if np.any(c_jam_obs):
+            if (73 <= self.waypoint_number <= 74) or (self.waypoint_number == 45) or (136 <= self.waypoint_number <= 137) or (190 <= self.waypoint_number <= 192) or (176 <= self.waypoint_number <= 176):
+                # jam条件 active でないこと none_jam_timer から 5s 経っていること）
+                if not self.jam_active:
+                    none_elapsed = (now - self.none_jam_timer).nanoseconds / 1e9
+                    print(f"none jam timer :{none_elapsed}")
+                    if none_elapsed > 4.0:
+                        self.jam_active = True
+                        self.jam_timer = now
+                        self.jam_last_print = -1
+                        print("Front obstacle detected. Timer started.")
+                        print(f"none jam timer :{none_elapsed}")
+
+                if self.jam_active:
+                    elapsed = (now - self.jam_timer).nanoseconds / 1e9  # 秒（float）
+
+                    #just showing passed time per sec
+                    sec = int(elapsed)
+                    if sec != self.jam_last_print:
+                        self.jam_last_print = sec
+                        print(f"time: {sec} sec")
+
+                    # stop before 20 sec passed
+                    if elapsed <= 50.0:
+                        speed = 0.0
+                        target_rad = 0.0
+                        return
+                    else: #20 sec passed 
+                        self.jam_active = False
+                        self.none_jam_timer = now
+                        print("ch obs persistence exceeded 20 sec -> FORCING EXIT FROM JAM PROCESS!!")
+        else:
+            # ch_obs is none   reset 
+            if self.jam_active:
+                print("Front obstacle cleared. Timer reset.")
+                self.none_jam_timer = now
+            self.jam_active = False
+
+        #---------------------------        
+        
         if ~np.any(ch_obs) :
             if np.any(lh_obs) and np.any(rh_obs) :  #真ん中　
                 target_theta = (target_rad) * (180 / math.pi)
@@ -411,15 +507,21 @@ def robot_ctrl(self):
                 print("--- Center --- After target_theta[deg]:",target_theta)
                             
             #or ((0 <= self.waypoint_number <= 3) and (0 - self.angle_diff <= theta_z <= 0 + self.angle_diff)) \
+            #((25 <= self.waypoint_number <= 28) and (-60 - self.angle_diff <= theta_z <= -60 + self.angle_diff)) \
             elif ~np.any(lh_obs) and np.any(rh_obs):   #右寄り　
                 speed = 0.25
-                if (6 <= self.waypoint_number <= 8) or (self.waypoint_number==15) or (self.waypoint_number==18) \
-                or ((50 <= self.waypoint_number <= 54) and (0 - self.angle_diff <= theta_z <= 0 + self.angle_diff)) \
-                or ((70 <= self.waypoint_number <= 73) and (-150 <= theta_z <= -150 - self.angle_diff) and (150  <= theta_z <= 150 + self.angle_diff)) :
+                if (
+                    (18 <= self.waypoint_number <= 20 and ((-150 - self.angle_diff <= theta_z <= -150) or (150  <= theta_z <= 150 + self.angle_diff)))
+                    or (139 <= self.waypoint_number <= 143 and 0 - self.angle_diff <= theta_z <= 0 + self.angle_diff)
+                    or (146 <= self.waypoint_number <= 151 and 0 - self.angle_diff <= theta_z <= 0 + self.angle_diff)
+                    or (179 <= self.waypoint_number <= 188 and ((-150 - self.angle_diff <= theta_z <= -150) or (150  <= theta_z <= 150 + self.angle_diff)))
+                    or (198 <= self.waypoint_number <= 208 and 90 - self.angle_diff <= theta_z <= 90 + self.angle_diff)
+                    or (33 <= self.waypoint_number <= 35 and -20 - self.angle_diff <= theta_z <= -20 + self.angle_diff)
+                ): #1 3 5 6 7 8
                     target_theta = (target_rad) * (180 / math.pi)
                     print("!!!RH!!!! Befor target_theta[deg]:",target_theta)
                     rh_obs_close = max(rh_obs[1,:]) # y0 rh min
-                    rh_dist = 0.65
+                    rh_dist = 0.65 # 0.65
                     cy = cf
                     cx = rh_obs_close + rh_dist
                     target_rad = math.atan2(cx, cf)
@@ -428,12 +530,15 @@ def robot_ctrl(self):
                     
             elif np.any(lh_obs) and ~np.any(rh_obs):  #左寄り 11 <= self.waypoint_number <= 12 \ 
                 speed = 0.25
-                if ((25 <= self.waypoint_number <= 28) and (-90 - self.angle_diff <= theta_z <= -90 + self.angle_diff)) \
-                or ((56 <= self.waypoint_number <= 60) and (-150 <= theta_z <= -150 - self.angle_diff) and (150  <= theta_z <= 150 + self.angle_diff)) :
+                if (
+                    (65 <= self.waypoint_number <= 72 and -90 - self.angle_diff <= theta_z <= -90 + self.angle_diff)
+                    or (162 <= self.waypoint_number <= 168 and ((-150 - self.angle_diff <= theta_z <= -150) or (150  <= theta_z <= 150 + self.angle_diff)))
+                    or (172 <= self.waypoint_number <= 175 and ((-150 - self.angle_diff <= theta_z <= -150) or (150  <= theta_z <= 150 + self.angle_diff)))
+                    ):    #2 4
                     target_theta = (target_rad) * (180 / math.pi)
                     print("!!!LH!!!! Befor target_theta[deg]:",target_theta)
                     lh_obs_close = min(lh_obs[1,:]) # y0 lh min
-                    lh_dist = 0.65
+                    lh_dist = -0.65 # 0.65
                     cy = cf
                     cx = lh_obs_close + lh_dist
                     target_rad = math.atan2(cx, cf)
@@ -577,6 +682,7 @@ def robot_ctrl(self):
         #################################################################################
         
         
+        
         #elif abs(target_theta)  > 90:
         #    speed = 0.2
         #else:
@@ -641,6 +747,7 @@ def sensim0(self, steering):
         return steering
     
     def get_odom_ref(self, msg):
+        global navigation_status
         self.ref_position_x = msg.pose.pose.position.x
         self.ref_position_y = msg.pose.pose.position.y
         self.ref_position_z = msg.pose.pose.position.z
@@ -679,6 +786,7 @@ def get_odom_ref(self, msg):
             if self.stop_xy[self.stop_num,4] > 0:
                 self.get_logger().info('####### stop flag on %f #######' % (self.stop_num))
                 self.stop_flag = 1;
+                navigation_status = "STOP"
                 #print(self.stop_num)
             else:
                 self.get_logger().info('####### through flag on %f #######' % (self.stop_num))
@@ -723,10 +831,13 @@ def obs_steer(self, msg):
             self.lh_obs = 0
         
         ch_obs = self.pcd_serch(points, 1.0,1.3,-0.4,0.4)
+
+        c_jam_obs = self.pcd_serch(points, 0.0,2.0,-0.6,0.6,0.6,1.0) #1.0,1.3,-0.4,0.4 #1.0,2.0,-0.8,0.8
             
         self.rh_obs = rh_obs
         self.lh_obs = lh_obs
         self.ch_obs = ch_obs
+        self.c_jam_obs = c_jam_obs
         
         #print(lh_obs.shape)
         #print("x0 min",min(lh_obs[0,:]))
@@ -737,8 +848,38 @@ def obs_steer(self, msg):
         #print("y0 rh min",max(rh_obs[1,:]))
         
         #global test obs rviz2 kesu
+        obs_jam_msg = point_cloud_intensity_msg(c_jam_obs.T, t_stamp, 'odom')
         obs_test_msg = point_cloud_intensity_msg(ch_obs.T, t_stamp, 'odom')
         self.pcd_test_publisher.publish(obs_test_msg) 
+        self.pcd_test_publisher.publish(obs_jam_msg)
+    
+    def low_obs_steer(self, msg):
+        
+        #print stamp message
+        #t_stamp = msg.header.stamp
+        #print(f"t_stamp ={t_stamp}")
+        #self.t_stamp = t_stamp
+        
+        #get pcd data
+        points = self.pointcloud2_to_array(msg)
+        #print(f"points ={points.shape}")
+        
+        #map_obs
+        if (
+            (self.waypoint_number == 45)
+            or (self.waypoint_number == 137)
+            or (self.waypoint_number == 177)
+            or (76 <= self.waypoint_number <= 78)
+            or (148 <= self.waypoint_number <= 153)
+            or (162 <= self.waypoint_number <= 167)
+            or (195 <= self.waypoint_number <= 198)
+            or (227 <= self.waypoint_number <= 232)
+        ): # nakaniwa 14~20
+            self.low_step_obs_points = points
+            #self.get_logger().info(f"#######Add Low Step#######: {self.low_step_obs_points}")
+        else:
+            self.low_step_obs_points = np.array([[],[],[],[]])
+            #self.get_logger().info(f"#######No Low Step#######: {self.low_step_obs_points}")
 
     # pcd_serch を z も考慮する形で置換（点群抽出用ユーティリティ）
     def pcd_serch(self, pointcloud, x_min, x_max, y_min, y_max, z_min=None, z_max=None):
@@ -828,6 +969,9 @@ def main(args=None):
     rclpy.init(args=args)
     # クラスのインスタンスを作成
     path_follower = PathFollower()
+    # GUI
+    gui_thread = Thread(target=start_gui, daemon=True)
+    gui_thread.start()
     # spin処理を実行、spinをしていないとROS 2のノードはデータを入出力することが出来ません。
     rclpy.spin(path_follower)
     # 明示的にノードの終了処理を行います。
diff --git a/try_navigation/try_navigation/potential_astar.py b/try_navigation/try_navigation/potential_astar.py
index 36c3374..22fb27a 100644
--- a/try_navigation/try_navigation/potential_astar.py
+++ b/try_navigation/try_navigation/potential_astar.py
@@ -79,6 +79,8 @@ def __init__(self):
         self.right_subscription = self.create_subscription(sensor_msgs.PointCloud2, '/line_buff_right', self.get_right_obs, qos_profile)
         self.left_subscription = self.create_subscription(sensor_msgs.PointCloud2, '/line_buff_left', self.get_left_obs, qos_profile)
         self.dot_subscription = self.create_subscription(sensor_msgs.PointCloud2, '/dotted_line', self.get_dot_obs, qos_profile)
+        self.low_step_subscription = self.create_subscription(sensor_msgs.PointCloud2, '/pcd_segment_low_step', self.get_low_step_obs, qos_profile)
+        self.shibafu_subscription = self.create_subscription(sensor_msgs.PointCloud2, '/pcd_segment_shibafu', self.get_shibafu_obs, qos_profile)
         self.subscription  # 警告を回避するために設置されているだけです。削除しても挙動はかわりません。
         #self.timer = self.create_timer(0.05, self.timer_callback)
         
@@ -102,7 +104,7 @@ def __init__(self):
         self.cg=20 #ポテンシャルの引力パラメータ
         self.lg=20 #ポテンシャルの引力パラメータ
         self.co=11 #ポテンシャルの斥力パラメータ SICKパラ目：co=11;lo=0.55;
-        self.lo=0.5#55 #0.5#0.9#ポテンシャルの斥力パラメータ
+        self.lo=0.45#55 #0.5#0.9#ポテンシャルの斥力パラメータ
         self.est_xy = [0,0]#自己位置仮入力
         self.wp_xy = [10,0]#ウェイポイント仮入力
         self.astar_path = [0,10]#ウェイポイント仮入力
@@ -187,7 +189,8 @@ def __init__(self):
         #tukuba_obs_z =np.linspace(0,0,860);
         #self.tsukuba_obs = np.array([tukuba_obs_x, tukuba_obs_y, tukuba_obs_z]);
         self.tsukuba_obs = np.array([[],[],[]])
-               
+        self.low_step_obs_points = np.array([[],[],[]])
+        self.shibafu_obs_points = np.array([[],[],[]])
         
         ################# IGVC SelfDrive Quolification line stop test #20250530# #################
         self.sd_line_stop_test = 0
@@ -358,6 +361,28 @@ def get_dot_obs(self, msg):
         #print(f"points ={points.shape}")
         
         self.dot_obs_points = np.vstack((points[0,:], points[1,:], points[2,:]))
+        
+    def get_low_step_obs(self, msg):
+        #print stamp message
+        t_stamp = msg.header.stamp
+        #print(f"t_stamp ={t_stamp}")
+        
+        #get pcd data
+        points = self.pointcloud2_to_array(msg)
+        #print(f"points ={points.shape}")
+        
+        self.low_step_obs_points = np.vstack((points[0,:], points[1,:], points[2,:]))
+        
+    def get_shibafu_obs(self, msg):
+        #print stamp message
+        t_stamp = msg.header.stamp
+        #print(f"t_stamp ={t_stamp}")
+        
+        #get pcd data
+        points = self.pointcloud2_to_array(msg)
+        #print(f"points ={points.shape}")
+        
+        self.shibafu_obs_points = np.vstack((points[0,:], points[1,:], points[2,:]))
             
     def potential_astar(self, msg):
         
@@ -368,7 +393,7 @@ def potential_astar(self, msg):
         #get pcd data
         points = self.pointcloud2_to_array(msg)
         #print(f"points ={points.shape}")
-        print(f"self.pot_obs_points.shape = {self.pot_obs_points.shape}")
+        #print(f"self.pot_obs_points.shape = {self.pot_obs_points.shape}")
         
         
         position_x=self.position_x; position_y=self.position_y; 
@@ -426,6 +451,36 @@ def potential_astar(self, msg):
             elif self.obs_info[self.waypoint_number][self.dotline_info] == 1:
                 dot_line_local = localization_xyz(self.dot_obs_points, position_x, position_y, theta_x, theta_y, theta_z)
         
+        #low_step_obs add(local)
+        low_step_local = np.array([[],[],[]])
+        if len(self.low_step_obs_points[0,:])>0:
+            if (
+                (self.waypoint_number == 45)
+                or (self.waypoint_number == 137)
+                or (self.waypoint_number == 177)
+                or (76 <= self.waypoint_number <= 78)
+                or (148 <= self.waypoint_number <= 153)
+                or (162 <= self.waypoint_number <= 167)
+                or (195 <= self.waypoint_number <= 198)
+                or (227 <= self.waypoint_number <= 232)
+            ): # nakaniwa 14~20
+                low_step_local = self.low_step_obs_points
+                print("!!!!!!!!Waypoint Low Step!!!!!!!!")
+        
+        #shibafu_obs add(local)
+        shibafu_local = np.array([[],[],[]])
+        if len(self.shibafu_obs_points[0,:])>0:
+            if (
+                (self.waypoint_number == 144)
+                or (self.waypoint_number == 170)
+                or (50 <= self.waypoint_number <= 72)
+                or (90 <= self.waypoint_number <= 123) 
+                or (200 <= self.waypoint_number <= 225)
+                or (227 <= self.waypoint_number <= 232)
+            ): # no otiba 101~123 nakaniwa 17~26
+                shibafu_local = self.shibafu_obs_points
+                print("!!!!!!!!Waypoint Shibafu!!!!!!!!")
+        
         self_radius = 0.8
         angle_min = -60 + 180
         angle_max = 60 + 180
@@ -447,20 +502,25 @@ def potential_astar(self, msg):
         """    
         
         # make map_obs   x1  x2  y1  y2
-        obs1 = make_obs(-45, 25, 97, 94) # siyakusyoura minami
+        obs1 = make_obs(-45, 25, 95.5, 92.5) # siyakusyoura minami
         obs2 = make_obs(  5, 20, 100, 102) # siyakusyoura kita
         obs3 = make_obs( 28, 20, 108, 102) # siyakusyoura kita2
-        obs4 = make_obs(-73,-75, 105, 29) # siyakusyo nisi
-        obs5 = make_obs( 61,100, 32, 41) # siyakusyo oudanhodou
-        obs6 = make_obs( 61,44, 32, 35) # siyakusyo oudanhodou
-        obs7 = make_obs(276,270, 28,-58) # 7-Eleven mae
-        obs8 = make_obs(-21,-39, 24, 27) # siyakusyo sibahu1
-        obs9 = make_obs(-51,-39, 37, 27) # siyakusyo sibahu2
-        obs10 = make_obs(36.5,36.5, 62, 96.5) # siyakusyo higasi
-        
+        obs4 = make_obs(-74.5,-76.5, 105, 29) # siyakusyo nisi
+        obs5 = make_obs( 61,100, 33.5, 42.5) # siyakusyo oudanhodou
+        obs6 = make_obs( 61,44, 33.5, 36.5) # siyakusyo oudanhodou
+        obs7 = make_obs(272,273,-65,-49.5) # 7-Eleven mae
+        obs8 = make_obs(-21,-39, 25.5, 28.5) # siyakusyo sibahu1
+        obs9 = make_obs(-51,-39, 38.5, 28.5) # siyakusyo sibahu2
+        obs10 = make_obs(38, 38, 62, 96.5) # siyakusyo higasi
+        obs11 = make_obs(465,485,-74.5, -74.5) # eki minami
+        obs12 = make_obs(3,4.8, 19, 23.5) # siyakusyo sibahu
+        obs13 = make_obs(-9,3,20, 19) # siyakusyo sibahu
+        obs14 = make_obs(273.06,269.82, -61.24, -61.25) # singou
+        obs15 = make_obs(270.96,270.52,-57.33, -61.25) # singou
+        obs16 = make_obs(269.96,273.12,-57.33, -57.05) # singou
 
-        #self.tsukuba_obs = np.hstack((obs1, obs4, obs5, obs6, obs7, obs8, obs9, obs10))
-        self.tsukuba_obs = np.hstack((obs7,obs1))
+        self.tsukuba_obs = np.hstack((obs1, obs4, obs5, obs6, obs7, obs8, obs9, obs10, obs11, obs12, obs13, obs14, obs15, obs16))
+        #self.tsukuba_obs = np.hstack((obs7,obs1))
         
         #map_obs add
         if len(self.tsukuba_obs[0,:])>0:
@@ -470,6 +530,7 @@ def potential_astar(self, msg):
             relative_point_rot, t_point_rot_matrix = rotation_xyz(relative_point, self.theta_x, self.theta_y, -self.theta_z)
         else:
             relative_point_rot = np.array([[],[],[]])
+        
         ###################################
                 
         #obs round&duplicated  :grid_size before:28239 after100:24592 after50:8894 after10:3879
@@ -501,6 +562,14 @@ def potential_astar(self, msg):
         
         if dot_line_local.shape[1] > 0:
             obs_points = np.insert(obs_points, len(obs_points[0,:]), dot_line_local.T, axis=1)
+            
+        if low_step_local.shape[1] > 0:
+            obs_points = np.insert(obs_points, len(obs_points[0,:]), low_step_local.T, axis=1)
+            print("!!!!!!!!Add Low Step!!!!!!!!")
+            
+        if shibafu_local.shape[1] > 0:
+            obs_points = np.insert(obs_points, len(obs_points[0,:]), shibafu_local.T, axis=1)
+            print("!!!!!!!!Add Shibafu!!!!!!!!")
         
         obs_points = np.insert(obs_points, len(obs_points[0,:]), relative_point_rot.T, axis=1)
         obs_points = np.insert(obs_points, len(obs_points[0,:]), self_radius_points.T, axis=1)
@@ -509,24 +578,24 @@ def potential_astar(self, msg):
         obs_xy_local = points_round[:,~pd.DataFrame({"x":points_round[0,:], "y":points_round[1,:]}).duplicated()]
         obs_xy = np.vstack((obs_xy_local[0,:], obs_xy_local[1,:]))
         
-        print(f"obs_points ={obs_points.shape}")
+        #print(f"obs_points ={obs_points.shape}")
         reflect_set = obs_points[2,~pd.DataFrame({"x":points_round[0,:], "y":points_round[1,:]}).duplicated()]
         #obs global
         obs_xy_rot, obs_rot_matrix = rotation_xyz(obs_xy_local, self.theta_x, self.theta_y, self.theta_z)
         obs_x_grobal = obs_xy_rot[0,:] + self.position_x
         obs_y_grobal = obs_xy_rot[1,:] + self.position_y
         obs_global = np.vstack((obs_x_grobal, obs_y_grobal, obs_xy_local[2,:], reflect_set) , dtype=np.float32)
-        print(f"obs_xy ={obs_xy.shape}")
-        print(f"obs_global ={obs_global.shape}")
-        print(f"obs_global ={obs_global.dtype}")
+        #print(f"obs_xy ={obs_xy.shape}")
+        #print(f"obs_global ={obs_global.shape}")
+        #print(f"obs_global ={obs_global.dtype}")
         #set self position
         #self.est_xy = [self.position_x, self.position_y]
-        print(f"self.est_xy ={self.est_xy}")
+        #print(f"self.est_xy ={self.est_xy}")
         
         astar_path = self.path_plan(obs_xy)
-        print(f"astar_path ={astar_path.shape}")
+        #print(f"astar_path ={astar_path.shape}")
         astar_path = np.vstack((astar_path, np.zeros([1,len(astar_path[0,:])]) ))
-        print(f"astar_path ={astar_path.shape}")
+        #print(f"astar_path ={astar_path.shape}")
         astar_path_rot, astar_path_rot_matrix = rotation_xyz(astar_path, self.theta_x, self.theta_y, self.theta_z)
         astar_path_x_grobal = astar_path_rot[0,:] + self.position_x
         astar_path_y_grobal = astar_path_rot[1,:] + self.position_y
@@ -587,7 +656,7 @@ def path_plan(self, obs_xy):
             
             astar_count = astar_count + 1 #ループカウント
             if astar_xy[astar_yn, astar_xn] <0.2:
-                self.get_logger().info(f"Goal: astar_path_x, astar_path_y ={astar_path_x, astar_path_y}")
+                #self.get_logger().info(f"Goal: astar_path_x, astar_path_y ={astar_path_x, astar_path_y}")
                 break
             if (astar_count > 100) and (astar_2nd==0):
                 astar_x =  np.arange(-9.9, 9.9,  0.3) +self.est_xy[0]	#%%Astarのxを定義
@@ -605,7 +674,7 @@ def path_plan(self, obs_xy):
                 co=self.co2nd
                 astar_2nd = 1
             if astar_count >200:
-                self.get_logger().info("Count Break")
+                #self.get_logger().info("Count Break")
                 break
                 
         #■  process : A-star Return
@@ -634,11 +703,11 @@ def path_plan(self, obs_xy):
         astar_judge_y = astar_y[astar_path_y_rev2] -  obs_xy[1].reshape(len(obs_xy[1]),1) #y-yo 斥力計算　探索ポイントｘ近場にある障害物を全て行列使って計算
         astar_judge_x2 = ( astar_judge_x * np.ones([len(obs_xy[0]),len(astar_x[astar_path_x_rev2])]) ) ** 2 #(x-xo)^2
         astar_judge_y2 = ( astar_judge_y * np.ones([len(obs_xy[1]),len(astar_y[astar_path_y_rev2])]) ) ** 2 #(y-yo)^2
-        self.get_logger().info(f"astar_judge_x2 ={len(astar_judge_x2)}")
-        self.get_logger().info(f"astar_path_x_rev2 ={len(astar_path_x_rev2)}")
+        #self.get_logger().info(f"astar_judge_x2 ={len(astar_judge_x2)}")
+        #self.get_logger().info(f"astar_path_x_rev2 ={len(astar_path_x_rev2)}")
         if len(astar_judge_x2) > 0:
             astar_judge_obs_ind = np.minimum.reduce( np.sqrt(astar_judge_x2 + astar_judge_y2) ) <1.8#1.6
-            self.get_logger().info(f"astar_judge_obs_ind ={astar_judge_obs_ind}")
+            #self.get_logger().info(f"astar_judge_obs_ind ={astar_judge_obs_ind}")
             astar_path_point_x = np.append(np.append(self.est_xy[0],astar_x[astar_path_x_rev2[astar_judge_obs_ind]]),self.wp_xy[0])
             astar_path_point_y = np.append(np.append(self.est_xy[1],astar_y[astar_path_y_rev2[astar_judge_obs_ind]]),self.wp_xy[1])
         else:
@@ -761,11 +830,30 @@ def path_msg(waypoints, stamp, parent_frame):
         wp_msg.poses.append(waypoint)
     return wp_msg
 
-def make_obs(x1, x2, y1, y2, n=860, z=0):
+def make_obs(x1, x2, y1, y2, n=860, z=0, thickness=1.5, lines=15): # 1.5m 0.1mkannkaku
     x = np.linspace(x1, x2, n)
     y = np.linspace(y1, y2, n)
     z = np.full(n, z)
-    return np.array([x, y, z])
+
+    dx = x2 - x1
+    dy = y2 - y1
+    length = np.hypot(dx, dy)
+    nx = -dy / length   # housen x
+    ny = dx / length    # housen y
+
+    # thickness = hutosa , lines = mitudo
+    offsets = np.linspace(-thickness/2, thickness/2, lines)
+
+    thick_points = []
+    for off in offsets:
+        xx = x + nx * off
+        yy = y + ny * off
+        zz = z.copy()
+        thick_points.append([xx, yy, zz])
+
+    thick_points = np.hstack(thick_points)
+    return thick_points
+    #return np.array([x, y, z])
 
 # mainという名前の関数です。C++のmain関数とは異なり、これは処理の開始地点ではありません。
 def main(args=None):