紧急通知:禁止一切关于政治&VPN翻墙等话题,发现相关帖子会立马删除封号
小鱼 ROS 2 新书上线!点击链接查看, 新书配套视频点击链接查看。
提问前必看的发帖注意事项—— 提问前必看!不符合要求的问题拒绝回答!!
社区使用指南—如何添加标签修改密码
小鱼 ROS 2 新书上线!点击链接查看, 新书配套视频点击链接查看。
提问前必看的发帖注意事项—— 提问前必看!不符合要求的问题拒绝回答!!
社区使用指南—如何添加标签修改密码
雷神m10雷达humble安装遇到问题,编译通过了,运行还是报错,大佬们给看一下
-
CMakeLists.txt
cmake_minimum_required(VERSION 3.5) project(lsm10_v2) # 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(rclcpp REQUIRED) find_package(rclpy REQUIRED) find_package(sensor_msgs REQUIRED) find_package(std_msgs REQUIRED) #find_package(dynamic_reconfigure REQUIRED) find_package(lsm10_v2 REQUIRED) find_package(Boost 1.65.1 REQUIRED COMPONENTS system filesystem thread) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() endif() rosidl_generate_interfaces(${PROJECT_NAME} "msg/Difop.msg" DEPENDENCIES std_msgs ADD_LINTER_TESTS ) include_directories( include src ${catkin_INCLUDE_DIRS} ${Eigen3_INCLUDE_DIRS} ) ament_export_dependencies(rosidl_default_runtime) add_executable(LSM10 src/lsiosr.cpp src/lsm10.cpp) include_directories(LSM10 ${Boost_INCLUDE_DIRS}) link_directories(LSM10 ${Boost_LIBRARY_DIRS}) target_link_libraries(LSM10 ${Boost_LIBRARIES}) ament_target_dependencies(LSM10 rclcpp rclpy std_msgs sensor_msgs lsm10_v2 Boost) target_link_libraries(LSM10 ${Boost_PROGRAM_OPTIONS_LIBRARY}) install(TARGETS LSM10 DESTINATION lib/${PROJECT_NAME} ) install( DIRECTORY launch DESTINATION share/${PROJECT_NAME} ) ament_package()
package.xml
<?xml version="1.0"?> <package format="3"> <name>lsm10_v2</name> <version>0.0.0</version> <description>The lsm10_v2 package--wheeltec@dongguan</description> <maintainer email="powrbv@gmail.com">tongsky</maintainer> <license>TODO</license> <buildtool_depend>ament_cmake</buildtool_depend> <test_depend>ament_lint_auto</test_depend> <test_depend>ament_lint_common</test_depend> <test_depend>eigen</test_depend> <test_depend>boost</test_depend> <buildtool_depend>eigen3_cmake_module</buildtool_depend> <build_depend>rosidl_default_generators</build_depend> <exec_depend>rosidl_default_runtime</exec_depend> <build_depend>dynamic_reconfigure</build_depend> <exec_depend>dynamic_reconfigure</exec_depend> <build_depend>message_generation</build_depend> <exec_depend>message_runtime</exec_depend> <depend>rclcpp</depend> <depend>rclpy</depend> <depend>sensor_msgs</depend> <depend>std_msgs</depend> <depend>tf</depend> <depend>tf2</depend> <member_of_group>rosidl_interface_packages</member_of_group> <export> <build_type>ament_cmake</build_type> </export> </package>
lsiosr.h
/************************************************************************************* @company: Copyright (C) 2021, Leishen Intelligent System, WHEELTEC (Dongguan) Co., Ltd @product: LSn10 @filename: lsiosr.cpp @brief: @version: date: author: comments: @v2.0 22-4-12 yao,Tues ROS2 *************************************************************************************/ #ifndef LSIOSR_H #define LSIOSR_H #include <memory> #include <sys/types.h> #include <sys/stat.h> #include <fcntl.h> #include <termios.h> #include <errno.h> #include <unistd.h> #include <string.h> #include <stdlib.h> #include <unistd.h> #include <stdint.h> #include <fstream> #include <iostream> //波特率 #define BAUD_2400 2400 #define BAUD_4800 4800 #define BAUD_9600 9600 #define BAUD_57600 57600 #define BAUD_115200 115200 #define BAUD_230400 230400 #define BAUD_460800 460800 //奇偶校验位 #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 ls { class LSIOSR{ public: static LSIOSR* instance(std::string name, int speed, int fd = 0); ~LSIOSR(); /* 从串口中读取数据 */ int read(char *buffer, int length, int timeout = 60); /* 向串口传数据 */ int send(const char* buffer, int length, int timeout = 60); /* 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
lsm10.h
/************************************************************************************* @company: Copyright (C) 2021, Leishen Intelligent System, WHEELTEC (Dongguan) Co., Ltd @product: LSM10 @filename: lsm10.h @brief: @version: date: author: comments: @v2.0 22-4-12 fu,Tues ROS2 *************************************************************************************/ #ifndef LSM10_H #define LSM10_H #include <boost/date_time/posix_time/posix_time.hpp> #include <boost/thread/thread.hpp> #include <iostream> #include <memory> #include <inttypes.h> #include <rcl/types.h> #include <string> #include "rclcpp/rclcpp.hpp" #include <sensor_msgs/msg/laser_scan.hpp> #include "lsm10_v2/lsiosr.h" #include "lsm10_v2/msg/difop.hpp" namespace ls { typedef struct { double degree; double range; double intensity; } ScanPoint; class LSM10 : public rclcpp::Node { public: LSM10(); ~LSM10(); /** * 实例化雷达 * port: 串口号, * baud_rate: 波特率 460800 */ static LSM10* instance(); /** * 获取雷达数据 * poins: 雷达点的数据。类型为ScanPoint数组 */ int getScan(std::vector<ScanPoint> &points, rclcpp::Time & scan_time, float &scan_duration); /** * 获取软件版本号 * version: 返回版本号 */ int getVersion(std::string &version); int getRate(); double getRPM(); private: void initParam(); void recvThread(); void pubScanThread(); void thread_tmp(); std::vector<ScanPoint> scan_points_; std::vector<ScanPoint> scan_points_bak_; LSIOSR * serial_; boost::thread *recv_thread_; boost::thread *pubscan_thread_; std::unique_ptr<boost::thread> test_thread_; std::unique_ptr<boost::thread> testscan_thread_; boost::mutex pubscan_mutex_; boost::mutex mutex_; boost::condition_variable pubscan_cond_; bool is_shutdown_; // shutdown recvthread int data_len_; int points_size_; int rpm_; double real_rpm_; //ros::Time pre_time_; //ros::Time time_; rclcpp::Time pre_time_; rclcpp::Time time_; std::string serial_port_; int baud_rate_; std::string scan_topic_; std::string frame_id_; //ros::NodeHandle n_; //ros::Publisher pub_; rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr pub_; //ros::Publisher device_pub; rclcpp::Publisher<lsm10_v2::msg::Difop>::SharedPtr device_pub; }; } #endif
lsiosr.cpp
/************************************************************************************* @company: Copyright (C) 2021, Leishen Intelligent System, WHEELTEC (Dongguan) Co., Ltd @product: LSM10 @filename: lsiosr.cpp @brief: @version: date: author: comments: @v2.0 22-4-12 yao,Tues ROS2 *************************************************************************************/ #include <lsm10_v2/lsiosr.h> using namespace std; //rclcpp::Node::SharedPtr nh_=nullptr; namespace ls { LSIOSR * LSIOSR::instance(std::string name, int speed, int fd) { // printf("jieru\n"); 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 2400: cfsetispeed(&newtio, B2400); cfsetospeed(&newtio, B2400); break; case 4800: cfsetispeed(&newtio, B4800); cfsetospeed(&newtio, B4800); break; case 9600: cfsetispeed(&newtio, B9600); cfsetospeed(&newtio, B9600); break; case 115200: cfsetispeed(&newtio, B115200); cfsetospeed(&newtio, B115200); break; case 230400: cfsetispeed(&newtio, B230400); cfsetospeed(&newtio, B230400); break; case 460800: cfsetispeed(&newtio, B460800); cfsetospeed(&newtio, B460800); break; default: cfsetispeed(&newtio, B9600); cfsetospeed(&newtio, B9600); 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(char *buffer, int length, int timeout) { //printf("portread \n"); memset(buffer, 0, length); int totalBytesRead = 0; int rc; int unlink = 0; 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; printf("open_port %s ERROR !\n", port_.c_str()); } printf("LSM10::Init\n"); return error_code; } int LSIOSR::close() { ::close(fd_); } std::string LSIOSR::getPort() { return port_; } int LSIOSR::setPortName(std::string name) { port_ = name; return 0; } }
lsm10.cpp
/************************************************************************************* @company: Copyright (C) 2021, Leishen Intelligent System, WHEELTEC (Dongguan) Co., Ltd @product: LSM10 @filename: lsm10.cpp @brief: @version: date: author: comments: @v2.0 22-4-12 yao,Tues ROS2 *************************************************************************************/ #include "lsm10_v2/lsm10.h" #include <stdio.h> #include <signal.h> #include "lsm10_v2/msg/difop.hpp" using std::placeholders::_1; using namespace std; rclcpp::Node::SharedPtr nh_=nullptr; namespace ls{ LSM10 * LSM10::instance() { static LSM10 obj; return &obj; } LSM10::LSM10():rclcpp::Node ("m10lidar") { int code = 0; initParam(); pub_ = create_publisher<sensor_msgs::msg::LaserScan>("scan", 3); device_pub = create_publisher<lsm10_v2::msg::Difop>("difop_information", 100); serial_ = LSIOSR::instance("/dev/wheeltec_lidar", 460800); code = serial_->init(); if(code != 0) { RCLCPP_INFO(this->get_logger(),"rclcpp is down."); rclcpp::shutdown(); exit(0); } recv_thread_ = new boost::thread(boost::bind(&LSM10::recvThread, this)); //test_thread_.reset( new boost::thread(boost::bind(&LSM10::recvThread, this))); // testscan_thread_.reset( new boost::thread(boost::bind(&LSM10::pubScanThread, this))); pubscan_thread_ = new boost::thread(boost::bind(&LSM10::pubScanThread, this)); } LSM10::~LSM10() { printf("start LSM10::~LSM10()\n"); is_shutdown_ = true; /* test_thread_->interrupt(); test_thread_->join(); test_thread_ = NULL; //delete pubscan_thread_; testscan_thread_->interrupt(); testscan_thread_->join(); testscan_thread_ = NULL; //delete pubscan_thread_; */ pubscan_thread_->interrupt(); pubscan_thread_->join(); //printf("1end LSM10::~LSM1000()\n"); pubscan_thread_ = NULL; delete pubscan_thread_; //printf("2end LSM10::~LSM1000()\n"); recv_thread_->interrupt(); recv_thread_->join(); //printf("3end LSM10::~LSM1000()\n"); recv_thread_ = NULL; delete recv_thread_; //printf("4end LSM10::~LSM1000()\n"); serial_->close(); serial_ = NULL; delete serial_; printf("end LSM10::~LSM10()\n"); } void LSM10::thread_tmp() { int test_rmp=0; printf("test thread\n"); } void LSM10::initParam() { pre_time_ = rclcpp::Node::now(); time_ = rclcpp::Node::now(); std::string scan_topic_ = "/scan"; std::string frame_id_ = "laser"; std::string serial_port_ = "/dev/wheeltec_lidar"; this->declare_parameter<std::string>("scan_topic", scan_topic_); this->get_parameter<std::string>("scan", scan_topic_); this->declare_parameter<std::string>("laser", "laser"); this->get_parameter<std::string>("laser", frame_id_); this->declare_parameter<std::string>("serial_port", "/dev/wheeltec_lidar"); this->get_parameter<std::string>("serial_port", serial_port_); this->declare_parameter<int>("baud_rate", 406800); this->get_parameter<int>("baud_rate", baud_rate_); this->declare_parameter<int>("rpm", 600); this->get_parameter<int>("rpm", rpm_); is_shutdown_ = false; data_len_ = 180; points_size_ = 360 * 42 / 15; scan_points_.resize(points_size_); } int LSM10::getScan(std::vector<ScanPoint> &points, rclcpp::Time & scan_time, float &scan_duration) { boost::unique_lock<boost::mutex> lock(mutex_); points.assign(scan_points_bak_.begin(), scan_points_bak_.end()); scan_time = pre_time_; scan_duration = (time_ - pre_time_).seconds(); } int LSM10::getVersion(std::string &version) { version = "lsm10_v1_0"; return 0; } double LSM10::getRPM() { return real_rpm_; } void LSM10::recvThread() { char * packet_bytes = new char[data_len_]; int idx = 0; double degree; boost::posix_time::ptime t1,t2; t1 = boost::posix_time::microsec_clock::universal_time(); //printf("rec\n"); while(!is_shutdown_&&rclcpp::ok()){ int count = serial_->read(packet_bytes, 90); //printf("rec2\n"); for (int i = 0; i < count; i++) { int k = packet_bytes[i]; k < 0 ? k += 256 : k; int y = packet_bytes[i + 1]; y < 0 ? y += 256 : y; int k_1 = packet_bytes[i + 2]; k_1 < 0 ? k_1 += 256 : k_1; int y_1 = packet_bytes[i + 3]; y_1 < 0 ? y_1 += 256 : y_1; if (k == 0xA5 && y == 0x5A) //应答距离 { if(i != 0) { serial_->read(packet_bytes + 90 - i, i); } int s = packet_bytes[i + 2]; s < 0 ? s += 256 : s; int z = packet_bytes[i + 3]; z < 0 ? z += 256 : z; boost::unique_lock<boost::mutex> lock(mutex_); //if ((s * 256 + z) / 100.f > 360) // degree = 0; //else degree = (s * 256 + z) / 100.f; //转速 lsm10_v2::msg::Difop Difop_data; s = packet_bytes[i + 4]; s < 0 ? s += 256 : s; z = packet_bytes[i + 5]; z < 0 ? z += 256 : z; Difop_data.motorspeed = float(2500000.0 / (s * 256 + z)); device_pub->publish(Difop_data); int invalidValue = 0; for (size_t num = 2; num < 86; num+=2) { int s = packet_bytes[i + num + 4]; s < 0 ? s += 256 : s; int z = packet_bytes[i + num + 5]; z < 0 ? z += 256 : z; if ((s * 256 + z) != 0xFFFF) { scan_points_[idx].range = double(s * 256 + (z)) / 1000.f; scan_points_[idx].intensity = 0; idx++; } else { invalidValue++; } } invalidValue = 42 - invalidValue; for (size_t i = 0; i < invalidValue; i++) { if ((degree + (15.0 / invalidValue * i)) > 360) scan_points_[idx-invalidValue+i].degree = degree + (15.0 / invalidValue * i) - 360; else scan_points_[idx-invalidValue+i].degree = degree + (15.0 / invalidValue * i); } lock.unlock(); if (degree > 359.5) { idx = 0; t2 = boost::posix_time::microsec_clock::universal_time(); boost::posix_time::millisec_posix_time_system_config::time_duration_type t_elapse; t_elapse = t2 - t1; real_rpm_ = 1000000.0 / t_elapse.ticks(); t1 = t2; boost::unique_lock<boost::mutex> lock(mutex_); scan_points_bak_.resize(scan_points_.size()); scan_points_bak_.assign(scan_points_.begin(), scan_points_.end()); for(int k=0; k<scan_points_.size(); k++) { scan_points_[k].range = 0; scan_points_[k].degree = 0; } pre_time_ = time_; lock.unlock(); pubscan_cond_.notify_one(); time_ = this->now(); } } else if (k == 0xA5 && y == 0xFF && k_1 == 00 && y_1 == 0x5A) { printf("elseif"); /*lsm10_v2::difopPtr Difop_data = lsm10_v2::difopPtr( new lsm10_v2::difop()); //温度 int s = packet_bytes[i + 12]; s < 0 ? s += 256 : s; int z = packet_bytes[i + 13]; z < 0 ? z += 256 : z; Difop_data->Temperature = (float(s * 256 + z) / 4096 * 330 - 50); //高压 s = packet_bytes[i + 14]; s < 0 ? s += 256 : s; z = packet_bytes[i + 15]; z < 0 ? z += 256 : z; Difop_data->HighPressure = (float(s * 256 + z) / 4096 * 3.3 * 101); //转速 s = packet_bytes[i + 16]; s < 0 ? s += 256 : s; z = packet_bytes[i + 17]; z < 0 ? z += 256 : z; Difop_data->MotorSpeed = (float(1000000 / (s * 256 + z) / 24)); device_pub.publish(Difop_data);*/ } } } if (packet_bytes) { packet_bytes = NULL; delete packet_bytes; } } void LSM10::pubScanThread() { //printf("pubscan0"); bool wait_for_wake = true; boost::unique_lock<boost::mutex> lock(pubscan_mutex_); while (!is_shutdown_&&rclcpp::ok()) { while (wait_for_wake) { pubscan_cond_.wait(lock); wait_for_wake = false; } //printf("pubscan1"); std::vector<ScanPoint> points; rclcpp::Time start_time; float scan_time; this->getScan(points, start_time, scan_time); int count = points.size(); if (count <= 0) continue; sensor_msgs::msg::LaserScan msg; msg.header.frame_id = "laser"; msg.header.stamp = start_time; msg.angle_min = 0.0; msg.angle_max = 2 * M_PI; msg.angle_increment = (msg.angle_max - msg.angle_min) / count; msg.range_min = 0.02; msg.range_max = 50; msg.ranges.resize(count); msg.intensities.resize(count); msg.scan_time = scan_time; msg.time_increment = scan_time / (double)(count - 1); for(int k=0; k<count; k++) { msg.ranges[k] = std::numeric_limits<float>::infinity(); msg.intensities[k] = 0; } for (int i = 0; i < count; i++) { int point_idx = 1008 - points[i].degree * 42 / 15; if(point_idx < 0 || point_idx > 1007) { continue; } if (points[i].range == 0.0) { msg.ranges[point_idx] = std::numeric_limits<float>::infinity(); msg.intensities[point_idx] = 0; // printf("pubscan2"); } else { double dist = points[i].range; msg.ranges[point_idx] = (float) dist; msg.intensities[point_idx] = points[i].intensity; // printf("pubscan3"); } } pub_->publish(msg); // printf("pubscan4"); wait_for_wake = true; } } } void handleSig(int signo) { rclcpp::shutdown(); exit(0); } int main(int argc, char *argv[]) { rclcpp::init(argc, argv); signal(SIGINT, handleSig); signal(SIGTERM, handleSig); ls::LSM10* lsm10 = ls::LSM10::instance(); //auto node=std::make_shared<ls::LSM10>(); auto node = rclcpp::Node::make_shared("lidar10"); rclcpp::spin(node); return 0; }
ls_m10.launch.py
#!/usr/bin/env python3 ''' @company: Copyright (C) 2022, WHEELTEC (Dongguan) Co., Ltd @product: LSM10 @filename: ls_m10.launch.py @brief: @version: date: author: comments: @v2.0 22-4-12 Tues ROS2 ''' import os from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.actions import LogInfo from launch.substitutions import LaunchConfiguration import launch_ros.actions def generate_launch_description(): frequency = LaunchConfiguration('frequency', default='5.5') serial_port_ = LaunchConfiguration('port', default='/dev/wheeltec_lidar') baud_rate_ = LaunchConfiguration('baud_rate_', default='460800') frame_id_ = LaunchConfiguration('frame_id', default='laser_link') scan_topic = LaunchConfiguration('scan_topic', default='scan') return LaunchDescription([ DeclareLaunchArgument('serial_port_', default_value=serial_port_, description='Specifying port to connected lidar'), DeclareLaunchArgument('baud_rate_', default_value=baud_rate_, description='Specifying serial baudrate to connected lidar'), DeclareLaunchArgument('frame_id_', default_value=frame_id_, description='Specifying frame_id of lidar. Default frame_id is \'laser\''), DeclareLaunchArgument('frequency', default_value=frequency, description='Specifying frequency property of lidar'), DeclareLaunchArgument('scan_topic', default_value=scan_topic, description='Specifying scan_topic property of lidar'), launch_ros.actions.Node( package='lsm10_v2', executable='LSM10', output='screen'), ])
雷达的代码都在这里了,以前在ros2 foxy版本编译好了就行,现在在foxy编译报错
一开始humble版本编译也报同样的错误,不知道为什么后来编译不报错了,但是运行还是报错
-
@毛哥成山轮胎机油保养 在 雷神m10雷达humble安装遇到问题,编译通过了,运行还是报错,大佬们给看一下 中说:
install(TARGETS
LSM10
DESTINATION lib/${PROJECT_NAME}
)检查下报错目录中是否存在可执行文件, /home/m/jtbot_ws/install/lsm10_v2/lib/lsm10_v2
-
@小鱼 当时我看了,确实没有可执行文件,但是原来这个驱动在foxy运行是正常的,这次装完系统我特意装了一个foxy虚拟机,在foxy上试了也不行,说明有些东西升级了,原来的东西不配套了。雷达售后给我又发了一个驱动,安装成功了,我把原来的humble 卸载了用一键装ros又装了一遍,没有问题,说明你的一键装ros也是正常的。
-