now I am making the node for my custom lidar. the struct of this lidar pointcloud is
#pragma pack(push, 1) typedef struct spherical { uint32_t timestamp; float intensity; float threshold; float theta; // vertical float pie; // horizontal float radius; // depth float speed; // speed vector with current angle uint32_t frame_cnt; // Frame counter }cspherical_t; #pragma pack(pop) and this one, and the code that I made to visualize is this code
#include <ros/ros.h> #include <sensor_msgs/PointCloud2.h> #include "VicsenLidar.h" #include <std_msgs/Float32.h> #include <pcl_ros/point_cloud.h> #include <pcl_conversions/pcl_conversions.h> ros::Publisher pointcloud_pub; ros::Publisher speed_pub; // Define your callback function to process the Lidar data void lidarframe_callback(const cspherical_t* points, size_t point_num) { // Add ROS_INFO statements for debugging ROS_INFO("Lidar callback called with %zu points.", point_num); // Create a PointCloud2 message sensor_msgs::PointCloud2 pointcloud_msg; // Fill in the header of the PointCloud2 message pointcloud_msg.header.stamp = ros::Time::now(); pointcloud_msg.header.frame_id = "fmcw"; // Adjust the frame_id as needed // Create a PCL PointCloud to hold the data pcl::PointCloud<pcl::PointXYZI> pcl_cloud; // Fill the PCL PointCloud with data from your Lidar callback for (size_t i = 0; i < point_num; i++) { pcl::PointXYZI point; float x = points[i].radius * sin(points[i].theta) * cos(points[i].pie); float y = points[i].radius * sin(points[i].theta) * sin(points[i].pie); float z = points[i].radius * cos(points[i].theta); point.x = x; point.y = y; point.z = z; point.intensity = points[i].intensity/100000; pcl_cloud.push_back(point); // 포인트 정보 출력 ROS_INFO("Point %zu: x=%.2f, y=%.2f, z=%.2f, intensity=%.2f, speed=%.2f", i , point.x, point.y, point.z, point.intensity, points[i].speed); } // Convert PCL PointCloud to PointCloud2 pcl::toROSMsg(pcl_cloud, pointcloud_msg); // Publish the PointCloud2 message on the "fmcw_pointcloud" topic pointcloud_pub.publish(pointcloud_msg); // Extract and publish speed values separately for (size_t i = 0; i < point_num; i++) { float speed = points[i].speed; // Extract speed value from packed data // Create a message for speed and publish it separately std_msgs::Float32 speed_msg; speed_msg.data = speed; speed_pub.publish(speed_msg); } } int main(int argc, char** argv) { // Initialize ROS ros::init(argc, argv, "fmcw_lidar_node"); ros::NodeHandle nh; // Create a publisher for the PointCloud2 data pointcloud_pub = nh.advertise<sensor_msgs::PointCloud2>("fmcw_pointcloud", 100); // Create a publisher for the speed data speed_pub = nh.advertise<std_msgs::Float32>("lidar_speed", 100); // Initialize the Vicsen Lidar vicsen_init(); /* 연결된 Vicsen Lidar를 찾습니다. 만약 라이다를 찾지 못하면 프로그램을 종료합니다. */ const char** ip_lists = vicsen_find(); if (!ip_lists) { std::cout << "Vicsen Lidar를 찾을 수 없습니다!" << std::endl; return 0; } // IPv4 주소만 사용 for (size_t i = 0; ip_lists[i] != NULL; i++) { // IPv4 주소인지 확인하려면 문자열로 변환하고 첫 번째 바이트를 확인합니다. std::string ip_string(ip_lists[i]); if (ip_string.find(".") != std::string::npos) { // 문자열에 '.'이 있으면 IPv4 주소로 간주 std::cout << "Vicsen IP (IPv4): " << ip_lists[i] << std::endl; /* IPv4 주소를 사용하여 Vicsen Lidar와 통신 또는 다른 작업을 수행하세요. */ } } /* Vicsen Lidar에 연결하여 제어 및 센서 데이터 수집합니다. */ vicsen_connect(ip_lists[0]); /* 센서 데이터를 수신할 때 호출할 콜백 함수를 등록합니다. */ set_frame_callback(lidarframe_callback); /* Vicsen Lidar 내부 FPGA의 온도를 얻습니다. */ float fpga_temp = get_fpga_temperature(); /* Vicsen Lidar 내 레이저 다이오드(LD)의 온도를 얻습니다. */ float ld_temp = get_ld_temperature(); /* Vicsen Lidar 장치의 시리얼 번호를 얻습니다. */ /* Vicsen Lidar 내 스캐너를 제어합니다. Preset: 0, framerate: 3. * preset에 대한 정보는 사용자 매뉴얼을 참조하시면 좋을 것 같습니다. */ set_scanner_preset(0, 3); /* 설정된 preset 및 framerate 정보를 얻습니다. */ uint8_t scanner_preset = get_scanner_preset(); float scanner_framerate = get_scanner_framerate(); /* Vicsen Lidar가 감지할 수 있는 최대 속도를 설정합니다. */ set_speed_limit(3); /* 설정된 최대 속도 제한을 얻습니다. */ float speed_limit = get_speed_limit(); /* Vicsen Lidar가 감지할 수 있는 점의 민감도를 설정합니다. * 점 감지 민감도에 대한 정보는 사용자 매뉴얼을 참조하십시오. */ set_threshold_value(3); /* 점 감지 민감도를 얻습니다. */ uint8_t threshold_value = get_threshold_value(); /* 감지된 점 정보를 통신할 포트를 설정합니다. * 이 포트 설정은 SDK의 UDP 서버와 Vicsen Lidar의 대상 포트 모두 변경합니다. */ set_udp_port(33232); /* Vicsen Lidar에서 사용 중인 포트 정보를 얻습니다. */ uint16_t port = get_udp_port(); /* 감지된 점 정보를 통신할 IP 주소를 설정합니다. */ set_udp_ipaddress("192.168.0.18"); /* Vicsen Lidar에서 사용 중인 IP 주소 정보를 얻습니다. */ const char* udp_ip = get_udp_ipaddress(); /* Vicsen Lidar와의 통신으로 얻은 정보를 표시합니다. */ std::cout << "FPGA temperature: " << fpga_temp << std::endl; std::cout << "LD temperature: " << ld_temp << std::endl; std::cout << "Scanner preset: " << (int)scanner_preset << std::endl; std::cout << "Scanner framerate: " << scanner_framerate << std::endl; std::cout << "Speed limit: " << speed_limit << std::endl; std::cout << "Point threshold: " << (int)threshold_value << std::endl; std::cout << "UDP port: " << (int)port << std::endl; std::cout << "UDP IP: " << udp_ip << std::endl; // ROS Timer를 사용하여 주기적으로 메시지를 발행 ros::Rate loop_rate(10); // 10hz로 설정, 0.1 초에 한번 publish. while (ros::ok()) { ros::spinOnce(); // 노드 이벤트 루프 처리 /* Vicsen Lidar와의 통신으로 정보를 주기적으로 업데이트 */ fpga_temp = get_fpga_temperature(); ld_temp = get_ld_temperature(); scanner_preset = get_scanner_preset(); scanner_framerate = get_scanner_framerate(); speed_limit = get_speed_limit(); threshold_value = get_threshold_value(); port = get_udp_port(); udp_ip = get_udp_ipaddress(); /* Vicsen Lidar와의 통신으로 얻은 정보를 실시간으로 출력 */ std::cout << "FPGA temperature: " << fpga_temp << std::endl; std::cout << "LD temperature: " << ld_temp << std::endl; std::cout << "Scanner preset: " << (int)scanner_preset << std::endl; std::cout << "Scanner framerate: " << scanner_framerate << std::endl; std::cout << "Speed limit: " << speed_limit << std::endl; std::cout << "Point threshold: " << (int)threshold_value << std::endl; std::cout << "UDP port: " << (int)port << std::endl; std::cout << "UDP IP: " << udp_ip << std::endl; loop_rate.sleep(); // 주기적으로 루프 실행 } // Deinitialize the Vicsen Lidar vicsen_deinit(); return 0; } and this is a error that I got when I use rosrun rviz rviz -f fmcw and adding the extra pointcloud2, with the frame "fmcw"
WARN] [1697793848.886709774]: Invalid argument passed to canTransform argument source_frame in tf2 frame_ids cannot be empty ``` 
