记录G4雷达的配置
系统环境为:Ubuntu22.04
1、安装雷达SDK
2、构建 G4 雷达 ROS2 项目工程文件
3、使用Rviz可视化界面显示
YDLidar SDK需要CMake 2.8.2+作为依赖项
sudo apt install cmake pkg-config
如果使用python API,您需要安装python和swig(3.0或更高版本)
sudo apt-get install python swig
sudo apt-get install python-pip
在YDLidar SDK目录中,运行以下命令来编译项目:
git clone https://github.com/YDLIDAR/YDLidar-SDK.git
cd YDLidar-SDK/build
cmake ..
make
sudo make install
注意:如果已经安装了python和swig,sudo make install命令也将安装python API,而无需执行以下操作,在这里建议大家还是使用一次 sudo make install 确认安装成功
至此,若在编译过程中未出现错误,即为SDK安装成功
ydlidar_ros2_driver依赖于ydlidar SDK库。如果从未安装过YDLidar SDK库,则必须首先安装YDLidar SDK库,具体的可以参考上一点:
(1)为github克隆ydlidar_ros2_driver包:
git clone https://github.com/YDLIDAR/ydlidar_ros2_driver.git ydlidar_ros2_ws/src/ydlidar_ros2_driver
(2)构建ydlidar_ros2_driver包
cd ydlidar_ros2_ws
colcon build --symlink-install
(3)程序包环境设置:
将工作空间添加到环境变量里面
source ./install/setup.bash
同样可以使用比较长久的方法:
echo "source ~/ydlidar_ros2_ws/install/setup.bash" >> ~/.bashrc
source ~/.bashrc
在这里 “~/ydlidar_ros2_ws/install/setup.bash路径要对应上自己的工作空间当中的setup.bash位置。
(4)确认要确认包路径已设置,请打印grep-i ROS变量
printenv | grep -i ROS
应该看到类似的内容:OLDPWD=/home/tony/ydlidar_ros2_ws/install
(5)创建串行端口别名[可选]
sudo chmod 0777 src/ydlidar_ros2_driver/startup/*
sudo sh src/ydlidar_ros2_driver/startup/initenv.sh
使用启动文件运行ydlidar_ros2_driver
ros2 launch ydlidar_ros2_driver ydlidar_launch.py
或者
ros2 launch $(ros2 pkg prefix ydlidar_ros2_driver)/share/ydlidar_ros2_driver/launch/ydlidar.py
如果想要运行可视化界面的话可以使用一下命令,单独在终端里运行即可
ros2 launch ydlidar_ros2_driver ydlidar_launch_view.py
ros2 run ydlidar_ros2_driver ydlidar_ros2_driver_client or ros2 topic echo /scan
在编译过程当中,出现了ydlidar_launch_view.py文件或者ydlidar_launch.py文件中的
[ERROR] [launch]: Caught exception in launch LifecycleNode: __init__() missing 1 required keyword-only argument: 'node_executable'
这样的错误警告,将两个文件当中含有node_[后缀名称]更改为[后缀名称即可]
例如,在本次报错当中,需要将LifecycleNode这个节点的node_executable更改为executable即可,其他的node_name同样的更改为 name
#!/usr/bin/python3
# Copyright 2020, EAIBOT
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import LifecycleNode
from launch_ros.actions import Node
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch.actions import LogInfo
import lifecycle_msgs.msg
import os
def generate_launch_description():
share_dir = get_package_share_directory('ydlidar_ros2_driver')
rviz_config_file = os.path.join(share_dir, 'config','ydlidar.rviz')
parameter_file = LaunchConfiguration('params_file')
# node_name = 'ydlidar_ros2_driver_node'
params_declare = DeclareLaunchArgument('params_file',
default_value=os.path.join(
share_dir, 'params', 'ydlidar.yaml'),
description='FPath to the ROS2 parameters file to use.')
driver_node = LifecycleNode(package='ydlidar_ros2_driver',
executable='ydlidar_ros2_driver_node',
name='ydlidar_ros2_driver_node',
output='screen',
emulate_tty=True,
parameters=[parameter_file],
namespace='/',
)
tf2_node = Node(package='tf2_ros',
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',
executable='rviz2',
name='rviz2',
arguments=['-d', rviz_config_file],
)
return LaunchDescription([
params_declare,
driver_node,
tf2_node,
rviz2_node,
])
上图对应的是 "ydlidar_launch_view.py"文件,然后ydlidar_launch.py文件中对应的两个参数,也就是node_executable和node_name 两个变量名字做出修改,修改后如下代码所示:
#!/usr/bin/python3
# Copyright 2020, EAIBOT
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import LifecycleNode
from launch_ros.actions import Node
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch.actions import LogInfo
import lifecycle_msgs.msg
import os
def generate_launch_description():
share_dir = get_package_share_directory('ydlidar_ros2_driver')
parameter_file = LaunchConfiguration('params_file')
# node_name = 'ydlidar_ros2_driver_node'
params_declare = DeclareLaunchArgument('params_file',
default_value=os.path.join(
share_dir, 'params', 'ydlidar.yaml'),
description='FPath to the ROS2 parameters file to use.')
driver_node = LifecycleNode(package='ydlidar_ros2_driver',
executable='ydlidar_ros2_driver_node',
name='ydlidar_ros2_driver_node',
output='screen',
emulate_tty=True,
parameters=[parameter_file],
namespace='/',
)
tf2_node = Node(package='tf2_ros',
executable='static_transform_publisher',
name='static_tf_pub_laser',
arguments=['0', '0', '0.02','0', '0', '0', '1','base_link','laser_frame'],
)
return LaunchDescription([
params_declare,
driver_node,
tf2_node,
])
建议使用 vscode 编辑器对文本进行更改,以便可以重复撤回和更加便携式的文本切换
然后就是在编译过程(colcon build --symlink-install)中,如果出现node节点链接错误的情况,需要更改当前目录下的
ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp 这个文件内容(直接复制替换即可)
/*
* YDLIDAR SYSTEM
* YDLIDAR ROS 2 Node
*
* Copyright 2017 - 2020 EAI TEAM
* http://www.eaibot.com
*
*/
/* Modified for Humble by @lghrainbow 10/2022 */
#ifdef _MSC_VER
#ifndef _USE_MATH_DEFINES
#define _USE_MATH_DEFINES
#endif
#endif
#include "src/CYdLidar.h"
#include
#include
#include
#include
#include "rclcpp/clock.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp/time_source.hpp"
#include "sensor_msgs/msg/laser_scan.hpp"
#include "std_srvs/srv/empty.hpp"
#include
#include
#include
#include
#define ROS2Verision "1.0.2" /* 1.0.1 modified */
int main(int argc, char *argv[]) {
rclcpp::init(argc, argv);
auto node = rclcpp::Node::make_shared("ydlidar_ros2_driver_node");
RCLCPP_INFO(node->get_logger(), "[YDLIDAR INFO] Current ROS Driver Version: %s\n", ((std::string)ROS2Verision).c_str());
CYdLidar laser;
std::string str_optvalue = "/dev/ydlidar";
node->declare_parameter<std::string>("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<std::string>("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<std::string>("frame_id");
node->get_parameter("frame_id", frame_id);
//int property/
/// lidar baudrate
int optval = 230400;
node->declare_parameter<int>("baudrate");
node->get_parameter("baudrate", optval);
laser.setlidaropt(LidarPropSerialBaudrate, &optval, sizeof(int));
/// tof lidar
optval = TYPE_TRIANGLE;
node->declare_parameter<int>("lidar_type");
node->get_parameter("lidar_type", optval);
laser.setlidaropt(LidarPropLidarType, &optval, sizeof(int));
/// device type
optval = YDLIDAR_TYPE_SERIAL;
node->declare_parameter<int>("device_type");
node->get_parameter("device_type", optval);
laser.setlidaropt(LidarPropDeviceType, &optval, sizeof(int));
/// sample rate
optval = 9;
node->declare_parameter<int>("sample_rate");
node->get_parameter("sample_rate", optval);
laser.setlidaropt(LidarPropSampleRate, &optval, sizeof(int));
/// abnormal count
optval = 4;
node->declare_parameter<int>("abnormal_check_count");
node->get_parameter("abnormal_check_count", optval);
laser.setlidaropt(LidarPropAbnormalCheckCount, &optval, sizeof(int));
//bool property/
/// fixed angle resolution
bool b_optvalue = false;
node->declare_parameter<bool>("fixed_resolution");
node->get_parameter("fixed_resolution", b_optvalue);
laser.setlidaropt(LidarPropFixedResolution, &b_optvalue, sizeof(bool));
/// rotate 180
b_optvalue = true;
node->declare_parameter<bool>("reversion");
node->get_parameter("reversion", b_optvalue);
laser.setlidaropt(LidarPropReversion, &b_optvalue, sizeof(bool));
/// Counterclockwise
b_optvalue = true;
node->declare_parameter<bool>("inverted");
node->get_parameter("inverted", b_optvalue);
laser.setlidaropt(LidarPropInverted, &b_optvalue, sizeof(bool));
b_optvalue = true;
node->declare_parameter<bool>("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<bool>("isSingleChannel");
node->get_parameter("isSingleChannel", b_optvalue);
laser.setlidaropt(LidarPropSingleChannel, &b_optvalue, sizeof(bool));
/// intensity
b_optvalue = false;
node->declare_parameter<bool>("intensity");
node->get_parameter("intensity", b_optvalue);
laser.setlidaropt(LidarPropIntenstiy, &b_optvalue, sizeof(bool));
/// Motor DTR
b_optvalue = false;
node->declare_parameter<bool>("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<float>("angle_max");
node->get_parameter("angle_max", f_optvalue);
laser.setlidaropt(LidarPropMaxAngle, &f_optvalue, sizeof(float));
f_optvalue = -180.0f;
node->declare_parameter<float>("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<float>("range_max");
node->get_parameter("range_max", f_optvalue);
laser.setlidaropt(LidarPropMaxRange, &f_optvalue, sizeof(float));
f_optvalue = 0.1f;
node->declare_parameter<float>("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<float>("frequency");
node->get_parameter("frequency", f_optvalue);
laser.setlidaropt(LidarPropScanFrequency, &f_optvalue, sizeof(float));
bool invalid_range_is_inf = false;
node->declare_parameter<bool>("invalid_range_is_inf");
node->get_parameter("invalid_range_is_inf", invalid_range_is_inf);
bool ret = laser.initialize();
if (ret) {
ret = laser.turnOn();
} else {
RCLCPP_ERROR(node->get_logger(), "%s\n", laser.DescribeError());
}
auto laser_pub = node->create_publisher<sensor_msgs::msg::LaserScan>("scan", rclcpp::QoS(rclcpp::SensorDataQoS()));
auto stop_scan_service =
[&laser](const std::shared_ptr<rmw_request_id_t> request_header,
const std::shared_ptr<std_srvs::srv::Empty::Request> req,
std::shared_ptr<std_srvs::srv::Empty::Response> response) -> bool
{
return laser.turnOff();
};
auto stop_service = node->create_service<std_srvs::srv::Empty>("stop_scan",stop_scan_service);
auto start_scan_service =
[&laser](const std::shared_ptr<rmw_request_id_t> request_header,
const std::shared_ptr<std_srvs::srv::Empty::Request> req,
std::shared_ptr<std_srvs::srv::Empty::Response> response) -> bool
{
return laser.turnOn();
};
auto start_service = node->create_service<std_srvs::srv::Empty>("start_scan",start_scan_service);
rclcpp::WallRate loop_rate(20);
while (ret && rclcpp::ok()) {
LaserScan scan;//
if (laser.doProcessSimple(scan)) {
auto scan_msg = std::make_shared<sensor_msgs::msg::LaserScan>();
scan_msg->header.stamp.sec = RCL_NS_TO_S(scan.stamp);
scan_msg->header.stamp.nanosec = scan.stamp - RCL_S_TO_NS(scan_msg->header.stamp.sec);
scan_msg->header.frame_id = frame_id;
scan_msg->angle_min = scan.config.min_angle;
scan_msg->angle_max = scan.config.max_angle;
scan_msg->angle_increment = scan.config.angle_increment;
scan_msg->scan_time = scan.config.scan_time;
scan_msg->time_increment = scan.config.time_increment;
scan_msg->range_min = scan.config.min_range;
scan_msg->range_max = scan.config.max_range;
int size = (scan.config.max_angle - scan.config.min_angle)/ scan.config.angle_increment + 1;
scan_msg->ranges.resize(size);
scan_msg->intensities.resize(size);
for(size_t i=0; i < scan.points.size(); i++) {
int index = std::ceil((scan.points[i].angle - scan.config.min_angle)/scan.config.angle_increment);
if(index >=0 && index < size) {
scan_msg->ranges[index] = scan.points[i].range;
scan_msg->intensities[index] = scan.points[i].intensity;
}
}
laser_pub->publish(*scan_msg);
} else {
RCLCPP_ERROR(node->get_logger(), "Failed to get scan");
}
if(!rclcpp::ok()) {
break;
}
rclcpp::spin_some(node);
loop_rate.sleep();
}
RCLCPP_INFO(node->get_logger(), "[YDLIDAR INFO] Now YDLIDAR is stopping .......");
laser.turnOff();
laser.disconnecting();
rclcpp::shutdown();
return 0;
}
完结撒花
Redamancy