From d93fe4573a6d4190abc30ae565d53d373e32ec9b Mon Sep 17 00:00:00 2001 From: Woojin Wie Date: Tue, 28 Nov 2023 18:24:14 +0900 Subject: [PATCH 1/3] frame_id by argument --- include/lipkg.h | 3 +++ src/lipkg.cpp | 2 +- src/main.cpp | 7 ++++++- 3 files changed, 10 insertions(+), 2 deletions(-) diff --git a/include/lipkg.h b/include/lipkg.h index b653f71..9db0b61 100644 --- a/include/lipkg.h +++ b/include/lipkg.h @@ -93,6 +93,8 @@ class LiPkg sensor_msgs::msg::LaserScan GetLaserScan() {return output;} void setStamp(rclcpp::Time timeStamp) {output.header.stamp = timeStamp;} + void setFrameId(std::string frameId) {frame_id = frameId;} + private: uint16_t mTimestamp; double mSpeed; @@ -105,6 +107,7 @@ class LiPkg FrameData mFrameData; sensor_msgs::msg::LaserScan output; void ToLaserscan(std::vector < PointData > src); + std::string frame_id; }; class LD00_LiPkg: public LiPkg diff --git a/src/lipkg.cpp b/src/lipkg.cpp index 836fb22..6a0ac91 100644 --- a/src/lipkg.cpp +++ b/src/lipkg.cpp @@ -230,7 +230,7 @@ void LiPkg::ToLaserscan(std::vector src) // Calculate the number of scanning points unsigned int beam_size = ceil((angle_max - angle_min) / angle_increment); // output.header.stamp = rclcpp::Time::now(); - output.header.frame_id = "base_scan"; + output.header.frame_id = frame_id; output.angle_min = angle_min; output.angle_max = angle_max; output.range_min = range_min; diff --git a/src/main.cpp b/src/main.cpp index d52c391..75c2350 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -30,10 +30,15 @@ int main(int argc, char ** argv) rclcpp::Publisher::SharedPtr lidar_pub; LiPkg * pkg; - std::string product; + std::string product, frame_id; int32_t ver = 8; pkg = new LD08_LiPkg; + node->declare_parameter("frame_id"); + node->get_parameter_or("frame_id", frame_id, "base_scan"); + + pkg->setFrameId(frame_id); + CmdInterfaceLinux cmd_port(ver); std::vector> device_list; std::string port_name; From 4ef45e98639e36629d4c7b6c2bbb7dd8853d430b Mon Sep 17 00:00:00 2001 From: Woojin Wie Date: Tue, 28 Nov 2023 18:47:22 +0900 Subject: [PATCH 2/3] Update ld08.launch.py --- launch/ld08.launch.py | 20 +++++++++++++------- 1 file changed, 13 insertions(+), 7 deletions(-) diff --git a/launch/ld08.launch.py b/launch/ld08.launch.py index cdee458..e17ccef 100644 --- a/launch/ld08.launch.py +++ b/launch/ld08.launch.py @@ -18,15 +18,21 @@ from launch import LaunchDescription +from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node def generate_launch_description(): + frame_id = LaunchConfiguration("frame_id", default="base_scan") - return LaunchDescription([ - Node( - package='ld08_driver', - executable='ld08_driver', - name='ld08_driver', - output='screen'), - ]) + return LaunchDescription( + [ + Node( + package="ld08_driver", + executable="ld08_driver", + name="ld08_driver", + parameters=[{"frame_id": frame_id}], + output="screen", + ), + ] + ) From d5bfb57d0f396ecf917f9dda7b5425fb71085201 Mon Sep 17 00:00:00 2001 From: kes Date: Wed, 29 Nov 2023 21:04:35 +0900 Subject: [PATCH 3/3] add namespace configuration --- launch/ld08.launch.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/launch/ld08.launch.py b/launch/ld08.launch.py index e17ccef..8e6b337 100644 --- a/launch/ld08.launch.py +++ b/launch/ld08.launch.py @@ -24,10 +24,12 @@ def generate_launch_description(): frame_id = LaunchConfiguration("frame_id", default="base_scan") + namespace = LaunchConfiguration('namespace') return LaunchDescription( [ Node( + namespace=namespace, package="ld08_driver", executable="ld08_driver", name="ld08_driver",