From b44f0c190917c06847502247e619d09726888da0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E9=99=8C=E6=AC=A2=E9=86=89?= <264854363@qq.com> Date: Sat, 6 Jun 2026 20:32:07 +0800 Subject: [PATCH] =?UTF-8?q?add=EF=BC=9A=E6=B7=BB=E5=8A=A0=E4=BA=86?= =?UTF-8?q?=E5=AE=9E=E8=BD=A6=E6=A8=A1=E5=BC=8F=EF=BC=8C=E6=9C=AA=E9=AA=8C?= =?UTF-8?q?=E8=AF=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/LSLIDAR_X_ROS2-20240228/src/README.md | 44 + .../src/lslidar_driver/CMakeLists.txt | 56 + .../include/lslidar_driver/input.h | 134 ++ .../include/lslidar_driver/lsiosr.h | 88 + .../include/lslidar_driver/lslidar_driver.h | 159 ++ .../launch/lslidar_double_launch.py | 50 + .../lslidar_driver/launch/lsm10_net_launch.py | 27 + .../launch/lsm10_uart_launch.py | 27 + .../launch/lsm10p_net_launch.py | 27 + .../launch/lsm10p_uart_launch.py | 27 + .../src/lslidar_driver/launch/lsn10_launch.py | 28 + .../lslidar_driver/launch/lsn10_net_launch.py | 28 + .../lslidar_driver/launch/lsn10p_launch.py | 28 + .../launch/lsn10p_net_launch.py | 28 + .../launch/viewer_scan_launch.py | 26 + .../src/lslidar_driver/package.xml | 36 + .../params/lidar_net_ros2/lsm10_net.yaml | 22 + .../params/lidar_net_ros2/lsm10p_net.yaml | 22 + .../params/lidar_net_ros2/lsn10_net.yaml | 25 + .../params/lidar_net_ros2/lsn10p_net.yaml | 25 + .../params/lidar_uart_ros2/lsm10.yaml | 28 + .../params/lidar_uart_ros2/lsm10_p.yaml | 28 + .../params/lidar_uart_ros2/lsn10.yaml | 26 + .../params/lidar_uart_ros2/lsn10p.yaml | 25 + .../src/lslidar_driver/rviz/lslidar.rviz | 161 ++ .../src/lslidar_driver/src/input.cc | 398 +++++ .../src/lslidar_driver/src/lsiosr.cpp | 400 +++++ .../src/lslidar_driver/src/lslidar_driver.cc | 1423 +++++++++++++++++ .../lslidar_driver/src/lslidar_driver_node.cc | 35 + .../src/lslidar_msgs/CMakeLists.txt | 39 + .../src/lslidar_msgs/msg/LslidarDifop.msg | 2 + .../src/lslidar_msgs/msg/LslidarPacket.msg | 5 + .../src/lslidar_msgs/msg/LslidarPoint.msg | 12 + .../src/lslidar_msgs/msg/LslidarScan.msg | 6 + .../src/lslidar_msgs/msg/LslidarSweep.msg | 4 + .../src/lslidar_msgs/package.xml | 25 + src/LSLIDAR_X_ROS2-20240228/src/version.txt | 20 + .../src/wheeltec_udev.sh | 33 + .../src/镭神Lsx雷达旋转角度.png | Bin 0 -> 6646 bytes .../wheeltec_lidar.launch.py | 62 + ...ltec_lidar.launch.py仅在WHEELTEC镜像中使用 | 0 src/gc_navigation2_real/CMakeLists.txt | 23 + .../behavior_tree/nav_to_pose_ackermann.xml | 37 + .../config/slam_toolbox_async_real.yaml | 78 + .../slam_toolbox_localization_real.yaml | 77 + .../launch/real_bringup.launch.py | 54 + .../launch/real_nav2_slam.launch.py | 110 ++ .../launch/real_nav2_slam_online.launch.py | 135 ++ .../launch/real_slam_mapping.launch.py | 103 ++ src/gc_navigation2_real/package.xml | 31 + .../params/gc_navigation_slam_real.yaml | 353 ++++ src/origincar_base/CMakeLists.txt | 89 ++ src/origincar_base/config/ekf.yaml | 222 +++ src/origincar_base/config/imu.yaml | 7 + .../origincar_base/Quaternion_Solution.h | 10 + .../include/origincar_base/origincar_base.h | 228 +++ .../__pycache__/testtwo.launch.cpython-38.pyc | Bin 0 -> 580 bytes .../launch/base_serial.launch.py | 47 + src/origincar_base/launch/ekf.launch.py | 33 + .../launch/origincar_bringup.launch.py | 95 ++ .../launch/robot_mode_description.launch.py | 24 + src/origincar_base/msg/Position.msg | 3 + src/origincar_base/package.xml | 44 + .../scripts/cmd_vel_to_ackermann_drive.py | 47 + .../src/Quaternion_Solution.cpp | 91 ++ src/origincar_base/src/origincar_base.cpp | 469 ++++++ src/origincar_description/urdf/origincar.urdf | 10 +- src/origincar_msg/CMakeLists.txt | 34 + src/origincar_msg/msg/Data.msg | 3 + src/origincar_msg/msg/Sign.msg | 1 + src/origincar_msg/package.xml | 21 + 71 files changed, 6113 insertions(+), 5 deletions(-) create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/README.md create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/CMakeLists.txt create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/include/lslidar_driver/input.h create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/include/lslidar_driver/lsiosr.h create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/include/lslidar_driver/lslidar_driver.h create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lslidar_double_launch.py create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsm10_net_launch.py create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsm10_uart_launch.py create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsm10p_net_launch.py create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsm10p_uart_launch.py create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsn10_launch.py create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsn10_net_launch.py create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsn10p_launch.py create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsn10p_net_launch.py create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/viewer_scan_launch.py create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/package.xml create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_net_ros2/lsm10_net.yaml create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_net_ros2/lsm10p_net.yaml create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_net_ros2/lsn10_net.yaml create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_net_ros2/lsn10p_net.yaml create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_uart_ros2/lsm10.yaml create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_uart_ros2/lsm10_p.yaml create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_uart_ros2/lsn10.yaml create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_uart_ros2/lsn10p.yaml create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/rviz/lslidar.rviz create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/input.cc create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/lsiosr.cpp create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/lslidar_driver.cc create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/lslidar_driver_node.cc create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/CMakeLists.txt create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/msg/LslidarDifop.msg create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/msg/LslidarPacket.msg create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/msg/LslidarPoint.msg create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/msg/LslidarScan.msg create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/msg/LslidarSweep.msg create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/package.xml create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/version.txt create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/wheeltec_udev.sh create mode 100644 src/LSLIDAR_X_ROS2-20240228/src/镭神Lsx雷达旋转角度.png create mode 100644 src/LSLIDAR_X_ROS2-20240228/wheeltec_lidar.launch.py create mode 100644 src/LSLIDAR_X_ROS2-20240228/wheeltec_lidar.launch.py仅在WHEELTEC镜像中使用 create mode 100644 src/gc_navigation2_real/CMakeLists.txt create mode 100644 src/gc_navigation2_real/behavior_tree/nav_to_pose_ackermann.xml create mode 100644 src/gc_navigation2_real/config/slam_toolbox_async_real.yaml create mode 100644 src/gc_navigation2_real/config/slam_toolbox_localization_real.yaml create mode 100644 src/gc_navigation2_real/launch/real_bringup.launch.py create mode 100644 src/gc_navigation2_real/launch/real_nav2_slam.launch.py create mode 100644 src/gc_navigation2_real/launch/real_nav2_slam_online.launch.py create mode 100644 src/gc_navigation2_real/launch/real_slam_mapping.launch.py create mode 100644 src/gc_navigation2_real/package.xml create mode 100644 src/gc_navigation2_real/params/gc_navigation_slam_real.yaml create mode 100644 src/origincar_base/CMakeLists.txt create mode 100644 src/origincar_base/config/ekf.yaml create mode 100644 src/origincar_base/config/imu.yaml create mode 100644 src/origincar_base/include/origincar_base/Quaternion_Solution.h create mode 100644 src/origincar_base/include/origincar_base/origincar_base.h create mode 100644 src/origincar_base/launch/__pycache__/testtwo.launch.cpython-38.pyc create mode 100644 src/origincar_base/launch/base_serial.launch.py create mode 100644 src/origincar_base/launch/ekf.launch.py create mode 100644 src/origincar_base/launch/origincar_bringup.launch.py create mode 100644 src/origincar_base/launch/robot_mode_description.launch.py create mode 100644 src/origincar_base/msg/Position.msg create mode 100644 src/origincar_base/package.xml create mode 100644 src/origincar_base/scripts/cmd_vel_to_ackermann_drive.py create mode 100644 src/origincar_base/src/Quaternion_Solution.cpp create mode 100644 src/origincar_base/src/origincar_base.cpp create mode 100644 src/origincar_msg/CMakeLists.txt create mode 100644 src/origincar_msg/msg/Data.msg create mode 100644 src/origincar_msg/msg/Sign.msg create mode 100644 src/origincar_msg/package.xml diff --git a/src/LSLIDAR_X_ROS2-20240228/src/README.md b/src/LSLIDAR_X_ROS2-20240228/src/README.md new file mode 100644 index 0000000..9871ecc --- /dev/null +++ b/src/LSLIDAR_X_ROS2-20240228/src/README.md @@ -0,0 +1,44 @@ +# lslidar + +## Description +The `lslidar package is a linux ROS2 driver for lslidar M10 ,M10_GPS,M10_P,M10_PLUS and N10. +The package is tested on Ubuntu 20.04 with ROS2 indigo. + +## Compling +This is a Catkin package. Make sure the package is on `ROS_PACKAGE_PATH` after cloning the package to your workspace. And the normal procedure for compling a catkin package will work. + +``` +cd your_work_space +colcon build +source install/setup.bash +ros2 launch lslidar_driver lslidar_launch.py +``` +open new terminal +ros2 topic pub -1 /lslidar_order std_msgs/msg/Int8 data:\ 1\ (open radar) +ros2 topic pub -1 /lslidar_order std_msgs/msg/Int8 data:\ 0\ (close radar) + + + + + +ros2 launch lslidar_driver lslidar_launch.py + +``` + +Note that this launch file launches both the driver, which is the only launch file needed to be used. + + +## FAQ + + +## Bug Report + +Prefer to open an issue. You can also send an E-mail to honghangli@lslidar.com + + + + +RERTION + + + diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/CMakeLists.txt b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/CMakeLists.txt new file mode 100644 index 0000000..85e13f6 --- /dev/null +++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/CMakeLists.txt @@ -0,0 +1,56 @@ +cmake_minimum_required(VERSION 3.5) +project(lslidar_driver) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +set(libpcap_LIBRARIES -lpcap) + +#set(FastRTPS_INCLUDE_DIR /opt/ros/foxy/include) +#set(FastRTPS_LIBRARY_RELEASE /opt/ros/foxy/lib/libfastrtps.so) + +#find_package(Boost REQUIRED COMPONENTS ) +find_package(Boost REQUIRED thread) +find_package(rclcpp REQUIRED) +find_package(PCL REQUIRED) +find_package(diagnostic_updater REQUIRED) +find_package(lslidar_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(ament_cmake REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclpy REQUIRED) +find_package(pcl_conversions REQUIRED) +find_package(sensor_msgs REQUIRED) +#find_package(PCL REQUIRED COMPONENTS common io) + +include_directories( + include + ${PCL_INCLUDE_DIRS} + ${PCL_COMMON_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} +) + +# Node +add_executable(lslidar_driver_node src/lslidar_driver_node.cc src/lslidar_driver.cc src/input.cc src/lsiosr.cpp) +target_link_libraries(lslidar_driver_node ${rclcpp_LIBRARIES} ${libpcap_LIBRARIES} ${Boost_LIBRARIES} Boost::thread) +ament_target_dependencies(lslidar_driver_node rclcpp std_msgs lslidar_msgs sensor_msgs diagnostic_updater pcl_conversions) + + +install(DIRECTORY launch params rviz + DESTINATION share/${PROJECT_NAME}) + +install(TARGETS + lslidar_driver_node + DESTINATION lib/${PROJECT_NAME} +) + +ament_export_dependencies(rclcpp pluginlib lslidar_msgs sensor_msgs pcl_conversions) +ament_export_include_directories(include ${PCL_COMMON_INCLUDE_DIRS}) + +ament_package() diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/include/lslidar_driver/input.h b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/include/lslidar_driver/input.h new file mode 100644 index 0000000..2715da4 --- /dev/null +++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/include/lslidar_driver/input.h @@ -0,0 +1,134 @@ +/* + * This file is part of lslidar_ch 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 . + * + * Input -- base class used to access the data independently of + * its source + * + * InputSocket -- derived class reads live data from the device + * via a UDP socket + * + * InputPCAP -- derived class provides a similar interface from a + * PCAP dump + */ + +#ifndef __LSLIDAR_INPUT_H_ +#define __LSLIDAR_INPUT_H_ + +#include +#include +#include +#include +#include "rclcpp/rclcpp.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace lslidar_driver +{ +static uint16_t MSOP_DATA_PORT_NUMBER = 2368; // lslidar default data port on PC +/** + * 从在线的网络数据或离线的网络抓包数据（pcap文件）中提取出lidar的原始数据，即packet数据包 + * @brief The Input class, + * + * @param private_nh 一个NodeHandled,用于通过节点传递参数 + * @param port + * @returns 0 if successful, + * -1 if end of file + * >0 if incomplete packet (is this possible?) + */ +class Input +{ +public: + Input(rclcpp::Node* private_nh, uint16_t port); + + virtual ~Input() + { + } + + virtual int getPacket(lslidar_msgs::msg::LslidarPacket::UniquePtr &packet) = 0; + + int getRpm(void); + int getReturnMode(void); + bool getUpdateFlag(void); + void clearUpdateFlag(void); + void UDP_order(const std_msgs::msg::Int8 msg); + void UDP_difop(); +protected: + rclcpp::Node* private_nh_; + uint16_t port_; + std::string devip_str_; + std::string lidar_name; + int cur_rpm_; + int return_mode_; + bool npkt_update_flag_; + bool add_multicast; + std::string group_ip; + int UDP_PORT_NUMBER_DIFOP; + int socket_id_difop; + int sockfd_; + std::string devip_str_difop; +}; + +/** @brief Live lslidar input from socket. */ +class InputSocket : public Input +{ +public: + InputSocket(rclcpp::Node* private_nh, uint16_t port = MSOP_DATA_PORT_NUMBER); + + virtual ~InputSocket(); + + virtual int getPacket(lslidar_msgs::msg::LslidarPacket::UniquePtr &packet); + +private: +private: + + in_addr devip_; + in_addr devip_difop; + //struct ip_mreq group; + +}; +class InputPCAP : public Input +{ +public: + InputPCAP(rclcpp::Node* private_nh,uint16_t port = MSOP_DATA_PORT_NUMBER, double packet_rate = 0.0, + std::string filename=""); + virtual ~InputPCAP(); + virtual int getPacket(lslidar_msgs::msg::LslidarPacket::UniquePtr &pkt); +private: + + rclcpp::Rate packet_rate_; + std::string filename_; + pcap_t *pcap_; + bpf_program pcap_packet_filter_; + char errbuf_[PCAP_ERRBUF_SIZE]; + bool empty_; + bool read_once_; + bool read_fast_; + double repeat_delay_; + }; +} + +#endif // __LSLIDAR_INPUT_H diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/include/lslidar_driver/lsiosr.h b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/include/lslidar_driver/lsiosr.h new file mode 100644 index 0000000..6109497 --- /dev/null +++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/include/lslidar_driver/lsiosr.h @@ -0,0 +1,88 @@ +/******************************************************* +@company: Copyright (C) 2021, Leishen Intelligent System +@product: LSM10_N10 +@filename: lsiosr.cpp +@brief: +@version: date: author: comments: +@v1.0 22-10-24 li new +*******************************************************/ +#ifndef LSIOSR_H +#define LSIOSR_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +//波特率 +#define BAUD_230400 230400 +#define BAUD_460800 460800 +#define BAUD_500000 500000 +#define BAUD_921600 921600 + +//奇偶校验位 +#define PARITY_ODD 'O' //奇数 +#define PARITY_EVEN 'E' //偶数 +#define PARITY_NONE 'N' //无奇偶校验位 + +//停止位 +#define STOP_BIT_1 1 +#define STOP_BIT_2 2 + +//数据位 +#define DATA_BIT_7 7 +#define DATA_BIT_8 8 + +namespace lslidar_driver +{ +class LSIOSR{ +public: + static LSIOSR* instance(std::string name, int speed, int fd = 0); + + ~LSIOSR(); + + /* 从串口中读取数据 */ + int read(unsigned char *buffer, int length, int timeout = 30); + + /* 向串口传数据 */ + int send(const char* buffer, int length, int timeout = 30); + + /* Empty serial port input buffer */ + void flushinput(); + + /* 串口初始化 */ + int init(); + + int close(); + + /* 获取串口号 */ + std::string getPort(); + + /* 设置串口号 */ + int setPortName(std::string name); + +private: + LSIOSR(std::string name, int speed, int fd); + + int waitWritable(int millis); + int waitReadable(int millis); + + /* 串口配置的函数 */ + int setOpt(int nBits, uint8_t nEvent, int nStop); + + std::string port_; + int baud_rate_; + + int fd_; +}; +} +#endif + diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/include/lslidar_driver/lslidar_driver.h b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/include/lslidar_driver/lslidar_driver.h new file mode 100644 index 0000000..34bb6d1 --- /dev/null +++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/include/lslidar_driver/lslidar_driver.h @@ -0,0 +1,159 @@ +/* + * 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 . + */ + +#ifndef LSLIDAR_DRIVER_H +#define LSLIDAR_DRIVER_H + +#include +#include +#include +#include + +#include +#include +#include +#include "rclcpp/rclcpp.hpp" +#include +#include "diagnostic_updater/diagnostic_updater.hpp" +#include "diagnostic_updater/publisher.hpp" +#include "lslidar_msgs/msg/lslidar_packet.hpp" +#include "std_msgs/msg/byte.hpp" + +#include "sensor_msgs/msg/point_cloud2.hpp" +#include "pcl_conversions/pcl_conversions.h" +#include "pcl/point_types.h" + +#include "time.h" +#include "input.h" +#include "lsiosr.h" +#include "sensor_msgs/msg/laser_scan.hpp" +namespace lslidar_driver { + +struct PointXYZIT { + PCL_ADD_POINT4D; + uint8_t intensity; + double timestamp; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW // make sure our new allocators are aligned +} EIGEN_ALIGN16; + +typedef struct { + double degree; + double range; + double intensity; +} ScanPoint; + +class LslidarDriver: public rclcpp::Node { +public: + LslidarDriver(); + LslidarDriver(const rclcpp::NodeOptions& options); + ~LslidarDriver(); + + bool initialize(); + bool polling(); + + typedef std::shared_ptr LslidarDriverPtr; + typedef std::shared_ptr LslidarDriverConstPtr; + +private: + uint64_t get_gps_stamp(struct tm t); + uint8_t N10_CalCRC8(unsigned char * p, int len); + bool loadParameters(); + bool createRosIO(); + void open_serial(); + void lidar_difop(); + void lidar_order(const std_msgs::msg::Int8::SharedPtr msg); + void data_processing(unsigned char *packet_bytes,int len); + void data_processing_2(unsigned char *packet_bytes,int len); + void difop_processing(unsigned char *packet_bytes); + void pubScanThread(); + void recvThread_crc(int &count,int &link_time); + int receive_data(unsigned char *packet_bytes); + int getScan(std::vector &points, rclcpp::Time &scan_time, float &scan_duration); + + boost::thread *pubscan_thread_ ; + boost::shared_ptr msop_input_; + boost::mutex mutex_; + boost::mutex pubscan_mutex_; + boost::condition_variable pubscan_cond_; + int UDP_PORT_NUMBER; + int count_num; + int package_points; + int data_bits_start; + int degree_bits_start; + int end_degree_bits_start; + int rpm_bits_start; + int baud_rate_; + int points_size_; + int idx = 0; + int link_time = 0; + int fixed_array_length;//cyy_add + + bool use_gps_ts; + bool is_start; + bool high_reflection; + bool compensation; + bool first_compensation = true; + bool pubScan; + bool pubPointCloud2; + + double min_range; + double max_range; + double angle_disable_min; + double angle_disable_max; + double angle_able_min; + double angle_able_max; + double last_degree = 0.0; + double degree_compensation = 0.0; + + uint16_t PACKET_SIZE ; + uint64_t sweep_end_time_gps; + uint64_t sweep_end_time_hardware; + uint64_t sub_second; + + std::string frame_id; + std::string interface_selection; + std::string scan_topic; + std::string lidar_name; + std::string serial_port_; + std::string dump_file; + std::string pointcloud_topic; + std::string in_file_name; + + tm pTime; + rclcpp::Time pre_time_; + rclcpp::Time time_; + std::vector scan_points_; + std::vector scan_points_bak_; + // Diagnostics updater + diagnostic_updater::Updater diagnostics; + std::shared_ptr diag_topic; + double diag_min_freq; + double diag_max_freq; + rclcpp::Publisher::SharedPtr scan_pub; + rclcpp::Publisher::SharedPtr point_cloud_pub; + rclcpp::Subscription::SharedPtr difop_switch; + LSIOSR * serial_; +}; +typedef PointXYZIT VPoint; +typedef pcl::PointCloud VPointCloud; + +} // namespace lslidar_driver +POINT_CLOUD_REGISTER_POINT_STRUCT(lslidar_driver::PointXYZIT, + (float, x, x)(float, y, y)(float, z, z)( + std::uint8_t, intensity, + intensity)(double, timestamp, timestamp)) +#endif // _LSLIDAR_DRIVER_H_ diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lslidar_double_launch.py b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lslidar_double_launch.py new file mode 100644 index 0000000..40ad793 --- /dev/null +++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lslidar_double_launch.py @@ -0,0 +1,50 @@ +#!/usr/bin/python3 +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import LifecycleNode +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument + +import lifecycle_msgs.msg +import os + +def generate_launch_description(): + + driver_dir_1 = os.path.join(get_package_share_directory('lslidar_driver_n10p'), 'params', 'lsx10_1.yaml') + driver_dir_2 = os.path.join(get_package_share_directory('lslidar_driver_n10p'), 'params', 'lsx10_2.yaml') + + driver_node_1 = LifecycleNode(package='lslidar_driver_n10p', + executable='lslidar_driver_node', + name='lslidar_driver_node', #设置激光数据topic名称 + output='screen', + emulate_tty=True, + namespace='lidar_1', + parameters=[driver_dir_1], + ) + + driver_node_2 = LifecycleNode(package='lslidar_driver_n10p', + executable='lslidar_driver_node', + name='lslidar_driver_node', #设置激光数据topic名称 + output='screen', + emulate_tty=True, + namespace='lidar_2', + parameters=[driver_dir_2], + ) + + rviz_dir = os.path.join(get_package_share_directory('lslidar_driver'), 'rviz', 'lslidar.rviz') + + rviz_node = Node( + package='rviz2', + namespace='', + executable='rviz2', + name='rviz2', + arguments=['-d', rviz_dir], + output='screen') + + return LaunchDescription([ + driver_node_1, + driver_node_2, + rviz_node, + ]) + diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsm10_net_launch.py b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsm10_net_launch.py new file mode 100644 index 0000000..7659d3f --- /dev/null +++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsm10_net_launch.py @@ -0,0 +1,27 @@ +#!/usr/bin/python3 +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import LifecycleNode +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument + +import lifecycle_msgs.msg +import os + +def generate_launch_description(): + + driver_dir = os.path.join(get_package_share_directory('lslidar_driver'), 'params', 'lidar_net_ros2','lsm10_net.yaml') + + driver_node = LifecycleNode(package='lslidar_driver', + executable='lslidar_driver_node', + name='lslidar_driver_node', + output='screen', + emulate_tty=True, + namespace='', + parameters=[driver_dir], + ) + return LaunchDescription([ + driver_node, + ]) + diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsm10_uart_launch.py b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsm10_uart_launch.py new file mode 100644 index 0000000..cd2fa16 --- /dev/null +++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsm10_uart_launch.py @@ -0,0 +1,27 @@ +#!/usr/bin/python3 +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import LifecycleNode +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument + +import lifecycle_msgs.msg +import os + +def generate_launch_description(): + + driver_dir = os.path.join(get_package_share_directory('lslidar_driver'), 'params', 'lidar_uart_ros2','lsm10.yaml') + + driver_node = LifecycleNode(package='lslidar_driver', + executable='lslidar_driver_node', + name='lslidar_driver_node', + output='screen', + emulate_tty=True, + namespace='', + parameters=[driver_dir], + ) + return LaunchDescription([ + driver_node, + ]) + diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsm10p_net_launch.py b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsm10p_net_launch.py new file mode 100644 index 0000000..e2468c8 --- /dev/null +++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsm10p_net_launch.py @@ -0,0 +1,27 @@ +#!/usr/bin/python3 +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import LifecycleNode +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument + +import lifecycle_msgs.msg +import os + +def generate_launch_description(): + + driver_dir = os.path.join(get_package_share_directory('lslidar_driver'), 'params', 'lidar_net_ros2','lsm10p_net.yaml') + + driver_node = LifecycleNode(package='lslidar_driver', + executable='lslidar_driver_node', + name='lslidar_driver_node', + output='screen', + emulate_tty=True, + namespace='', + parameters=[driver_dir], + ) + return LaunchDescription([ + driver_node, + ]) + diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsm10p_uart_launch.py b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsm10p_uart_launch.py new file mode 100644 index 0000000..10d1763 --- /dev/null +++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsm10p_uart_launch.py @@ -0,0 +1,27 @@ +#!/usr/bin/python3 +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import LifecycleNode +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument + +import lifecycle_msgs.msg +import os + +def generate_launch_description(): + + driver_dir = os.path.join(get_package_share_directory('lslidar_driver'), 'params', 'lidar_uart_ros2','lsm10_p.yaml') + + driver_node = LifecycleNode(package='lslidar_driver', + executable='lslidar_driver_node', + name='lslidar_driver_node', + output='screen', + emulate_tty=True, + namespace='', + parameters=[driver_dir], + ) + return LaunchDescription([ + driver_node, + ]) + diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsn10_launch.py b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsn10_launch.py new file mode 100644 index 0000000..ae9366e --- /dev/null +++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsn10_launch.py @@ -0,0 +1,28 @@ +#!/usr/bin/python3 +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import LifecycleNode +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument + +import lifecycle_msgs.msg +import os + +def generate_launch_description(): + + driver_dir = os.path.join(get_package_share_directory('lslidar_driver'), 'params','lidar_uart_ros2', 'lsn10.yaml') + + driver_node = LifecycleNode(package='lslidar_driver', + executable='lslidar_driver_node', + name='lslidar_driver_node', #设置激光数据topic名称 + output='screen', + emulate_tty=True, + namespace='', + parameters=[driver_dir], + ) + + return LaunchDescription([ + driver_node, + ]) + diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsn10_net_launch.py b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsn10_net_launch.py new file mode 100644 index 0000000..9a6a4d8 --- /dev/null +++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsn10_net_launch.py @@ -0,0 +1,28 @@ +#!/usr/bin/python3 +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import LifecycleNode +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument + +import lifecycle_msgs.msg +import os + +def generate_launch_description(): + + driver_dir = os.path.join(get_package_share_directory('lslidar_driver'), 'params','lidar_net_ros2', 'lsn10_net.yaml') + + driver_node = LifecycleNode(package='lslidar_driver', + executable='lslidar_driver_node', + name='lslidar_driver_node', #设置激光数据topic名称 + output='screen', + emulate_tty=True, + namespace='', + parameters=[driver_dir], + ) + + return LaunchDescription([ + driver_node, + ]) + diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsn10p_launch.py b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsn10p_launch.py new file mode 100644 index 0000000..758b03b --- /dev/null +++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsn10p_launch.py @@ -0,0 +1,28 @@ +#!/usr/bin/python3 +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import LifecycleNode +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument + +import lifecycle_msgs.msg +import os + +def generate_launch_description(): + + driver_dir = os.path.join(get_package_share_directory('lslidar_driver'), 'params','lidar_uart_ros2', 'lsn10p.yaml') + + driver_node = LifecycleNode(package='lslidar_driver', + executable='lslidar_driver_node', + name='lslidar_driver_node', #设置激光数据topic名称 + output='screen', + emulate_tty=True, + namespace='', + parameters=[driver_dir], + ) + + return LaunchDescription([ + driver_node, + ]) + diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsn10p_net_launch.py b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsn10p_net_launch.py new file mode 100644 index 0000000..966bda9 --- /dev/null +++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsn10p_net_launch.py @@ -0,0 +1,28 @@ +#!/usr/bin/python3 +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import LifecycleNode +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument + +import lifecycle_msgs.msg +import os + +def generate_launch_description(): + + driver_dir = os.path.join(get_package_share_directory('lslidar_driver'), 'params','lidar_net_ros2', 'lsn10p_net.yaml') + + driver_node = LifecycleNode(package='lslidar_driver', + executable='lslidar_driver_node', + name='lslidar_driver_node', #设置激光数据topic名称 + output='screen', + emulate_tty=True, + namespace='', + parameters=[driver_dir], + ) + + return LaunchDescription([ + driver_node, + ]) + diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/viewer_scan_launch.py b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/viewer_scan_launch.py new file mode 100644 index 0000000..c82e76d --- /dev/null +++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/viewer_scan_launch.py @@ -0,0 +1,26 @@ +#!/usr/bin/python3 +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import LifecycleNode +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument + +import lifecycle_msgs.msg +import os + +def generate_launch_description(): + + rviz2_config = os.path.join(get_package_share_directory('lslidar_driver'),'rviz','lslidar.rviz') + + rviz2_node = Node( + package='rviz2', + executable='rviz2', + name='rviz2', + arguments=['-d',rviz2_config], + output='screen') + + return LaunchDescription([ + rviz2_node, + ]) + diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/package.xml b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/package.xml new file mode 100644 index 0000000..2d88422 --- /dev/null +++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/package.xml @@ -0,0 +1,36 @@ + + + lslidar_driver + 1.2.0 + ROS device driver for Leishen lidar. + Nick Shu + Nick Shu + GNU General Public License V3.0 + + ament_cmake + + rclcpp + std_msgs + lslidar_msgs + pcl_conversions + rclpy + libpcap + libpcl-all-dev + pluginlib + sensor_msgs + + rclcpp + std_msgs + lslidar_msgs + pcl_conversions + rclpy + libpcap + libpcl-all + pluginlib + sensor_msgs + + diagnostic_updater + + ament_cmake + + diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_net_ros2/lsm10_net.yaml b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_net_ros2/lsm10_net.yaml new file mode 100644 index 0000000..6e91feb --- /dev/null +++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_net_ros2/lsm10_net.yaml @@ -0,0 +1,22 @@ +/lslidar_driver_node: + ros__parameters: + frame_id: laser #激光坐标 + group_ip: 224.1.1.2 + add_multicast: false + device_ip: 192.168.1.200 #雷达目的ip + device_ip_difop: 192.168.1.102 #雷达源IP + msop_port: 2368 #雷达目的端口号 + difop_port: 2369 #雷达源端口号 + lidar_name: M10 #雷达选择:M10 M10_P M10_PLUS M10_GPS N10 + ceil_increase: -1 #Lsm10*时改值应设置为-1 + angle_disable_min: 0.0 #单角度裁剪开始值 + angle_disable_max: 0.0 #单角度裁剪结束值 + truncated_mode_: 0 #多角度裁剪开关：值为0时表示不使用多角度裁剪，默认为0 + #值为1表示使用多角度裁剪，同时angle_disable_min与angle_disable_max设为 0，角度值在/lslidar_driver.cc中修改 + min_range: 0.0 #雷达接收距离最小值 + max_range: 200.0 #雷达接收距离最大值 + use_gps_ts: false #雷达是否使用GPS授时 + scan_topic: /scan #设置激光数据topic名称 + interface_selection: net #接口选择:net 为网口,serial 为串口。 + serial_port_: /dev/wheeltec_laser #串口连接时的串口号 +# pcap: /home/ls/work/2211/M10_P_gps.pcap #雷达是否使用pcap包读取功能 diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_net_ros2/lsm10p_net.yaml b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_net_ros2/lsm10p_net.yaml new file mode 100644 index 0000000..a8e30ce --- /dev/null +++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_net_ros2/lsm10p_net.yaml @@ -0,0 +1,22 @@ +/lslidar_driver_node: + ros__parameters: + frame_id: laser #激光坐标 + group_ip: 224.1.1.2 + add_multicast: false + device_ip: 192.168.1.200 #雷达目的ip + device_ip_difop: 192.168.1.102 #雷达源IP + msop_port: 2368 #雷达目的端口号 + difop_port: 2369 #雷达源端口号 + lidar_name: M10_P #雷达选择:M10 M10_P M10_PLUS M10_GPS N10 + ceil_increase: -1 #Lsm10*时改值应设置为-1 + angle_disable_min: 0.0 #单角度裁剪开始值 + angle_disable_max: 0.0 #单角度裁剪结束值 + truncated_mode_: 0 #多角度裁剪开关：值为0时表示不使用多角度裁剪，默认为0 + #值为1表示使用多角度裁剪，同时angle_disable_min与angle_disable_max设为 0，角度值在/lslidar_driver.cc中修改 + min_range: 0.0 #雷达接收距离最小值 + max_range: 200.0 #雷达接收距离最大值 + use_gps_ts: false #雷达是否使用GPS授时 + scan_topic: /scan #设置激光数据topic名称 + interface_selection: net #接口选择:net 为网口,serial 为串口。 + serial_port_: /dev/wheeltec_laser #串口连接时的串口号 +# pcap: /home/ls/work/2211/M10_P_gps.pcap #雷达是否使用pcap包读取功能 diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_net_ros2/lsn10_net.yaml b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_net_ros2/lsn10_net.yaml new file mode 100644 index 0000000..f211d48 --- /dev/null +++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_net_ros2/lsn10_net.yaml @@ -0,0 +1,25 @@ +/lslidar_driver_node: + ros__parameters: + frame_id: laser #激光坐标 + group_ip: 224.1.1.2 + add_multicast: false + device_ip: 192.168.1.200 #雷达源IP + device_ip_difop: 192.168.1.102 #雷达目的ip + msop_port: 2368 #雷达目的端口号 + difop_port: 2369 #雷达源端口号 + lidar_name: N10 #雷达选择:M10 M10_P M10_PLUS M10_GPS N10 L10 N10_P + angle_disable_min: 0.0 #角度裁剪开始值 + angle_disable_max: 0.0 #角度裁剪结束值 + min_range: 0.2 #雷达接收距离最小值 + max_range: 200.0 #雷达接收距离最大值 + use_gps_ts: false #雷达是否使用GPS授时 + scan_topic: /scan #设置激光数据topic名称 + interface_selection: net #接口选择:net 为网口,serial 为串口。 + serial_port_: /dev/wheeltec_laser #串口连接时的串口号 + high_reflection: false #M10_P雷达需填写该值,若不确定，请联系技术支持。 + compensation: false #M10系列是否使用角度补偿功能 + pubScan: true #是否发布scan话题 + pubPointCloud2: false #是否发布pointcloud2话题 + pointcloud_topic: /lslidar_point_cloud #设置激光数据topic名称 +# pcap: /home/ls/1.pcap #雷达是否使用pcap包读取功能 +# in_file_name: /home/ls/1.txt #雷达是否使用txt文件读取功能 \ No newline at end of file diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_net_ros2/lsn10p_net.yaml b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_net_ros2/lsn10p_net.yaml new file mode 100644 index 0000000..ffd83bb --- /dev/null +++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_net_ros2/lsn10p_net.yaml @@ -0,0 +1,25 @@ +/lslidar_driver_node: + ros__parameters: + frame_id: laser #激光坐标 + group_ip: 224.1.1.2 + add_multicast: false + device_ip: 192.168.1.200 #雷达源IP + device_ip_difop: 192.168.1.102 #雷达目的ip + msop_port: 2368 #雷达目的端口号 + difop_port: 2369 #雷达源端口号 + lidar_name: N10_P #雷达选择:M10 M10_P M10_PLUS M10_GPS N10 L10 N10_P + angle_disable_min: 120.0 #角度裁剪开始值 + angle_disable_max: 230.0 #角度裁剪结束值 + min_range: 0.2 #雷达接收距离最小值 + max_range: 200.0 #雷达接收距离最大值 + use_gps_ts: false #雷达是否使用GPS授时 + scan_topic: /scan #设置激光数据topic名称 + interface_selection: net #接口选择:net 为网口,serial 为串口。 + serial_port_: /dev/wheeltec_laser #串口连接时的串口号 + high_reflection: false #M10_P雷达需填写该值,若不确定，请联系技术支持。 + compensation: false #M10系列是否使用角度补偿功能 + pubScan: true #是否发布scan话题 + pubPointCloud2: false #是否发布pointcloud2话题 + pointcloud_topic: /lslidar_point_cloud #设置激光数据topic名称 +# pcap: /home/ls/1.pcap #雷达是否使用pcap包读取功能 +# in_file_name: /home/ls/1.txt #雷达是否使用txt文件读取功能 \ No newline at end of file diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_uart_ros2/lsm10.yaml b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_uart_ros2/lsm10.yaml new file mode 100644 index 0000000..203f6b5 --- /dev/null +++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_uart_ros2/lsm10.yaml @@ -0,0 +1,28 @@ +/lslidar_driver_node: + ros__parameters: + frame_id: laser #激光坐标 + group_ip: 224.1.1.2 + add_multicast: false + device_ip: 192.168.1.200 #雷达目的ip + device_ip_difop: 192.168.1.102 #雷达源IP + msop_port: 2368 #雷达目的端口号 + difop_port: 2369 #雷达源端口号 + lidar_name: M10 #雷达选择:M10 M10_P M10_PLUS M10_GPS N10 + ceil_increase: -1 #Lsm10*时改值应设置为-1 + angle_disable_min: 0.0 #单角度裁剪开始值 + angle_disable_max: 0.0 #单角度裁剪结束值 + truncated_mode_: 0 #多角度裁剪开关：值为0时表示不使用多角度裁剪，默认为0 + #值为1表示使用多角度裁剪，同时angle_disable_min与angle_disable_max设为 0，角度值在/lslidar_driver.cc中修改 + min_range: 0.0 #雷达接收距离最小值 + max_range: 200.0 #雷达接收距离最大值 + use_gps_ts: false #雷达是否使用GPS授时 + scan_topic: /scan #设置激光数据topic名称 + interface_selection: serial #接口选择:net 为网口,serial 为串口。 + serial_port_: /dev/wheeltec_laser #串口连接时的串口号 + high_reflection: false #M10_P雷达需填写该值,若不确定，请联系技术支持。 + compensation: false #M10系列是否使用角度补偿功能 + pubScan: true #是否发布scan话题 + pubPointCloud2: false #是否发布pointcloud2话题 + pointcloud_topic: /lslidar_point_cloud #设置激光数据topic名称 +# pcap: /home/ls/1.pcap #雷达是否使用pcap包读取功能 +# in_file_name: /home/ls/1.txt #雷达是否使用txt文件读取功能 \ No newline at end of file diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_uart_ros2/lsm10_p.yaml b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_uart_ros2/lsm10_p.yaml new file mode 100644 index 0000000..009e5fb --- /dev/null +++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_uart_ros2/lsm10_p.yaml @@ -0,0 +1,28 @@ +/lslidar_driver_node: + ros__parameters: + frame_id: laser #激光坐标 + group_ip: 224.1.1.2 + add_multicast: false + device_ip: 192.168.1.200 #雷达目的ip + device_ip_difop: 192.168.1.102 #雷达源IP + msop_port: 2368 #雷达目的端口号 + difop_port: 2369 #雷达源端口号 + lidar_name: M10_P #雷达选择:M10 M10_P M10_PLUS M10_GPS N10 + ceil_increase: -1 #Lsm10*时改值应设置为-1 + angle_disable_min: 0.0 #单角度裁剪开始值 + angle_disable_max: 0.0 #单角度裁剪结束值 + truncated_mode_: 0 #多角度裁剪开关：值为0时表示不使用多角度裁剪，默认为0 + #值为1表示使用多角度裁剪，同时angle_disable_min与angle_disable_max设为 0，角度值在/lslidar_driver.cc中修改 + min_range: 0.0 #雷达接收距离最小值 + max_range: 200.0 #雷达接收距离最大值 + use_gps_ts: false #雷达是否使用GPS授时 + scan_topic: /scan #设置激光数据topic名称 + interface_selection: serial #接口选择:net 为网口,serial 为串口。 + serial_port_: /dev/wheeltec_laser #串口连接时的串口号 + high_reflection: false #M10_P雷达需填写该值,若不确定，请联系技术支持。 + compensation: false #M10系列是否使用角度补偿功能 + pubScan: true #是否发布scan话题 + pubPointCloud2: false #是否发布pointcloud2话题 + pointcloud_topic: /lslidar_point_cloud #设置激光数据topic名称 +# pcap: /home/ls/1.pcap #雷达是否使用pcap包读取功能 +# in_file_name: /home/ls/1.txt #雷达是否使用txt文件读取功能 \ No newline at end of file diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_uart_ros2/lsn10.yaml b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_uart_ros2/lsn10.yaml new file mode 100644 index 0000000..ea0e78f --- /dev/null +++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_uart_ros2/lsn10.yaml @@ -0,0 +1,26 @@ +/lslidar_driver_node: + ros__parameters: + frame_id: laser_link #激光坐标 + group_ip: 224.1.1.2 + add_multicast: false + device_ip: 192.168.1.200 #雷达源IP + device_ip_difop: 192.168.1.102 #雷达目的ip + msop_port: 2368 #雷达目的端口号 + difop_port: 2369 #雷达源端口号 + lidar_name: N10 #雷达选择:M10 M10_P M10_PLUS M10_GPS N10 L10 N10_P + angle_disable_min: 0.0 #角度裁剪开始值 + angle_disable_max: 0.0 #角度裁剪结束值 + min_range: 0.15 #雷达接收距离最小值 + max_range: 200.0 #雷达接收距离最大值 + use_gps_ts: false #雷达是否使用GPS授时 + scan_topic: /scan #设置激光数据topic名称 + interface_selection: serial #接口选择:net 为网口,serial 为串口。 + serial_port_: /dev/ttyCH343USB0 #串口连接时的串口号 + high_reflection: false #M10_P雷达需填写该值,若不确定，请联系技术支持。 + compensation: false #M10系列是否使用角度补偿功能 + pubScan: true #是否发布scan话题 + pubPointCloud2: false #是否发布pointcloud2话题 + pointcloud_topic: /lslidar_point_cloud #设置激光数据topic名称 + fixed_array_length: 450 +# pcap: /home/ls/1.pcap #雷达是否使用pcap包读取功能 +# in_file_name: /home/ls/1.txt #雷达是否使用txt文件读取功能 \ No newline at end of file diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_uart_ros2/lsn10p.yaml b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_uart_ros2/lsn10p.yaml new file mode 100644 index 0000000..8f1c6c9 --- /dev/null +++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_uart_ros2/lsn10p.yaml @@ -0,0 +1,25 @@ +/lslidar_driver_node: + ros__parameters: + frame_id: laser #激光坐标 + group_ip: 224.1.1.2 + add_multicast: false + device_ip: 192.168.1.200 #雷达源IP + device_ip_difop: 192.168.1.102 #雷达目的ip + msop_port: 2368 #雷达目的端口号 + difop_port: 2369 #雷达源端口号 + lidar_name: N10_P #雷达选择:M10 M10_P M10_PLUS M10_GPS N10 L10 N10_P + angle_disable_min: 120.0 #角度裁剪开始值 + angle_disable_max: 230.0 #角度裁剪结束值 + min_range: 0.2 #雷达接收距离最小值 + max_range: 200.0 #雷达接收距离最大值 + use_gps_ts: false #雷达是否使用GPS授时 + scan_topic: /scan #设置激光数据topic名称 + interface_selection: serial #接口选择:net 为网口,serial 为串口。 + serial_port_: /dev/wheeltec_laser #串口连接时的串口号 + high_reflection: false #M10_P雷达需填写该值,若不确定，请联系技术支持。 + compensation: false #M10系列是否使用角度补偿功能 + pubScan: true #是否发布scan话题 + pubPointCloud2: false #是否发布pointcloud2话题 + pointcloud_topic: /lslidar_point_cloud #设置激光数据topic名称 +# pcap: /home/ls/1.pcap #雷达是否使用pcap包读取功能 +# in_file_name: /home/ls/1.txt #雷达是否使用txt文件读取功能 \ No newline at end of file diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/rviz/lslidar.rviz b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/rviz/lslidar.rviz new file mode 100644 index 0000000..4b104c7 --- /dev/null +++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/rviz/lslidar.rviz @@ -0,0 +1,161 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /LaserScan1 + Splitter Ratio: 0.3441176414489746 + Tree Height: 617 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + 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/LaserScan + 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: LaserScan + 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: scan + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: laser + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 3.635173797607422 + 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 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.8653978705406189 + Target Frame: + Value: Orbit (rviz) + Yaw: 3.095397710800171 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 846 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000156000002f4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002f4000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002f4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002f4000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000292000002f400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1283 + X: 406 + Y: 152 diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/input.cc b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/input.cc new file mode 100644 index 0000000..cfa40a8 --- /dev/null +++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/input.cc @@ -0,0 +1,398 @@ +#include "lslidar_driver/input.h" + +extern volatile sig_atomic_t flag; +namespace lslidar_driver +{ + static const size_t packet_size_input = 400; + //////////////////////////////////////////////////////////////////////// + // Input base class implementation + //////////////////////////////////////////////////////////////////////// + + /** @brief constructor + * + * @param private_nh ROS private handle for calling node. + * @param port UDP port number. + */ + Input::Input(rclcpp::Node *private_nh, uint16_t port) : private_nh_(private_nh), port_(port) { + npkt_update_flag_ = false; + cur_rpm_ = 0; + return_mode_ = 1; + devip_str_difop = std::string("192.168.1.200"); + devip_str_ = std::string("192.168.1.102"); + lidar_name = std::string("M10"); + add_multicast = false; + group_ip = std::string("224.1.1.2"); + UDP_PORT_NUMBER_DIFOP = 2369; + + + private_nh->declare_parameter("device_ip","192.168.1.102"); + private_nh->declare_parameter("device_ip_difop","192.168.1.200"); + private_nh->declare_parameter("add_multicast",false); + private_nh->declare_parameter("group_ip","224.1.1.2"); + private_nh->declare_parameter("difop_port",2369); + + + private_nh->get_parameter("lidar_name", lidar_name); + private_nh->get_parameter("device_ip", devip_str_); + private_nh->get_parameter("add_multicast", add_multicast); + private_nh->get_parameter("group_ip", group_ip); + private_nh->get_parameter("difop_port", UDP_PORT_NUMBER_DIFOP); + private_nh->get_parameter("device_ip_difop", devip_str_difop); + + if (!devip_str_.empty()) + RCLCPP_INFO(private_nh->get_logger(), "[driver][input] accepting packets from IP address: %s port: %d", + devip_str_.c_str(),port); + } + + /** @brief constructor + * + * @param private_nh ROS private handle for calling node. + * @param port UDP port number + */ + InputSocket::InputSocket(rclcpp::Node *private_nh, uint16_t port) : Input(private_nh, port) { + sockfd_ = -1; + + if (!devip_str_.empty()) { + inet_aton(devip_str_.c_str(), &devip_); + inet_aton(devip_str_difop.c_str(), &devip_difop); + } + + RCLCPP_INFO(private_nh_->get_logger(), "[driver][socket] Opening UDP socket: port %d", port); + sockfd_ = socket(PF_INET, SOCK_DGRAM, 0); + if (sockfd_ == -1) { + perror("socket"); // TODO: ROS_ERROR errno + return; + } + + int opt = 1; + if (setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (const void *) &opt, sizeof(opt))) { + perror("setsockopt error!\n"); + return; + } + + sockaddr_in my_addr; // my address information + memset(&my_addr, 0, sizeof(my_addr)); // initialize to zeros + my_addr.sin_family = AF_INET; // host byte order + my_addr.sin_port = htons(port); // port in network byte order + my_addr.sin_addr.s_addr = INADDR_ANY; // automatically fill in my IP + + if (bind(sockfd_, (sockaddr * ) & my_addr, sizeof(sockaddr)) == -1) { + perror("bind"); // TODO: ROS_ERROR errno + return; + } + + if (add_multicast) { + struct ip_mreq group; + group.imr_multiaddr.s_addr = inet_addr(group_ip.c_str()); + group.imr_interface.s_addr = htonl(INADDR_ANY); + + if (setsockopt(sockfd_, IPPROTO_IP, IP_ADD_MEMBERSHIP, (char *) &group, sizeof(group)) < 0) { + perror("Adding multicast group error "); + close(sockfd_); + exit(1); + } else + printf("Adding multicast group...OK.\n"); + } + if (fcntl(sockfd_, F_SETFL, O_NONBLOCK | FASYNC) < 0) { + perror("non-block"); + return; + } + } + + /** @brief destructor */ + InputSocket::~InputSocket(void) { + (void) close(sockfd_); + } + + void Input::UDP_difop() + { + sockaddr_in server_sai; + server_sai.sin_family = AF_INET; // IPV4 协议族 + server_sai.sin_port = htons(UDP_PORT_NUMBER_DIFOP); + server_sai.sin_addr.s_addr = inet_addr(devip_str_.c_str()); + 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 = sendto(sockfd_, data, 188, 0, (struct sockaddr *)&server_sai, sizeof(struct sockaddr)); + if (rtn < 0) printf("start scan error !\n"); + else return; + } + return; + } + + void Input::UDP_order(const std_msgs::msg::Int8 msg) + { + int i = msg.data; + sockaddr_in server_sai; + server_sai.sin_family = AF_INET; // IPV4 协议族 + server_sai.sin_port = htons(UDP_PORT_NUMBER_DIFOP); + server_sai.sin_addr.s_addr = inet_addr(devip_str_.c_str()); + int rtn = 0; + for (int k = 0; k < 10; k++) + { + 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"){ + if (i <= 1){ //雷达启停 + data[184] = 0x01; + data[185] = char(i); + } + else if (i == 2){ //雷达点云不滤波 + data[181] = 0x0A; + data[184] = 0x06; + data[185] = 0x01; + } + else if (i == 3){ //雷达点云正常滤波 + data[181] = 0x0B; + data[184] = 0x06; + data[185] = 0x01; + } + else if (i == 4){ //雷达近距离滤波 + data[181] = 0x0C; + data[184] = 0x06; + 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"){ + 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 = sendto(sockfd_, data, 188, 0, (struct sockaddr *)&server_sai, sizeof(struct sockaddr)); + if (rtn < 0) + { + printf("start scan error !\n"); + } + else + { + if (i == 1) + usleep(3000000); + return; + } + } + return; + } + + + + int InputSocket::getPacket(lslidar_msgs::msg::LslidarPacket::UniquePtr &packet) + { + int q = 0; + struct pollfd fds[1]; + fds[0].fd = sockfd_; + fds[0].events = POLLIN; + static const int POLL_TIMEOUT = 2000; // one second (in msec) + + sockaddr_in sender_address{}; + socklen_t sender_address_len = sizeof(sender_address); + while (flag == 1) + { + // poll() until input available + do { + int retval = poll(fds, 1, POLL_TIMEOUT); + if (retval < 0) // poll() error? + { + if (errno != EINTR) + RCLCPP_ERROR(private_nh_->get_logger(), "[driver][socket] poll() error: %s", strerror(errno)); + return 0; + } + if (retval == 0) // poll() timeout? + { + RCLCPP_WARN(private_nh_->get_logger(), "lslidar poll() timeout, port: %d",port_); + return 0; + } + if ((fds[0].revents & POLLERR) || (fds[0].revents & POLLHUP) || (fds[0].revents & POLLNVAL)) // device error? + { + RCLCPP_ERROR(private_nh_->get_logger(),"poll() reports lslidar error"); + return 0; + } + } while ((fds[0].revents & POLLIN) == 0); + + // Receive packets that should now be available from the + // socket using a blocking read. + ssize_t nbytes = recvfrom(sockfd_, &packet->data[0], packet_size_input, 0, + (sockaddr *)&sender_address, &sender_address_len); + // ROS_DEBUG_STREAM("incomplete lslidar packet read: " + // << nbytes << " bytes"); + q = (int)nbytes; + if (nbytes < 0) + { + if (errno != EWOULDBLOCK) + { + perror("recvfail"); + RCLCPP_ERROR(private_nh_->get_logger(),"recvfail"); + return 1; + } + } + else if ((size_t)nbytes <= packet_size_input || (size_t)nbytes >= 50) + { + + // read successful, + // if packet is not from the lidar scanner we selected by IP, + // continue otherwise we are done + if (devip_str_ != "" && sender_address.sin_addr.s_addr != devip_.s_addr) + continue; + else + break; // done + } + + } + if (flag == 0) + { + abort(); + } + + return q; + } + InputPCAP::InputPCAP(rclcpp::Node *private_nh, uint16_t port, double packet_rate, std::string filename) : Input(private_nh, port), + packet_rate_(packet_rate), + filename_(filename) + { + pcap_ = NULL; + empty_ = true; + read_once_ = false; + read_fast_ = false; + repeat_delay_ = 0.0; + private_nh->get_parameter("read_once", read_once_); + private_nh->get_parameter("read_fast", read_fast_); + private_nh->get_parameter("repeat_delay", repeat_delay_); + + if (read_once_) + RCLCPP_WARN(private_nh_->get_logger(),"Read input file only once."); + if (read_fast_) + RCLCPP_WARN(private_nh_->get_logger(),"Read input file as quickly as possible."); + if (repeat_delay_ > 0.0) + RCLCPP_WARN(private_nh_->get_logger(),"Delay %.3f seconds before repeating input file.", repeat_delay_); + + RCLCPP_INFO(private_nh_->get_logger(),"Opening PCAP file %s",filename_.c_str()); + if ((pcap_ = pcap_open_offline(filename_.c_str(), errbuf_)) == NULL) + { + RCLCPP_WARN(private_nh_->get_logger(),"Error opening lslidar socket dump file."); + return; + } + std::stringstream filter; + if (devip_str_ != "") + { + filter << "src host " << devip_str_ << "&&"; + } + filter << "udp dst port " << port; + pcap_compile(pcap_, &pcap_packet_filter_, filter.str().c_str(), 1, PCAP_NETMASK_UNKNOWN); + } + + InputPCAP::~InputPCAP(void) + { + pcap_close(pcap_); + } + + int InputPCAP::getPacket(lslidar_msgs::msg::LslidarPacket::UniquePtr &pkt) + { + struct pcap_pkthdr *header; + const u_char *pkt_data; + while (flag == 1) + { + int res; + if ((res = pcap_next_ex(pcap_, &header, &pkt_data)) >= 0) + { + // skip packets not for the correct port and from the selected IP address + if (!devip_str_.empty() && (0 == pcap_offline_filter(&pcap_packet_filter_, header, pkt_data))) + continue; + + if (read_fast_ == false) + packet_rate_.sleep(); + mempcpy(&pkt->data[0], pkt_data + 42, packet_size_input); + empty_ = false; + return 0; + } + if (empty_) + { + RCLCPP_WARN(private_nh_->get_logger(),"Error %d reading lslidar packet: %s", res, pcap_geterr(pcap_)); + return -1; + } + if (read_once_) + { + RCLCPP_WARN(private_nh_->get_logger(),"end of file reached -- done reading."); + return -1; + } + if (repeat_delay_ > 0.0) + { + RCLCPP_WARN(private_nh_->get_logger(),"end of file reached -- delaying %.3f seconds.", repeat_delay_); + usleep(rint(repeat_delay_ * 1000000.0)); + } + RCLCPP_WARN(private_nh_->get_logger(),"replayding lsliar dump file"); + + pcap_close(pcap_); + pcap_ = pcap_open_offline(filename_.c_str(), errbuf_); + empty_ = true; + } + if (flag == 0) + { + abort(); + } + return 0; + } + +} // namespace diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/lsiosr.cpp b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/lsiosr.cpp new file mode 100644 index 0000000..d560b12 --- /dev/null +++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/lsiosr.cpp @@ -0,0 +1,400 @@ +/******************************************************* +@company: Copyright (C) 2022, Leishen Intelligent System +@product: LSM10 and N10 +@filename: lsiosr.cpp +@brief: +@version: date: author: comments: +@v1.0 21-2-4 yao new +*******************************************************/ +#include "lslidar_driver/lsiosr.h" + +namespace lslidar_driver { + +LSIOSR * LSIOSR::instance(std::string name, int speed, int fd) +{ + static LSIOSR obj(name, speed, fd); + return &obj; +} + +LSIOSR::LSIOSR(std::string port, int baud_rate, int fd):port_(port), baud_rate_(baud_rate), fd_(fd) +{ + printf("port = %s, baud_rate = %d\n", port.c_str(), baud_rate); +} + +LSIOSR::~LSIOSR() +{ + close(); +} +/* 串口配置的函数 */ +int LSIOSR::setOpt(int nBits, uint8_t nEvent, int nStop) +{ + struct termios newtio, oldtio; + /*保存测试现有串口参数设置，在这里如果串口号等出错，会有相关的出错信息*/ + if (tcgetattr(fd_, &oldtio) != 0) + { + perror("SetupSerial 1"); + return -1; + } + bzero(&newtio, sizeof(newtio)); + /*步骤一，设置字符大小*/ + newtio.c_cflag |= CLOCAL; //如果设置，modem 的控制线将会被忽略。如果没有设置，则 open()函数会阻塞直到载波检测线宣告 modem 处于摘机状态为止。 + newtio.c_cflag |= CREAD; //使端口能读取输入的数据 + /*设置每个数据的位数*/ + switch (nBits) + { + case 7: + newtio.c_cflag |= CS7; + break; + case 8: + newtio.c_cflag |= CS8; + break; + } + /*设置奇偶校验位*/ + switch (nEvent) + { + case 'O': //奇数 + newtio.c_iflag |= (INPCK | ISTRIP); + newtio.c_cflag |= PARENB; //使能校验，如果不设PARODD则是偶校验 + newtio.c_cflag |= PARODD; //奇校验 + break; + case 'E': //偶数 + newtio.c_iflag |= (INPCK | ISTRIP); + newtio.c_cflag |= PARENB; + newtio.c_cflag &= ~PARODD; + break; + case 'N': //无奇偶校验位 + newtio.c_cflag &= ~PARENB; + break; + } + /*设置波特率*/ + switch (baud_rate_) + { + case 230400: + cfsetispeed(&newtio, B230400); + cfsetospeed(&newtio, B230400); + break; + case 460800: + cfsetispeed(&newtio, B460800); + cfsetospeed(&newtio, B460800); + break; + case 500000: + cfsetispeed(&newtio, B500000); + cfsetospeed(&newtio, B500000); + break; + case 921600: + cfsetispeed(&newtio, B921600); + cfsetospeed(&newtio, B921600); + break; + default: + cfsetispeed(&newtio, B460800); + cfsetospeed(&newtio, B460800); + break; + } + + /* + * 设置停止位 + * 设置停止位的位数， 如果设置，则会在每帧后产生两个停止位， 如果没有设置，则产生一个 + * 停止位。一般都是使用一位停止位。需要两位停止位的设备已过时了。 + * */ + if (nStop == 1) + newtio.c_cflag &= ~CSTOPB; + else if (nStop == 2) + newtio.c_cflag |= CSTOPB; + /*设置等待时间和最小接收字符*/ + newtio.c_cc[VTIME] = 0; + newtio.c_cc[VMIN] = 0; + /*处理未接收字符*/ + tcflush(fd_, TCIFLUSH); + /*激活新配置*/ + if ((tcsetattr(fd_, TCSANOW, &newtio)) != 0) + { + perror("serial set error"); + return -1; + } + + return 0; +} + +void LSIOSR::flushinput() { + tcflush(fd_, TCIFLUSH); +} + +/* 从串口中读取数据 */ +int LSIOSR::read(unsigned char *buffer, int length, int timeout) +{ + memset(buffer, 0, length); + + int totalBytesRead = 0; + int rc; + int unlink = 0; + unsigned char* pb = buffer; + + if (timeout > 0) + { + rc = waitReadable(timeout); + if (rc <= 0) + { + return (rc == 0) ? 0 : -1; + } + + int retry = 3; + while (length > 0) + { + rc = ::read(fd_, pb, (size_t)length); + + if (rc > 0) + { + length -= rc; + pb += rc; + totalBytesRead += rc; + + if (length == 0) + { + break; + } + } + else if (rc < 0) + { + printf("error \n"); + retry--; + if (retry <= 0) + { + break; + } + } + unlink++; + rc = waitReadable(20); + if(unlink > 10) + return -1; + + if (rc <= 0) + { + break; + } + } + } + else + { + rc = ::read(fd_, pb, (size_t)length); + + if (rc > 0) + { + totalBytesRead += rc; + } + else if ((rc < 0) && (errno != EINTR) && (errno != EAGAIN)) + { + printf("read error\n"); + return -1; + } + } + + return totalBytesRead; +} + +int LSIOSR::waitReadable(int millis) +{ + if (fd_ < 0) + { + return -1; + } + int serial = fd_; + + fd_set fdset; + struct timeval tv; + int rc = 0; + + while (millis > 0) + { + if (millis < 5000) + { + tv.tv_usec = millis % 1000 * 1000; + tv.tv_sec = millis / 1000; + + millis = 0; + } + else + { + tv.tv_usec = 0; + tv.tv_sec = 5; + + millis -= 5000; + } + + FD_ZERO(&fdset); + FD_SET(serial, &fdset); + + rc = select(serial + 1, &fdset, NULL, NULL, &tv); + if (rc > 0) + { + rc = (FD_ISSET(serial, &fdset)) ? 1 : -1; + break; + } + else if (rc < 0) + { + rc = -1; + break; + } + } + + return rc; +} + + +int LSIOSR::waitWritable(int millis) +{ + if (fd_ < 0) + { + return -1; + } + int serial = fd_; + + fd_set fdset; + struct timeval tv; + int rc = 0; + + while (millis > 0) + { + if (millis < 5000) + { + tv.tv_usec = millis % 1000 * 1000; + tv.tv_sec = millis / 1000; + + millis = 0; + } + else + { + tv.tv_usec = 0; + tv.tv_sec = 5; + + millis -= 5000; + } + + FD_ZERO(&fdset); + FD_SET(serial, &fdset); + + rc = select(serial + 1, NULL, &fdset, NULL, &tv); + if (rc > 0) + { + rc = (FD_ISSET(serial, &fdset)) ? 1 : -1; + break; + } + else if (rc < 0) + { + rc = -1; + break; + } + } + + return rc; +} + +/* 向串口中发送数据 */ +int LSIOSR::send(const char* buffer, int length, int timeout) +{ + if (fd_ < 0) + { + return -1; + } + + if ((buffer == 0) || (length <= 0)) + { + return -1; + } + + int totalBytesWrite = 0; + int rc; + char* pb = (char*)buffer; + + + if (timeout > 0) + { + rc = waitWritable(timeout); + if (rc <= 0) + { + return (rc == 0) ? 0 : -1; + } + + int retry = 3; + while (length > 0) + { + rc = write(fd_, pb, (size_t)length); + if (rc > 0) + { + length -= rc; + pb += rc; + totalBytesWrite += rc; + + if (length == 0) + { + break; + } + } + else + { + retry--; + if (retry <= 0) + { + break; + } + } + + rc = waitWritable(50); + if (rc <= 0) + { + break; + } + } + } + else + { + rc = write(fd_, pb, (size_t)length); + if (rc > 0) + { + totalBytesWrite += rc; + } + else if ((rc < 0) && (errno != EINTR) && (errno != EAGAIN)) + { + return -1; + } + } + + return totalBytesWrite; +} + +int LSIOSR::init() +{ + int error_code = 0; + + fd_ = open(port_.c_str(), O_RDWR|O_NOCTTY|O_NDELAY); + if (0 < fd_) + { + error_code = 0; + setOpt(DATA_BIT_8, PARITY_NONE, STOP_BIT_1);//设置串口参数 + //printf("open_port %s OK !\n", port_.c_str()); + } + else + { + error_code = -1; + } + + return error_code; +} + +int LSIOSR::close() +{ + ::close(fd_); + return 0; +} + +std::string LSIOSR::getPort() +{ + return port_; +} + +int LSIOSR::setPortName(std::string name) +{ + port_ = name; + return 0; +} + +} 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 new file mode 100644 index 0000000..c22c0f7 --- /dev/null +++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/lslidar_driver.cc @@ -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/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/lslidar_driver_node.cc b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/lslidar_driver_node.cc new file mode 100644 index 0000000..6a89f99 --- /dev/null +++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/lslidar_driver_node.cc @@ -0,0 +1,35 @@ +/* + * 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 "rclcpp/rclcpp.hpp" +#include "lslidar_driver/lslidar_driver.h" + +using namespace lslidar_driver; +volatile sig_atomic_t flag = 1; + +int main(int argc, char* argv[]) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + + while (rclcpp::ok() && node->polling()) { + rclcpp::spin_some(node); + } + //rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/CMakeLists.txt b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/CMakeLists.txt new file mode 100644 index 0000000..e6a603e --- /dev/null +++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/CMakeLists.txt @@ -0,0 +1,39 @@ +cmake_minimum_required(VERSION 3.5) +project(lslidar_msgs) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(std_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +rosidl_generate_interfaces(lslidar_msgs + "msg/LslidarDifop.msg" + "msg/LslidarPacket.msg" + "msg/LslidarPoint.msg" + "msg/LslidarScan.msg" + "msg/LslidarSweep.msg" + DEPENDENCIES builtin_interfaces std_msgs + ) + +ament_package() diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/msg/LslidarDifop.msg b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/msg/LslidarDifop.msg new file mode 100644 index 0000000..f377c75 --- /dev/null +++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/msg/LslidarDifop.msg @@ -0,0 +1,2 @@ +int64 temperature +int64 rpm diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/msg/LslidarPacket.msg b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/msg/LslidarPacket.msg new file mode 100644 index 0000000..d77ee45 --- /dev/null +++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/msg/LslidarPacket.msg @@ -0,0 +1,5 @@ +# Raw Leishen LIDAR packet. + +builtin_interfaces/Time stamp # packet timestamp +uint8[2000] data # packet contents + diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/msg/LslidarPoint.msg b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/msg/LslidarPoint.msg new file mode 100644 index 0000000..3132167 --- /dev/null +++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/msg/LslidarPoint.msg @@ -0,0 +1,12 @@ +# Time when the point is captured +float32 time + +# Converted distance in the sensor frame +float64 x +float64 y +float64 z + +# Raw measurement from Leishen M10 +float64 azimuth +float64 distance +float64 intensity diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/msg/LslidarScan.msg b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/msg/LslidarScan.msg new file mode 100644 index 0000000..0a3891c --- /dev/null +++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/msg/LslidarScan.msg @@ -0,0 +1,6 @@ +# Altitude of all the points within this scan +float64 altitude + +# The valid points in this scan sorted by azimuth +# from 0 to 359.99 +LslidarPoint[] points diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/msg/LslidarSweep.msg b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/msg/LslidarSweep.msg new file mode 100644 index 0000000..9cfe4b7 --- /dev/null +++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/msg/LslidarSweep.msg @@ -0,0 +1,4 @@ +std_msgs/Header header + +# The 0th scan is at the bottom +LslidarScan[16] scans diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/package.xml b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/package.xml new file mode 100644 index 0000000..8001ad5 --- /dev/null +++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/package.xml @@ -0,0 +1,25 @@ + + + lslidar_msgs + 1.2.0 + ROS message definitions for Leishen LIDARs. + Nick Shu + Nick Shu + GNU General Public License V3.0 + + ament_cmake + rosidl_default_generators + + rosidl_default_runtime + builtin_interfaces + + std_msgs + ament_lint_auto + ament_lint_common + + rosidl_interface_packages + + + ament_cmake + + diff --git a/src/LSLIDAR_X_ROS2-20240228/src/version.txt b/src/LSLIDAR_X_ROS2-20240228/src/version.txt new file mode 100644 index 0000000..7a2b9d4 --- /dev/null +++ b/src/LSLIDAR_X_ROS2-20240228/src/version.txt @@ -0,0 +1,20 @@ +版本变更 +/*************************************************************** +初始版本: LSLIDAR_M10_N10_V2.5.0_221104_ROS2 +变更内容: + 1.实现M10/M10_P/M10_PLUS/N10/M10_GPS网口和串口传输数据生成点云功能 + 2.实现点云角度裁剪和距离过滤功能 + 3.可以通过lslidar_order话题控制雷达启停 + 4.支持读取pcap包 + +更改日期: 2022-11-04 +***************************************************************/ + +/*************************************************************** +初始版本: LSLIDAR_M10_N10_V2.5.0_221111_ROS2 +变更内容: + 1.针对M10和M10_GPS雷达出货后发现的点云问题进行驱动补救 + +更改日期: 2022-11-11 +***************************************************************/ + diff --git a/src/LSLIDAR_X_ROS2-20240228/src/wheeltec_udev.sh b/src/LSLIDAR_X_ROS2-20240228/src/wheeltec_udev.sh new file mode 100644 index 0000000..17aaa46 --- /dev/null +++ b/src/LSLIDAR_X_ROS2-20240228/src/wheeltec_udev.sh @@ -0,0 +1,33 @@ +#CP2102 串口号0002 设置别名为wheeltec_controller +echo 'KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60",ATTRS{serial}=="0002", MODE:="0777", GROUP:="dialout", SYMLINK+="wheeltec_controller"' >/etc/udev/rules.d/wheeltec_controller.rules +#CH9102，同时系统安装了对应驱动 串口号0002 设置别名为wheeltec_controller +echo 'KERNEL=="ttyCH343USB*", ATTRS{idVendor}=="1a86", ATTRS{idProduct}=="55d4",ATTRS{serial}=="0002", MODE:="0777", GROUP:="dialout", SYMLINK+="wheeltec_controller"' >/etc/udev/rules.d/wheeltec_controller2.rules +#CH9102，同时系统没有安装对应驱动 串口号0002 设置别名为wheeltec_controller +echo 'KERNEL=="ttyACM*", ATTRS{idVendor}=="1a86", ATTRS{idProduct}=="55d4",ATTRS{serial}=="0002", MODE:="0777", GROUP:="dialout", SYMLINK+="wheeltec_controller"' >/etc/udev/rules.d/wheeltec_controller3.rules + +#CP2102 串口号0001 设置别名为wheeltec_lidar +echo 'KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60",ATTRS{serial}=="0001", MODE:="0777", GROUP:="dialout", SYMLINK+="wheeltec_lidar"' >/etc/udev/rules.d/wheeltec_lidar.rules +#CH9102，同时系统安装了对应驱动 串口号0001 设置别名为wheeltec_lidar +echo 'KERNEL=="ttyCH343USB*", ATTRS{idVendor}=="1a86", ATTRS{idProduct}=="55d4",ATTRS{serial}=="54B8001974", MODE:="0777", GROUP:="dialout", SYMLINK+="wheeltec_lidar"' >/etc/udev/rules.d/wheeltec_lidar2.rules +#CH9102，同时系统没有安装对应驱动 串口号0001 设置别名为wheeltec_lidar +echo 'KERNEL=="ttyACM*", ATTRS{idVendor}=="1a86", ATTRS{idProduct}=="55d4",ATTRS{serial}=="0001", MODE:="0777", GROUP:="dialout", SYMLINK+="wheeltec_lidar"' >/etc/udev/rules.d/wheeltec_lidar3.rules + +#CP2102 串口号0003 设置别名为wheeltec_FDI_IMU_GNSS +echo 'KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60",ATTRS{serial}=="0003", MODE:="0777", GROUP:="dialout", SYMLINK+="wheeltec_FDI_IMU_GNSS"' >/etc/udev/rules.d/wheeltec_fdi_imu_gnss.rules +#CH9102，同时系统安装了对应驱动 串口号0003 设置别名为wheeltec_FDI_IMU_GNSS +echo 'KERNEL=="ttyCH343USB*", ATTRS{idVendor}=="1a86", ATTRS{idProduct}=="55d4",ATTRS{serial}=="0003", MODE:="0777", GROUP:="dialout", SYMLINK+="wheeltec_FDI_IMU_GNSS"' >/etc/udev/rules.d/wheeltec_fdi_imu_gnss2.rules +#CH9102，同时系统没有安装对应驱动 串口号0003 设置别名为wheeltec_FDI_IMU_GNSS +echo 'KERNEL=="ttyACM*", ATTRS{idVendor}=="1a86", ATTRS{idProduct}=="55d4",ATTRS{serial}=="0003", MODE:="0777", GROUP:="dialout", SYMLINK+="wheeltec_FDI_IMU_GNSS"' >/etc/udev/rules.d/wheeltec_fdi_imu_gnss3.rules + +echo 'SUBSYSTEM=="video4linux",ATTR{name}=="GENERAL WEBCAM",ATTR{index}=="0",MODE:="0777",SYMLINK+="RgbCam"' >>/etc/udev/rules.d/camera.rules +echo 'SUBSYSTEM=="video4linux",ATTR{name}=="GENERAL WEBCAM: GENERAL WEBCAM",ATTR{index}=="0",MODE:="0777",SYMLINK+="RgbCam"' >>/etc/udev/rules.d/camera.rules +echo 'SUBSYSTEM=="video4linux",ATTR{name}=="Astra Pro HD Camera: Astra Pro ",ATTR{index}=="0",MODE:="0777",SYMLINK+="Astra_Pro"' >>/etc/udev/rules.d/camera.rules +echo 'SUBSYSTEM=="video4linux",ATTR{name}=="USB 2.0 Camera: USB Camera",ATTR{index}=="0",MODE:="0777",SYMLINK+="Astra_Dabai"' >>/etc/udev/rules.d/camera.rules +echo 'SUBSYSTEM=="video4linux",ATTR{name}=="USB 2.0 Camera",ATTR{index}=="0",MODE:="0777",SYMLINK+="Astra_Gemini"' >>/etc/udev/rules.d/camera.rules +echo 'SUBSYSTEM=="video4linux",ATTR{name}=="Intel(R) RealSense(TM) Depth Ca",ATTR{index}=="0",MODE:="0777",SYMLINK+="realsense"' >>/etc/udev/rules.d/camera.rules + +service udev reload +sleep 2 +service udev restart + + diff --git a/src/LSLIDAR_X_ROS2-20240228/src/镭神Lsx雷达旋转角度.png b/src/LSLIDAR_X_ROS2-20240228/src/镭神Lsx雷达旋转角度.png new file mode 100644 index 0000000000000000000000000000000000000000..aa247855f25076075025218ad73d877a247427a8 GIT binary patch literal 6646 zcmdT}dpMM9*Qc6@?UZ2)L$VW*;}&7aIgwK;#}Q^IBr^`f@J;W&zVCbc zVX7^0oomck>E3#?75s7T3Qb12DbBCW&mzOlweyzunTA)J?1f<_)k~l{d7*-U?I(#- zo0BtZuic=pOvRrHJ)xB7Yn@aG2d$tYr`nOcRd+m1qEu=mDodEuoSq@+i#<(Uy29Ww za0W}q@rXsEJ>NQ0CD0J=rKBoOP$(V^da%+iRpPkvZIrSe%hNq#DJGOgq6k0dOazgr z@7`?Low0@8c)b3x#WAz|F=M5=D!Jv(^5`(l5e%`&IOE!BIfK(BX=Y6O2u-Wne1+W@ z_VHGypbLcA@e@Z8B$V8JenM>IU@j|-1T3lB9$Rzcs&V&lzZ8*4k)uhw_}R}>iWnv( zGYU63e(*@X?+3s8UdrjeZY}zpbDjk;(ir%MFOd$cqo{Acnlhiz9fP zC#ujJ4Ph&u_2nfD4wdo|#Z25Rm_B#=dMkRE%ITcnXQ=pu$0_4XgpX%K4i+ZfC5;Nx zVrHU)$>EKt$}bOzuK2HhkT40Fgp6>gu`4Mt+-247bQhKCKVag1d-01sCb zYyYR4#r`5X?ju^!5s@O)^O{Wuu=}0wJcive_7>#Ts(yC%8n~c2oxnQX>@Ewfm-%W8 zSA3KW_}~`zJ1YiEyvY~~$a?#Fe$r&9%3o)zIZ<>h*AIY(7RNY(J16m~TS-m8C=1iH zXRy!LuW_d5Dff{(VWQ2M6P>>i55IhYV9!(Xk!_Bm%{af#Ux@?FFA(c2$|EGtQN+04 zxwuv9u=cZT_P33CO0S`oG$>{{4cF-ILmz1 zF~#T;_wIUJ(C_8I@T(qzn1_3adv`o8{5lgCj8FLeaxh%Lnkvqze6Fzhu%6O>L02`W zKZ|H>_Pduy^BPN@h@s~pQtpAqz227%$wO)PWZu1gVUWS9h0rRGSKGqRF)@eoVq0=S z177bd7ZFg}eHoA7*9#ZnheK>92oXdkRphunJyyf?Hm!>s>{Y}vp#eVRg|_5?+L<$v z232nGDkkQv)2|b6#mD2a3!8rv1 zO)PFMK-Hkb>>Qj>olf;MUA6I3%PoH^J_w5jg>BS?pLhT$)oU>Gg~w~BGrcHe7_&}G zE-h`gm}nj2HQw#LbE#2{igC-ksHA49$0!xR&C=PEbN8W{QZSLbm;-jT5CZ_ot&y+BEeoQhi@`X9LqIq_iemL#Z2a% zh3A4$)Ijv;rU@N!LKG2rgnj`&-!Nl5x^Z^$hnOM%L^4E+%dH$_7aj!O(QoFWJ9j%oy%Q^5z1ieGHi{ZJ8gfis-{m?xx9pM!X zGtrg1C}KkfCPWW8u4Vepd&Lt`TN4g0oj&BGWYi;GchRiSoTcg0fAQ;;GTiU5)JvL$ z);epsRaZ6Rf03qKAX@(}(_gAgc{|!5V}`xTI8MJM(-pJO6HRQm2Ebn)YW9S?+O8yV zuUCd6OQ)rrlQpoY`Tig% zb6f-$*kY_Rjd8ET7WFRTy^@tC&! z>tR9c`SmzG*7;T9z!ZG)(_(G%w}tw&SUn^O-w^o3X0=*fav_M_?Q&G#4bAMJg* zAIVBb)gBu*sAK7-1)6ckax0bHqC|5H^n@Hh2)Z&jKu0aaovw1QE00aI@Ny}c_AU!{ z7tiu$*Sm0ED>=QtP3%;~MlXaE5!-D#44vNHR#fE7UnR3s67%zVps9Aipy6Q@tSt@W znE#;1ST}Tt*1jOIu6shdM?77Zp* zo|mENt4K%bj+9dv1De$^D=by~j(X@Iqi+6Znl7m8l;od;{-8mf>HrfFs(7;c;^15z zl!wJ+^r*kH)Rkyc!WTws$V*4z9j2OZKT}u#wCy(XbSkKqD+_Q`D>|^|#$c_(HtnvN6R5m~)M*9y+{Qx1d~}ml7DhacW^}jEz6< zWUY5{`w<%%&ZTIS70W50oVn&Fc?{sN>zPJ+X@$!ar93rU@4%h0`PHGVl?4?aCo7ec z+*UR!3Gr4o$MtTexlWsMkrJ`%7hqs%zvJ@1P;cLl{3S>PTiY>Lwre=Adg#3CZ838r zUX7)4Paft-_}7;UXff`YZXhrqx&Pd5*nqu$89`VfDT3>tY<^DMF$_xpeWLH*Yz&<48N|Bx;R1?^uPa2$0yEqL-|FySwi z|6`HbzP+)+nB7`m)*+*K+|`E^13|#pSl%rYHc?sG0+qD6y}3$likyN>lF_^0R_FRH zA`>c~9Xl)wZaO2g%~kfCDs1sYT>_$UG*|#gbaQEfMC;19iOa1IzQbWQO-AnfE4kZ9 zCCVwXc=y_`>+WzP#m_CR3KCzI5_qhuB@feXz8uWW$FC z)7yOC@tusAsFj?mvmGvVDC7pmes^penIk*5$~`m~KQTJ7><(w#zmp_ggKt`Y&OC6p zi@N6pGUP>g;JAZ*m2D@n+|^1UrE2*}wG`f7MfE`Pa+a zzNYzvq1E5F`e3h*Wm{in8FE~S?2+`F%CW;+ed>_Dc2yIOpNGu<>LYI`l8qlsIOyzp z^*@D=MgrpAq8`cXM1qe_rt^*Dn<1=tzohjB*_uz>STY$p2CQ<|Ncr+#N`c{P|F)t!HGVFwIM+`pLq7_X7a=apmCiASO#Aet0Q8O3 z!PV#m&|KbKO!)TZ{AM#77vp4ON$Py+uw_^sE^%fLzb^rBm;yy^g zZR81!(Swi$gq4-(roJ*`Bg_!pPo3-n#4!o;oOpJIV}8bKV)9`uf}Nn{MJ_oUq>qh} zduLfhDe?F1j0Mh}&Pz7O+mf487V>Ud7n_5XQX$bNr1b@btdR&a@wORnHRv9Chomit zYnoE&;ueKHkbk?g_#_XMY#uFoUjKlQ%{8v+e&md78T24sP|8*e@rhh%?-qsA$iHh{ zqTTkTNGVz~M?afIxEq+n{n%7#;TCluNB>Z&?K$p8Bi?7FGMGO7mQ>dm`-Q((WXZI9 z8O%yN6Tz1doo##^KurDt7d%l^YM2ixV&4JhJ#>HxodE_7QFox=G)!3j!?aU2{3^OL ze;`rslr&ZRo_cpF9@)c{0yOEK%8AJvA>fgN5^GR|S}sW4EyGq~E1PpfBm#ywBXM87 zyBLjp%asJI>sme$as)C4$mUSwT})!$VF)bM5g0T;nxi84xAkqeJ5~M}I$02{e?(SC zO~ZeHUjD?rKVma`_`J=a4iA9~pD4ZTqK5T=Z`ceTYeTe#zc0NUS;m(IZ2wt4M^^!U zM}u`l4i2N5r{&x$UaHP5vB&3B#Q_z&Vx!&RhZ{Q1Da%NQrFsDS%XKKdqedc19Z#a`ScK3YFz#8nXTwYa zn;}PUX~fv;UGqI*`oeof5m84DKj+h-nr|F&4+N=-H$#qHYAmdp9&`d3QNJVW2TI?O zwR-&i9Eu;YQwvKOO6Ebs4gvZDxy?TC3fmRqQM5AMr13jAXB{yeD~bOp3{4If^XV|i zKMl_YMN}CV#*tkbQB~7)ClykU_{)om)Q3~rKBS8Sdvo4a6FaMAD0P!fJ)>1J?AN~% zNvr`M*{%9M8g;Z%hW*Co`d!vn{sOT(mpU5iHK1_5CxB=j<;9|KA!n^YpR&IC{k;1Sh9ok5fA$D^&gYZDA+|h1U$u^A#arlb8|!pmXz+cSDycEf5~lV@ z1)3UjvHp3pEZqg~{PgqBOAgoVV(|w_`GSy&GO6BE~Wrf8sp@fScGLzoZgQkw> zW%q={({Ht3xR4oNMGu-ho|g|geTXhdzVIRAdM2G=cHt(WjKF$jaY2TS@}o0MFT5hN zg!#;S`bXy7swMd&IWNlwyg6!!39(pWG5y_NCBQ|__gXQA76(g}L)JLe8q?R|Kb@yq z{qb?JV-T;owM~L$Sj zdG+UurM5*Z$ES?X}HnPh4Lr+C5;ueQF6~u+sj$yNV=lM7BOkGhJ{g zGUp{M%yL$3uk7tttteLIw}g>x>ELzrE9`pW>YwlZI}~fu@m{G>tuC!Q)Lfk!9m*%h ztkwvvL#yKR=03yNUkZaGKF1%gXj;NDB8)L8jY*@zJ_t6K!O7p)5aw-c7jjePq(=sd z;Xk*|j(}G5D?UxEj{3pPK0Hoa$D2l}i1i<CNf5S2$A!OWez~lix#(oxfM))>PhFutux@ z=-(-_g5tYccOZ0r;28l&ny>nr=`Zfn{$3}i;~&Z%u9Q!Wg+vJ2cZ=!8BrUI@e9&Og zzn(9$xRD>*dSc78FFI9`?AZvsF~%%!X3SQRt`l@H@+QFD*PgdmxXQ<`&U|5_KaqI# zB|f{&4qNoNEw44Mw!(Z4AqDrMNTWt`%j^2SrEln7DKt1tGfB0TA#O6MvqR#D2@ELz kyk%8A7UB4N|Q#f1Q^osamyKdTXNuywYnwz`b@FT^_5k^lez literal 0 HcmV?d00001 diff --git a/src/LSLIDAR_X_ROS2-20240228/wheeltec_lidar.launch.py b/src/LSLIDAR_X_ROS2-20240228/wheeltec_lidar.launch.py new file mode 100644 index 0000000..f38b80a --- /dev/null +++ b/src/LSLIDAR_X_ROS2-20240228/wheeltec_lidar.launch.py @@ -0,0 +1,62 @@ +import os +from pathlib import Path +import launch_ros.actions +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import (DeclareLaunchArgument, GroupAction, + IncludeLaunchDescription, SetEnvironmentVariable) +from launch.launch_description_sources import PythonLaunchDescriptionSource + +def generate_launch_description(): + Lslidar_dir = get_package_share_directory('lslidar_driver') + Lslidar_launch_dir = os.path.join(Lslidar_dir, 'launch') + + + Ld14_dir = get_package_share_directory('ldlidar_sl_ros2') + Ld14_launch_dir = os.path.join(Ld14_dir, 'launch') + + Ld06_dir = get_package_share_directory('ldlidar_stl_ros2') + Ld06_launch_dir = os.path.join(Ld14_dir, 'launch') + + + Lsm10_m10_uart = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(Lslidar_launch_dir, 'lsm10_uart_launch.py')),) + + Lsm10_m10_net = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(Lslidar_launch_dir, 'lsm10_net_launch.py')),) + + Lsm10_m10p_uart = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(Lslidar_launch_dir, 'lsm10p_uart_launch.py')),) + + Lsm10_m10p_net = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(Lslidar_launch_dir, 'lsm10p_net_launch.py')),) + + Lsn10 = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(Lslidar_launch_dir, 'lsn10_launch.py')),) + Lsn10p = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(Lslidar_launch_dir, 'lsn10p_launch.py')),) + + Lsn10_net = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(Lslidar_launch_dir, 'lsn10_net_launch.py')),) + Lsn10p_net = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(Lslidar_launch_dir, 'lsn10p_net_launch.py')),) + + Ld14 = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(Ld14_launch_dir, 'ld14.launch.py')),) + Ld06 = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(Ld06_launch_dir, 'ld06.launch.py')),) + + # Create the launch description and populate + ld = LaunchDescription() + ''' + Please select your lidar here, options include: + Lsm10_m10p_uart、Lsm10_m10p_net、Lsm10_m10_uart、Lsm10_m10_net、Lsn10、Lsn10p,Lsn10_net、Lsn10p_net,ld14、Ld06. + 1.If you are using LS* lidar (including lsn10, lsm10*), please don't forget to + modify the tf conversion parameters of robot_mode_description.launch.py + according to the user guide file. + 2.If you are using m10 lidar, please pay attention to distinguish whether it is m10p or not. + ''' + ld.add_action(Lsn10) + + return ld + diff --git a/src/LSLIDAR_X_ROS2-20240228/wheeltec_lidar.launch.py仅在WHEELTEC镜像中使用 b/src/LSLIDAR_X_ROS2-20240228/wheeltec_lidar.launch.py仅在WHEELTEC镜像中使用 new file mode 100644 index 0000000..e69de29 diff --git a/src/gc_navigation2_real/CMakeLists.txt b/src/gc_navigation2_real/CMakeLists.txt new file mode 100644 index 0000000..8a939d4 --- /dev/null +++ b/src/gc_navigation2_real/CMakeLists.txt @@ -0,0 +1,23 @@ +cmake_minimum_required(VERSION 3.8) +project(gc_navigation2_real) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) + +install( + DIRECTORY launch config params maps behavior_tree + DESTINATION share/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/src/gc_navigation2_real/behavior_tree/nav_to_pose_ackermann.xml b/src/gc_navigation2_real/behavior_tree/nav_to_pose_ackermann.xml new file mode 100644 index 0000000..c3343f7 --- /dev/null +++ b/src/gc_navigation2_real/behavior_tree/nav_to_pose_ackermann.xml @@ -0,0 +1,37 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/gc_navigation2_real/config/slam_toolbox_async_real.yaml b/src/gc_navigation2_real/config/slam_toolbox_async_real.yaml new file mode 100644 index 0000000..85b0e2a --- /dev/null +++ b/src/gc_navigation2_real/config/slam_toolbox_async_real.yaml @@ -0,0 +1,78 @@ +slam_toolbox: + ros__parameters: + + # ============================ Solver 插件参数 ====================================== + solver_plugin: solver_plugins::CeresSolver + ceres_linear_solver: SPARSE_NORMAL_CHOLESKY + ceres_preconditioner: SCHUR_JACOBI + ceres_trust_strategy: LEVENBERG_MARQUARDT + ceres_dogleg_type: TRADITIONAL_DOGLEG + ceres_loss_function: None + + # ============================ ROS 基础参数（实车）=================================== + odom_frame: odom_combined # 实车：EKF 融合后的里程计坐标系 + map_frame: map # 地图坐标系 + base_frame: base_footprint # 机器人基座坐标系（与 URDF 根帧一致） + scan_topic: /scan # 激光雷达话题（实车镭神雷达） + use_map_saver: true # 启用地图保存功能 + mode: mapping # mapping = 定位 + 实时建图 + + # ============================ 已有地图加载 ========================================== + # map_file_name: "" # 从零开始建图，留空 + map_start_pose: [0.0, 0.0, 0.0] # 初始位姿 [x, y, yaw] + + # ============================ 调试与性能 =========================================== + debug_logging: false + throttle_scans: 1 # 每 N 帧处理一次（实车每秒10帧左右） + transform_publish_period: 0.02 # TF 发布周期（秒） + map_update_interval: 3.0 # /map 话题更新间隔（秒） + resolution: 0.05 # 地图分辨率（米/像素） + restamp_tf: false + min_laser_range: 0.15 # 实车雷达盲区约 0.12m，设 0.15 过滤近处噪点 + max_laser_range: 12.0 # 实车雷达最大有效距离（镭神 N10 ≈ 10-15m） + minimum_time_interval: 0.3 # 最小处理时间间隔（实车可适当放宽） + transform_timeout: 0.2 # TF 查找超时 + tf_buffer_duration: 30.0 # TF 缓冲区时长 + stack_size_to_use: 40000000 # 栈大小（序列化大地图需要） + enable_interactive_mode: true # 交互模式：允许外部工具编辑地图 + + # ============================ 通用建图参数 ========================================= + use_scan_matching: true + use_scan_barycenter: true + minimum_travel_distance: 0.3 # 实车最小移动距离（米），小于仿真以更频繁更新 + minimum_travel_heading: 0.3 # 最小转向角度（弧度） + check_min_dist_and_heading_precisely: false + scan_buffer_size: 10 # 扫描缓冲区大小 + scan_buffer_maximum_scan_distance: 10.0 + link_match_minimum_response_fine: 0.1 + link_scan_maximum_distance: 1.5 + + # ============================ 回环检测参数 ========================================= + do_loop_closing: true # 启用回环检测 + loop_match_minimum_chain_size: 10 + loop_match_maximum_variance_coarse: 3.0 + loop_match_minimum_response_coarse: 0.35 + loop_match_minimum_response_fine: 0.45 + loop_search_maximum_distance: 3.0 + + # ============================ 相关性匹配参数 ======================================= + correlation_search_space_dimension: 0.5 + correlation_search_space_resolution: 0.01 + correlation_search_space_smear_deviation: 0.1 + + loop_search_space_dimension: 8.0 + loop_search_space_resolution: 0.05 + loop_search_space_smear_deviation: 0.03 + + # ============================ 扫描匹配器参数 ======================================== + distance_variance_penalty: 0.5 + angle_variance_penalty: 1.0 + + fine_search_angle_offset: 0.00349 + coarse_search_angle_offset: 0.349 + coarse_angle_resolution: 0.0349 + minimum_angle_penalty: 0.9 + minimum_distance_penalty: 0.5 + use_response_expansion: true + min_pass_through: 2 + occupancy_threshold: 0.1 diff --git a/src/gc_navigation2_real/config/slam_toolbox_localization_real.yaml b/src/gc_navigation2_real/config/slam_toolbox_localization_real.yaml new file mode 100644 index 0000000..7466496 --- /dev/null +++ b/src/gc_navigation2_real/config/slam_toolbox_localization_real.yaml @@ -0,0 +1,77 @@ +slam_toolbox: + ros__parameters: + + # ============================ Solver 插件参数 ====================================== + solver_plugin: solver_plugins::CeresSolver + ceres_linear_solver: SPARSE_NORMAL_CHOLESKY + ceres_preconditioner: SCHUR_JACOBI + ceres_trust_strategy: LEVENBERG_MARQUARDT + ceres_dogleg_type: TRADITIONAL_DOGLEG + ceres_loss_function: None + + # ============================ ROS 基础参数（实车定位模式）============================ + odom_frame: odom_combined # 实车：EKF 融合后的里程计坐标系 + map_frame: map + base_frame: base_footprint + scan_topic: /scan + mode: localization # 纯定位模式，不更新地图 + + # ============================ 已有地图加载 ========================================== + # 由 launch 文件通过 map_file_name 参数传入 .posegraph 路径 + # map_file_name: "" + map_start_pose: [0.0, 0.0, 0.0] + + # ============================ 调试与性能 =========================================== + debug_logging: false + throttle_scans: 2 # 每2帧处理一次（实车激光较慢时使用） + transform_publish_period: 0.02 + map_update_interval: 5.0 + resolution: 0.05 + restamp_tf: false + min_laser_range: 0.15 # 过滤雷达盲区噪点 + max_laser_range: 12.0 # 实车雷达最大有效距离 + minimum_time_interval: 0.3 + transform_timeout: 0.2 + tf_buffer_duration: 30.0 + stack_size_to_use: 40000000 + + # ============================ 通用参数 ============================================= + use_scan_matching: true + use_scan_barycenter: true + minimum_travel_distance: 0.3 + minimum_travel_heading: 0.3 + check_min_dist_and_heading_precisely: false + scan_buffer_size: 10 + scan_buffer_maximum_scan_distance: 10.0 + link_match_minimum_response_fine: 0.1 + link_scan_maximum_distance: 1.5 + + # 定位模式关闭回环检测 + do_loop_closing: false + loop_match_minimum_chain_size: 10 + loop_match_maximum_variance_coarse: 3.0 + loop_match_minimum_response_coarse: 0.35 + loop_match_minimum_response_fine: 0.45 + loop_search_maximum_distance: 3.0 + + # ============================ 相关性匹配参数 ======================================= + correlation_search_space_dimension: 0.5 + correlation_search_space_resolution: 0.01 + correlation_search_space_smear_deviation: 0.1 + + loop_search_space_dimension: 8.0 + loop_search_space_resolution: 0.05 + loop_search_space_smear_deviation: 0.03 + + # ============================ 扫描匹配器参数 ======================================== + distance_variance_penalty: 0.5 + angle_variance_penalty: 1.0 + + fine_search_angle_offset: 0.00349 + coarse_search_angle_offset: 0.349 + coarse_angle_resolution: 0.0349 + minimum_angle_penalty: 0.9 + minimum_distance_penalty: 0.5 + use_response_expansion: true + min_pass_through: 2 + occupancy_threshold: 0.1 diff --git a/src/gc_navigation2_real/launch/real_bringup.launch.py b/src/gc_navigation2_real/launch/real_bringup.launch.py new file mode 100644 index 0000000..096a166 --- /dev/null +++ b/src/gc_navigation2_real/launch/real_bringup.launch.py @@ -0,0 +1,54 @@ +#!/usr/bin/env python3 +# ============================================================================ +# real_bringup.launch.py +# 功能：实车基础 bringup — 底盘驱动 + 激光雷达 + TF 树 +# +# 适用场景： +# - 单独调试底盘通信和传感器数据 +# - 遥控测试（无导航） +# - 作为其他 launch 文件的基础模块 +# +# 启动内容： +# origincar_base bringup — 串口驱动 + EKF + IMU 滤波 + TF + robot_state_publisher +# lslidar_driver — 镭神激光雷达 → /scan +# +# 使用方式： +# ros2 launch gc_navigation2_real real_bringup.launch.py +# ============================================================================ + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument +from launch.launch_description_sources import PythonLaunchDescriptionSource + + +def generate_launch_description(): + """生成 LaunchDescription，启动实车底盘 + 雷达 + TF（无导航）""" + + # ============================ 1. 包路径 ========================================= + origincar_base_dir = get_package_share_directory('origincar_base') + + # ============================ 2. 实车底盘 bringup =============================== + # 注：origincar_bringup 已包含 robot_state_publisher + joint_state_publisher + 静态 TF + origincar_bringup = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [origincar_base_dir, '/launch', '/origincar_bringup.launch.py']), + ) + + # ============================ 3. 镭神激光雷达驱动 =============================== + lslidar_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [get_package_share_directory('lslidar_driver'), + '/launch', '/lsn10_launch.py']), + ) + + # ============================ 4. 组装 LaunchDescription ========================= + return LaunchDescription([ + DeclareLaunchArgument( + 'use_sim_time', + default_value='false', + description='使用仿真时间（实车必须为 false）'), + + origincar_bringup, + lslidar_launch, + ]) diff --git a/src/gc_navigation2_real/launch/real_nav2_slam.launch.py b/src/gc_navigation2_real/launch/real_nav2_slam.launch.py new file mode 100644 index 0000000..c9cf7ab --- /dev/null +++ b/src/gc_navigation2_real/launch/real_nav2_slam.launch.py @@ -0,0 +1,110 @@ +#!/usr/bin/env python3 +# ============================================================================ +# real_nav2_slam.launch.py +# 功能：实车导航 — 已知静态地图 + SLAM 定位 + Nav2 导航 +# +# 架构： +# origincar_base (bringup) — 底盘串口驱动 + EKF 里程计 + IMU 滤波 + TF +# lslidar_driver — 镭神激光雷达驱动 → /scan +# map_server — 加载 yaml+pgm 全量静态地图 → /map +# slam_toolbox (localization) — 加载 .posegraph，激光扫描匹配定位 +# 发布 map→odom_combined TF（替代 AMCL） +# ⚠️ /map 话题重映射为 /slam_map，避免冲突 +# navigation_launch — Nav2 导航栈 +# global_costmap 从 /map 获取全量静态地图 +# +# 使用方式： +# ros2 launch gc_navigation2_real real_nav2_slam.launch.py +# ============================================================================ + +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + """生成 LaunchDescription，启动实车底盘 + 雷达 + SLAM定位 + Nav2 导航""" + ld = LaunchDescription() + + # =============================1. 包路径=========================================== + pkg_dir = get_package_share_directory('gc_navigation2_real') + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + origincar_base_dir = get_package_share_directory('origincar_base') + + # =============================2. 参数============================================= + use_sim_time = LaunchConfiguration('use_sim_time', default='false') + map_yaml_path = LaunchConfiguration('map', default=os.path.join( + pkg_dir, 'maps', 'my_map.yaml')) + nav2_param_path = LaunchConfiguration('params_file', default=os.path.join( + pkg_dir, 'params', 'gc_navigation_slam_real.yaml')) + slam_params_file = os.path.join( + pkg_dir, 'config', 'slam_toolbox_localization_real.yaml') + posegraph_path = os.path.join( + pkg_dir, 'maps', 'my_map.posegraph') + + # =============================3. 实车底盘 bringup ================================ + origincar_bringup = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [origincar_base_dir, '/launch', '/origincar_bringup.launch.py']), + ) + + # =============================4. 镭神激光雷达驱动 ================================ + lslidar_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [get_package_share_directory('lslidar_driver'), + '/launch', '/lsn10_launch.py']), + ) + + # =============================5. map_server：全量静态地图 ========================== + map_server_node = Node( + package='nav2_map_server', + executable='map_server', + name='map_server', + output='screen', + parameters=[{'yaml_filename': map_yaml_path, 'use_sim_time': False}], + ) + map_server_lifecycle = Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_map', + output='screen', + parameters=[{'use_sim_time': False, 'autostart': True, + 'node_names': ['map_server']}], + ) + + # =============================6. slam_toolbox 定位模式（替代 AMCL）================== + # ⚠️ /map 重映射到 /slam_map，避免覆盖 map_server 的全量地图 + slam_toolbox_node = Node( + package='slam_toolbox', + executable='localization_slam_toolbox_node', + name='slam_toolbox', + output='screen', + parameters=[slam_params_file, + {'use_sim_time': False, + 'map_file_name': posegraph_path}], + remappings=[('/map', '/slam_map')], + ) + + # =============================7. Nav2 导航栈 ===================================== + navigation_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [nav2_bringup_dir, '/launch', '/navigation_launch.py']), + launch_arguments={ + 'use_sim_time': use_sim_time, + 'params_file': nav2_param_path, + }.items(), + ) + + # =============================8. 组装============================================= + ld.add_action(origincar_bringup) + ld.add_action(lslidar_launch) + ld.add_action(map_server_node) + ld.add_action(map_server_lifecycle) + ld.add_action(slam_toolbox_node) + ld.add_action(navigation_launch) + + return ld diff --git a/src/gc_navigation2_real/launch/real_nav2_slam_online.launch.py b/src/gc_navigation2_real/launch/real_nav2_slam_online.launch.py new file mode 100644 index 0000000..248d160 --- /dev/null +++ b/src/gc_navigation2_real/launch/real_nav2_slam_online.launch.py @@ -0,0 +1,135 @@ +#!/usr/bin/env python3 +# ============================================================================ +# real_nav2_slam_online.launch.py +# 功能：实车导航 — 已知全局地图 + SLAM 在线更新 + Nav2 导航 +# +# 架构： +# ┌─ /map（全量静态地图） +# map_server ──────────┤ +# └─ 来自 my_map.yaml + my_map.pgm +# +# ┌─ map→odom_combined TF（激光扫描匹配定位） +# slam_toolbox ────────┤ +# (async mapping) └─ /slam_map（实时建图结果，重映射避免冲突） +# +# navigation ┌─ global_costmap/static_layer ← /map（全量地图） +# ├─ local_costmap/obstacle_layer ← /scan（实时避障） +# └─ planner + controller + behavior +# +# 关键设计： +# 1. map_server 提供全量静态底图 → Foxglove 中看到完整房间布局 +# 2. slam_toolbox 定位 + 实时建图，/slam_map 可单独查看更新效果 +# 3. 实时避障通过 costmap 的 obstacle_layer 实现（不依赖地图更新） +# 4. 定期调用 /slam_toolbox/serialize_map 保存更新后的地图 +# +# 与仿真版的区别： +# - 无 Gazebo，无 spawn_entity +# - 使用 origincar_base bringup（串口驱动 + EKF + IMU） +# - 使用 lslidar_driver（镭神实车雷达） +# - use_sim_time: False +# - TF 帧适配实车（odom_combined, base_footprint） +# +# 使用方式： +# ros2 launch gc_navigation2_real real_nav2_slam_online.launch.py +# ============================================================================ + +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from launch.actions import ExecuteProcess + + +def generate_launch_description(): + """启动实车底盘 + 雷达 + 静态地图 + SLAM 在线定位建图 + Nav2 导航""" + ld = LaunchDescription() + + # =============================1. 包路径=========================================== + pkg_dir = get_package_share_directory('gc_navigation2_real') + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + origincar_base_dir = get_package_share_directory('origincar_base') + + # =============================2. 参数============================================= + use_sim_time = LaunchConfiguration('use_sim_time', default='false') + map_yaml_path = os.path.join(pkg_dir, 'maps', 'my_map.yaml') + nav2_param_path = LaunchConfiguration('params_file', default=os.path.join( + pkg_dir, 'params', 'gc_navigation_slam_real.yaml')) + slam_params_file = os.path.join(pkg_dir, 'config', 'slam_toolbox_async_real.yaml') + posegraph_path = os.path.join(pkg_dir, 'maps', 'my_map.posegraph') + + # =============================3. 实车底盘 bringup ================================ + origincar_bringup = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [origincar_base_dir, '/launch', '/origincar_bringup.launch.py']), + ) + + # =============================4. 镭神激光雷达驱动 ================================ + lslidar_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [get_package_share_directory('lslidar_driver'), + '/launch', '/lsn10_launch.py']), + ) + + # =============================5. map_server：全量静态地图 ========================== + map_server_node = Node( + package='nav2_map_server', + executable='map_server', + name='map_server', + output='screen', + parameters=[{'yaml_filename': map_yaml_path, 'use_sim_time': False}], + ) + map_server_lifecycle = Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_map', + output='screen', + parameters=[{'use_sim_time': False, 'autostart': True, + 'node_names': ['map_server']}], + ) + + # =============================6. slam_toolbox：定位 + 实时建图======================== + # ⚠️ /map 重映射到 /slam_map，避免覆盖 map_server 的全量地图 + slam_toolbox_node = Node( + package='slam_toolbox', + executable='async_slam_toolbox_node', + name='slam_toolbox', + output='screen', + parameters=[slam_params_file, {'use_sim_time': False}], + remappings=[('/map', '/slam_map')], + ) + + # =============================7. 加载已有序列化地图 ================================= + load_map_cmd = ExecuteProcess( + cmd=[ + 'bash', '-c', + 'sleep 8 && ' + 'ros2 service call /slam_toolbox/deserialize_map ' + 'slam_toolbox/srv/DeserializePoseGraph ' + "'{filename: \"%s\", match_type: 1}'" % posegraph_path, + ], + output='screen', + ) + + # =============================8. Nav2 导航栈 ===================================== + navigation_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [nav2_bringup_dir, '/launch', '/navigation_launch.py']), + launch_arguments={ + 'use_sim_time': use_sim_time, + 'params_file': nav2_param_path, + }.items(), + ) + + # =============================9. 组装============================================ + ld.add_action(origincar_bringup) + ld.add_action(lslidar_launch) + ld.add_action(map_server_node) + ld.add_action(map_server_lifecycle) + ld.add_action(slam_toolbox_node) + ld.add_action(load_map_cmd) + ld.add_action(navigation_launch) + + return ld diff --git a/src/gc_navigation2_real/launch/real_slam_mapping.launch.py b/src/gc_navigation2_real/launch/real_slam_mapping.launch.py new file mode 100644 index 0000000..f957e03 --- /dev/null +++ b/src/gc_navigation2_real/launch/real_slam_mapping.launch.py @@ -0,0 +1,103 @@ +#!/usr/bin/env python3 +# ============================================================================ +# real_slam_mapping.launch.py +# 功能：实车 SLAM 建图启动文件（无 Gazebo 仿真） +# +# 适用场景： +# - 在实际场地遥控机器人建立环境地图 +# - 在已有地图基础上增量更新 +# - 单独调试 slam_toolbox 建图参数 +# +# 架构： +# origincar_base (bringup) — 底盘串口驱动 + EKF 里程计 + IMU 滤波 + TF +# lslidar_driver — 镭神激光雷达驱动 → /scan +# async_slam_toolbox_node — 激光 SLAM 建图（mapping 模式） +# 发布 map→odom_combined TF +# 发布 /map 话题（实时地图） +# +# 使用方式： +# ros2 launch gc_navigation2_real real_slam_mapping.launch.py +# +# 保存地图： +# ros2 service call /slam_toolbox/serialize_map slam_toolbox/srv/SerializePoseGraph \ +# "{filename: '/home/guoch/test_ws/src/gc_navigation2_real/maps/my_map'}" +# ros2 run nav2_map_server map_saver_cli -f +# ============================================================================ + +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from launch.launch_description_sources import PythonLaunchDescriptionSource + + +def generate_launch_description(): + """生成 LaunchDescription，启动实车底盘 + 雷达 + slam_toolbox 建图""" + + # ============================ 1. 包路径 ========================================= + pkg_dir = get_package_share_directory('gc_navigation2_real') + origincar_base_dir = get_package_share_directory('origincar_base') + + # ============================ 2. 声明启动参数 ==================================== + use_sim_time = LaunchConfiguration('use_sim_time', default='false') + + # slam_params_file: slam_toolbox 配置文件 + slam_params_file = LaunchConfiguration( + 'slam_params_file', + default=os.path.join(pkg_dir, 'config', 'slam_toolbox_async_real.yaml')) + + # map_file_name: 可选，加载已有序列化地图继续建图 + map_file_name = LaunchConfiguration('map_file_name', default='') + + # ============================ 3. 实车底盘 bringup =============================== + # 启动 origincar_base：串口驱动 + EKF + IMU 滤波 + TF 发布 + # 注：origincar_bringup 已包含 robot_state_publisher + joint_state_publisher + 静态 TF + origincar_bringup = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [origincar_base_dir, '/launch', '/origincar_bringup.launch.py']), + ) + + # ============================ 4. 镭神激光雷达驱动 =============================== + # 启动 lslidar_driver → /scan 话题 + lslidar_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [get_package_share_directory('lslidar_driver'), + '/launch', '/lsn10_launch.py']), + ) + + # ============================ 5. slam_toolbox 建图节点 ============================ + slam_toolbox_node = Node( + package='slam_toolbox', + executable='async_slam_toolbox_node', + name='slam_toolbox', + output='screen', + parameters=[ + slam_params_file, + { + 'use_sim_time': False, + 'map_file_name': map_file_name, + }, + ], + ) + + # ============================ 6. 组装 LaunchDescription ========================= + return LaunchDescription([ + DeclareLaunchArgument( + 'use_sim_time', + default_value='false', + description='使用仿真时间（实车必须为 false）'), + DeclareLaunchArgument( + 'slam_params_file', + default_value=os.path.join(pkg_dir, 'config', 'slam_toolbox_async_real.yaml'), + description='slam_toolbox 参数配置文件路径'), + DeclareLaunchArgument( + 'map_file_name', + default_value='', + description='已有序列化地图路径（.posegraph），留空则从零开始建图'), + + origincar_bringup, + lslidar_launch, + slam_toolbox_node, + ]) diff --git a/src/gc_navigation2_real/package.xml b/src/gc_navigation2_real/package.xml new file mode 100644 index 0000000..bbc6199 --- /dev/null +++ b/src/gc_navigation2_real/package.xml @@ -0,0 +1,31 @@ + + + + gc_navigation2_real + 0.0.0 + Real-world navigation package: SLAM + Nav2 for OriginCar on physical robot (no Gazebo) + guoch + TODO: License declaration + + ament_cmake + + rclcpp + slam_toolbox + nav2_bringup + origincar_base + origincar_description + origincar_msg + robot_state_publisher + joint_state_publisher + robot_localization + imu_filter_madgwick + tf2_ros + lslidar_driver + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/gc_navigation2_real/params/gc_navigation_slam_real.yaml b/src/gc_navigation2_real/params/gc_navigation_slam_real.yaml new file mode 100644 index 0000000..56b8948 --- /dev/null +++ b/src/gc_navigation2_real/params/gc_navigation_slam_real.yaml @@ -0,0 +1,353 @@ +# ============================================================================ +# gc_navigation_slam_real.yaml +# 实车 Nav2 导航参数（SLAM 定位方案，无 AMCL） +# +# 与仿真版的区别： +# - use_sim_time: False（使用系统时钟） +# - TF 帧适配实车（odom_combined, base_footprint） +# - 速度/加速度参数适当降低以保证实车安全 +# - 激光雷达参数匹配镭神 N10 实车性能 +# ============================================================================ + +slam_toolbox: + ros__parameters: + use_sim_time: False + +bt_navigator: + ros__parameters: + use_sim_time: False + global_frame: map + robot_base_frame: base_footprint + odom_topic: /odom_combined + bt_loop_duration: 50 + default_server_timeout: 20 + # 阿克曼底盘专用 BT：移除 Spin，恢复行为 = 清代价地图 → 后退 → 等待 + default_nav_to_pose_bt_xml: /home/guoch/test_ws/install/gc_navigation2_real/share/gc_navigation2_real/behavior_tree/nav_to_pose_ackermann.xml + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + +bt_navigator_rclcpp_node: + ros__parameters: + use_sim_time: False + +controller_server: + ros__parameters: + use_sim_time: False + controller_frequency: 20.0 + FollowPath: + plugin: "nav2_mppi_controller::MPPIController" + time_steps: 36 + model_dt: 0.05 + batch_size: 1000 + vx_std: 0.2 + vy_std: 0.0 + wz_std: 0.4 + vx_max: 0.4 # 实车最大线速度降低（安全） + vx_min: -0.25 # 实车最大倒车速度降低 + vy_max: 0.0 + wz_max: 1.5 # 实车最大角速度降低 + iteration_count: 1 + temperature: 0.3 + gamma: 0.015 + motion_model: "Ackermann" + visualize: false + TrajectoryVisualizer: + trajectory_step: 5 + time_step: 3 + AckermannConstraints: + min_turning_r: 0.4 + critics: ["ConstraintCritic", "CostCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"] + ConstraintCritic: + enabled: true + cost_power: 1 + cost_weight: 4.0 + GoalCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + threshold_to_consider: 1.4 + GoalAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 3.0 + threshold_to_consider: 0.5 + PreferForwardCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + threshold_to_consider: 0.5 + CostCritic: + enabled: true + cost_power: 1 + cost_weight: 3.81 + critical_cost: 300.0 + consider_footprint: true + collision_cost: 1000000.0 + near_goal_distance: 1.0 + trajectory_point_step: 2 + PathAlignCritic: + enabled: true + cost_power: 1 + cost_weight: 14.0 + max_path_occupancy_ratio: 0.05 + trajectory_point_step: 4 + threshold_to_consider: 0.5 + offset_from_furthest: 20 + use_path_orientations: false + PathFollowCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + offset_from_furthest: 5 + threshold_to_consider: 1.4 + PathAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 2.0 + offset_from_furthest: 4 + threshold_to_consider: 0.5 + max_angle_to_furthest: 1.0 + forward_preference: true + +controller_server_rclcpp_node: + ros__parameters: + use_sim_time: False + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + transform_tolerance: 0.5 + global_frame: odom_combined # 实车：EKF 融合后的里程计坐标系 + robot_base_frame: base_footprint # 实车：URDF 根帧 + use_sim_time: False + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + footprint: "[[0.14, 0.085], + [0.14, -0.085], + [-0.14, -0.085], + [-0.14, 0.085]]" + footprint_padding: 0.02 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + map_subscribe_transient_local: True + always_send_full_costmap: True + local_costmap_client: + ros__parameters: + use_sim_time: False + local_costmap_rclcpp_node: + ros__parameters: + use_sim_time: False + +global_costmap: + global_costmap: + ros__parameters: + use_sim_time: False + transform_tolerance: 0.5 + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_footprint # 实车：与 URDF 根帧一致 + footprint: "[[0.14, 0.085], + [0.14, -0.085], + [-0.14, -0.085], + [-0.14, 0.085]]" + footprint_padding: 0.02 + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + global_costmap_client: + ros__parameters: + use_sim_time: False + global_costmap_rclcpp_node: + ros__parameters: + use_sim_time: False + +map_saver: + ros__parameters: + use_sim_time: False + save_map_timeout: 5.0 + free_thresh_default: 0.25 + occupied_thresh_default: 0.65 + map_subscribe_transient_local: True + +planner_server: + ros__parameters: + planner_plugins: ["GridBased"] + use_sim_time: False + + GridBased: + plugin: "nav2_smac_planner/SmacPlannerHybrid" + downsample_costmap: false + downsampling_factor: 1 + tolerance: 0.25 + allow_unknown: true + max_iterations: 1000000 + max_on_approach_iterations: 1000 + max_planning_time: 5.0 + motion_model_for_search: "REEDS_SHEPP" + angle_quantization_bins: 72 + analytic_expansion_ratio: 3.5 + analytic_expansion_max_length: 3.0 + minimum_turning_radius: 0.40 + reverse_penalty: 1.3 + change_penalty: 0.0 + non_straight_penalty: 1.2 + cost_penalty: 2.0 + retrospective_penalty: 0.015 + lookup_table_size: 20.0 + cache_obstacle_heuristic: false + viz_expansions: false + smooth_path: True + + smoother: + max_iterations: 1000 + w_smooth: 0.3 + w_data: 0.2 + tolerance: 1.0e-10 + do_refinement: true + refinement_num: 2 + +planner_server_rclcpp_node: + ros__parameters: + use_sim_time: False + +smoother_server: + ros__parameters: + use_sim_time: False + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "wait"] + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + backup_dist: 0.8 + backup_speed: 0.18 + wait: + plugin: "nav2_behaviors/Wait" + wait_duration: 0.5 + global_frame: odom_combined # 实车：EKF 融合后的里程计坐标系 + robot_base_frame: base_footprint # 实车：URDF 根帧 + transform_tolerance: 0.5 + use_sim_time: False + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: False + +waypoint_follower: + ros__parameters: + loop_rate: 20 + use_sim_time: False + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 diff --git a/src/origincar_base/CMakeLists.txt b/src/origincar_base/CMakeLists.txt new file mode 100644 index 0000000..e02128e --- /dev/null +++ b/src/origincar_base/CMakeLists.txt @@ -0,0 +1,89 @@ +cmake_minimum_required(VERSION 3.5) +project(origincar_base) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +#find_package(actionlib REQUIRED) +#find_package(actionlib_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) + +find_package(nav2_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclpy REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(std_srvs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(ackermann_msgs REQUIRED) # CHANGE +find_package(origincar_msg REQUIRED) # CHANGE +find_package(serial REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(origincar_description REQUIRED) +find_package(robot_localization REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/Position.msg" + ) +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +include_directories( + include + ${robot_localization_INCLUDE_DIRS} +) +set(origincar_base_node_SRCS + src/origincar_base.cpp + src/Quaternion_Solution.cpp +) + +add_executable(origincar_base_node src/origincar_base.cpp src/Quaternion_Solution.cpp) +ament_target_dependencies(origincar_base_node tf2_ros tf2 tf2_geometry_msgs rclcpp std_msgs geometry_msgs robot_localization nav_msgs std_srvs sensor_msgs ackermann_msgs serial origincar_msg origincar_description) + +#add_executable(testNode src/test.cpp src/Quaternion_Solution.cpp) +#ament_target_dependencies(testNode rclcpp std_msgs nav_msgs std_srvs sensor_msgs ackermann_msgs serial origincar_msg) + +install(PROGRAMS scripts/cmd_vel_to_ackermann_drive.py DESTINATION lib/${PROJECT_NAME}) + +install(TARGETS +origincar_base_node + +DESTINATION lib/${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install( + DIRECTORY launch config + DESTINATION share/${PROJECT_NAME} +) +ament_package() diff --git a/src/origincar_base/config/ekf.yaml b/src/origincar_base/config/ekf.yaml new file mode 100644 index 0000000..4c15af1 --- /dev/null +++ b/src/origincar_base/config/ekf.yaml @@ -0,0 +1,222 @@ +### ekf config file ### +ekf_filter_node: + ros__parameters: +# ͨóʼλ + use_pose_with_covariance_stamped: true # ͨ + pose0: /set_pose # ָ + pose0_config: [true, true, false, # ʹ x, y + false, false, true] # ʹ yaw + pose0_differential: false +# The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin +# computation until it receives at least one message from one of the inputs. It will then run continuously at the +# frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified. + frequency: 30.0 + +# The period, in seconds, after which we consider a sensor to have timed out. In this event, we carry out a predict +# cycle on the EKF without correcting it. This parameter can be thought of as the minimum frequency with which the +# filter will generate new output. Defaults to 1 / frequency if not specified. + sensor_timeout: 2.0 + +# ekf_localization_node and ukf_localization_node both use a 3D omnidirectional motion model. If this parameter is +# set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar +# environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected +# by, for example, an IMU. Defaults to false if unspecified. + two_d_mode: true + +# Use this parameter to provide an offset to the transform generated by ekf_localization_node. This can be used for +# future dating the transform, which is required for interaction with some other packages. Defaults to 0.0 if +# unspecified. + transform_time_offset: 0.0 + +# Use this parameter to provide specify how long the tf listener should wait for a transform to become available. +# Defaults to 0.0 if unspecified. + transform_timeout: 0.2 + +# If you're having trouble, try setting this to true, and then echo the /diagnostics_agg topic to see if the node is +# unhappy with any settings or data. + print_diagnostics: false + +# Debug settings. Not for the faint of heart. Outputs a ludicrous amount of information to the file specified by +# debug_out_file. I hope you like matrices! Please note that setting this to true will have strongly deleterious +# effects on the performance of the node. Defaults to false if unspecified. + debug: false + +# Defaults to "robot_localization_debug.txt" if unspecified. Please specify the full path. + debug_out_file: /path/to/debug/file.txt + +# Whether to broadcast the transformation over the /tf topic. Defaults to true if unspecified. + publish_tf: true + +# Whether to publish the acceleration state. Defaults to false if unspecified. + publish_acceleration: false + +# REP-105 (http://www.ros.org/reps/rep-0105.html) specifies four principal coordinate frames: base_link, odom, map, and +# earth. base_link is the coordinate frame that is affixed to the robot. Both odom and map are world-fixed frames. +# The robot's position in the odom frame will drift over time, but is accurate in the short term and should be +# continuous. The odom frame is therefore the best frame for executing local motion plans. The map frame, like the odom +# frame, is a world-fixed coordinate frame, and while it contains the most globally accurate position estimate for your +# robot, it is subject to discrete jumps, e.g., due to the fusion of GPS data or a correction from a map-based +# localization node. The earth frame is used to relate multiple map frames by giving them a common reference frame. +# ekf_localization_node and ukf_localization_node are not concerned with the earth frame. +# Here is how to use the following settings: +# 1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system. +# 1a. If your system does not have a map_frame, just remove it, and make sure "world_frame" is set to the value of +# odom_frame. +# 2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set +# "world_frame" to your odom_frame value. This is the default behavior for robot_localization's state estimation nodes. +# 3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates +# from landmark observations) then: +# 3a. Set your "world_frame" to your map_frame value +# 3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state +# estimation node from robot_localization! However, that instance should *not* fuse the global data. + map_frame: map # Defaults to "map" if unspecified + odom_frame: odom_combined # Defaults to "odom" if unspecified + base_link_frame: base_footprint # Defaults to "base_link" if unspecified + world_frame: odom_combined # Defaults to the value of odom_frame if unspecified + +# The filter accepts an arbitrary number of inputs from each input message type (nav_msgs/Odometry, +# geometry_msgs/PoseWithCovarianceStamped, geometry_msgs/TwistWithCovarianceStamped, +# sensor_msgs/Imu). To add an input, simply append the next number in the sequence to its "base" name, e.g., odom0, +# odom1, twist0, twist1, imu0, imu1, imu2, etc. The value should be the topic name. These parameters obviously have no +# default values, and must be specified. + odom0: odom +# Each sensor reading updates some or all of the filter's state. These options give you greater control over which +# values from each measurement are fed to the filter. For example, if you have an odometry message as input, but only +# want to use its Z position value, then set the entire vector to false, except for the third entry. The order of the +# values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Note that not some message types +# do not provide some of the state variables estimated by the filter. For example, a TwistWithCovarianceStamped message +# has no pose information, so the first six values would be meaningless in that case. Each vector defaults to all false +# if unspecified, effectively making this parameter required for each sensor. + odom0_config: [true, false, false, + false, false, false, + true, true, false, + false, false, true, + false, false, false] + +# If you have high-frequency data or are running with a low frequency parameter value, then you may want to increase +# the size of the subscription queue so that more measurements are fused. + odom0_queue_size: 10 + +# [ADVANCED] Large messages in ROS can exhibit strange behavior when they arrive at a high frequency. This is a result +# of Nagle's algorithm. This option tells the ROS subscriber to use the tcpNoDelay option, which disables Nagle's +# algorithm. + odom0_nodelay: false + +# [ADVANCED] When measuring one pose variable with two sensors, a situation can arise in which both sensors under- +# report their covariances. This can lead to the filter rapidly jumping back and forth between each measurement as they +# arrive. In these cases, it often makes sense to (a) correct the measurement covariances, or (b) if velocity is also +# measured by one of the sensors, let one sensor measure pose, and the other velocity. However, doing (a) or (b) isn't +# always feasible, and so we expose the differential parameter. When differential mode is enabled, all absolute pose +# data is converted to velocity data by differentiating the absolute pose measurements. These velocities are then +# integrated as usual. NOTE: this only applies to sensors that provide pose measurements; setting differential to true +# for twist measurements has no effect. + odom0_differential: true + +# [ADVANCED] When the node starts, if this parameter is true, then the first measurement is treated as a "zero point" +# for all future measurements. While you can achieve the same effect with the differential paremeter, the key +# difference is that the relative parameter doesn't cause the measurement to be converted to a velocity before +# integrating it. If you simply want your measurements to start at 0 for a given sensor, set this to true. + odom0_relative: false + +# [ADVANCED] If your data is subject to outliers, use these threshold settings, expressed as Mahalanobis distances, to +# control how far away from the current vehicle state a sensor measurement is permitted to be. Each defaults to +# numeric_limits::max() if unspecified. It is strongly recommended that these parameters be removed if not +# required. Data is specified at the level of pose and twist variables, rather than for each variable in isolation. +# For messages that have both pose and twist data, the parameter specifies to which part of the message we are applying +# the thresholds. +# odom0_pose_rejection_threshold: 5.0 +# odom0_twist_rejection_threshold: 1.0 + + imu0: /imu/data_raw + imu0_config: [false, false, false, + false, false, true, + false, false, false, + false, false, true, + false, false, false] + + imu0_nodelay: false + imu0_differential: false + imu0_relative: true + imu0_queue_size: 10 + imu0_pose_rejection_threshold: 20.0 # Note the difference in parameter names + imu0_twist_rejection_threshold: 1.542 # + imu0_linear_acceleration_rejection_threshold: 10.0 # + +# [ADVANCED] Some IMUs automatically remove acceleration due to gravity, and others don't. If yours doesn't, please set +# this to true, and *make sure* your data conforms to REP-103, specifically, that the data is in ENU frame. + imu0_remove_gravitational_acceleration: true + +# [ADVANCED] The EKF and UKF models follow a standard predict/correct cycle. During prediction, if there is no +# acceleration reference, the velocity at time t+1 is simply predicted to be the same as the velocity at time t. During +# correction, this predicted value is fused with the measured value to produce the new velocity estimate. This can be +# problematic, as the final velocity will effectively be a weighted average of the old velocity and the new one. When +# this velocity is the integrated into a new pose, the result can be sluggish covergence. This effect is especially +# noticeable with LIDAR data during rotations. To get around it, users can try inflating the process_noise_covariance +# for the velocity variable in question, or decrease the variance of the variable in question in the measurement +# itself. In addition, users can also take advantage of the control command being issued to the robot at the time we +# make the prediction. If control is used, it will get converted into an acceleration term, which will be used during +# predicition. Note that if an acceleration measurement for the variable in question is available from one of the +# inputs, the control term will be ignored. +# Whether or not we use the control input during predicition. Defaults to false. + use_control: false +# Whether the input (assumed to be cmd_vel) is a geometry_msgs/Twist or geometry_msgs/TwistStamped message. Defaults to +# false. + stamped_control: false +# The last issued control command will be used in prediction for this period. Defaults to 0.2. + control_timeout: 0.2 +# Which velocities are being controlled. Order is vx, vy, vz, vroll, vpitch, vyaw. + control_config: [true, false, false, false, false, true] +# Places limits on how large the acceleration term will be. Should match your robot's kinematics. + acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4] +# Acceleration and deceleration limits are not always the same for robots. + deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5] +# If your robot cannot instantaneously reach its acceleration limit, the permitted change can be controlled with these +# gains + acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9] +# If your robot cannot instantaneously reach its deceleration limit, the permitted change can be controlled with these +# gains + deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0] +# [ADVANCED] The process noise covariance matrix can be difficult to tune, and can vary for each application, so it is +# exposed as a configuration parameter. This matrix represents the noise we add to the total error after each +# prediction step. The better the omnidirectional motion model matches your system, the smaller these values can be. +# However, if users find that a given variable is slow to converge, one approach is to increase the +# process_noise_covariance diagonal value for the variable in question, which will cause the filter's predicted error +# to be larger, which will cause the filter to trust the incoming measurement more during correction. The values are +# ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below if +# unspecified. + process_noise_covariance: [0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.04, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.015] +# [ADVANCED] This represents the initial value for the state estimate error covariance matrix. Setting a diagonal +# value (variance) to a large value will result in rapid convergence for initial measurements of the variable in +# question. Users should take care not to use large values for variables that will not be measured directly. The values +# are ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below +#if unspecified. + initial_estimate_covariance: [1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9] + diff --git a/src/origincar_base/config/imu.yaml b/src/origincar_base/config/imu.yaml new file mode 100644 index 0000000..edbf7de --- /dev/null +++ b/src/origincar_base/config/imu.yaml @@ -0,0 +1,7 @@ +imu_filter_madgwick: + ros__parameters: + fixed_frame: "base_footprint" + use_mag: false + publish_tf: false + world_frame: "enu" + orientation_stddev: 0.05 diff --git a/src/origincar_base/include/origincar_base/Quaternion_Solution.h b/src/origincar_base/include/origincar_base/Quaternion_Solution.h new file mode 100644 index 0000000..b839cec --- /dev/null +++ b/src/origincar_base/include/origincar_base/Quaternion_Solution.h @@ -0,0 +1,10 @@ + +#ifndef __QUATERNION_SOLUTION_H_ +#define __QUATERNION_SOLUTION_H_ +#include "origincar_base.h" +float InvSqrt(float number); +void Quaternion_Solution(float gx, float gy, float gz, float ax, float ay, float az); +extern volatile float q0, q1, q2, q3; +#endif + + diff --git a/src/origincar_base/include/origincar_base/origincar_base.h b/src/origincar_base/include/origincar_base/origincar_base.h new file mode 100644 index 0000000..456ce18 --- /dev/null +++ b/src/origincar_base/include/origincar_base/origincar_base.h @@ -0,0 +1,228 @@ + +#ifndef _ORIGINCAR_BASE_H_ +#define _ORIGINCAR_BASE_H_ + +#include +#include +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/string.hpp" +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "tf2/LinearMath/Transform.h" +#include "tf2/LinearMath/Quaternion.h" +#include +#include "ackermann_msgs/msg/ackermann_drive_stamped.hpp" +#include "origincar_msg/msg/data.hpp" +#include "origincar_msg/msg/sign.hpp" // 匹配信号发送 +#include +#include +using namespace std; + + +#define SEND_DATA_CHECK 1 //Send data check flag bits //发送数据校验标志位 +#define READ_DATA_CHECK 0 //Receive data to check flag bits //接收数据校验标志位 +#define FRAME_HEADER 0X7B //Frame head //帧头 +#define FRAME_TAIL 0X7D //Frame tail //帧尾 +#define RECEIVE_DATA_SIZE 24 //The length of the data sent by the lower computer //下位机发送过来的数据的长度 +#define SEND_DATA_SIZE 11 //The length of data sent by ROS to the lower machine //ROS向下位机发送的数据的长度 +#define PI 3.1415926f //PI //圆周率 + +#define GYROSCOPE_RATIO 0.00026644f + +#define ACCEl_RATIO 1671.84f + +extern sensor_msgs::msg::Imu Mpu6050; + +const double odom_pose_covariance[36] = {1e-3, 0, 0, 0, 0, 0, + 0, 1e-3, 0, 0, 0, 0, + 0, 0, 1e6, 0, 0, 0, + 0, 0, 0, 1e6, 0, 0, + 0, 0, 0, 0, 1e6, 0, + 0, 0, 0, 0, 0, 1e3 }; + +const double odom_pose_covariance2[36] = {1e-9, 0, 0, 0, 0, 0, + 0, 1e-3, 1e-9, 0, 0, 0, + 0, 0, 1e6, 0, 0, 0, + 0, 0, 0, 1e6, 0, 0, + 0, 0, 0, 0, 1e6, 0, + 0, 0, 0, 0, 0, 1e-9 }; + +const double odom_twist_covariance[36] = {1e-3, 0, 0, 0, 0, 0, + 0, 1e-3, 0, 0, 0, 0, + 0, 0, 1e6, 0, 0, 0, + 0, 0, 0, 1e6, 0, 0, + 0, 0, 0, 0, 1e6, 0, + 0, 0, 0, 0, 0, 1e3 }; + +const double odom_twist_covariance2[36] = {1e-9, 0, 0, 0, 0, 0, + 0, 1e-3, 1e-9, 0, 0, 0, + 0, 0, 1e6, 0, 0, 0, + 0, 0, 0, 1e6, 0, 0, + 0, 0, 0, 0, 1e6, 0, + 0, 0, 0, 0, 0, 1e-9} ; + +typedef struct __Vel_Pos_Data_ +{ + float X; + float Y; + float Z; + +} Vel_Pos_Data; + +typedef struct __MPU6050_DATA_ +{ + short accele_x_data; + short accele_y_data; + short accele_z_data; + short gyros_x_data; + short gyros_y_data; + short gyros_z_data; + +} MPU6050_DATA; + +typedef struct _SEND_DATA_ +{ + uint8_t tx[SEND_DATA_SIZE]; + float X_speed; + float Y_speed; + float Z_speed; + unsigned char Frame_Tail; +} SEND_DATA; + +typedef struct _RECEIVE_DATA_ +{ + uint8_t rx[RECEIVE_DATA_SIZE]; + uint8_t Flag_Stop; + unsigned char Frame_Header; + float X_speed; + float Y_speed; + float Z_speed; + float Power_Voltage; + unsigned char Frame_Tail; +} RECEIVE_DATA; + +class origincar_base : public rclcpp::Node + +{ +public: + origincar_base(); + ~origincar_base(); + void Control(); + void Publish_Odom(); + +public : + serial::Serial Stm32_Serial; + +private: + void declare_parameters(); + void get_parameters(); + + + void Cmd_Vel_Callback(const geometry_msgs::msg::Twist::SharedPtr twist_aux); + void Akm_Cmd_Vel_Callback(const ackermann_msgs::msg::AckermannDriveStamped::SharedPtr akm_ctl); + + void Publish_ImuSensor(); + void Publish_Voltage(); + auto createQuaternionMsgFromYaw(double yaw); + + bool Get_Sensor_Data(); + unsigned char Check_Sum(unsigned char Count_Number,unsigned char mode); + short IMU_Trans(uint8_t Data_High,uint8_t Data_Low); + float Odom_Trans(uint8_t Data_High,uint8_t Data_Low); + + void Sign_Switch_Callback(const std_msgs::msg::Int32::SharedPtr sign_switch); + +private: + void reset_filter() + { + auto msg = std::make_shared(); + + // 设置头信息 + msg->header.stamp = this->now(); + msg->header.frame_id = "odom_combined"; + + // 重置位姿 + msg->pose.pose.position.x = 0.54; + msg->pose.pose.position.y = 0.2; + msg->pose.pose.orientation.w = 1.0; + + // 设置关键协方差 + std::array cov = {0}; + cov[0] = 0.5; // x + cov[7] = 0.5; // y + cov[35] = 0.5; // yaw + msg->pose.covariance = cov; + + // 发布重置指令 + pose_pub_->publish(*msg); + RCLCPP_WARN(this->get_logger(), "FILTER RESET TRIGGERED!"); + } + + rclcpp::Publisher::SharedPtr pose_pub_; + rclcpp::Time _Now, _Last_Time; + float Sampling_Time; + rclcpp::Subscription::SharedPtr Cmd_Vel_Sub; + rclcpp::Subscription::SharedPtr Akm_Cmd_Vel_Sub; + + rclcpp::Publisher::SharedPtr odom_publisher; + rclcpp::Publisher::SharedPtr voltage_publisher; + rclcpp::Publisher::SharedPtr imu_publisher; + + rclcpp::Publisher::SharedPtr test_publisher; + + rclcpp::Publisher::SharedPtr robotpose_publisher; + rclcpp::Publisher::SharedPtr robotvel_publisher; + // rclcpp::Publisher::SharedPtr tf_pub_; + + std::shared_ptr tf_bro; + rclcpp::Publisher::SharedPtr tf_pub_; + rclcpp::TimerBase::SharedPtr test_timer; + + rclcpp::TimerBase::SharedPtr odom_timer; + rclcpp::TimerBase::SharedPtr imu_timer; + rclcpp::TimerBase::SharedPtr voltage_timer; + + rclcpp::TimerBase::SharedPtr robotpose_timer; + rclcpp::TimerBase::SharedPtr robotvel_timer; + + std::unique_ptr tf_broadcaster_; + + rclcpp::Subscription::SharedPtr Sign_Switch_Sub; + + string usart_port_name, robot_frame_id, gyro_frame_id, odom_frame_id, akm_cmd_vel, test; + std::string cmd_vel; + int serial_baud_rate; + RECEIVE_DATA Receive_Data; + SEND_DATA Send_Data; + + Vel_Pos_Data Robot_Pos; + Vel_Pos_Data Robot_Vel; + MPU6050_DATA Mpu6050_Data; + float Power_voltage; + size_t count_; +}; + + +#endif //_ORIGINCAR_BASE_H_ diff --git a/src/origincar_base/launch/__pycache__/testtwo.launch.cpython-38.pyc b/src/origincar_base/launch/__pycache__/testtwo.launch.cpython-38.pyc new file mode 100644 index 0000000000000000000000000000000000000000..5150cceb7f83651085e6abd70f276ec18f86c64c GIT binary patch literal 580 zcmZWmO-lnY5KVU5U9oIc)RPyld+CB_5x*{ih#+3PEZfa=w#SNHqppu89jtVh5u+>0qJ{7J z;ZXgL!#>AV(aKE|-`H@fs+V33oQuPmkesE|CJaV};W~dRxB=T0x&~>Z)*Yo(x@)Gf zjd)?2vbHAbT0mr(LTf|s1h%1oirHog|sRWlMcQD&(0SQ{_j z^|}kujK-Cbg=61{Oz^-KK5w&HOd*@q09-@)8{|@`qfE&%(?&MAX!0KkR)#`ne%4Hb z(t0KaJ{>$h+k*-kz>^^NT+Y__=(-=-Mj*69`*ai6o?D-C3%N9q5xO>BkLaU57VO6H PR4~Kg#66gDun~R(_A{M9 literal 0 HcmV?d00001 diff --git a/src/origincar_base/launch/base_serial.launch.py b/src/origincar_base/launch/base_serial.launch.py new file mode 100644 index 0000000..848e78d --- /dev/null +++ b/src/origincar_base/launch/base_serial.launch.py @@ -0,0 +1,47 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch.conditions import IfCondition, UnlessCondition +import launch_ros.actions + +def generate_launch_description(): + akmcar = LaunchConfiguration('akmcar', default='false') + + robot_parameters = [ + {'usart_port_name': '/dev/ttyACM0', + 'serial_baud_rate': 115200, + 'robot_frame_id': 'base_link', + 'odom_frame_id': 'odom', + 'cmd_vel': 'cmd_vel', + 'product_number': 0} + ] + + return LaunchDescription([ + DeclareLaunchArgument( + 'akmcar', + default_value='false', + description='Use simulation (Gazebo) clock if true' + ), + + launch_ros.actions.Node( + condition=IfCondition(akmcar), + package='origincar_base', + executable='origincar_base_node', + parameters=robot_parameters + [{'akm_cmd_vel': 'ackermann_cmd'}], + remappings=[('/cmd_vel', 'cmd_vel')], + ), + + launch_ros.actions.Node( + condition=IfCondition(akmcar), + package='origincar_base', + executable='cmd_vel_to_ackermann_drive.py', + name='cmd_vel_to_ackermann_drive', + ), + + launch_ros.actions.Node( + condition=UnlessCondition(akmcar), + package='origincar_base', + executable='origincar_base_node', + parameters=robot_parameters + [{'akm_cmd_vel': 'none'}], + ) + ]) diff --git a/src/origincar_base/launch/ekf.launch.py b/src/origincar_base/launch/ekf.launch.py new file mode 100644 index 0000000..51713ba --- /dev/null +++ b/src/origincar_base/launch/ekf.launch.py @@ -0,0 +1,33 @@ +#!/usr/bin/python3 + +import os +import launch +import launch_ros +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + + package_name = 'origincar_base' + ld = launch.LaunchDescription() + pkg_share = launch_ros.substitutions.FindPackageShare(package=package_name).find(package_name) + robot_localization_file_path = os.path.join(pkg_share, 'config/ekf.yaml') + use_sim_time = LaunchConfiguration('use_sim_time') + + # Start robot localization using an Extended Kalman filter + robot_localization_node = Node( + package='robot_localization', + executable='ekf_node', + name='ekf_filter_node', + output='screen', + remappings=[("odometry/filtered", "odom_combined")], + parameters=[ + robot_localization_file_path, + {'use_sim_time': use_sim_time} + ]) + + ld.add_action(launch.actions.DeclareLaunchArgument(name='use_sim_time', default_value='false', + description='Flag to disable use_sim_time')) + ld.add_action(robot_localization_node) + return ld diff --git a/src/origincar_base/launch/origincar_bringup.launch.py b/src/origincar_base/launch/origincar_bringup.launch.py new file mode 100644 index 0000000..35e023e --- /dev/null +++ b/src/origincar_base/launch/origincar_bringup.launch.py @@ -0,0 +1,95 @@ +import os +from pathlib import Path +import launch +from launch.actions import SetEnvironmentVariable +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import (DeclareLaunchArgument, GroupAction, + IncludeLaunchDescription, SetEnvironmentVariable) +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PythonExpression +from launch_ros.actions import PushRosNamespace +import launch_ros.actions +from launch.conditions import UnlessCondition + +def generate_launch_description(): + # Get the launch directory + bringup_dir = get_package_share_directory('origincar_base') + launch_dir = os.path.join(bringup_dir, 'launch') + ekf_config = Path(get_package_share_directory('origincar_base'), 'config', 'ekf.yaml') + imu_config = Path(get_package_share_directory('origincar_base'), 'config', 'imu.yaml') + + + carto_slam = LaunchConfiguration('carto_slam', default='false') + carto_slam_dec = DeclareLaunchArgument('carto_slam',default_value='false') + + akmcar = LaunchConfiguration('akmcar', default='true') + akmcar_dec = DeclareLaunchArgument('akmcar', default_value='true', + description='阿克曼底盘模式 (true=阿克曼, false=差速)') + + origincar_base = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(launch_dir, 'base_serial.launch.py')), + launch_arguments={'akmcar': akmcar}.items(), + ) + + choose_car = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(launch_dir, 'robot_mode_description.launch.py')), + ) + + + 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( + package='tf2_ros', + executable='static_transform_publisher', + name='base_to_gyro', + arguments=['0', '0', '0','0', '0','0','base_footprint','gyro_link'], + ) + + link_to_laser = launch_ros.actions.Node( + package='tf2_ros', + executable='static_transform_publisher', + name='link_to_laser', + arguments=['0', '0', '0','0', '0','0','base_link','laser'], + ) + + imu_filter_node = launch_ros.actions.Node( + package='imu_filter_madgwick', + executable='imu_filter_madgwick_node', + parameters=[imu_config] + ) + + robot_ekf = launch_ros.actions.Node( + condition=UnlessCondition(carto_slam), + package='robot_localization', + executable='ekf_node', + parameters=[ekf_config], + remappings=[("odometry/filtered", "odom_combined")] + ) + + joint_state_publisher_node = launch_ros.actions.Node( + package='joint_state_publisher', + executable='joint_state_publisher', + name='joint_state_publisher', + ) + + ld = LaunchDescription() + + 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) + ld.add_action(imu_filter_node) + ld.add_action(robot_ekf) + + return ld + diff --git a/src/origincar_base/launch/robot_mode_description.launch.py b/src/origincar_base/launch/robot_mode_description.launch.py new file mode 100644 index 0000000..4bce9d9 --- /dev/null +++ b/src/origincar_base/launch/robot_mode_description.launch.py @@ -0,0 +1,24 @@ +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 + +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() + + ld.add_action(origincar_description) + return ld \ No newline at end of file diff --git a/src/origincar_base/msg/Position.msg b/src/origincar_base/msg/Position.msg new file mode 100644 index 0000000..2fe2697 --- /dev/null +++ b/src/origincar_base/msg/Position.msg @@ -0,0 +1,3 @@ +float32 angle_x +float32 angle_y +float32 distance diff --git a/src/origincar_base/package.xml b/src/origincar_base/package.xml new file mode 100644 index 0000000..5e569f3 --- /dev/null +++ b/src/origincar_base/package.xml @@ -0,0 +1,44 @@ + + + + origincar_base + 0.0.0 + ROS2 origincar_base for origincar_base + ps-micro + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + tf2_geometry_msgs + + + rosidl_default_generators + rosidl_interface_packages + tf2_geometry_msgs + + rosidl_default_runtime + ament_index_cpp + geometry_msgs + rclcpp + rclcpp_action + sensor_msgs + std_msgs + std_srvs + tf2 + tf2_ros + rclpy + serial + nav2_msgs + nav_msgs + ackermann_msgs + origincar_msg + origincar_description + robot_localization + + boost + + ament_cmake + + diff --git a/src/origincar_base/scripts/cmd_vel_to_ackermann_drive.py b/src/origincar_base/scripts/cmd_vel_to_ackermann_drive.py new file mode 100644 index 0000000..bae5266 --- /dev/null +++ b/src/origincar_base/scripts/cmd_vel_to_ackermann_drive.py @@ -0,0 +1,47 @@ +#!/usr/bin/env python3 + +import math +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import Twist +from ackermann_msgs.msg import AckermannDriveStamped +from rclpy.qos import QoSProfile + +class CmdVel2AckermannDriveNode(Node): + def __init__(self): + super().__init__('cmd_vel_to_ackermann_drive') + self.publisher = self.create_publisher(AckermannDriveStamped, '/ackermann_cmd', QoSProfile(depth=10)) + self.subscription = self.create_subscription(Twist, 'cmd_vel', self.cmd_callback, QoSProfile(depth=10)) + self.wheelbase = 0.143 #轮距 + self.frame_id = 'odom_combined' + self.cmd_angle_instead_rotvel = False + + def convert_trans_rot_vel_to_steering_angle(self, vel, omega): + if omega == 0 or vel == 0: + return 0 + radius = vel / omega + return math.atan(self.wheelbase / radius) + + def cmd_callback(self, data): + vel = data.linear.x + if self.cmd_angle_instead_rotvel: + steering = data.angular.z + else: + steering = 1.0 * self.convert_trans_rot_vel_to_steering_angle(vel, data.angular.z) + + msg = AckermannDriveStamped() + msg.header.stamp = self.get_clock().now().to_msg() + msg.header.frame_id = self.frame_id + msg.drive.steering_angle = steering + msg.drive.speed = vel + self.publisher.publish(msg) + +def main(): + rclpy.init() + node = CmdVel2AckermannDriveNode() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/src/origincar_base/src/Quaternion_Solution.cpp b/src/origincar_base/src/Quaternion_Solution.cpp new file mode 100644 index 0000000..56cf830 --- /dev/null +++ b/src/origincar_base/src/Quaternion_Solution.cpp @@ -0,0 +1,91 @@ + +#include "origincar_base/Quaternion_Solution.h" +#define SAMPLING_FREQ 20.0f // 采样频率 +/************************************** +Date: May 31, 2020 +Function: 平方根倒数 求四元数用到 +***************************************/ +float InvSqrt(float number) +{ + volatile long i; + volatile float x, y; + volatile const float f = 1.5F; + x = number * 0.5F; + y = number; + i = * (( long * ) &y); + i = 0x5f375a86 - ( i >> 1 ); + y = * (( float * ) &i); + y = y * ( f - ( x * y * y ) ); + + return y; +} +/************************************** +Date: May 31, 2020 +Function: 四元数解算 +***************************************/ +volatile float twoKp = 1.0f; // 2 * proportional gain (Kp) +volatile float twoKi = 0.0f; // 2 * integral gain (Ki) +volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // quaternion of sensor frame relative to auxiliary frame +volatile float integralFBx = 0.0f, integralFBy = 0.0f, integralFBz = 0.0f; // integral error terms scaled by Ki +void Quaternion_Solution(float gx, float gy, float gz, float ax, float ay, float az) +{ + float recipNorm; + float halfvx, halfvy, halfvz; + float halfex, halfey, halfez; + float qa, qb, qc; + // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) + if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { + // 首先把加速度计采集到的值(三维向量)转化为单位向量，即向量除以模 + recipNorm = InvSqrt(ax * ax + ay * ay + az * az); + ax *= recipNorm; + ay *= recipNorm; + az *= recipNorm; + // 把四元数换算成方向余弦中的第三行的三个元素 + halfvx = q1 * q3 - q0 * q2; + halfvy = q0 * q1 + q2 * q3; + halfvz = q0 * q0 - 0.5f + q3 * q3; + //误差是估计的重力方向和测量的重力方向的交叉乘积之和 + halfex = (ay * halfvz - az * halfvy); + halfey = (az * halfvx - ax * halfvz); + halfez = (ax * halfvy - ay * halfvx); + // 计算并应用积分反馈（如果启用） + if(twoKi > 0.0f) { + integralFBx += twoKi * halfex * (1.0f / SAMPLING_FREQ); // integral error scaled by Ki + integralFBy += twoKi * halfey * (1.0f / SAMPLING_FREQ); + integralFBz += twoKi * halfez * (1.0f / SAMPLING_FREQ); + gx += integralFBx; // apply integral feedback + gy += integralFBy; + gz += integralFBz; + } + else { + integralFBx = 0.0f; // prevent integral windup + integralFBy = 0.0f; + integralFBz = 0.0f; + } + // Apply proportional feedback + gx += twoKp * halfex; + gy += twoKp * halfey; + gz += twoKp * halfez; + } + // Integrate rate of change of quaternion + gx *= (0.5f * (1.0f / SAMPLING_FREQ)); // pre-multiply common factors + gy *= (0.5f * (1.0f / SAMPLING_FREQ)); + gz *= (0.5f * (1.0f / SAMPLING_FREQ)); + qa = q0; + qb = q1; + qc = q2; + q0 += (-qb * gx - qc * gy - q3 * gz); + q1 += (qa * gx + qc * gz - q3 * gy); + q2 += (qa * gy - qb * gz + q3 * gx); + q3 += (qa * gz + qb * gy - qc * gx); + // Normalise quaternion + recipNorm = InvSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); + q0 *= recipNorm; + q1 *= recipNorm; + q2 *= recipNorm; + q3 *= recipNorm; + Mpu6050.orientation.w = q0; + Mpu6050.orientation.x = q1; + Mpu6050.orientation.y = q2; + Mpu6050.orientation.z = q3; +} diff --git a/src/origincar_base/src/origincar_base.cpp b/src/origincar_base/src/origincar_base.cpp new file mode 100644 index 0000000..4a39723 --- /dev/null +++ b/src/origincar_base/src/origincar_base.cpp @@ -0,0 +1,469 @@ +#include "origincar_base/origincar_base.h" +#include "rclcpp/rclcpp.hpp" +#include "origincar_base/Quaternion_Solution.h" +#include "ackermann_msgs/msg/ackermann_drive_stamped.hpp" +#include "origincar_msg/msg/data.hpp" +#include "robot_localization/srv/set_pose.hpp" +#include + +using std::placeholders::_1; +using namespace std; +void sigintHandler(int sig); +sensor_msgs::msg::Imu Mpu6050; +rclcpp::Node::SharedPtr node_handle = nullptr; +int Init_imu_num = 0; +float gyro_z_sum=0; + +int main(int argc, char *argv[]) +{ + rclcpp::init(argc, argv); + signal(SIGINT, sigintHandler); + origincar_base Robot_Control; + Robot_Control.Control(); + rclcpp::shutdown(); + return 0; +} + + + + +short origincar_base::IMU_Trans(uint8_t Data_High,uint8_t Data_Low) +{ + short transition_16; + transition_16 = 0; + transition_16 |= Data_High<<8; + transition_16 |= Data_Low; + return transition_16; +} + +float origincar_base::Odom_Trans(uint8_t Data_High,uint8_t Data_Low) +{ + float data_return; + short transition_16; + transition_16 = 0; + transition_16 |= Data_High<<8; + transition_16 |= Data_Low; + data_return = (transition_16 / 1000)+(transition_16 % 1000)*0.001; + return data_return; + } + +void origincar_base::Akm_Cmd_Vel_Callback(const ackermann_msgs::msg::AckermannDriveStamped::SharedPtr akm_ctl) +{ + short transition; + std::cout<<"linerx"<drive.speed<drive.steering_angle<drive.speed*1000; + Send_Data.tx[4] = transition; + Send_Data.tx[3] = transition>>8; + + transition=0; + transition = akm_ctl->drive.steering_angle*1000/2; + Send_Data.tx[8] = transition; + Send_Data.tx[7] = transition>>8; + + Send_Data.tx[9]=Check_Sum(9,SEND_DATA_CHECK); + Send_Data.tx[10]=FRAME_TAIL; + + try { + Stm32_Serial.write(Send_Data.tx,sizeof (Send_Data.tx)); + } catch (serial::IOException& e) { + RCLCPP_ERROR(this->get_logger(),("Unable to send data through serial port")); + } +} + +void origincar_base::Cmd_Vel_Callback(const geometry_msgs::msg::Twist::SharedPtr twist_aux) +{ + RCLCPP_INFO(this->get_logger(),"linarx: %.2f, angularz: %.2f ", twist_aux->linear.x, twist_aux->angular.z); + std::cout<<"linerx"<linear.x<angular.z<linear.x*1000; + Send_Data.tx[4] = transition; + Send_Data.tx[3] = transition>>8; + + transition=0; + transition = twist_aux->linear.y*1000; + Send_Data.tx[6] = transition; + Send_Data.tx[5] = transition>>8; + + transition=0; + transition = (twist_aux->angular.z)*1000; + Send_Data.tx[8] = transition; + Send_Data.tx[7] = transition>>8; + + Send_Data.tx[9]=Check_Sum(9,SEND_DATA_CHECK); + Send_Data.tx[10]=FRAME_TAIL; + + try + { + Stm32_Serial.write(Send_Data.tx,sizeof (Send_Data.tx)); + } catch (serial::IOException& e) { + RCLCPP_ERROR(this->get_logger(),("Unable to send data through serial port")); + } +} + +void origincar_base::Sign_Switch_Callback(const std_msgs::msg::Int32::SharedPtr sign_switch) +{ + (void)sign_switch; + if (sign_switch->data == -1) { + memset(&Robot_Pos, 0, sizeof(Robot_Pos)); + memset(&Mpu6050, 0, sizeof(Mpu6050)); + q0 = 0; + q1 = 0; + q2 = 0; + q3 = 0; + Robot_Pos.X = 0.54; + Robot_Pos.Y = 0.2; + Robot_Pos.Z = 0; + Robot_Vel.X = 0; + Robot_Vel.Y = 0; + Robot_Vel.Z = 0; + memset(&Robot_Vel, 0, sizeof(Robot_Vel)); + reset_filter(); + } + else if(sign_switch->data == -2) { + memset(&Robot_Pos, 0, sizeof(Robot_Pos)); + Robot_Pos.X = 2.54; + Robot_Pos.Y = 2.5; + Robot_Pos.Z = -1.5708; + Robot_Vel.X = 0; + Robot_Vel.Y = 0; + Robot_Vel.Z = 0; + memset(&Robot_Vel, 0, sizeof(Robot_Vel)); + reset_filter(); + } + /* + else if (sign_switch->data == 6) { + memset(&Robot_Pos, 0, sizeof(Robot_Pos)); + Robot_Pos.X = 2; + Robot_Pos.Y = 2; + memset(&Robot_Vel, 0, sizeof(Robot_Vel)); + }*/ + +} + +void origincar_base::Publish_ImuSensor() +{ + sensor_msgs::msg::Imu Imu_Data_Pub; + Imu_Data_Pub.header.stamp = rclcpp::Node::now(); + Imu_Data_Pub.header.frame_id = gyro_frame_id; + + Imu_Data_Pub.orientation.x = Mpu6050.orientation.x; + Imu_Data_Pub.orientation.y = Mpu6050.orientation.y; + Imu_Data_Pub.orientation.z = Mpu6050.orientation.z; + Imu_Data_Pub.orientation.w = Mpu6050.orientation.w; + Imu_Data_Pub.orientation_covariance[0] = 1e6; + Imu_Data_Pub.orientation_covariance[4] = 1e6; + Imu_Data_Pub.orientation_covariance[8] = 1e-6; + Imu_Data_Pub.angular_velocity.x = Mpu6050.angular_velocity.x; + Imu_Data_Pub.angular_velocity.y = Mpu6050.angular_velocity.y; + Imu_Data_Pub.angular_velocity.z = Mpu6050.angular_velocity.z; + Imu_Data_Pub.angular_velocity_covariance[0] = 1e6; + Imu_Data_Pub.angular_velocity_covariance[4] = 1e6; + Imu_Data_Pub.angular_velocity_covariance[8] = 1e-6; + Imu_Data_Pub.linear_acceleration.x = Mpu6050.linear_acceleration.x; + Imu_Data_Pub.linear_acceleration.y = Mpu6050.linear_acceleration.y; + Imu_Data_Pub.linear_acceleration.z = Mpu6050.linear_acceleration.z; + + imu_publisher->publish(Imu_Data_Pub); + +} + +void origincar_base::Publish_Odom() +{ + tf2::Quaternion q; + q.setRPY(0,0,Robot_Pos.Z); + geometry_msgs::msg::Quaternion odom_quat=tf2::toMsg(q); + + origincar_msg::msg::Data robotpose; + origincar_msg::msg::Data robotvel; + nav_msgs::msg::Odometry odom; + + odom.header.stamp = rclcpp::Node::now(); + odom.header.frame_id = odom_frame_id; + odom.child_frame_id = robot_frame_id; + + odom.pose.pose.position.x = Robot_Pos.X; + odom.pose.pose.position.y = Robot_Pos.Y; + + odom.pose.pose.position.z = 0.0; + odom.pose.pose.orientation = odom_quat; + //odom_quat; + + + odom.twist.twist.linear.x = Robot_Vel.X; + odom.twist.twist.linear.y = Robot_Vel.Y; + odom.twist.twist.angular.z = Robot_Vel.Z; + + robotpose.x = Robot_Pos.X; + robotpose.y = Robot_Pos.Y; + robotpose.z = Robot_Pos.Z; + + robotvel.x = Robot_Vel.X; + robotvel.y = Robot_Vel.Y; + robotvel.z = Robot_Vel.Z; + + geometry_msgs::msg::TransformStamped t; + + t.header.stamp = rclcpp::Node::now(); + t.header.frame_id = odom_frame_id; + t.child_frame_id = robot_frame_id; + + t.transform.translation.x = Robot_Pos.X; + t.transform.translation.y = Robot_Pos.Y; + t.transform.translation.z = 0.0; + + t.transform.rotation = odom_quat; + tf_broadcaster_->sendTransform(t); + odom_publisher->publish(odom); + robotpose_publisher->publish(robotpose); + robotvel_publisher->publish(robotvel); +} + +void origincar_base::Publish_Voltage() +{ + std_msgs::msg::Float32 voltage_msgs; + static float Count_Voltage_Pub = 0; + + if (Count_Voltage_Pub++ > 10) { + Count_Voltage_Pub = 0; + voltage_msgs.data = Power_voltage; + voltage_publisher->publish(voltage_msgs); + } +} + +unsigned char origincar_base::Check_Sum(unsigned char Count_Number,unsigned char mode) +{ + unsigned char check_sum = 0, k; + + if (mode == 0) { + for(k=0; k < Count_Number; k++) { + check_sum = check_sum^Receive_Data.rx[k]; + } + } else if (mode == 1) { + for (k=0; k < Count_Number; k++) { + check_sum = check_sum^Send_Data.tx[k]; + } + } + + return check_sum; +} + +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)); + for (j = 0; j < 24; j++) { + if (Receive_Data_Pr[j] == FRAME_HEADER) + Header_Pos=j; + else if (Receive_Data_Pr[j] == FRAME_TAIL) + Tail_Pos = j; + } + + if (Tail_Pos == (Header_Pos + 23)) { + memcpy(Receive_Data.rx, Receive_Data_Pr, sizeof(Receive_Data_Pr)); + } else if (Header_Pos == (1 + Tail_Pos)) { + for (j = 0;j < 24; j++) + Receive_Data.rx[j] = Receive_Data_Pr[(j+Header_Pos) % 24]; + } else { + return false; + } + + Receive_Data.Frame_Header = Receive_Data.rx[0]; + Receive_Data.Frame_Tail = Receive_Data.rx[23]; + if (Receive_Data.Frame_Header == FRAME_HEADER) { + if (Receive_Data.Frame_Tail == FRAME_TAIL) { + if (Receive_Data.rx[22] == Check_Sum(22,READ_DATA_CHECK)||(Header_Pos == (1 + Tail_Pos))) { + Receive_Data.Flag_Stop=Receive_Data.rx[1]; + Robot_Vel.X = Odom_Trans(Receive_Data.rx[2],Receive_Data.rx[3]); + + Robot_Vel.Y = Odom_Trans(Receive_Data.rx[4],Receive_Data.rx[5]); + + //Robot_Vel.Z = Odom_Trans(Receive_Data.rx[6],Receive_Data.rx[7]); + + Mpu6050_Data.accele_x_data = IMU_Trans(Receive_Data.rx[8],Receive_Data.rx[9]); + Mpu6050_Data.accele_y_data = IMU_Trans(Receive_Data.rx[10],Receive_Data.rx[11]); + Mpu6050_Data.accele_z_data = IMU_Trans(Receive_Data.rx[12],Receive_Data.rx[13]); + Mpu6050_Data.gyros_x_data = IMU_Trans(Receive_Data.rx[14],Receive_Data.rx[15]); + Mpu6050_Data.gyros_y_data = IMU_Trans(Receive_Data.rx[16],Receive_Data.rx[17]); + Mpu6050_Data.gyros_z_data = IMU_Trans(Receive_Data.rx[18],Receive_Data.rx[19]); + + Mpu6050.linear_acceleration.x = Mpu6050_Data.accele_x_data / ACCEl_RATIO; + Mpu6050.linear_acceleration.y = Mpu6050_Data.accele_y_data / ACCEl_RATIO; + Mpu6050.linear_acceleration.z = Mpu6050_Data.accele_z_data / ACCEl_RATIO; + + Mpu6050.angular_velocity.x = Mpu6050_Data.gyros_x_data * GYROSCOPE_RATIO; + Mpu6050.angular_velocity.y = Mpu6050_Data.gyros_y_data * GYROSCOPE_RATIO; + + if(Init_imu_num<100) + { + Init_imu_num += 1; + gyro_z_sum += Mpu6050_Data.gyros_z_data ; + Mpu6050.angular_velocity.z = 0; + } + else + { + Mpu6050.angular_velocity.z = (Mpu6050_Data.gyros_z_data - (gyro_z_sum / (float)Init_imu_num))*GYROSCOPE_RATIO; + //RCLCPP_INFO(this->get_logger(),"gyro_z_sum: %.2f, err: %.2f, gyroz: %.2f ", gyro_z_sum ,(gyro_z_sum / (float)Init_imu_num), Mpu6050.angular_velocity.z); + } + Robot_Vel.Z = Mpu6050.angular_velocity.z; + transition_16 = 0; + transition_16 |= Receive_Data.rx[20]<<8; + transition_16 |= Receive_Data.rx[21]; + Power_voltage = transition_16/1000+(transition_16 % 1000)*0.001; + + return true; + } + } + } + + return false; +} + +void origincar_base::Control() +{ + rclcpp::Time current_time, last_time; + current_time = rclcpp::Node::now(); + last_time = rclcpp::Node::now(); + while(rclcpp::ok()) { + current_time = rclcpp::Node::now(); + Sampling_Time = (current_time - last_time).seconds(); + if (true == Get_Sensor_Data()) { + Robot_Pos.X+=1.03*(Robot_Vel.X * cos(Robot_Pos.Z) - Robot_Vel.Y * sin(Robot_Pos.Z)) * Sampling_Time; + Robot_Pos.Y+=1.01*(Robot_Vel.X * sin(Robot_Pos.Z) + Robot_Vel.Y * cos(Robot_Pos.Z)) * Sampling_Time;//1.125 + Robot_Pos.Z+= Mpu6050.angular_velocity.z * Sampling_Time; + + Quaternion_Solution(Mpu6050.angular_velocity.x, Mpu6050.angular_velocity.y, Mpu6050.angular_velocity.z,\ + Mpu6050.linear_acceleration.x, Mpu6050.linear_acceleration.y, Mpu6050.linear_acceleration.z); + Publish_ImuSensor(); + Publish_Voltage(); + Publish_Odom(); + rclcpp::spin_some(this->get_node_base_interface()); + } + last_time = current_time; + } +} + +origincar_base::origincar_base() +: rclcpp::Node ("origincar_base") +{ + memset(&Robot_Pos, 0, sizeof(Robot_Pos)); + memset(&Robot_Vel, 0, sizeof(Robot_Vel)); + memset(&Receive_Data, 0, sizeof(Receive_Data)); + memset(&Send_Data, 0, sizeof(Send_Data)); + memset(&Mpu6050_Data, 0, sizeof(Mpu6050_Data)); + Robot_Pos.X = 0.54; + Robot_Pos.Y = 0.2; + + int serial_baud_rate = 115200; + + this->declare_parameter("usart_port_name", "/dev/ttyCH343USB0"); + this->declare_parameter("cmd_vel", "cmd_vel"); + this->declare_parameter("akm_cmd_vel", "ackermann_cmd"); + this->declare_parameter("odom_frame_id", "odom"); + this->declare_parameter("robot_frame_id", "base_link"); + this->declare_parameter("gyro_frame_id", "gyro_link"); + + this->get_parameter("serial_baud_rate", serial_baud_rate); + this->get_parameter("usart_port_name", usart_port_name); + this->get_parameter("cmd_vel", cmd_vel); + this->get_parameter("akm_cmd_vel", akm_cmd_vel); + 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); + + odom_publisher = create_publisher("odom", 10); + + imu_publisher = create_publisher("imu/data_raw", 10); + + voltage_publisher = create_publisher("PowerVoltage", 1); + + robotpose_publisher = create_publisher("robotpose", 10); + + robotvel_publisher = create_publisher("robotvel", 10); + + pose_pub_ = create_publisher( + "/set_pose", + rclcpp::SystemDefaultsQoS().reliable()); + + tf_bro = std::make_shared(this); + tf_broadcaster_ = std::make_unique(*this); + Cmd_Vel_Sub = create_subscription( + cmd_vel, 1, std::bind(&origincar_base::Cmd_Vel_Callback, this, _1)); + Akm_Cmd_Vel_Sub = create_subscription( + akm_cmd_vel, 1, std::bind(&origincar_base::Akm_Cmd_Vel_Callback, this, _1)); + + Sign_Switch_Sub = create_subscription( + "/sign4return", 1, std::bind(&origincar_base::Sign_Switch_Callback, this, _1)); + try { + Stm32_Serial.setPort("/dev/ttyACM0"); + Stm32_Serial.setBaudrate(serial_baud_rate); + serial::Timeout _time = serial::Timeout::simpleTimeout(2000); + Stm32_Serial.setTimeout(_time); + Stm32_Serial.open(); + } catch (serial::IOException& e) { + RCLCPP_ERROR(this->get_logger(),"origincar_base can not open serial port,Please check the serial port cable! "); + } + if(Stm32_Serial.isOpen()) { + RCLCPP_INFO(this->get_logger(),"origincar_base serial port opened"); + } +} + + +void sigintHandler(int sig) +{ + sig = sig; + printf("OriginBot shutdown...\n"); + serial::Serial Stm32_Serial; + Stm32_Serial.setPort("/dev/ttyACM0"); + Stm32_Serial.setBaudrate(115200); + serial::Timeout _time = serial::Timeout::simpleTimeout(2000); + Stm32_Serial.setTimeout(_time); + Stm32_Serial.open(); + SEND_DATA Send_Data; + if (Stm32_Serial.isOpen()) { + Send_Data.tx[0]=FRAME_HEADER; + Send_Data.tx[1] = 0; + Send_Data.tx[2] = 0; + + Send_Data.tx[4] = 0; + Send_Data.tx[3] = 0; + + Send_Data.tx[6] = 0; + Send_Data.tx[5] = 0; + + Send_Data.tx[7] = 0; + Send_Data.tx[8] = 0; + int check_sum = 0; + for (int k = 0; k < 9; k++) { + check_sum = check_sum^Send_Data.tx[k]; + } + Send_Data.tx[9]=check_sum; + Send_Data.tx[10]=FRAME_TAIL; + + try { + Stm32_Serial.write(Send_Data.tx,sizeof (Send_Data.tx)); + } catch (serial::IOException& e) { + } + + } + // 关闭ROS2接口，清除资源 + rclcpp::shutdown(); +} + +origincar_base::~origincar_base() +{ + RCLCPP_INFO(this->get_logger(),"Shutting down"); +} \ No newline at end of file diff --git a/src/origincar_description/urdf/origincar.urdf b/src/origincar_description/urdf/origincar.urdf index 28bc794..8664a97 100644 --- a/src/origincar_description/urdf/origincar.urdf +++ b/src/origincar_description/urdf/origincar.urdf @@ -83,7 +83,7 @@ - + @@ -136,7 +136,7 @@ - + @@ -205,7 +205,7 @@ - + @@ -239,7 +239,7 @@ - + @@ -265,7 +265,7 @@ - + diff --git a/src/origincar_msg/CMakeLists.txt b/src/origincar_msg/CMakeLists.txt new file mode 100644 index 0000000..8697fbf --- /dev/null +++ b/src/origincar_msg/CMakeLists.txt @@ -0,0 +1,34 @@ +cmake_minimum_required(VERSION 3.5) + +project(origincar_msg) + +# Default to C++14 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + + +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +find_package(std_msgs REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/Data.msg" + "msg/Sign.msg" + DEPENDENCIES std_msgs + ADD_LINTER_TESTS +) + +ament_export_dependencies(rosidl_default_runtime) + +ament_package() diff --git a/src/origincar_msg/msg/Data.msg b/src/origincar_msg/msg/Data.msg new file mode 100644 index 0000000..161f45e --- /dev/null +++ b/src/origincar_msg/msg/Data.msg @@ -0,0 +1,3 @@ +float32 x +float32 y +float32 z diff --git a/src/origincar_msg/msg/Sign.msg b/src/origincar_msg/msg/Sign.msg new file mode 100644 index 0000000..189d1f8 --- /dev/null +++ b/src/origincar_msg/msg/Sign.msg @@ -0,0 +1 @@ +int32 sign_data \ No newline at end of file diff --git a/src/origincar_msg/package.xml b/src/origincar_msg/package.xml new file mode 100644 index 0000000..bd08ee6 --- /dev/null +++ b/src/origincar_msg/package.xml @@ -0,0 +1,21 @@ + + + + origincar_msg + 0.0.0 + TODO: Package description + ps-micro + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + + + ament_cmake + +