From 51ea5bd52ede9d736712473dc770e8e5a2f98ccf Mon Sep 17 00:00:00 2001 From: Orange <2314753575@qq.com> Date: Sat, 13 Jun 2026 17:03:06 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E5=A4=8D=E4=BA=86=E9=9B=B7=E8=BE=BE?= =?UTF-8?q?=E6=89=AB=E6=8F=8F=E8=8C=83=E5=9B=B4=E7=9A=84bug=EF=BC=8C?= =?UTF-8?q?=E4=BF=AE=E5=A4=8D=E4=BA=86=E9=87=8C=E7=A8=8B=E8=AE=A1=E6=95=B0?= =?UTF-8?q?=E6=8D=AE=E7=9A=84=E5=8F=91=E5=B8=83=E8=80=85=E5=86=B2=E7=AA=81?= =?UTF-8?q?=E7=9A=84=E9=97=AE=E9=A2=98=E3=80=82=E7=9B=AE=E5=89=8D=E5=AD=98?= =?UTF-8?q?=E5=9C=A8=E9=97=AE=E9=A2=98=EF=BC=9Aodom=E5=9D=90=E6=A0=87?= =?UTF-8?q?=E7=B3=BB=E5=9C=A8=E8=BF=90=E5=8A=A8=E8=BF=87=E7=A8=8B=E4=B8=AD?= =?UTF-8?q?=E8=BF=98=E6=98=AF=E4=BC=9A=E9=A3=98?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .vscode/browse.vc.db-shm | Bin 32768 -> 32768 bytes .../src/lslidar_driver/src/lslidar_driver.cc | 6 +- .../lslidar_driver/src/lslidar_driver.cc.bak | 1423 +++++++++++++++++ .../gc_nav2_with_slam_online_real.launch.py | 13 +- src/origincar_base/config/ekf.yaml | 2 +- .../include/origincar_base/origincar_base.h | 1 + .../launch/origincar_bringup.launch.py | 18 +- .../launch/robot_mode_description.launch.py | 39 +- .../scripts/cmd_vel_to_ackermann_drive.py | 0 src/origincar_base/src/origincar_base.cpp | 12 +- src/origincar_description/urdf/origincar.urdf | 30 - 调试记录.log | 152 +- 12 files changed, 1624 insertions(+), 72 deletions(-) create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/lslidar_driver.cc.bak mode change 100644 => 100755 src/origincar_base/scripts/cmd_vel_to_ackermann_drive.py diff --git a/.vscode/browse.vc.db-shm b/.vscode/browse.vc.db-shm index fe9ac2845eca6fe6da8a63cd096d9cf9e24ece10..2d36fffbf5c78e009a7448b50193df68fe1f4655 100644 GIT binary patch delta 224 zcmZo@U}|V!N|AV^%K!qh-i!>43=9e&_Sf@_3#7cT?>JDDG)CQVk0&N1`{9#Sq)aVvC-X?nUR5e zW1~M4Bg@3b)yzx`JQEw|GjeQf{LRY9xv_CA8zb|^#&S+Z){Twdxf$8O;_Mq6lX)4r ICN{nV0ARyBp#T5? delta 80 zcmZo@U}|V!;+1%$%K!t66Awy@TChv7nNGgVi7p`mlYpuI4+Il)WHw&Vaou>q#|i+d CnG}Tp diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/lslidar_driver.cc b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/lslidar_driver.cc index c22c0f7..a141253 100644 --- a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/lslidar_driver.cc +++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/lslidar_driver.cc @@ -1134,7 +1134,7 @@ namespace lslidar_driver { auto scan = sensor_msgs::msg::LaserScan::UniquePtr(new sensor_msgs::msg::LaserScan()); //int scan_num = ceil((angle_able_max - angle_able_min) / 360 * count_num) + 1; - int scan_num = fixed_array_length;//cyy_addcyy_add + int scan_num = ceil((angle_able_max - angle_able_min) / 360 * count_num) + 1; std::vector points; rclcpp::Time start_time; @@ -1160,7 +1160,7 @@ namespace lslidar_driver scan->angle_min = 2 * M_PI * angle_able_min / 360; scan->angle_max = 2 * M_PI * angle_able_max / 360; } - scan->angle_increment = 2 * M_PI / (double)(fixed_array_length - 1); + scan->angle_increment = 2 * M_PI / (double)(scan_num - 1); scan->range_min = min_range; scan->range_max = max_range; @@ -1169,7 +1169,7 @@ namespace lslidar_driver scan->intensities.reserve(scan_num); scan->intensities.assign(scan_num, std::numeric_limits::infinity()); scan->scan_time = 0.1; - scan->time_increment = 0.1 / (double)(fixed_array_length - 1); + scan->time_increment = 0.1 / (double)(scan_num - 1); int start_num = floor(angle_able_min * count_num / 360); int end_num = floor(angle_able_max * count_num / 360); diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/lslidar_driver.cc.bak b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/lslidar_driver.cc.bak new file mode 100644 index 0000000..c22c0f7 --- /dev/null +++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/lslidar_driver.cc.bak @@ -0,0 +1,1423 @@ +/* + * This file is part of lslidar driver. + * + * The driver is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * The driver is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with the driver. If not, see . + */ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "lslidar_driver/lslidar_driver.h" +#include + +int truncated_mode_=0; //多角度屏蔽开关：默认为0，如果需要屏蔽多个角度，则truncated_mode_赋值为1。 + +int scan_crop_min[]={0,180}; //雷达屏蔽角度，这里屏蔽角度为135°到225°， + //如果要多角度屏蔽，如10~30，50~60，改为： + //scan_angle_min[]={10，50};scan_angle_max[]={30，60}; +int scan_crop_max[]={90,270}; //修改后编译即可 + + +namespace lslidar_driver +{ + + static void my_hander(int sig) + { + printf("sig: %d", sig); + abort(); + } + LslidarDriver::LslidarDriver() : LslidarDriver(rclcpp::NodeOptions()) {} + LslidarDriver::LslidarDriver(const rclcpp::NodeOptions &options) : Node("lslidar_driver_node", options), diagnostics(this) + { + signal(SIGINT, my_hander); + + if (!this->initialize()) + RCLCPP_ERROR(this->get_logger(), "Could not initialize the driver..."); + else + RCLCPP_INFO(this->get_logger(), "Successfully initialize driver..."); + } + + LslidarDriver::~LslidarDriver() + { + return; + } + + bool LslidarDriver::loadParameters() + { + pubscan_thread_ = new boost::thread(boost::bind(&LslidarDriver::pubScanThread, this)); + interface_selection = std::string("net"); + frame_id = std::string("laser_link"); + scan_topic = std::string("/scan"); + lidar_name = std::string("M10"); + pointcloud_topic = std::string("/lslidar_point_cloud"); + is_start = true; + min_range = 0.3; + max_range = 100.0; + use_gps_ts = true; + compensation = true; + pubScan = true; + pubPointCloud2 = true; + angle_disable_min = 0.0; + angle_disable_max = 0.0; + // 添加恒定输出长度参数 + fixed_array_length = 450; //cyy_addcyy_add + this->declare_parameter("fixed_array_length", 450);//cyy_addcyy_add + + + this->declare_parameter("lidar_name", "M10"); + this->declare_parameter("frame_id", "laser_link"); + this->declare_parameter("scan_topic", "/scan"); + this->declare_parameter("pointcloud_topic", "/lslidar_point_cloud"); + this->declare_parameter("min_range", 0.3); + this->declare_parameter("max_range", 100.0); + this->declare_parameter("use_gps_ts", false); + this->declare_parameter("high_reflection", false); + this->declare_parameter("compensation", false); + this->declare_parameter("pubScan", true); + this->declare_parameter("pubPointCloud2", false); + this->declare_parameter("angle_disable_min", 0.0); + this->declare_parameter("angle_disable_max", 0.0); + this->declare_parameter("interface_selection", "net"); + + this->get_parameter("fixed_array_length", fixed_array_length);//cyy_addcyy_add + this->get_parameter("lidar_name", lidar_name); + this->get_parameter("frame_id", frame_id); + this->get_parameter("high_reflection", high_reflection); + this->get_parameter("scan_topic", scan_topic); + this->get_parameter("min_range", min_range); + this->get_parameter("max_range", max_range); + this->get_parameter("use_gps_ts", use_gps_ts); + this->get_parameter("compensation", compensation); + this->get_parameter("pointcloud_topic", pointcloud_topic); + this->get_parameter("pubScan", pubScan); + this->get_parameter("pubPointCloud2", pubPointCloud2); + this->get_parameter("angle_disable_min", angle_disable_min); + this->get_parameter("angle_disable_max", angle_disable_max); + this->get_parameter("interface_selection", interface_selection); + while (angle_disable_min < 0) + angle_disable_min += 360; + while (angle_disable_max < 0) + angle_disable_max += 360; + while (angle_disable_min > 360) + angle_disable_min -= 360; + while (angle_disable_max > 360) + angle_disable_max -= 360; + if (angle_disable_max == angle_disable_min) + { + angle_able_min = 0; + angle_able_max = 360; + } + else + { + if (angle_disable_min < angle_disable_max && angle_disable_min != 0.0) + { + angle_able_min = angle_disable_max; + angle_able_max = angle_disable_min + 360; + } + if (angle_disable_min < angle_disable_max && angle_disable_min == 0.0) + { + angle_able_min = angle_disable_max; + angle_able_max = 360; + } + if (angle_disable_min > angle_disable_max) + { + angle_able_min = angle_disable_max; + angle_able_max = angle_disable_min; + } + } + count_num = 0; + + scan_points_.resize(6000); + + if (lidar_name == "M10") + { + use_gps_ts = false; + PACKET_SIZE = 92; + package_points = 42; + data_bits_start = 6; + degree_bits_start = 2; + rpm_bits_start = 4; + baud_rate_ = 460800; + points_size_ = 1008; + } + else if (lidar_name == "M10_P") + { + PACKET_SIZE = 160; + package_points = 70; + data_bits_start = 8; + degree_bits_start = 4; + rpm_bits_start = 6; + baud_rate_ = 500000; + points_size_ = 2000; + } + else if (lidar_name == "M10_PLUS") + { + PACKET_SIZE = 104; + package_points = 41; + data_bits_start = 8; + degree_bits_start = 4; + rpm_bits_start = 6; + points_size_ = 5000; + baud_rate_ = 921600; + } + else if (lidar_name == "M10_GPS") + { + PACKET_SIZE = 102; + package_points = 42; + data_bits_start = 6; + degree_bits_start = 2; + rpm_bits_start = 4; + baud_rate_ = 460800; + points_size_ = 1008; + } + else if (lidar_name == "N10") + { + PACKET_SIZE = 58; + package_points = 16; + data_bits_start = 7; + degree_bits_start = 5; + end_degree_bits_start = 55; + baud_rate_ = 230400; + points_size_ = 2000; + use_gps_ts = false; + compensation = false; + } + else if (lidar_name == "M10_DOUBLE") + { + PACKET_SIZE = 300; + package_points = 70; + data_bits_start = 8; + degree_bits_start = 4; + rpm_bits_start = 6; + points_size_ = 3000; + baud_rate_ = 921600; + } + else if (lidar_name == "N10_P") + { + PACKET_SIZE = 108; + package_points = 16; + data_bits_start = 7; + degree_bits_start = 5; + end_degree_bits_start = 105; + baud_rate_ = 460800; + points_size_ = 2000; + use_gps_ts = false; + compensation = false; + } + else if (lidar_name == "L10") + { + PACKET_SIZE = 58; + package_points = 16; + data_bits_start = 7; + degree_bits_start = 5; + end_degree_bits_start = 55; + baud_rate_ = 230400; + points_size_ = 2000; + use_gps_ts = false; + compensation = false; + } + RCLCPP_INFO_STREAM(this->get_logger(), "Lidar is " << lidar_name.c_str()); + + if (pubScan) + scan_pub = this->create_publisher(scan_topic, 10); + if (pubPointCloud2) + point_cloud_pub = this->create_publisher(pointcloud_topic, 10); + difop_switch = this->create_subscription("lslidar_order", 1, std::bind(&LslidarDriver::lidar_order, this, std::placeholders::_1)); // 转速输入 + return true; + } + + void LslidarDriver::lidar_difop() + { + if (lidar_name == "L10" || lidar_name == "N10" || lidar_name == "N10_P") + return; + if (interface_selection == "net") + msop_input_->UDP_difop(); + else + { + for (int k = 0; k < 10; k++) + { + unsigned char data[188] = {0x00}; + data[0] = 0xA5; + data[1] = 0x5A; + data[2] = 0x55; + data[184] = 0x08; + data[185] = 0x01; + data[186] = 0xFA; + data[187] = 0xFB; + int rtn = serial_->send((const char *)data, 188); + if (rtn < 0) + printf("start scan error !\n"); + else + return; + } + } + return; + } + + void LslidarDriver::lidar_order(const std_msgs::msg::Int8::SharedPtr msg) + { + if (lidar_name == "L10") + return; + int i = msg->data; + if (i == 0) + is_start = false; + else + is_start = true; + if (interface_selection == "net") + msop_input_->UDP_order(*msg); + else + { + int i = msg->data; + for (int k = 0; k < 10; k++) + { + int rtn; + unsigned char data[188] = {0x00}; + data[0] = 0xA5; + data[1] = 0x5A; + data[2] = 0x55; + data[186] = 0xFA; + data[187] = 0xFB; + + if (lidar_name == "M10" || lidar_name == "M10_GPS" || lidar_name == "M10_P" || lidar_name == "M10_DOUBLE") + { + if (i <= 1) + { // 雷达启停 + data[184] = 0x01; + data[185] = char(i); + } + else if (i == 2) + { // 雷达点云不滤波 + data[181] = 0x0A; + data[184] = 0x06; + if (is_start) + data[185] = 0x01; + } + else if (i == 3) + { // 雷达点云正常滤波 + data[181] = 0x0B; + data[184] = 0x06; + if (is_start) + data[185] = 0x01; + } + else if (i == 4) + { // 雷达近距离滤波 + data[181] = 0x0C; + data[184] = 0x06; + if (is_start) + data[185] = 0x01; + } + else if (i == 100) + { // 接收设备包 + data[184] = 0x08; + data[185] = 0x01; + } + else + return; + } + else if (lidar_name == "M10_PLUS") + { + data[184] = 0x0A; + data[185] = 0x01; + if (i == 5) + { + data[141] = 0x01; + data[142] = 0x2c; + } + else if (i == 6) + { + data[141] = 0x01; + data[142] = 0x68; + } + else if (i == 8) + { + data[141] = 0x01; + data[142] = 0xe0; + } + else if (i == 10) + { + data[141] = 0x02; + data[142] = 0x58; + } + else if (i == 12) + { + data[141] = 0x02; + data[142] = 0xd0; + } + else if (i == 15) + { + data[141] = 0x03; + data[142] = 0x84; + } + else if (i == 20) + { + data[141] = 0x04; + data[142] = 0xb0; + } + else if (i <= 1) + { + data[184] = 0x01; + data[185] = char(i); + } + else if (i == 100) // 接收设备包 + { + data[184] = 0x08; + data[185] = 0x01; + } + else + return; + } + else if (lidar_name == "N10" || lidar_name == "N10_P") + { + if (i <= 1) + { + data[185] = char(i); + data[184] = 0x01; + } + else if (i >= 6 && i <= 12) + { + data[172] = char(i); + data[184] = 0x0a; + data[185] = 0X01; + } + else + return; + } + rtn = serial_->send((const char *)data, 188); + if (rtn < 0) + printf("start scan error !\n"); + else + { + if (i == 1) + usleep(1000000); // 1.0s + if (i == 0) + is_start = false; + if (i == 1) + is_start = true; + return; + } + } + return; + } + } + + void LslidarDriver::open_serial() + { + diagnostics.setHardwareID("Lslidar"); + int code = 0; + serial_port_ = std::string("/dev/ttyUSB0"); + this->declare_parameter("serial_port_", "/dev/ttyUSB0"); + this->get_parameter("serial_port_", serial_port_); + serial_ = LSIOSR::instance(serial_port_, baud_rate_); + code = serial_->init(); + if (code != 0) + { + printf("open_port %s ERROR !\n", serial_port_.c_str()); + rclcpp::shutdown(); + exit(0); + } + printf("open_port %s OK !\n", serial_port_.c_str()); + } + + bool LslidarDriver::createRosIO() + { + UDP_PORT_NUMBER = 2368; + this->declare_parameter("msop_port", 2368); + this->get_parameter("msop_port", UDP_PORT_NUMBER); + RCLCPP_INFO_STREAM(this->get_logger(), "Opening UDP socket: port " << UDP_PORT_NUMBER); + dump_file = std::string(""); + this->declare_parameter("pcap", ""); + this->get_parameter("pcap", dump_file); + // ROS diagnostics + diagnostics.setHardwareID("Lslidar"); + + const double diag_freq = 12 * 24; + diag_max_freq = diag_freq; + diag_min_freq = diag_freq; + RCLCPP_INFO(this->get_logger(), "expected frequency: %.3f (Hz)", diag_freq); + + using namespace diagnostic_updater; + diag_topic.reset(new TopicDiagnostic( + "lslidar_packets", diagnostics, + FrequencyStatusParam(&diag_min_freq, &diag_max_freq, 0.1, 10), + TimeStampStatusParam())); + + int hz = 10; + if (lidar_name == "M10_P") + hz = 12; + else if (lidar_name == "M10_PLUS") + hz = 20; + + double packet_rate = hz * 24; + if (dump_file != "") + { + msop_input_.reset(new lslidar_driver::InputPCAP(this, UDP_PORT_NUMBER, packet_rate, dump_file)); + } + else + { + msop_input_.reset(new lslidar_driver::InputSocket(this, UDP_PORT_NUMBER)); + } + + // Output + return true; + } + + int LslidarDriver::getScan(std::vector &points, rclcpp::Time &scan_time, float &scan_duration) + { + boost::unique_lock lock(mutex_); + points.assign(scan_points_bak_.begin(), scan_points_bak_.end()); + scan_time = pre_time_; + scan_duration = time_.seconds() - pre_time_.seconds(); + return 1; + } + + uint64_t LslidarDriver::get_gps_stamp(struct tm t) + { + + uint64_t ptime = static_cast(timegm(&t)); + return ptime; + } + + bool LslidarDriver::initialize() + { + if (!loadParameters()) + { + RCLCPP_ERROR(this->get_logger(), "Cannot load all required ROS parameters..."); + return false; + } + if (interface_selection == "net") + { + if (!createRosIO()) + { + RCLCPP_ERROR(this->get_logger(), "Cannot create all ROS IO..."); + return false; + } + } + else + { + in_file_name = std::string(""); + this->declare_parameter("in_file_name", ""); + this->get_parameter("in_file_name", in_file_name); + if (in_file_name == "") + open_serial(); + else + { + RCLCPP_INFO_STREAM(this->get_logger(), "Opening txt file " << in_file_name.c_str()); + std::ifstream file_reader(in_file_name); + if (!file_reader.is_open()) + { + RCLCPP_ERROR(this->get_logger(), "Cannot open the file"); + return false; + } + } + } + RCLCPP_INFO(this->get_logger(), "Initialised lslidar without error"); + return true; + } + + void LslidarDriver::recvThread_crc(int &count, int &link_time) + { + if (count <= 0) + link_time++; + else + link_time = 0; + + if (link_time > 150) + { + serial_->close(); + int ret = serial_->init(); + if (ret < 0) + { + RCLCPP_ERROR(this->get_logger(), "serial open fail"); + usleep(200000); + } + link_time = 0; + } + } + + int LslidarDriver::receive_data(unsigned char *packet_bytes) + { + int link_time = 0; + int len_H = 0; + int len_L = 0; + int len = 0; + int count_2 = 0; + int count = 0; + while (count <= 0) + { + count = serial_->read(packet_bytes, 1); + LslidarDriver::recvThread_crc(count, link_time); + } + if (packet_bytes[0] != 0xA5) + return 0; + + while (count_2 <= 0) + { + count_2 = serial_->read(packet_bytes + count, 1); + if (count_2 >= 0) + count += count_2; + LslidarDriver::recvThread_crc(count_2, link_time); + } + + count_2 = 0; + if (packet_bytes[1] != 0x5A) + return 0; + while (count_2 <= 0) + { + count_2 = serial_->read(packet_bytes + count, 2); + if (count_2 >= 0) + count += count_2; + LslidarDriver::recvThread_crc(count_2, link_time); + } + + count_2 = 0; + + if (lidar_name == "M10") + len = 92; + else if (lidar_name == "M10_GPS") + len = 102; + else if (lidar_name == "N10_P") + len = 108; + else if (lidar_name == "N10" || lidar_name == "L10") + len = packet_bytes[2]; + else + { + len_H = packet_bytes[2]; + len_L = packet_bytes[3]; + len = len_H * 256 + len_L; + } + if (lidar_name == "M10" || lidar_name == "M10_DOUBLE" || lidar_name == "M10_GPS" || lidar_name == "M10_P" || lidar_name == "M10_PLUS") + { + if (packet_bytes[2] == 0x55 && packet_bytes[3] == 0x00) + len = 188; + } + while (count < len) + { + count_2 = serial_->read(packet_bytes + count, len - count); + if (count_2 >= 0) + count += count_2; + LslidarDriver::recvThread_crc(count_2, link_time); + } + if (lidar_name == "N10" || lidar_name == "L10" || lidar_name == "N10_P") + { + if (packet_bytes[PACKET_SIZE - 1] != N10_CalCRC8(packet_bytes, PACKET_SIZE - 1)) + return 0; + } + return len; + } + + uint8_t LslidarDriver::N10_CalCRC8(unsigned char *p, int len) + { + uint8_t crc = 0; + int sum = 0; + + for (int i = 0; i < len; i++) + { + sum += uint8_t(p[i]); + } + crc = sum & 0xff; + return crc; + } + + void LslidarDriver::difop_processing(unsigned char *packet_bytes) // 处理设备包的数据 + { + int s = packet_bytes[173]; + int z = packet_bytes[174]; + int degree_temp = s & 0x7F; + int sign_temp = s & 0x80; + degree_compensation = double(degree_temp * 256 + z) / 100.f; + if (sign_temp) + degree_compensation = -degree_compensation; + first_compensation = false; + printf("degree_compensation = %f\n", degree_compensation); + return; + } + + void LslidarDriver::data_processing(unsigned char *packet_bytes, int len) // 处理每一包的数据 + { + double degree; + double end_degree; + double degree_interval = 15.0; + boost::posix_time::ptime t1, t2; + t1 = boost::posix_time::microsec_clock::universal_time(); + + int s = packet_bytes[degree_bits_start]; + int z = packet_bytes[degree_bits_start + 1]; + + degree = (s * 256 + z) / 100.f + degree_compensation; + degree = (degree < 0) ? degree + 360 : degree; + degree = (degree > 360) ? degree - 360 : degree; + if (lidar_name == "N10" || lidar_name == "L10") + { + int s_e = packet_bytes[end_degree_bits_start]; + int z_e = packet_bytes[end_degree_bits_start + 1]; + + end_degree = (s_e * 256 + z_e) / 100.f; + end_degree = (end_degree > 360) ? end_degree - 360 : end_degree; + + if (degree > end_degree) + degree_interval = end_degree + 360 - degree; + else + degree_interval = end_degree - degree; + } + + // boost::unique_lock lock(mutex_); + if (lidar_name == "M10_PLUS" || lidar_name == "M10_P") + { + PACKET_SIZE = len; + package_points = (PACKET_SIZE - 20) / 2; + } + int invalidValue = 0; + int point_len = 2; + if (lidar_name == "N10" || lidar_name == "L10") + point_len = 3; + + if (lidar_name == "M10_GPS" || lidar_name == "M10") + { + int err_data_84 = packet_bytes[84]; + int err_data_85 = packet_bytes[85]; + if ((err_data_84 * 256 + err_data_85) == 0xFFFF || packet_bytes[86] >= 0xF5) + { + packet_bytes[86] = 0xFF; + packet_bytes[87] = 0xFF; + } + } + + for (int num = 0; num < point_len * package_points; num += point_len) + { + int s = packet_bytes[num + data_bits_start]; + int z = packet_bytes[num + data_bits_start + 1]; + if ((s * 256 + z) == 0xFFFF) + invalidValue++; + } + + if (use_gps_ts && lidar_name != "N10") + { + pTime.tm_year = packet_bytes[PACKET_SIZE - 12] + 2000 - 1900; // x+2000 + pTime.tm_mon = packet_bytes[PACKET_SIZE - 11] - 1; // 1-12 + pTime.tm_mday = packet_bytes[PACKET_SIZE - 10]; // 1-31 + pTime.tm_hour = packet_bytes[PACKET_SIZE - 9]; // 0-23 + pTime.tm_min = packet_bytes[PACKET_SIZE - 8]; // 0-59 + pTime.tm_sec = packet_bytes[PACKET_SIZE - 7]; // 0-59 + sub_second = (packet_bytes[PACKET_SIZE - 6] * 256 + packet_bytes[PACKET_SIZE - 5]) * 1000000 + (packet_bytes[PACKET_SIZE - 4] * 256 + packet_bytes[PACKET_SIZE - 3]) * 1000; + sweep_end_time_gps = get_gps_stamp(pTime); + sweep_end_time_hardware = sub_second % 1000000000; + } + invalidValue = package_points - invalidValue; + if (lidar_name == "N10" || lidar_name == "L10") + invalidValue--; + if (invalidValue <= 1) + { + delete packet_bytes; + return; + } + + for (int num = 0; num < package_points; num++) + { + int s = packet_bytes[num * point_len + data_bits_start]; + int z = packet_bytes[num * point_len + data_bits_start + 1]; + int y = 0; + if (lidar_name == "N10" || lidar_name == "L10") + y = packet_bytes[num * point_len + data_bits_start + 2]; + int dist_temp = s & 0x7F; + int inten_temp = s & 0x80; + + if ((s * 256 + z) != 0xFFFF) + { + if (lidar_name == "N10" || lidar_name == "L10") + { + scan_points_[idx].range = double(s * 256 + (z)) / 1000.f; + scan_points_[idx].intensity = int(y); + } + else if ((lidar_name == "M10_P" || lidar_name == "M10_PLUS") && !high_reflection) + { + scan_points_[idx].range = double(s * 256 + (z)) / 1000.f; + scan_points_[idx].intensity = 0; + } + else + { + scan_points_[idx].range = double(dist_temp * 256 + (z)) / 1000.f; + if (inten_temp) + scan_points_[idx].intensity = 255; + else + scan_points_[idx].intensity = 0; + } + if ((degree + (degree_interval / invalidValue * num)) > 360) + scan_points_[idx].degree = degree + (degree_interval / invalidValue * num) - 360; + else + scan_points_[idx].degree = degree + (degree_interval / invalidValue * num); + } + else + continue; + + if ((scan_points_[idx].degree < last_degree && scan_points_[idx].degree < 5 && last_degree > 355) || idx >= points_size_) + { + last_degree = scan_points_[idx].degree; + count_num = idx; + idx = 0; + for (long unsigned int k = 0; k < scan_points_.size(); k++) + { + if (scan_points_[k].range < min_range || scan_points_[k].range > max_range) + scan_points_[k].range = 0; + } + boost::unique_lock lock(mutex_); + scan_points_bak_.resize(scan_points_.size()); + scan_points_bak_.assign(scan_points_.begin(), scan_points_.end()); + for (long unsigned int k = 0; k < scan_points_.size(); k++) + { + scan_points_[k].range = 0; + scan_points_[k].degree = 0; + scan_points_[k].intensity = 0; + } + pre_time_ = time_; + lock.unlock(); + pubscan_cond_.notify_one(); + time_ = get_clock()->now(); + } + else + { + last_degree = scan_points_[idx].degree; + idx++; + } + } + packet_bytes = {0x00}; + if (packet_bytes) + { + packet_bytes = NULL; + delete packet_bytes; + } + } + + void LslidarDriver::data_processing_2(unsigned char *packet_bytes, int len) // 处理每一包的数据 + { + double degree; + double end_degree; + double degree_interval = 15.0; + boost::posix_time::ptime t1, t2; + t1 = boost::posix_time::microsec_clock::universal_time(); + + int s = packet_bytes[degree_bits_start]; + int z = packet_bytes[degree_bits_start + 1]; + + degree = (s * 256 + z) / 100.f + degree_compensation; + degree = (degree < 0) ? degree + 360 : degree; + degree = (degree > 360) ? degree - 360 : degree; + if (lidar_name == "N10_P") + { + int s_e = packet_bytes[end_degree_bits_start]; + int z_e = packet_bytes[end_degree_bits_start + 1]; + + end_degree = (s_e * 256 + z_e) / 100.f; + end_degree = (end_degree > 360) ? end_degree - 360 : end_degree; + + if (degree > end_degree) + degree_interval = end_degree + 360 - degree; + else + degree_interval = end_degree - degree; + } + + // boost::unique_lock lock(mutex_); + if (lidar_name == "M10_DOUBLE") + { + PACKET_SIZE = len; + package_points = (PACKET_SIZE - 20) / 4; + } + int invalidValue = 0; + int point_len = 4; + if (lidar_name == "N10_P") + point_len = 6; + + for (int num = 0; num < point_len * package_points; num += point_len) + { + int s = packet_bytes[num + data_bits_start]; + int z = packet_bytes[num + data_bits_start + 1]; + if ((s * 256 + z) == 0xFFFF) + invalidValue++; + } + + if (use_gps_ts) + { + pTime.tm_year = packet_bytes[PACKET_SIZE - 12] + 2000 - 1900; // x+2000 + pTime.tm_mon = packet_bytes[PACKET_SIZE - 11] - 1; // 1-12 + pTime.tm_mday = packet_bytes[PACKET_SIZE - 10]; // 1-31 + pTime.tm_hour = packet_bytes[PACKET_SIZE - 9]; // 0-23 + pTime.tm_min = packet_bytes[PACKET_SIZE - 8]; // 0-59 + pTime.tm_sec = packet_bytes[PACKET_SIZE - 7]; // 0-59 + sub_second = (packet_bytes[PACKET_SIZE - 6] * 256 + packet_bytes[PACKET_SIZE - 5]) * 1000000 + (packet_bytes[PACKET_SIZE - 4] * 256 + packet_bytes[PACKET_SIZE - 3]) * 1000; + sweep_end_time_gps = get_gps_stamp(pTime); + sweep_end_time_hardware = sub_second % 1000000000; + } + invalidValue = package_points - invalidValue; + if (lidar_name == "N10_P") + invalidValue--; + if (invalidValue <= 1) + { + delete packet_bytes; + return; + } + + for (int num = 0; num < package_points; num++) + { + int s = packet_bytes[num * point_len + data_bits_start]; + int z = packet_bytes[num * point_len + data_bits_start + 1]; + int y = 0; + if (lidar_name == "N10_P") + y = packet_bytes[num * point_len + data_bits_start + 2]; + + if ((s * 256 + z) != 0xFFFF) + { + scan_points_[idx].range = double(s * 256 + (z)) / 1000.f; + if (lidar_name == "N10_P") + scan_points_[idx].intensity = int(y); + else + scan_points_[idx].intensity = 0; + s = packet_bytes[num * point_len + data_bits_start + point_len / 2]; + z = packet_bytes[num * point_len + data_bits_start + point_len / 2 + 1]; + if (lidar_name == "N10_P") + y = packet_bytes[num * point_len + data_bits_start + point_len / 2 + 2]; + + scan_points_[idx + 3000].range = double(s * 256 + (z)) / 1000.f; + if (lidar_name == "N10_P") + scan_points_[idx + 3000].intensity = int(y); + else + scan_points_[idx + 3000].intensity = 0; + + if ((degree + (degree_interval / invalidValue * num)) > 360) + scan_points_[idx].degree = degree + (degree_interval / invalidValue * num) - 360; + else + scan_points_[idx].degree = degree + (degree_interval / invalidValue * num); + } + else + continue; + if (((scan_points_[idx].degree < last_degree && scan_points_[idx].degree < 5 && last_degree > 355) || idx >= points_size_) && idx > 10) + { + last_degree = scan_points_[idx].degree; + count_num = idx; + idx = 0; + for (int k = 0; k < count_num; k++) + { + if (angle_able_max > 360) + { + if ((360 - scan_points_[k].degree) > (angle_able_max - 360) && (360 - scan_points_[k].degree) < angle_able_min) + { + scan_points_[k].range = 0; + scan_points_[k + 3000].range = 0; + } + } + else + { + if ((360 - scan_points_[k].degree) > angle_able_max || (360 - scan_points_[k].degree) < angle_able_min) + { + scan_points_[k].range = 0; + scan_points_[k + 3000].range = 0; + } + } + if (scan_points_[k].range < min_range || scan_points_[k].range > max_range) + scan_points_[k].range = 0; + if (scan_points_[k + 3000].range < min_range || scan_points_[k + 3000].range > max_range) + scan_points_[k + 3000].range = 0; + } + boost::unique_lock lock(mutex_); + scan_points_bak_.resize(scan_points_.size()); + scan_points_bak_.assign(scan_points_.begin(), scan_points_.end()); + for (long unsigned int k = 0; k < scan_points_.size(); k++) + { + scan_points_[k].range = 0; + scan_points_[k].degree = 0; + scan_points_[k].intensity = 0; + } + pre_time_ = time_; + lock.unlock(); + pubscan_cond_.notify_one(); + time_ = get_clock()->now(); + } + else + { + last_degree = scan_points_[idx].degree; + idx++; + } + } + packet_bytes = {0x00}; + if (packet_bytes) + { + packet_bytes = NULL; + delete packet_bytes; + } + } + + void LslidarDriver::pubScanThread() + { + bool wait_for_wake = true; + boost::unique_lock lock(pubscan_mutex_); + + while (rclcpp::ok()) + { + + while (wait_for_wake) + { + pubscan_cond_.wait(lock); + wait_for_wake = false; + } + if (lidar_name == "N10_P" || lidar_name == "M10_DOUBLE") + { + if (pubScan) + { + auto scan = sensor_msgs::msg::LaserScan::UniquePtr(new sensor_msgs::msg::LaserScan()); + ////int scan_num = count_num * 2; + int scan_num = count_num ; + + std::vector points; + rclcpp::Time start_time; + float scan_time; + this->getScan(points, start_time, scan_time); + scan->header.frame_id = frame_id; + if (use_gps_ts) + { + scan->header.stamp = rclcpp::Time(sweep_end_time_gps, sweep_end_time_hardware); + } + else + { + scan->header.stamp = this->now(); // timestamp will obtained from sweep data stamp + } + + scan->angle_min = 0; + scan->angle_max = 2 * M_PI; + scan->angle_increment = 2 * M_PI / (double)(count_num); + scan->range_min = min_range; + scan->range_max = max_range; + scan->ranges.reserve(scan_num); + scan->ranges.assign(scan_num, std::numeric_limits::infinity()); + scan->intensities.reserve(scan_num); + scan->intensities.assign(scan_num, std::numeric_limits::infinity()); + // scan->scan_time = scan_time; + // scan->time_increment = scan_time / (double)(count_num); + + for (int k = 0; k < scan_num; k++) + { + scan->ranges[k] = std::numeric_limits::infinity(); + scan->intensities[k] = 0; + } + + for (int i = 0; i < count_num; i++) + { + int point_idx = round((360 - points[i].degree) * count_num / 360); + if (points[i].range == 0.0) + { + scan->ranges[point_idx] = std::numeric_limits::infinity(); + scan->intensities[point_idx] = 0; + } + else + { + double dist = points[i].range; + scan->ranges[point_idx] = (float)dist; + scan->intensities[point_idx] = points[i].intensity; + } + + if(truncated_mode_){ + int len=sizeof(scan_crop_max) / sizeof(scan_crop_max[0]) ; + for(int j=0;j=(scan_crop_min[j]*count_num / 360)) && (point_idx<=(scan_crop_max[j]*count_num / 360))){ + scan->ranges[point_idx] = std::numeric_limits::infinity(); + scan->intensities[point_idx] = 0; + } + } + } + /* + if (points[i + 3000].range == 0.0) + { + scan->ranges[point_idx + count_num] = std::numeric_limits::infinity(); + scan->intensities[point_idx + count_num] = 0; + } + else + { + double dist = points[i+3000].range; + scan->ranges[point_idx + count_num] = (float)dist; + scan->intensities[point_idx + count_num] = points[i + 3000].intensity; + }*/ + } + scan_pub->publish(std::move(scan)); + } + if (pubPointCloud2) + { + std::vector points; + rclcpp::Time start_time; + float scan_time; + this->getScan(points, start_time, scan_time); + VPointCloud::Ptr point_cloud(new VPointCloud()); + if (use_gps_ts) + { + start_time = rclcpp::Time(sweep_end_time_gps, sweep_end_time_hardware); + } + double timestamp = start_time.seconds(); + point_cloud->header.stamp = static_cast(timestamp * 1e6); + point_cloud->header.frame_id = frame_id; + point_cloud->height = 1; + // printf("now = %f\n",timestamp); + for (uint16_t i = 0; i < count_num; i++) + { + // printf("degree = %f\n",points[i].degree); + double degree = 360.0 - points[i].degree; + bool pass_point = false; + if (angle_able_max < 360) + { + if (degree < angle_able_min || degree > angle_able_max) + pass_point = true; + } + else + { + if (degree < angle_able_min && degree > (angle_able_max - 360)) + pass_point = true; + } + if (points[i].range < 0.001) + pass_point = true; + if (!pass_point) + { + // printf("degree = %f\n",degree); + // printf("angle_able_min = %f\nangle_able_max=%f\n",angle_able_min,angle_able_max); + VPoint point; + int point_idx = round(degree * count_num / 360); + point.timestamp = timestamp - point_idx * (scan_time / count_num); + // printf("timestamp = %f\n",point.timestamp); + point.x = points[i].range * cos(M_PI / 180 * points[i].degree); + point.y = -points[i].range * sin(M_PI / 180 * points[i].degree); + point.z = 0; + point.intensity = points[i].intensity; + point_cloud->points.push_back(point); + ++point_cloud->width; + } + if (points[i + 3000].range < 0.001) + pass_point = true; + if (!pass_point) + { + // printf("degree = %f\n",degree); + // printf("angle_able_min = %f\nangle_able_max=%f\n",angle_able_min,angle_able_max); + VPoint point; + int point_idx = round(degree * count_num / 360); + point.timestamp = timestamp - point_idx * (scan_time / count_num); + // printf("timestamp = %f\n",point.timestamp); + point.x = points[i + 3000].range * cos(M_PI / 180 * points[i].degree); + point.y = -points[i + 3000].range * sin(M_PI / 180 * points[i].degree); + point.z = 0; + point.intensity = points[i + 3000].intensity; + point_cloud->points.push_back(point); + ++point_cloud->width; + } + } + sensor_msgs::msg::PointCloud2 pc_msg; + pcl::toROSMsg(*point_cloud, pc_msg); + point_cloud_pub->publish(pc_msg); + } + } + else + { + if (pubScan) + { + auto scan = sensor_msgs::msg::LaserScan::UniquePtr(new sensor_msgs::msg::LaserScan()); + //int scan_num = ceil((angle_able_max - angle_able_min) / 360 * count_num) + 1; + int scan_num = fixed_array_length;//cyy_addcyy_add + + std::vector points; + rclcpp::Time start_time; + float scan_time; + this->getScan(points, start_time, scan_time); + scan->header.frame_id = frame_id; + if (use_gps_ts) + { + scan->header.stamp = rclcpp::Time(sweep_end_time_gps, sweep_end_time_hardware); + } + else + { + scan->header.stamp = this->now(); // timestamp will obtained from sweep data stamp + } + + if (angle_able_max > 360) + { + scan->angle_min = 2 * M_PI * (angle_able_min - 360) / 360; + scan->angle_max = 2 * M_PI * (angle_able_max - 360) / 360; + } + else + { + scan->angle_min = 2 * M_PI * angle_able_min / 360; + scan->angle_max = 2 * M_PI * angle_able_max / 360; + } + scan->angle_increment = 2 * M_PI / (double)(fixed_array_length - 1); + + scan->range_min = min_range; + scan->range_max = max_range; + scan->ranges.reserve(scan_num); + scan->ranges.assign(scan_num, std::numeric_limits::infinity()); + scan->intensities.reserve(scan_num); + scan->intensities.assign(scan_num, std::numeric_limits::infinity()); + scan->scan_time = 0.1; + scan->time_increment = 0.1 / (double)(fixed_array_length - 1); + + int start_num = floor(angle_able_min * count_num / 360); + int end_num = floor(angle_able_max * count_num / 360); + + for (int i = 0; i < count_num; i++) + { + int point_idx = round((360 - points[i].degree) * count_num / 360); + if (point_idx < (end_num - count_num)) + point_idx += count_num; + point_idx = point_idx - start_num; + if (point_idx < 0 || point_idx >= scan_num) + continue; + if (points[i].range == 0.0) + { + scan->ranges[point_idx] = std::numeric_limits::infinity(); + } + else + { + double dist = points[i].range; + scan->ranges[point_idx] = (float)dist; + } + scan->intensities[point_idx] = points[i].intensity; + + if(truncated_mode_){ + int len=sizeof(scan_crop_max) / sizeof(scan_crop_max[0]) ; + for(int j=0;j=(scan_crop_min[j]*count_num / 360)) && (point_idx<=(scan_crop_max[j]*count_num / 360))){ + scan->ranges[point_idx] = std::numeric_limits::infinity(); + scan->intensities[point_idx] = 0; + } + } + } + } + + + scan_pub->publish(std::move(scan)); + } + if (pubPointCloud2) + { + std::vector points; + rclcpp::Time start_time; + float scan_time; + this->getScan(points, start_time, scan_time); + VPointCloud::Ptr point_cloud(new VPointCloud()); + if (use_gps_ts) + { + start_time = rclcpp::Time(sweep_end_time_gps, sweep_end_time_hardware); + } + double timestamp = start_time.seconds(); + point_cloud->header.stamp = static_cast(timestamp * 1e6); + point_cloud->header.frame_id = frame_id; + point_cloud->height = 1; + for (uint16_t i = 0; i < count_num; i++) + { + double degree = 360.0 - points[i].degree; + bool pass_point = false; + if (angle_able_max < 360) + { + if (degree < angle_able_min || degree > angle_able_max) + pass_point = true; + } + else + { + if (degree < angle_able_min && degree > (angle_able_max - 360)) + pass_point = true; + } + if (points[i].range < 0.001) + pass_point = true; + if (!pass_point) + { + // printf("degree = %f\n",degree); + // printf("angle_able_min = %f\nangle_able_max=%f\n",angle_able_min,angle_able_max); + VPoint point; + int point_idx = round(degree * count_num / 360); + point.timestamp = timestamp - point_idx * (scan_time / count_num); + // printf("timestamp = %f\n",point.timestamp); + point.x = points[i].range * cos(M_PI / 180 * points[i].degree); + point.y = -points[i].range * sin(M_PI / 180 * points[i].degree); + point.z = 0; + point.intensity = points[i].intensity; + point_cloud->points.push_back(point); + ++point_cloud->width; + } + } + sensor_msgs::msg::PointCloud2 pc_msg; + pcl::toROSMsg(*point_cloud, pc_msg); + point_cloud_pub->publish(pc_msg); + } + } + count_num = 0; + wait_for_wake = true; + if (first_compensation && compensation) + { + lidar_difop(); + } + } + } + + bool LslidarDriver::polling() + { + if (!is_start) + return true; + // Allocate a new shared pointer for zero-copy sharing with other nodelets. + unsigned char *packet_bytes = new unsigned char[500]; + int len = 0; + bool difop = false; + if (interface_selection == "net") + { + auto packet = lslidar_msgs::msg::LslidarPacket::UniquePtr( + new lslidar_msgs::msg::LslidarPacket()); + + std_msgs::msg::Byte msg; + while (true) + { + difop = false; + len = 0; + // keep reading until full packet received + len = msop_input_->getPacket(packet); + if (packet->data[0] == 0x5a) + { + if (lidar_name == "N10" || lidar_name == "L10") + len = 58; + else if (lidar_name == "M10") + len = 92; + else if (lidar_name == "N10_P") + len = 108; + else if (lidar_name == "M10_GPS") + len = 102; + else + { + int len_H = packet->data[1]; + int len_L = packet->data[2]; + len = len_H * 256 + len_L; + } + for (int i = len - 1; i > 0; i--) + packet->data[i] = packet->data[i - 1]; + packet->data[0] = 0xa5; + } + + if (lidar_name == "N10" || lidar_name == "L10") + len = 58; + else if (lidar_name == "M10") + len = 92; + else if (lidar_name == "N10_P") + len = 108; + else if (lidar_name == "M10_GPS") + len = 102; + else + { + int len_H = packet->data[2]; + int len_L = packet->data[3]; + len = len_H * 256 + len_L; + } + if ((lidar_name == "M10" || lidar_name == "M10_DOUBLE" || lidar_name == "M10_GPS" || lidar_name == "M10_P" || lidar_name == "M10_PLUS") && compensation) + { + if (packet->data[2] == 0x55 && packet->data[3] == 0x00 && packet->data[186] == 0xFA && packet->data[187] == 0xFB) + { + len = 188; + difop = true; + } + } + + if (len <= 0 || len >= 1000 || packet->data[0] != 0xa5 || packet->data[1] != 0x5a) + continue; + for (int i = 0; i < len; i++) + { + packet_bytes[i] = packet->data[i]; + } + if ((lidar_name == "N10" || lidar_name == "L10" || lidar_name == "N10_P") && packet_bytes[len - 1] != N10_CalCRC8(packet_bytes, len - 1)) + continue; + break; + } + } + else + { + if (in_file_name != "") // 读txt文件功能 + { + int usleep_time = round(1000000 / 10 / 24) - 135; + while (true) + { + std::ifstream file_reader(in_file_name); + while (file_reader.peek() != EOF) + { + std::string line; + std::getline(file_reader, line, '\n'); + for (long unsigned int i = 0; i < line.size() - 1; i++) + { + line[i] = line[i] - 48; + if (line[i] > 9) + line[i] = line[i] - 39; + } + + for (long unsigned int i = 0; i < (line.size() - 1) / 2; i++) + { + packet_bytes[i] = line[i * 2] * 16 + line[i * 2 + 1]; + } + if (lidar_name == "N10" || lidar_name == "L10") + len = 58; + else if (lidar_name == "M10") + len = 92; + else if (lidar_name == "N10_P") + len = 108; + else if (lidar_name == "M10_GPS") + len = 102; + else + { + int len_H = packet_bytes[2]; + int len_L = packet_bytes[3]; + len = len_H * 256 + len_L; + } + if (lidar_name == "N10_P" || lidar_name == "M10_DOUBLE") + LslidarDriver::data_processing_2(packet_bytes, len); + else + LslidarDriver::data_processing(packet_bytes, len); + usleep(usleep_time); + } + } + return false; + } + else + { + while (true) + { + difop = false; + len = 0; + len = LslidarDriver::receive_data(packet_bytes); + if ((lidar_name == "M10" || lidar_name == "M10_DOUBLE" || lidar_name == "M10_GPS" || lidar_name == "M10_P" || lidar_name == "M10_PLUS") && compensation) + { + if (packet_bytes[2] == 0x55 && packet_bytes[3] == 0x00 && packet_bytes[186] == 0xFA && packet_bytes[187] == 0xFB) + difop = true; + } + if (len == 0) + continue; + break; + } + } + } + if (difop) + LslidarDriver::difop_processing(packet_bytes); + else + { + if (lidar_name == "N10_P" || lidar_name == "M10_DOUBLE") + LslidarDriver::data_processing_2(packet_bytes, len); + else + LslidarDriver::data_processing(packet_bytes, len); + } + delete packet_bytes; + return true; + } + +} // namespace lslidar_driver diff --git a/src/gc_navigation2_slamtoolbox/launch/gc_nav2_with_slam_online_real.launch.py b/src/gc_navigation2_slamtoolbox/launch/gc_nav2_with_slam_online_real.launch.py index a37bb2d..4a684fb 100644 --- a/src/gc_navigation2_slamtoolbox/launch/gc_nav2_with_slam_online_real.launch.py +++ b/src/gc_navigation2_slamtoolbox/launch/gc_nav2_with_slam_online_real.launch.py @@ -1,6 +1,7 @@ # ============================================================================ # gc_nav2_with_slam_online_real.launch.py # 功能：真机实时建图 — slam_toolbox 从零建图 + Nav2 导航 +# 依赖：需先启动 origincar_bringup（底盘驱动 + EKF） # ============================================================================ import os @@ -29,7 +30,7 @@ def generate_launch_description(): pkg_dir, 'params', 'gc_navigation_slam.yaml')) slam_params_file = os.path.join(pkg_dir, 'config', 'slam_toolbox_mapping.yaml') - # === 3. slam_toolbox：实时建图 → 直接发布到 /map === + # === 3. slam_toolbox：实时建图 → 发布 map→odom === slam_toolbox_node = Node( package='slam_toolbox', executable='async_slam_toolbox_node', @@ -49,7 +50,7 @@ def generate_launch_description(): }.items(), ) - # === 5. 机器人模型 + TF === + # === 5. 机器人模型（URDF 已定义 base_footprint→base_link，无需 static TF）=== model = DeclareLaunchArgument( name='model', default_value=os.path.join(origincar_urdf_dir, 'urdf', 'origincar.urdf')) @@ -63,17 +64,9 @@ def generate_launch_description(): 'use_sim_time': use_sim_time, 'publish_frequency': 30.0}], ) - base_footprint_tf = Node( - package='tf2_ros', - executable='static_transform_publisher', - name='base_footprint_to_base_link', - arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link'], - ) - # === 6. 组装 === ld.add_action(model) ld.add_action(robot_state_publisher) - ld.add_action(base_footprint_tf) ld.add_action(slam_toolbox_node) ld.add_action(navigation_launch) diff --git a/src/origincar_base/config/ekf.yaml b/src/origincar_base/config/ekf.yaml index a0d236f..0dbaded 100644 --- a/src/origincar_base/config/ekf.yaml +++ b/src/origincar_base/config/ekf.yaml @@ -13,7 +13,7 @@ ekf_filter_node: map_frame: map odom_frame: odom - base_link_frame: base_link + base_link_frame: base_footprint world_frame: odom odom0: odom diff --git a/src/origincar_base/include/origincar_base/origincar_base.h b/src/origincar_base/include/origincar_base/origincar_base.h index 456ce18..d5ebe91 100644 --- a/src/origincar_base/include/origincar_base/origincar_base.h +++ b/src/origincar_base/include/origincar_base/origincar_base.h @@ -212,6 +212,7 @@ private: rclcpp::Subscription::SharedPtr Sign_Switch_Sub; string usart_port_name, robot_frame_id, gyro_frame_id, odom_frame_id, akm_cmd_vel, test; + bool publish_tf_; std::string cmd_vel; int serial_baud_rate; RECEIVE_DATA Receive_Data; diff --git a/src/origincar_base/launch/origincar_bringup.launch.py b/src/origincar_base/launch/origincar_bringup.launch.py index 35e023e..0b56728 100644 --- a/src/origincar_base/launch/origincar_bringup.launch.py +++ b/src/origincar_base/launch/origincar_bringup.launch.py @@ -37,12 +37,6 @@ def generate_launch_description(): ) - base_to_link = launch_ros.actions.Node( - package='tf2_ros', - executable='static_transform_publisher', - name='base_to_link', - arguments=['0', '0', '0','0', '0','0','base_footprint','base_link'],#arguments=['0.41', '0.12', '0','0', '0','0','base_footprint','base_link'], - ) base_to_gyro = launch_ros.actions.Node( @@ -73,10 +67,21 @@ def generate_launch_description(): remappings=[("odometry/filtered", "odom_combined")] ) + # 从 URDF 生成 robot_description，供 joint_state_publisher 和 robot_state_publisher 共用 + from launch_ros.parameter_descriptions import ParameterValue + from launch.substitutions import Command + + robot_description = ParameterValue( + Command(['xacro ', os.path.join( + get_package_share_directory('origincar_description'), + 'urdf', 'origincar.urdf')]), + value_type=str) + joint_state_publisher_node = launch_ros.actions.Node( package='joint_state_publisher', executable='joint_state_publisher', name='joint_state_publisher', + parameters=[{'robot_description': robot_description}], ) ld = LaunchDescription() @@ -84,7 +89,6 @@ def generate_launch_description(): ld.add_action(carto_slam_dec) ld.add_action(akmcar_dec) ld.add_action(origincar_base) - ld.add_action(base_to_link) ld.add_action(base_to_gyro) ld.add_action(joint_state_publisher_node) ld.add_action(choose_car) diff --git a/src/origincar_base/launch/robot_mode_description.launch.py b/src/origincar_base/launch/robot_mode_description.launch.py index 4bce9d9..a55cacb 100644 --- a/src/origincar_base/launch/robot_mode_description.launch.py +++ b/src/origincar_base/launch/robot_mode_description.launch.py @@ -1,24 +1,27 @@ import os -from pathlib import Path -import launch_ros.actions -import launch from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import (DeclareLaunchArgument, GroupAction,LogInfo, - IncludeLaunchDescription, SetEnvironmentVariable) -from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterValue +from launch.substitutions import Command + def generate_launch_description(): - origincar_description = GroupAction([ - launch_ros.actions.Node( - package='robot_state_publisher', - executable='robot_state_publisher', - name='robot_state_publisher', - arguments=[os.path.join(get_package_share_directory('origincar_description'),'urdf','origincar.urdf')] - ) - ]) - - ld = LaunchDescription() + urdf_path = os.path.join( + get_package_share_directory('origincar_description'), + 'urdf', 'origincar.urdf') + + robot_description = ParameterValue( + Command(['xacro ', urdf_path]), value_type=str) - ld.add_action(origincar_description) - return ld \ No newline at end of file + robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + parameters=[{'robot_description': robot_description, + 'publish_frequency': 30.0}], + ) + + ld = LaunchDescription() + ld.add_action(robot_state_publisher) + return ld diff --git a/src/origincar_base/scripts/cmd_vel_to_ackermann_drive.py b/src/origincar_base/scripts/cmd_vel_to_ackermann_drive.py old mode 100644 new mode 100755 diff --git a/src/origincar_base/src/origincar_base.cpp b/src/origincar_base/src/origincar_base.cpp index 4a39723..2a972d6 100644 --- a/src/origincar_base/src/origincar_base.cpp +++ b/src/origincar_base/src/origincar_base.cpp @@ -225,7 +225,9 @@ void origincar_base::Publish_Odom() t.transform.translation.z = 0.0; t.transform.rotation = odom_quat; - tf_broadcaster_->sendTransform(t); + if (publish_tf_) { + tf_broadcaster_->sendTransform(t); + } odom_publisher->publish(odom); robotpose_publisher->publish(robotpose); robotvel_publisher->publish(robotvel); @@ -264,7 +266,11 @@ bool origincar_base::Get_Sensor_Data() { short transition_16 = 0, j = 0, Header_Pos = 0, Tail_Pos = 0; uint8_t Receive_Data_Pr[RECEIVE_DATA_SIZE] = {0}; - Stm32_Serial.read(Receive_Data_Pr,sizeof (Receive_Data_Pr)); + try { + Stm32_Serial.read(Receive_Data_Pr,sizeof (Receive_Data_Pr)); + } catch (const serial::SerialException& e) { + return false; + } for (j = 0; j < 24; j++) { if (Receive_Data_Pr[j] == FRAME_HEADER) Header_Pos=j; @@ -375,6 +381,7 @@ origincar_base::origincar_base() this->declare_parameter("odom_frame_id", "odom"); this->declare_parameter("robot_frame_id", "base_link"); this->declare_parameter("gyro_frame_id", "gyro_link"); + this->declare_parameter("publish_tf", false); this->get_parameter("serial_baud_rate", serial_baud_rate); this->get_parameter("usart_port_name", usart_port_name); @@ -383,6 +390,7 @@ origincar_base::origincar_base() this->get_parameter("odom_frame_id", odom_frame_id); this->get_parameter("robot_frame_id", robot_frame_id); this->get_parameter("gyro_frame_id", gyro_frame_id); + this->get_parameter("publish_tf", publish_tf_); odom_publisher = create_publisher("odom", 10); diff --git a/src/origincar_description/urdf/origincar.urdf b/src/origincar_description/urdf/origincar.urdf index c441917..8664a97 100644 --- a/src/origincar_description/urdf/origincar.urdf +++ b/src/origincar_description/urdf/origincar.urdf @@ -4,12 +4,6 @@ -<<<<<<< HEAD - - - -======= ->>>>>>> mo_new @@ -89,11 +83,7 @@ -<<<<<<< HEAD - -======= ->>>>>>> mo_new @@ -146,11 +136,7 @@ -<<<<<<< HEAD - -======= ->>>>>>> mo_new @@ -219,11 +205,7 @@ -<<<<<<< HEAD - -======= ->>>>>>> mo_new @@ -257,11 +239,7 @@ -<<<<<<< HEAD - -======= ->>>>>>> mo_new @@ -287,11 +265,7 @@ -<<<<<<< HEAD - -======= ->>>>>>> mo_new @@ -336,10 +310,6 @@ -<<<<<<< HEAD - -======= ->>>>>>> mo_new diff --git a/调试记录.log b/调试记录.log index 4518581..e1c185b 100644 --- a/调试记录.log +++ b/调试记录.log @@ -16,4 +16,154 @@ Bus 001 Device 001: ID 1d6b:0002 Linux Foundation 2.0 root hub 16:48 -雷达串口问题解决，原因是type-c转接线的芯片问题，换了一条转接线后雷达串口设备正常显示为ttyACM0和ttyACM1了。 \ No newline at end of file +雷达串口问题解决，原因是type-c转接线的芯片问题，换了一条转接线后雷达串口设备正常显示为ttyACM0和ttyACM1了。 +============================================================ +2026/6/11 — TF树修复 & base驱动稳定性修复 +============================================================ + +## 问题1: SLAM Toolbox Message Filter 丢帧 +**现象**: `Message Filter dropping message: frame laser_link ... reason discarding message because the queue is full` +**原因**: `slam_toolbox_mapping.yaml` 中 `scan_queue_size` 偏小，激光雷达 10Hz、minimum_time_interval 0.5s，队列缓冲不足 +**修复**: 将 `scan_queue_size` 适当调大即可（属配置调优项，未做硬编码修改） + +## 问题2: SLAM Toolbox Message Filter 时间戳过期 +**现象**: `Message Filter dropping message: frame laser_link ... reason the timestamp on the message is earlier than all the data in the transform cache` +**原因**: 激光帧时间戳早于 TF 缓存最早数据，通常刚启动时 TF 尚未就绪或处理积压 +**修复**: 随问题3 TF树修复后缓解 + +## 问题3: TF 树断连 — bt_navigator 找不到 map→base_footprint (核心问题) +**现象**: `Could not find a connection between map and base_footprint because they are not part of the same tree. Tf has two or more unconnected trees.` +**根因**: 两个节点同时发布 `odom→base_link` TF: + - base 驱动 (origincar_base.cpp:228) — 原始轮式里程计 + - EKF (ekf.yaml: base_link_frame=base_link, publish_tf=true) — 融合 odom+IMU + 两节点争夺同一条 TF，导致树撕裂 +**修复** (4处改动): + 1. ekf.yaml: base_link_frame 从 base_link 改为 base_footprint + → EKF 发布 odom→base_footprint，不再与 base 驱动争抢 + 2. origincar_base.cpp: 新增 publish_tf 参数，默认 false + → base 驱动默认不再广播 TF + 3. gc_nav2_with_slam_online_real.launch.py: 移除重复的 base_footprint→base_link static TF + → URDF 已定义该关节 (z=-0.09)，static TF 提供 identity (z=0) 造成冲突 + 4. origincar_bringup.launch.py: 移除 base_to_link static TF（同上） +**修复后 TF 树**: + map → odom → base_footprint → base_link → chassis_link → borad_link → laser_link + ↑ ↑ ↑ ↑ + SLAM EKF融合 URDF URDF + +## 问题4: cmd_vel_to_ackermann_drive.py 找不到 +**现象**: `executable cmd_vel_to_ackermann_drive.py not found on the libexec directory` +**原因**: 源文件缺执行权限 (644)，--symlink-install 时安装路径继承源文件权限 +**修复**: chmod +x scripts/cmd_vel_to_ackermann_drive.py + +## 问题5: base 驱动串口异常崩溃 +**现象**: `terminate called after throwing serial::SerialException: device reports readiness to read but returned no data` +**原因**: Get_Sensor_Data() 中 Stm32_Serial.read() 抛异常未捕获 +**修复**: origincar_base.cpp 中 Stm32_Serial.read() 包裹 try-catch，捕获 SerialException 后 return false + +## 问题6: URDF 文件 git 冲突未解决 +**现象**: xacro 解析失败 — XML parsing error: not well-formed (invalid token) +**原因**: origincar.urdf 存在 8 处 git merge 冲突标记 (HEAD vs mo_new)，涉及轮距参数 (0.0841 vs 0.0715) +**修复**: 清除所有冲突标记，采用 mo_new 分支轮距值 + +## 问题7: joint_state_publisher 找不到 robot_description +**现象**: `Waiting for robot_description to be published on the robot_description topic...` +**原因**: robot_mode_description.launch.py 使用旧式命令行参数传 URDF，未发布到 topic +**修复**: 改为参数方式传递 robot_description，同时传给 joint_state_publisher + +## 编译记录 + - 命令: colcon build --packages-select origincar_base --symlink-install + - 结果: 通过 + +================================================================================ +日期: 2026-06-13 +问题: LSLiDAR /scan 可视化中出现空扇形面 +================================================================================ + +【现象】 +在 rviz/foxglove 中查看 /scan 话题时，显示中凭空出现一个约60°的扇形区域 +没有任何点（inf），物体轮廓在该边界处像是被"切断"一般。但实际雷达能够检测 +到360°所有方向的物体。 + +【根因分析】 +驱动代码 lslidar_driver.cc 中，N10 雷达走的是 else 分支（Path 2）。 +第1137行被硬编码为: + int scan_num = fixed_array_length; // 450 + +而原始正确公式（被注释掉）为: + //int scan_num = ceil((angle_able_max - angle_able_min) / 360 * count_num) + 1; + +问题机制: + - LiDAR 硬件每圈输出约374个数据点 (分辨率 ~0.96°/点)，完整覆盖360° + - 但 fixed_array_length=450 强制分配了450个bin (分辨率 0.80°/点) + - 索引映射 point_idx = round((360-degree) * count_num / 360) 最大只能到373 + - 所以 indices 374-449 (78个bin, ~60°) 永远是 inf + - 所有真实数据被"压缩"到 0-299° 范围内显示 + +同时 angle_increment 和 time_increment 也使用了 fixed_array_length +导致与实际数据不匹配。 + +【修复内容】 +文件: src/lslidar_driver/src/lslidar_driver.cc + +修改1 (line 1137): + - 修复前: int scan_num = fixed_array_length;//cyy_addcyy_add + + 修复后: int scan_num = ceil((angle_able_max - angle_able_min) / 360 * count_num) + 1; + +修改2 (line 1163): + - 修复前: scan->angle_increment = 2 * M_PI / (double)(fixed_array_length - 1); + + 修复后: scan->angle_increment = 2 * M_PI / (double)(scan_num - 1); + +修改3 (line 1172): + - 修复前: scan->time_increment = 0.1 / (double)(fixed_array_length - 1); + + 修复后: scan->time_increment = 0.1 / (double)(scan_num - 1); + +【验证】 +编译通过。需要重启 lslidar_driver_node 使修复生效: + ros2 lifecycle set lslidar_driver_node shutdown + 然后重新启动 launch 文件。 + +================================================================================ +日期: 2026-06-13 +问题: LSLiDAR /scan 可视化中出现空扇形面 +================================================================================ + +【现象】 +在 rviz/foxglove 中查看 /scan 话题时，显示中凭空出现一个约60°的扇形区域 +没有任何点（inf），物体轮廓在该边界处像是被切断一般。但实际雷达能够检测 +到360°所有方向的物体。 + +【根因分析】 +驱动代码 lslidar_driver.cc 中，N10 雷达走的是 else 分支（Path 2）。 +第1137行被硬编码为: + int scan_num = fixed_array_length; // 450 + +而原始正确公式（被注释掉）为: + //int scan_num = ceil((angle_able_max - angle_able_min) / 360 * count_num) + 1; + +问题机制: + - LiDAR 硬件每圈输出约374个数据点 (分辨率 ~0.96°/点)，完整覆盖360° + - 但 fixed_array_length=450 强制分配了450个bin (分辨率 0.80°/点) + - 索引映射 point_idx = round((360-degree) * count_num / 360) 最大只能到373 + - 所以 indices 374-449 (78个bin, ~60°) 永远是 inf + - 所有真实数据被压缩到 0-299° 范围内显示 + +同时 angle_increment 和 time_increment 也使用了 fixed_array_length +导致与实际数据不匹配。 + +【修复内容】 +文件: src/lslidar_driver/src/lslidar_driver.cc + +修改1 (line 1137): + - 修复前: int scan_num = fixed_array_length; + + 修复后: int scan_num = ceil((angle_able_max - angle_able_min) / 360 * count_num) + 1; + +修改2 (line 1163): + - 修复前: scan->angle_increment = 2 * M_PI / (double)(fixed_array_length - 1); + + 修复后: scan->angle_increment = 2 * M_PI / (double)(scan_num - 1); + +修改3 (line 1172): + - 修复前: scan->time_increment = 0.1 / (double)(fixed_array_length - 1); + + 修复后: scan->time_increment = 0.1 / (double)(scan_num - 1); + +【验证】 +编译通过。需要重启 lslidar_driver_node 使修复生效。