From 6d6e46db6baff6f024769694bdb06ed4a0d965ea Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kal=20=E8=B5=96=E5=86=A0=E5=AE=8F?= Date: Fri, 16 Sep 2022 13:51:55 +0800 Subject: [PATCH] feat(*): migrate to ros2 humble --- launch/ydlidar_launch.py | 10 ++++---- launch/ydlidar_launch_view.py | 14 +++++----- params/ydlidar.yaml | 6 ++--- src/ydlidar_ros2_driver_node.cpp | 44 ++++++++++++++++---------------- 4 files changed, 37 insertions(+), 37 deletions(-) diff --git a/launch/ydlidar_launch.py b/launch/ydlidar_launch.py index 92d064b..579c4f4 100644 --- a/launch/ydlidar_launch.py +++ b/launch/ydlidar_launch.py @@ -36,16 +36,16 @@ def generate_launch_description(): description='FPath to the ROS2 parameters file to use.') driver_node = LifecycleNode(package='ydlidar_ros2_driver', - node_executable='ydlidar_ros2_driver_node', - node_name='ydlidar_ros2_driver_node', + executable='ydlidar_ros2_driver_node', + name='ydlidar_ros2_driver_node', output='screen', emulate_tty=True, parameters=[parameter_file], - node_namespace='/', + namespace='/', ) tf2_node = Node(package='tf2_ros', - node_executable='static_transform_publisher', - node_name='static_tf_pub_laser', + executable='static_transform_publisher', + name='static_tf_pub_laser', arguments=['0', '0', '0.02','0', '0', '0', '1','base_link','laser_frame'], ) diff --git a/launch/ydlidar_launch_view.py b/launch/ydlidar_launch_view.py index a37a99c..ede6764 100644 --- a/launch/ydlidar_launch_view.py +++ b/launch/ydlidar_launch_view.py @@ -37,21 +37,21 @@ def generate_launch_description(): description='FPath to the ROS2 parameters file to use.') driver_node = LifecycleNode(package='ydlidar_ros2_driver', - node_executable='ydlidar_ros2_driver_node', - node_name='ydlidar_ros2_driver_node', + executable='ydlidar_ros2_driver_node', + name='ydlidar_ros2_driver_node', output='screen', emulate_tty=True, parameters=[parameter_file], - node_namespace='/', + namespace='/', ) tf2_node = Node(package='tf2_ros', - node_executable='static_transform_publisher', - node_name='static_tf_pub_laser', + executable='static_transform_publisher', + name='static_tf_pub_laser', arguments=['0', '0', '0.02','0', '0', '0', '1','base_link','laser_frame'], ) rviz2_node = Node(package='rviz2', - node_executable='rviz2', - node_name='rviz2', + executable='rviz2', + name='rviz2', arguments=['-d', rviz_config_file], ) diff --git a/params/ydlidar.yaml b/params/ydlidar.yaml index cef0f58..586d993 100644 --- a/params/ydlidar.yaml +++ b/params/ydlidar.yaml @@ -4,11 +4,11 @@ ydlidar_ros2_driver_node: frame_id: laser_frame ignore_array: "" baudrate: 230400 - lidar_type: 1 - device_type: 0 + lidar_type: 1 # 0:TYPE_TOF, 1:TYPE_TRIANGLE 2:TYPE_TOF_NET + device_type: 0 # 0:YDLIDAR_TYPE_SERIAL, 1:YDLIDAR_TYPE_TCP sample_rate: 9 abnormal_check_count: 4 - resolution_fixed: true + fixed_resolution: true reversion: true inverted: true auto_reconnect: true diff --git a/src/ydlidar_ros2_driver_node.cpp b/src/ydlidar_ros2_driver_node.cpp index 918eaed..80f7a6d 100644 --- a/src/ydlidar_ros2_driver_node.cpp +++ b/src/ydlidar_ros2_driver_node.cpp @@ -41,45 +41,45 @@ int main(int argc, char *argv[]) { CYdLidar laser; std::string str_optvalue = "/dev/ydlidar"; - node->declare_parameter("port"); + node->declare_parameter("port"); node->get_parameter("port", str_optvalue); ///lidar port laser.setlidaropt(LidarPropSerialPort, str_optvalue.c_str(), str_optvalue.size()); ///ignore array str_optvalue = ""; - node->declare_parameter("ignore_array"); + node->declare_parameter("ignore_array"); node->get_parameter("ignore_array", str_optvalue); laser.setlidaropt(LidarPropIgnoreArray, str_optvalue.c_str(), str_optvalue.size()); std::string frame_id = "laser_frame"; - node->declare_parameter("frame_id"); + node->declare_parameter("frame_id"); node->get_parameter("frame_id", frame_id); //////////////////////int property///////////////// /// lidar baudrate int optval = 230400; - node->declare_parameter("baudrate"); + node->declare_parameter("baudrate"); node->get_parameter("baudrate", optval); laser.setlidaropt(LidarPropSerialBaudrate, &optval, sizeof(int)); /// tof lidar optval = TYPE_TRIANGLE; - node->declare_parameter("lidar_type"); + node->declare_parameter("lidar_type"); node->get_parameter("lidar_type", optval); laser.setlidaropt(LidarPropLidarType, &optval, sizeof(int)); /// device type optval = YDLIDAR_TYPE_SERIAL; - node->declare_parameter("device_type"); + node->declare_parameter("device_type"); node->get_parameter("device_type", optval); laser.setlidaropt(LidarPropDeviceType, &optval, sizeof(int)); /// sample rate optval = 9; - node->declare_parameter("sample_rate"); + node->declare_parameter("sample_rate"); node->get_parameter("sample_rate", optval); laser.setlidaropt(LidarPropSampleRate, &optval, sizeof(int)); /// abnormal count optval = 4; - node->declare_parameter("abnormal_check_count"); + node->declare_parameter("abnormal_check_count"); node->get_parameter("abnormal_check_count", optval); laser.setlidaropt(LidarPropAbnormalCheckCount, &optval, sizeof(int)); @@ -87,66 +87,66 @@ int main(int argc, char *argv[]) { //////////////////////bool property///////////////// /// fixed angle resolution bool b_optvalue = false; - node->declare_parameter("fixed_resolution"); + node->declare_parameter("fixed_resolution"); node->get_parameter("fixed_resolution", b_optvalue); laser.setlidaropt(LidarPropFixedResolution, &b_optvalue, sizeof(bool)); /// rotate 180 b_optvalue = true; - node->declare_parameter("reversion"); + node->declare_parameter("reversion"); node->get_parameter("reversion", b_optvalue); laser.setlidaropt(LidarPropReversion, &b_optvalue, sizeof(bool)); /// Counterclockwise b_optvalue = true; - node->declare_parameter("inverted"); + node->declare_parameter("inverted"); node->get_parameter("inverted", b_optvalue); laser.setlidaropt(LidarPropInverted, &b_optvalue, sizeof(bool)); b_optvalue = true; - node->declare_parameter("auto_reconnect"); + node->declare_parameter("auto_reconnect"); node->get_parameter("auto_reconnect", b_optvalue); laser.setlidaropt(LidarPropAutoReconnect, &b_optvalue, sizeof(bool)); /// one-way communication b_optvalue = false; - node->declare_parameter("isSingleChannel"); + node->declare_parameter("isSingleChannel"); node->get_parameter("isSingleChannel", b_optvalue); laser.setlidaropt(LidarPropSingleChannel, &b_optvalue, sizeof(bool)); /// intensity b_optvalue = false; - node->declare_parameter("intensity"); + node->declare_parameter("intensity"); node->get_parameter("intensity", b_optvalue); laser.setlidaropt(LidarPropIntenstiy, &b_optvalue, sizeof(bool)); /// Motor DTR b_optvalue = false; - node->declare_parameter("support_motor_dtr"); + node->declare_parameter("support_motor_dtr"); node->get_parameter("support_motor_dtr", b_optvalue); laser.setlidaropt(LidarPropSupportMotorDtrCtrl, &b_optvalue, sizeof(bool)); //////////////////////float property///////////////// /// unit: ° float f_optvalue = 180.0f; - node->declare_parameter("angle_max"); + node->declare_parameter("angle_max"); node->get_parameter("angle_max", f_optvalue); laser.setlidaropt(LidarPropMaxAngle, &f_optvalue, sizeof(float)); f_optvalue = -180.0f; - node->declare_parameter("angle_min"); + node->declare_parameter("angle_min"); node->get_parameter("angle_min", f_optvalue); laser.setlidaropt(LidarPropMinAngle, &f_optvalue, sizeof(float)); /// unit: m f_optvalue = 64.f; - node->declare_parameter("range_max"); + node->declare_parameter("range_max"); node->get_parameter("range_max", f_optvalue); laser.setlidaropt(LidarPropMaxRange, &f_optvalue, sizeof(float)); f_optvalue = 0.1f; - node->declare_parameter("range_min"); + node->declare_parameter("range_min"); node->get_parameter("range_min", f_optvalue); laser.setlidaropt(LidarPropMinRange, &f_optvalue, sizeof(float)); /// unit: Hz f_optvalue = 10.f; - node->declare_parameter("frequency"); + node->declare_parameter("frequency"); node->get_parameter("frequency", f_optvalue); laser.setlidaropt(LidarPropScanFrequency, &f_optvalue, sizeof(float)); bool invalid_range_is_inf = false; - node->declare_parameter("invalid_range_is_inf"); + node->declare_parameter("invalid_range_is_inf"); node->get_parameter("invalid_range_is_inf", invalid_range_is_inf); @@ -157,7 +157,7 @@ int main(int argc, char *argv[]) { RCLCPP_ERROR(node->get_logger(), "%s\n", laser.DescribeError()); } - auto laser_pub = node->create_publisher("scan", rclcpp::SensorDataQoS()); + auto laser_pub = node->create_publisher("scan", rclcpp::QoS(rclcpp::SensorDataQoS())); auto stop_scan_service = [&laser](const std::shared_ptr request_header,