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/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 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/orange_bringup/launch/orange_robot.launch.xml b/orange_bringup/launch/orange_robot.launch.xml index 4824862..841e94d 100644 --- a/orange_bringup/launch/orange_robot.launch.xml +++ b/orange_bringup/launch/orange_robot.launch.xml @@ -58,13 +58,42 @@ - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -81,14 +110,18 @@ --> - - + + + + - + + + - + 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_description/xacro/orange_robot.xacro b/orange_description/xacro/orange_robot.xacro index 9e7afe4..ba8393f 100644 --- a/orange_description/xacro/orange_robot.xacro +++ b/orange_description/xacro/orange_robot.xacro @@ -7,17 +7,24 @@ + - + + + + + + + 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..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,24 +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.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 @@ -46,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) @@ -62,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 @@ -73,7 +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.theta = self.theta + 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) @@ -96,7 +108,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) @@ -124,13 +136,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 += 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] + #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.tsukuba_theta}") self.send_request() self.is_acquiring = False @@ -250,7 +268,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 @@ -259,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 @@ -358,6 +377,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}") @@ -366,13 +387,13 @@ 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 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 @@ -409,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: @@ -419,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/orange_gnss/orange_gnss/gnss_odom_publisher_ttyUSB.py b/orange_gnss/orange_gnss/gnss_odom_publisher_ttyUSB.py index 7e2973d..9993e41 100644 --- a/orange_gnss/orange_gnss/gnss_odom_publisher_ttyUSB.py +++ b/orange_gnss/orange_gnss/gnss_odom_publisher_ttyUSB.py @@ -19,15 +19,21 @@ 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.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= 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 @@ -42,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("-------------------------") @@ -62,7 +68,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,10 +114,13 @@ 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 = 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.send_request() self.is_acquiring = False @@ -233,12 +246,13 @@ 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----------!!!") - self.first_heading = robotheading + #self.first_heading = robotheading + self.first_heading = self.tsukuba_theta self.count = 1 relative_heading = robotheading - self.first_heading @@ -347,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 new file mode 100644 index 0000000..bab7625 --- /dev/null +++ b/orange_gnss/orange_gnss/odom_combination.py @@ -0,0 +1,128 @@ +#!/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/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) + + self.position_x = 0.0 + self.position_y = 0.0 + self.theta_z = 0.0 + self.theta = 0.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 + 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 # 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 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 + pos_theta = self.theta_z + self.init_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 + + # 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) + + # 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", ], }, ) 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") diff --git a/pcd_convert/pcd_convert/pcd_height_segmentation.py b/pcd_convert/pcd_convert/pcd_height_segmentation.py index 28d2610..1fe5e3d 100644 --- a/pcd_convert/pcd_convert/pcd_height_segmentation.py +++ b/pcd_convert/pcd_convert/pcd_height_segmentation.py @@ -38,10 +38,11 @@ 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 - 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,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 = 550/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 = 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] + #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 @@ -98,9 +106,15 @@ 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) + #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 bbc1679..0a42f81 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_X = 180.5 #[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])}") @@ -134,7 +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.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 new file mode 100644 index 0000000..071d957 --- /dev/null +++ b/pcd_convert/pcd_convert/pcd_rotation_lidar2.py @@ -0,0 +1,233 @@ +#!/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 = 750/1000; #self.MID360_HIGHT =1.7483812830163088 + + #上下反転 LiDAR init + 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 = 1 + 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}") + #print(f"self.THETA_INIT_Y ={self.THETA_INIT_Y}") + + + #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..3d9e743 100644 --- a/pcd_convert/setup.py +++ b/pcd_convert/setup.py @@ -45,7 +45,9 @@ 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_reflect_segmentation = pcd_convert.pcd_reflect_segmentation:main', + 'pcd_rotation_lidar2 = pcd_convert.pcd_rotation_lidar2:main' ], }, ) 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: 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: 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/launch/try_navigation.launch.py b/try_navigation/launch/try_navigation.launch.py index e4591ed..ce8cbfb 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,32 @@ 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/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', + default_value= '0', + description='waypoint start index' + ) + return LaunchDescription([ + declare_odom_arg, declare_waypoint_arg, declare_waypoint_start_arg, + #rviz2 Node(package='rviz2', executable='rviz2', @@ -38,6 +66,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', @@ -54,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', @@ -68,6 +113,9 @@ def generate_launch_description(): executable='gps_waypoint', name='gps_waypoint', output='screen', + parameters=[{'odom': odom}, + {'waypoint_path': waypoint_path}, + {'waypoint_start_index': waypoint_start_index}], arguments=[], ), # $ ros2 run navigation_control gps_waypoint @@ -85,15 +133,17 @@ def generate_launch_description(): executable='potential_astar', name='potential_astar_node', output='screen', + parameters=[{'odom': odom}], arguments=[], ), #robot ctrl - Node(package='try_navigation', - executable='path_follower', - name='path_follower_node', - output='screen', - arguments=[], - ), + #Node(package='try_navigation', + # executable='path_follower', + # name='path_follower_node', + # output='screen', + # parameters=[{'odom': odom}], + # arguments=[], + #), #navigation start Node(package='navigation_control', @@ -103,12 +153,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/setup.py b/try_navigation/setup.py index 4a20ab2..0226f3c 100644 --- a/try_navigation/setup.py +++ b/try_navigation/setup.py @@ -58,7 +58,10 @@ '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', + '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 c7b2699..8cd10c2 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.00 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', self.sensor_a_callback, 10) + 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( @@ -88,6 +89,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 +134,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( @@ -163,13 +175,20 @@ def sensor_b_callback(self, data): def determination_of_R(self): if self.GpsXY is not None: - if (-30= 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] @@ -434,7 +482,8 @@ def publish_fused_value(self): self.diff_yaw_buff = [] self.get_logger().info(f"!!!!!!!!!!!!!!!!!!!!!! offsetyaw: {self.offsetyaw}!!!!!!!!!!!!!!!!!!!!!!!!!!") - if self.GpsXY is not None and self.gps_rr_flag:#self.Number_of_satellites > #22 : + #ここからカルマンフィルタ　:GPS情報が受信できている（Noneではない）＆受信精度が良い区間（sefl.gps_rr_flag=1）時に処理を行う + 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( @@ -450,7 +499,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) @@ -464,7 +513,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) ########################################### @@ -474,6 +525,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] @@ -488,13 +540,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) @@ -507,11 +561,14 @@ def publish_fused_value(self): self.robot_yaw -= 2 * np.pi ########## Change the written line ####### - kalf_speed = self.Speed * self.kalf_speed_param + #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) ########################################## + #カルマンフィルタ結果(fused_value)のｘｙ座標反映。カルマンフィルタで角度の計算は行っていないので、角度オフセット補正した値のまま。 robot_orientation = self.yaw_to_orientation(self.robot_yaw) self.robot_orientationz = robot_orientation[0] self.robot_orientationw = robot_orientation[1] @@ -523,6 +580,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 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 f07f857..c9e911a 100644 --- a/try_navigation/try_navigation/path_follower.py +++ b/try_navigation/try_navigation/path_follower.py @@ -15,7 +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クラスのインスタンスを作成する際に呼び出されます。 @@ -42,16 +61,17 @@ 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.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) - self.waypoint_number_subscription = self.create_subscription(Int32,'/waypoint_number', self.get_waypoint_number, qos_profile_sub) + self.waypoint_number_sub = self.create_subscription(Int32,'/waypoint_number', self.get_waypoint_number, qos_profile_sub) self.subscription # 警告を回避するために設置されているだけです。削除しても挙動はかわりません。 # タイマーを0.05秒（50ミリ秒）ごとに呼び出す @@ -60,7 +80,10 @@ 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 self.path_plan = np.array([[0],[0],[0]]) @@ -131,14 +154,54 @@ 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 - - [ 999, 999, 999, 999, 0.0] ]) # + #[ 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 + self.t_stamp = 0 + + # 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# ################# @@ -165,12 +228,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] @@ -181,15 +244,16 @@ def __init__(self): self.previous_status = None self.human_status = None ################################################################## - + # 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): @@ -283,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# ################# @@ -296,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 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) + 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) + + #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 ( + (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 # 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(lh_obs) and ~np.any(rh_obs): #左寄り 11 <= self.waypoint_number <= 12 \ + speed = 0.25 + 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 # 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 + lim_steer = 20 #lim_steer = 30 24/11/29 ok #if abs(target_theta) < 10 and 0.0 < speed and speed < 0.5: 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 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) : speed = 0.10 - + ################# IGVC SelfDrive Full #20250601# ################# if self.sd_full_flag == 1: @@ -493,12 +682,13 @@ def robot_ctrl(self): ################################################################################# + #elif abs(target_theta) > 90: # speed = 0.2 #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 @@ -557,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 @@ -572,15 +763,34 @@ 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 >= 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 >= 81: # after singou + if self.stop_num <= 6: + self.stop_num = 7 + elif self.waypoint_number >= 57: # after dourotan2 + 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; + navigation_status = "STOP" + #print(self.stop_num) 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 @@ -600,6 +810,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) @@ -608,21 +819,79 @@ 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,-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, -0.2,1,-0.7,0) + 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: 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,:])) + #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_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}") - 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 rotation_xyz(pointcloud, theta_x, theta_y, theta_z): theta_x = math.radians(theta_x) @@ -658,6 +927,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): @@ -665,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/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() diff --git a/try_navigation/try_navigation/potential_astar.py b/try_navigation/try_navigation/potential_astar.py index d55fbfb..22fb27a 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) + 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) @@ -73,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) @@ -96,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]#ウェイポイント仮入力 @@ -176,6 +184,13 @@ 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]); + 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 @@ -347,6 +362,28 @@ def get_dot_obs(self, msg): 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): #print stamp message @@ -356,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; @@ -414,6 +451,45 @@ 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 + 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: @@ -424,6 +500,37 @@ def potential_astar(self, msg): else: relative_point_rot = np.array([[],[],[]]) """ + + # make map_obs x1 x2 y1 y2 + 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(-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, obs11, obs12, obs13, obs14, obs15, obs16)) + #self.tsukuba_obs = np.hstack((obs7,obs1)) + + #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 @@ -455,30 +562,40 @@ 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,:]), 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()] 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 @@ -539,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を定義 @@ -557,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 @@ -586,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: @@ -713,6 +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, 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) + + 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):