From c937c2d10d06eea8690aff740249902b7658b902 Mon Sep 17 00:00:00 2001 From: Dominik Polic Date: Thu, 30 Jun 2022 19:53:47 +0200 Subject: [PATCH] Proposed fix for issue #1 This is a proposed fix for the issue #1, No ability to set frame_id in the scan message. It's only for the ROS1 version of the driver, but similar solution could probably be implemented for the ROS2 version as well. --- src/ld08_driver.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/ld08_driver.cpp b/src/ld08_driver.cpp index dad59e0..8273b58 100644 --- a/src/ld08_driver.cpp +++ b/src/ld08_driver.cpp @@ -29,6 +29,9 @@ int main(int argc , char **argv) strcpy(product_ver,"LD08"); ros::init(argc, argv, product_ver); ros::NodeHandle nh; /* create a ROS Node */ + ros::NodeHandle priv_nh("~"); + std::string frame_id; + priv_nh.param("frame_id", frame_id, std::string("base_scan")); char topic_name[20]={0}; strcat(topic_name,product_ver); strcat(topic_name,"/scan"); @@ -59,7 +62,7 @@ int main(int argc , char **argv) cmd_port.Open(port_name); sensor_msgs::LaserScan scan; scan.header.stamp = ros::Time::now(); - scan.header.frame_id = "base_scan"; + scan.header.frame_id = frame_id; scan.range_min = 0.0; scan.range_max = 100.0;