diff --git a/src/rplidar_node.cpp b/src/rplidar_node.cpp index 28948546..1e2bfe08 100644 --- a/src/rplidar_node.cpp +++ b/src/rplidar_node.cpp @@ -44,14 +44,15 @@ #define _countof(_Array) (int)(sizeof(_Array) / sizeof(_Array[0])) #endif -#define DEG2RAD(x) ((x)*M_PI/180.) +#define DEG2RAD(x) ((x) * M_PI / 180.) #define ROS2VERSION "1.0.1" -enum { - LIDAR_A_SERIES_MINUM_MAJOR_ID = 0, - LIDAR_S_SERIES_MINUM_MAJOR_ID = 5, - LIDAR_T_SERIES_MINUM_MAJOR_ID = 8, +enum +{ + LIDAR_A_SERIES_MINUM_MAJOR_ID = 0, + LIDAR_S_SERIES_MINUM_MAJOR_ID = 5, + LIDAR_T_SERIES_MINUM_MAJOR_ID = 8, }; using namespace sl; @@ -60,40 +61,38 @@ bool need_exit = false; class RPlidarNode : public rclcpp::Node { - public: - RPlidarNode(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()) - : Node("rplidar_node", options) +public: + RPlidarNode(const rclcpp::NodeOptions &options = rclcpp::NodeOptions()) + : Node("rplidar_node", options) { - - } - private: +private: void init_param() { - this->declare_parameter("channel_type","serial"); + this->declare_parameter("channel_type", "serial"); this->declare_parameter("tcp_ip", "192.168.0.7"); this->declare_parameter("tcp_port", 20108); - this->declare_parameter("udp_ip","192.168.11.2"); - this->declare_parameter("udp_port",8089); + this->declare_parameter("udp_ip", "192.168.11.2"); + this->declare_parameter("udp_port", 8089); this->declare_parameter("serial_port", "/dev/ttyUSB0"); - this->declare_parameter("serial_baudrate",1000000); - this->declare_parameter("frame_id","laser_frame"); + this->declare_parameter("serial_baudrate", 1000000); + this->declare_parameter("frame_id", "laser_frame"); this->declare_parameter("inverted", false); this->declare_parameter("angle_compensate", false); this->declare_parameter("flip_x_axis", false); this->declare_parameter("auto_standby", false); - this->declare_parameter("topic_name",std::string("scan")); - this->declare_parameter("scan_mode",std::string()); - this->declare_parameter("scan_frequency",10); - + this->declare_parameter("topic_name", std::string("scan")); + this->declare_parameter("scan_mode", std::string()); + this->declare_parameter("scan_frequency", 10); + this->get_parameter_or("channel_type", channel_type, "serial"); - this->get_parameter_or("tcp_ip", tcp_ip, "192.168.0.7"); + this->get_parameter_or("tcp_ip", tcp_ip, "192.168.0.7"); this->get_parameter_or("tcp_port", tcp_port, 20108); - this->get_parameter_or("udp_ip", udp_ip, "192.168.11.2"); + this->get_parameter_or("udp_ip", udp_ip, "192.168.11.2"); this->get_parameter_or("udp_port", udp_port, 8089); - this->get_parameter_or("serial_port", serial_port, "/dev/ttyUSB0"); - this->get_parameter_or("serial_baudrate", serial_baudrate, 1000000/*256000*/);//ros run for A1 A2, change to 256000 if A3 + this->get_parameter_or("serial_port", serial_port, "/dev/ttyUSB0"); + this->get_parameter_or("serial_baudrate", serial_baudrate, 1000000 /*256000*/); // ros run for A1 A2, change to 256000 if A3 this->get_parameter_or("frame_id", frame_id, "laser_frame"); this->get_parameter_or("inverted", inverted, false); this->get_parameter_or("angle_compensate", angle_compensate, false); @@ -101,61 +100,70 @@ class RPlidarNode : public rclcpp::Node this->get_parameter_or("auto_standby", auto_standby, false); this->get_parameter_or("topic_name", topic_name, "scan"); this->get_parameter_or("scan_mode", scan_mode, std::string()); - if(channel_type == "udp") + if (channel_type == "udp") this->get_parameter_or("scan_frequency", scan_frequency, 20.0); else this->get_parameter_or("scan_frequency", scan_frequency, 10.0); } - bool getRPLIDARDeviceInfo(ILidarDriver * drv) + bool getRPLIDARDeviceInfo(ILidarDriver *drv) { - sl_result op_result; + sl_result op_result; sl_lidar_response_device_info_t devinfo; op_result = drv->getDeviceInfo(devinfo); - if (SL_IS_FAIL(op_result)) { - if (op_result == SL_RESULT_OPERATION_TIMEOUT) { - RCLCPP_ERROR(this->get_logger(),"Error, operation time out. SL_RESULT_OPERATION_TIMEOUT! "); - } else { - RCLCPP_ERROR(this->get_logger(),"Error, unexpected error, code: %x",op_result); + if (SL_IS_FAIL(op_result)) + { + if (op_result == SL_RESULT_OPERATION_TIMEOUT) + { + RCLCPP_ERROR(this->get_logger(), "Error, operation time out. SL_RESULT_OPERATION_TIMEOUT! "); + } + else + { + RCLCPP_ERROR(this->get_logger(), "Error, unexpected error, code: %x", op_result); } return false; } // print out the device serial number, firmware and hardware version number.. - char sn_str[37] = {'\0'}; - for (int pos = 0; pos < 16 ;++pos) { - sprintf(sn_str + (pos * 2),"%02X", devinfo.serialnum[pos]); + char sn_str[37] = {'\0'}; + for (int pos = 0; pos < 16; ++pos) + { + sprintf(sn_str + (pos * 2), "%02X", devinfo.serialnum[pos]); } - RCLCPP_INFO(this->get_logger(),"RPLidar S/N: %s",sn_str); - RCLCPP_INFO(this->get_logger(),"Firmware Ver: %d.%02d",devinfo.firmware_version>>8, devinfo.firmware_version & 0xFF); - RCLCPP_INFO(this->get_logger(),"Hardware Rev: %d",(int)devinfo.hardware_version); + RCLCPP_INFO(this->get_logger(), "RPLidar S/N: %s", sn_str); + RCLCPP_INFO(this->get_logger(), "Firmware Ver: %d.%02d", devinfo.firmware_version >> 8, devinfo.firmware_version & 0xFF); + RCLCPP_INFO(this->get_logger(), "Hardware Rev: %d", (int)devinfo.hardware_version); return true; } - bool checkRPLIDARHealth(ILidarDriver * drv) + bool checkRPLIDARHealth(ILidarDriver *drv) { - sl_result op_result; + sl_result op_result; sl_lidar_response_device_health_t healthinfo; op_result = drv->getHealth(healthinfo); - if (SL_IS_OK(op_result)) { - RCLCPP_INFO(this->get_logger(),"RPLidar health status : %d", healthinfo.status); - switch (healthinfo.status) { - case SL_LIDAR_STATUS_OK: - RCLCPP_INFO(this->get_logger(),"RPLidar health status : OK."); - return true; - case SL_LIDAR_STATUS_WARNING: - RCLCPP_INFO(this->get_logger(),"RPLidar health status : Warning."); - return true; - case SL_LIDAR_STATUS_ERROR: - RCLCPP_ERROR(this->get_logger(),"Error, RPLidar internal error detected. Please reboot the device to retry."); - return false; - default: - RCLCPP_ERROR(this->get_logger(),"Error, Unknown internal error detected. Please reboot the device to retry."); - return false; + if (SL_IS_OK(op_result)) + { + RCLCPP_INFO(this->get_logger(), "RPLidar health status : %d", healthinfo.status); + switch (healthinfo.status) + { + case SL_LIDAR_STATUS_OK: + RCLCPP_INFO(this->get_logger(), "RPLidar health status : OK."); + return true; + case SL_LIDAR_STATUS_WARNING: + RCLCPP_INFO(this->get_logger(), "RPLidar health status : Warning."); + return true; + case SL_LIDAR_STATUS_ERROR: + RCLCPP_ERROR(this->get_logger(), "Error, RPLidar internal error detected. Please reboot the device to retry."); + return false; + default: + RCLCPP_ERROR(this->get_logger(), "Error, Unknown internal error detected. Please reboot the device to retry."); + return false; } - } else { - RCLCPP_ERROR(this->get_logger(),"Error, cannot retrieve RPLidar health code: %x", op_result); + } + else + { + RCLCPP_ERROR(this->get_logger(), "Error, cannot retrieve RPLidar health code: %x", op_result); return false; } } @@ -166,7 +174,8 @@ class RPlidarNode : public rclcpp::Node (void)req; (void)res; - if (auto_standby) { + if (auto_standby) + { RCLCPP_INFO( this->get_logger(), "Ingnoring stop_motor request because rplidar_node is in 'auto standby' mode"); @@ -174,20 +183,21 @@ class RPlidarNode : public rclcpp::Node } RCLCPP_DEBUG(this->get_logger(), "Call to '%s'", __FUNCTION__); - - //RCLCPP_DEBUG(this->get_logger(),"Stop motor"); + + // RCLCPP_DEBUG(this->get_logger(),"Stop motor"); this->stop(); - //drv->setMotorSpeed(0); + // drv->setMotorSpeed(0); return true; } bool start_motor(const std::shared_ptr req, - std::shared_ptr res) + std::shared_ptr res) { (void)req; (void)res; - if (auto_standby) { + if (auto_standby) + { RCLCPP_INFO( this->get_logger(), "Ingnoring start_motor request because rplidar_node is in 'auto standby' mode"); @@ -219,21 +229,20 @@ class RPlidarNode : public rclcpp::Node return true; #endif - } - static float getAngle(const sl_lidar_response_measurement_node_hq_t& node) + static float getAngle(const sl_lidar_response_measurement_node_hq_t &node) { return node.angle_z_q14 * 90.f / 16384.f; } - void publish_scan(rclcpp::Publisher::SharedPtr& pub, - sl_lidar_response_measurement_node_hq_t *nodes, - size_t node_count, rclcpp::Time start, - double scan_time, bool inverted, bool flip_X_axis, - float angle_min, float angle_max, - float max_distance, - std::string frame_id) + void publish_scan(rclcpp::Publisher::SharedPtr &pub, + sl_lidar_response_measurement_node_hq_t *nodes, + size_t node_count, rclcpp::Time start, + double scan_time, bool inverted, bool flip_X_axis, + float angle_min, float angle_max, + float max_distance, + std::string frame_id) { static int scan_count = 0; auto scan_msg = std::make_shared(); @@ -242,33 +251,39 @@ class RPlidarNode : public rclcpp::Node scan_msg->header.frame_id = frame_id; scan_count++; - bool reversed = (angle_max > angle_min); - if ( reversed ) { - scan_msg->angle_min = M_PI - angle_max; - scan_msg->angle_max = M_PI - angle_min; - } else { - scan_msg->angle_min = M_PI - angle_min; - scan_msg->angle_max = M_PI - angle_max; + bool reversed = (angle_max < angle_min); + if (reversed) + { + scan_msg->angle_min = angle_max; + scan_msg->angle_max = angle_min; + } + else + { + scan_msg->angle_min = angle_min; + scan_msg->angle_max = angle_max; } - scan_msg->angle_increment = (scan_msg->angle_max - scan_msg->angle_min) / (double)(node_count-1); + scan_msg->angle_increment = (scan_msg->angle_max - scan_msg->angle_min) / (double)(node_count - 1); scan_msg->scan_time = scan_time; - scan_msg->time_increment = scan_time / (double)(node_count-1); + scan_msg->time_increment = scan_time / (double)(node_count - 1); scan_msg->range_min = 0.15; - scan_msg->range_max = max_distance;//8.0; + scan_msg->range_max = max_distance; // 8.0; scan_msg->intensities.resize(node_count); scan_msg->ranges.resize(node_count); bool reverse_data = (!inverted && reversed) || (inverted && !reversed); size_t scan_midpoint = node_count / 2; - for (size_t i = 0; i < node_count; i++) { + for (size_t i = 0; i < node_count; i++) + { float read_value = (float)nodes[i].dist_mm_q2 / 4.0f / 1000; size_t apply_index = i; - if (reverse_data) { + if (reverse_data) + { apply_index = node_count - 1 - i; } - if (flip_X_axis) { + if (flip_X_axis) + { if (apply_index >= scan_midpoint) apply_index = apply_index - scan_midpoint; else @@ -285,34 +300,43 @@ class RPlidarNode : public rclcpp::Node pub->publish(*scan_msg); } - bool set_scan_mode() { - sl_result op_result; + bool set_scan_mode() + { + sl_result op_result; LidarScanMode current_scan_mode; - if (scan_mode.empty()) { + if (scan_mode.empty()) + { op_result = drv->startScan(false /* not force scan */, true /* use typical scan mode */, 0, ¤t_scan_mode); } - else { + else + { std::vector allSupportedScanModes; op_result = drv->getAllSupportedScanModes(allSupportedScanModes); - if (SL_IS_OK(op_result)) { + if (SL_IS_OK(op_result)) + { sl_u16 selectedScanMode = sl_u16(-1); - for (std::vector::iterator iter = allSupportedScanModes.begin(); iter != allSupportedScanModes.end(); iter++) { - if (iter->scan_mode == scan_mode) { + for (std::vector::iterator iter = allSupportedScanModes.begin(); iter != allSupportedScanModes.end(); iter++) + { + if (iter->scan_mode == scan_mode) + { selectedScanMode = iter->id; break; } } - if (selectedScanMode == sl_u16(-1)) { + if (selectedScanMode == sl_u16(-1)) + { RCLCPP_ERROR(this->get_logger(), "scan mode `%s' is not supported by lidar, supported modes:", scan_mode.c_str()); - for (std::vector::iterator iter = allSupportedScanModes.begin(); iter != allSupportedScanModes.end(); iter++) { + for (std::vector::iterator iter = allSupportedScanModes.begin(); iter != allSupportedScanModes.end(); iter++) + { RCLCPP_ERROR(this->get_logger(), "\t%s: max_distance: %.1f m, Point number: %.1fK", iter->scan_mode, - iter->max_distance, (1000 / iter->us_per_sample)); + iter->max_distance, (1000 / iter->us_per_sample)); } op_result = SL_RESULT_OPERATION_FAIL; } - else { + else + { op_result = drv->startScanExpress(false /* not force scan */, selectedScanMode, 0, ¤t_scan_mode); } } @@ -320,14 +344,14 @@ class RPlidarNode : public rclcpp::Node if (SL_IS_OK(op_result)) { - //default frequent is 10 hz (by motor pwm value), current_scan_mode.us_per_sample is the number of scan point per us + // default frequent is 10 hz (by motor pwm value), current_scan_mode.us_per_sample is the number of scan point per us int points_per_circle = (int)(1000 * 1000 / current_scan_mode.us_per_sample / scan_frequency); angle_compensate_multiple = points_per_circle / 360.0 + 1; if (angle_compensate_multiple < 1) angle_compensate_multiple = 1.0; max_distance = (float)current_scan_mode.max_distance; RCLCPP_INFO(this->get_logger(), "current scan mode: %s, sample rate: %d Khz, max_distance: %.1f m, scan frequency:%.1f Hz, ", - current_scan_mode.scan_mode, (int)(1000 / current_scan_mode.us_per_sample + 0.5), max_distance, scan_frequency); + current_scan_mode.scan_mode, (int)(1000 / current_scan_mode.us_per_sample + 0.5), max_distance, scan_frequency); return true; } else @@ -338,13 +362,15 @@ class RPlidarNode : public rclcpp::Node } bool start() { - if (nullptr == drv) { + if (nullptr == drv) + { return false; } RCLCPP_INFO(this->get_logger(), "Start"); drv->setMotorSpeed(); - if (!set_scan_mode()) { + if (!set_scan_mode()) + { this->stop(); RCLCPP_ERROR(this->get_logger(), "Failed to set scan mode"); return false; @@ -355,7 +381,8 @@ class RPlidarNode : public rclcpp::Node void stop() { - if (nullptr == drv) { + if (nullptr == drv) + { return; } @@ -365,56 +392,69 @@ class RPlidarNode : public rclcpp::Node is_scanning = false; } -public: +public: int work_loop() - { + { init_param(); int ver_major = SL_LIDAR_SDK_VERSION_MAJOR; int ver_minor = SL_LIDAR_SDK_VERSION_MINOR; int ver_patch = SL_LIDAR_SDK_VERSION_PATCH; - RCLCPP_INFO(this->get_logger(),"RPLidar running on ROS2 package rplidar_ros. RPLIDAR SDK Version:%d.%d.%d",ver_major,ver_minor,ver_patch); - - sl_result op_result; + RCLCPP_INFO(this->get_logger(), "RPLidar running on ROS2 package rplidar_ros. RPLIDAR SDK Version:%d.%d.%d", ver_major, ver_minor, ver_patch); + + sl_result op_result; // create the driver instance drv = *createLidarDriver(); - if (nullptr == drv) { + if (nullptr == drv) + { /* don't start spinning without a driver object */ RCLCPP_ERROR(this->get_logger(), "Failed to construct driver"); return -1; } - IChannel* _channel; - if(channel_type == "tcp"){ + IChannel *_channel; + if (channel_type == "tcp") + { _channel = *createTcpChannel(tcp_ip, tcp_port); } - else if(channel_type == "udp"){ + else if (channel_type == "udp") + { _channel = *createUdpChannel(udp_ip, udp_port); } - else{ + else + { _channel = *createSerialPortChannel(serial_port, serial_baudrate); } - if (SL_IS_FAIL((drv)->connect(_channel))) { - if(channel_type == "tcp"){ - RCLCPP_ERROR(this->get_logger(),"Error, cannot connect to the ip addr %s with the tcp port %s.",tcp_ip.c_str(),std::to_string(tcp_port).c_str()); + if (SL_IS_FAIL((drv)->connect(_channel))) + { + if (channel_type == "tcp") + { + RCLCPP_ERROR(this->get_logger(), "Error, cannot connect to the ip addr %s with the tcp port %s.", tcp_ip.c_str(), std::to_string(tcp_port).c_str()); } - else if(channel_type == "udp"){ - RCLCPP_ERROR(this->get_logger(),"Error, cannot connect to the ip addr %s with the udp port %s.",udp_ip.c_str(),std::to_string(udp_port).c_str()); + else if (channel_type == "udp") + { + RCLCPP_ERROR(this->get_logger(), "Error, cannot connect to the ip addr %s with the udp port %s.", udp_ip.c_str(), std::to_string(udp_port).c_str()); } - else{ - RCLCPP_ERROR(this->get_logger(),"Error, cannot bind to the specified serial port %s.",serial_port.c_str()); + else + { + RCLCPP_ERROR(this->get_logger(), "Error, cannot bind to the specified serial port %s.", serial_port.c_str()); } - delete drv; drv = nullptr; + delete drv; + drv = nullptr; return -1; } - + // get rplidar device info - if (!getRPLIDARDeviceInfo(drv)) { - delete drv; drv = nullptr; + if (!getRPLIDARDeviceInfo(drv)) + { + delete drv; + drv = nullptr; return -1; } // check health... - if (!checkRPLIDARHealth(drv)) { - delete drv; drv = nullptr; + if (!checkRPLIDARHealth(drv)) + { + delete drv; + drv = nullptr; return -1; } @@ -422,43 +462,52 @@ class RPlidarNode : public rclcpp::Node op_result = drv->getDeviceInfo(devinfo); bool scan_frequency_tunning_after_scan = false; - if( (devinfo.model>>4) > LIDAR_S_SERIES_MINUM_MAJOR_ID){ + if ((devinfo.model >> 4) > LIDAR_S_SERIES_MINUM_MAJOR_ID) + { scan_frequency_tunning_after_scan = true; } - if(!scan_frequency_tunning_after_scan){ //for RPLIDAR A serials - //start RPLIDAR A serials rotate by pwm + if (!scan_frequency_tunning_after_scan) + { // for RPLIDAR A serials + // start RPLIDAR A serials rotate by pwm drv->setMotorSpeed(600); } /* start motor and scanning */ - if (!auto_standby && !this->start()) { - delete drv; drv = nullptr; + if (!auto_standby && !this->start()) + { + delete drv; + drv = nullptr; return -1; } scan_pub = this->create_publisher(topic_name, rclcpp::QoS(rclcpp::KeepLast(10))); - stop_motor_service = this->create_service("stop_motor", - std::bind(&RPlidarNode::stop_motor,this,std::placeholders::_1,std::placeholders::_2)); - start_motor_service = this->create_service("start_motor", - std::bind(&RPlidarNode::start_motor,this,std::placeholders::_1,std::placeholders::_2)); + stop_motor_service = this->create_service("stop_motor", + std::bind(&RPlidarNode::stop_motor, this, std::placeholders::_1, std::placeholders::_2)); + start_motor_service = this->create_service("start_motor", + std::bind(&RPlidarNode::start_motor, this, std::placeholders::_1, std::placeholders::_2)); - //drv->setMotorSpeed(); + // drv->setMotorSpeed(); rclcpp::Time start_scan_time; rclcpp::Time end_scan_time; double scan_duration; - while (rclcpp::ok() && !need_exit) { + while (rclcpp::ok() && !need_exit) + { sl_lidar_response_measurement_node_hq_t nodes[8192]; - size_t count = _countof(nodes); + size_t count = _countof(nodes); - if (auto_standby) { - if (scan_pub->get_subscription_count() > 0 && !is_scanning) { + if (auto_standby) + { + if (scan_pub->get_subscription_count() > 0 && !is_scanning) + { this->start(); } - else if (scan_pub->get_subscription_count() == 0) { - if (is_scanning) { + else if (scan_pub->get_subscription_count() == 0) + { + if (is_scanning) + { this->stop(); } } @@ -469,74 +518,89 @@ class RPlidarNode : public rclcpp::Node end_scan_time = this->now(); scan_duration = (end_scan_time - start_scan_time).seconds(); - if (op_result == SL_RESULT_OK) { - if(scan_frequency_tunning_after_scan) { //Set scan frequency(For Slamtec Tof lidar) - RCLCPP_ERROR(this->get_logger(), "set lidar scan frequency to %.1f Hz(%.1f Rpm) ",scan_frequency,scan_frequency*60); - drv->setMotorSpeed(scan_frequency*60); //rpm + if (op_result == SL_RESULT_OK) + { + if (scan_frequency_tunning_after_scan) + { // Set scan frequency(For Slamtec Tof lidar) + RCLCPP_ERROR(this->get_logger(), "set lidar scan frequency to %.1f Hz(%.1f Rpm) ", scan_frequency, scan_frequency * 60); + drv->setMotorSpeed(scan_frequency * 60); // rpm scan_frequency_tunning_after_scan = false; continue; } op_result = drv->ascendScanData(nodes, count); float angle_min = DEG2RAD(0.0f); float angle_max = DEG2RAD(359.0f); - if (op_result == SL_RESULT_OK) { - if (angle_compensate) { - //const int angle_compensate_multiple = 1; - const int angle_compensate_nodes_count = 360*angle_compensate_multiple; + if (op_result == SL_RESULT_OK) + { + if (angle_compensate) + { + // const int angle_compensate_multiple = 1; + const int angle_compensate_nodes_count = 360 * angle_compensate_multiple; int angle_compensate_offset = 0; auto angle_compensate_nodes = new sl_lidar_response_measurement_node_hq_t[angle_compensate_nodes_count]; - memset(angle_compensate_nodes, 0, angle_compensate_nodes_count*sizeof(sl_lidar_response_measurement_node_hq_t)); + memset(angle_compensate_nodes, 0, angle_compensate_nodes_count * sizeof(sl_lidar_response_measurement_node_hq_t)); size_t i = 0, j = 0; - for( ; i < count; i++ ) { - if (nodes[i].dist_mm_q2 != 0) { + for (; i < count; i++) + { + if (nodes[i].dist_mm_q2 != 0) + { float angle = getAngle(nodes[i]); int angle_value = (int)(angle * angle_compensate_multiple); - if ((angle_value - angle_compensate_offset) < 0) angle_compensate_offset = angle_value; - for (j = 0; j < angle_compensate_multiple; j++) { - int angle_compensate_nodes_index = angle_value-angle_compensate_offset + j; - if(angle_compensate_nodes_index >= angle_compensate_nodes_count) + if ((angle_value - angle_compensate_offset) < 0) + angle_compensate_offset = angle_value; + for (j = 0; j < angle_compensate_multiple; j++) + { + int angle_compensate_nodes_index = angle_value - angle_compensate_offset + j; + if (angle_compensate_nodes_index >= angle_compensate_nodes_count) angle_compensate_nodes_index = angle_compensate_nodes_count - 1; angle_compensate_nodes[angle_compensate_nodes_index] = nodes[i]; } } } - + publish_scan(scan_pub, angle_compensate_nodes, angle_compensate_nodes_count, - start_scan_time, scan_duration, inverted, flip_x_axis, - angle_min, angle_max, max_distance, - frame_id); + start_scan_time, scan_duration, inverted, flip_x_axis, + angle_min, angle_max, max_distance, + frame_id); - if (angle_compensate_nodes) { + if (angle_compensate_nodes) + { delete[] angle_compensate_nodes; angle_compensate_nodes = nullptr; } - } else { + } + else + { int start_node = 0, end_node = 0; int i = 0; // find the first valid node and last valid node - while (nodes[i++].dist_mm_q2 == 0); - start_node = i-1; - i = count -1; - while (nodes[i--].dist_mm_q2 == 0); - end_node = i+1; + while (nodes[i++].dist_mm_q2 == 0) + ; + start_node = i - 1; + i = count - 1; + while (nodes[i--].dist_mm_q2 == 0) + ; + end_node = i + 1; angle_min = DEG2RAD(getAngle(nodes[start_node])); angle_max = DEG2RAD(getAngle(nodes[end_node])); - publish_scan(scan_pub, &nodes[start_node], end_node-start_node +1, - start_scan_time, scan_duration, inverted, flip_x_axis, - angle_min, angle_max, max_distance, - frame_id); + publish_scan(scan_pub, &nodes[start_node], end_node - start_node + 1, + start_scan_time, scan_duration, inverted, flip_x_axis, + angle_min, angle_max, max_distance, + frame_id); } - } else if (op_result == SL_RESULT_OPERATION_FAIL) { + } + else if (op_result == SL_RESULT_OPERATION_FAIL) + { // All the data is invalid, just publish them float angle_min = DEG2RAD(0.0f); float angle_max = DEG2RAD(359.0f); publish_scan(scan_pub, nodes, count, - start_scan_time, scan_duration, inverted, flip_x_axis, - angle_min, angle_max, max_distance, - frame_id); + start_scan_time, scan_duration, inverted, flip_x_axis, + angle_min, angle_max, max_distance, + frame_id); } } @@ -546,12 +610,16 @@ class RPlidarNode : public rclcpp::Node // done! drv->setMotorSpeed(0); drv->stop(); - RCLCPP_INFO(this->get_logger(),"Stop motor"); - if (drv) { delete drv; drv = nullptr; } + RCLCPP_INFO(this->get_logger(), "Stop motor"); + if (drv) + { + delete drv; + drv = nullptr; + } return 0; } - private: +private: rclcpp::Publisher::SharedPtr scan_pub; rclcpp::Service::SharedPtr start_motor_service; rclcpp::Service::SharedPtr stop_motor_service; @@ -570,7 +638,7 @@ class RPlidarNode : public rclcpp::Node bool flip_x_axis = false; bool auto_standby = false; float max_distance = 8.0; - size_t angle_compensate_multiple = 1;//it stand of angle compensate at per 1 degree + size_t angle_compensate_multiple = 1; // it stand of angle compensate at per 1 degree std::string scan_mode; float scan_frequency; /* State */ @@ -585,14 +653,12 @@ void ExitHandler(int sig) need_exit = true; } - -int main(int argc, char * argv[]) +int main(int argc, char *argv[]) { - rclcpp::init(argc, argv); - auto rplidar_node = std::make_shared(rclcpp::NodeOptions()); - signal(SIGINT,ExitHandler); - int ret = rplidar_node->work_loop(); - rclcpp::shutdown(); - return ret; + rclcpp::init(argc, argv); + auto rplidar_node = std::make_shared(rclcpp::NodeOptions()); + signal(SIGINT, ExitHandler); + int ret = rplidar_node->work_loop(); + rclcpp::shutdown(); + return ret; } -