目录
2.运行carto时报错找不到cartographyer_node节点
一、安装篇
在安装源码过程中可能出现的问题有很多下面我将简单的列出我遇到的问题以及解决思路
1.下载过程中用到git clone 时连接拒绝问题
解决:这类问题出现的问题是是使用了proxy代理
git config --global --unset http.proxy
只要取消proxy代理就行了
2.cmake出现的问题
在编译工作空间的时候经常会出现cmake报错。但是cmake出错的问题有很多下面我将进行归类
(1)缺少库(lib**)或者缺少功能包..
出现这类问题很正常,解决办法也很简单只需要把缺少的东西安装上就行了,如果不知道安装什么。首先去看报错处需要什么,然后到ROS.org官网上寻找这个功能包或者库的具体信息。随后跟着官方的教程下载就好了一般是使用 sudo apt-get install (包的名字)
(2)找不到Eigen3的问题
实际上出现这个错误的原因就是功能包内的cmakelist文件中find_package出现了问题,出现问题的原因有两个:一是真的没有找eigen的路径,第二个是因为eigen的版本不符合要求,有些要求eigen3.3以上,有些就是3.3以下非常恶心!
原因一解决办法:
找到该功能包的cmakelist文件在find_package(Eigen3)的上面加上一句设置寻找eigen3文件的路径即可,可以参照该博主的文章
https://blog.csdn.net/weixin_44023934/article/details/121701318
二、文件配置篇
1.运行carto警告[ WARN] [1668694281.982229116]: Could not compute submap fading: "map" passed to lookupTransform argument target_frame does not exist.
并且在rvizi中发现多处报错Submaps: For frame [map]: Frame [map] does not exist"
解决办法:
出现此类问题大概率是文件配置问题 若接入了两个以上的传感器例如激光雷达和imu 建议去检查一下雷达的启动文件或者imu的启动文件 frame_Id是否正确 或者去查看 revo_lds.lua是否配置正确。如果只是接入了单雷达出现此类错误可能是启用了imu数据造成的。一下附上我的文件给大家进行比对。
启动文件.launch一般是在工作空间的scr下launch文件夹中。
首先是弄清自己雷达的数据topic名称和frame_id,可以在雷达启动文件中查看,这里我以镭神lsN10的启动文件lsn10.launch为例:
<launch>
<node name="lsn10" pkg="lsn10" type="lsn10" output="screen" >
<param name="scan_topic" value="horizontal_laser_2d"/> #设置激光数据topic名称
<param name="frame_id" value="horizontal_laser_link"/> #激光坐标
<param name="serial_port" value="/dev/ttyUSB0"/> #雷达连接的串口
<param name="baud_rate" value="230400" /> #雷达连接的串口波特率
<param name="angle_disable_min" value="0.0"/> #雷达裁剪角度开始值
<param name="angle_disable_max" value="0.0"/> #雷达裁剪角度结束值
<param name="min_range" value="0.0"/> #雷达点云距离最小值
<param name="max_range" value="100.0"/> #雷达点云距离最大值
</node>
</launch>
如果接入了imu的话也需查看imu的启动文件:
<launch>
<node pkg="fdilink_ahrs" name="ahrs_driver" type="ahrs_driver" output="screen" >
<!-- 是否输出debug信息 -->
<param name="debug" value="false"/>
<!-- 串口设备,可通过rules.d配置固定。
若使用DETA100,则value="/dev/wheeltec_ch340"
若使用WHEELTEC N系列,则不需要改动 -->
<param name="port" value="/dev/fdilink_ahrs"/>
<!-- 波特率 -->
<param name="baud" value="921600"/>
<!-- 发布的imu话题名 -->
<param name="imu_topic" value="imu"/>
<!-- 发布的imu话题中的frame_id -->
<param name="imu_frame" value="imu_link"/>
<!-- 地磁北的yaw角 -->
<param name="mag_pose_2d_topic" value="/mag_pose_2d"/>
<!-- 欧拉角 -->
<param name="Euler_angles_pub_" value="/euler_angles"/>
<!-- 磁力计磁场强度 -->
<param name="Magnetic_pub_" value="/magnetic"/>
<!-- 发布的数据基于不同设备有不同的坐标系 -->
<param name="device_type" value="1"/> <!-- 0: origin_data, 1: for single imu or ucar in ROS, 2:for Xiao in ROS -->
</node>
</launch>
这里我们明确了
发布的话题名 | frame_id | |
雷达 | horizontal_laser_2d | horizontal_laser_link |
imu | imu | imu_link |
然后我们打开revo_lds.lua
-- Copyright 2016 The Cartographer Authors
--
-- 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.
include "map_builder.lua"
include "trajectory_builder.lua"
options = {
map_builder = MAP_BUILDER,
trajectory_builder = TRAJECTORY_BUILDER,
map_frame = "map",
tracking_frame = "base_link",
published_frame = "base_link",
odom_frame = "odom",
provide_odom_frame = true,
publish_frame_projected_to_2d = false,
use_pose_extrapolator = true,
use_odometry = false,
use_nav_sat = false,
use_landmarks = false,
num_laser_scans = 1,
num_multi_echo_laser_scans = 0,
num_subdivisions_per_laser_scan = 1,
num_point_clouds = 0,
lookup_transform_timeout_sec = 0.2,
submap_publish_period_sec = 0.3,
pose_publish_period_sec = 5e-3,
trajectory_publish_period_sec = 30e-3,
rangefinder_sampling_ratio = 1.,
odometry_sampling_ratio = 1.,
fixed_frame_pose_sampling_ratio = 1.,
imu_sampling_ratio = 1.,
landmarks_sampling_ratio = 1.,
}
MAP_BUILDER.use_trajectory_builder_2d = true
TRAJECTORY_BUILDER_2D.submaps.num_range_data = 35
TRAJECTORY_BUILDER_2D.min_range = 0.3
TRAJECTORY_BUILDER_2D.max_range = 8.
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 1.
TRAJECTORY_BUILDER_2D.use_imu_data = true
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.1
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.translation_delta_cost_weight = 10.
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.rotation_delta_cost_weight = 1e-1
POSE_GRAPH.optimization_problem.huber_scale = 1e2
POSE_GRAPH.optimize_every_n_nodes = 35
POSE_GRAPH.constraint_builder.min_score = 0.65
return options
其中 tracking_frame = "base_link",
published_frame = "base_link",
都要改成base_link
若用到了imu则TRAJECTORY_BUILDER_2D.use_imu_data = true 这里改成true,如果没有imu数据发布而启用了imu可能会报错。如果不用imu,单雷达的话就改为false。
然后打开demo_revo_lds.lauch文件进行配置
<!--
Copyright 2016 The Cartographer Authors
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.
-->
<launch>
<param name="/use_sim_time" value="false" />
<param name="robot_description"
textfile="$(find cartographer_ros)/urdf/backpack_2d.urdf" />
<node name="robot_state_publisher" pkg="robot_state_publisher"
type="robot_state_publisher" />
<node name="cartographer_node" pkg="cartographer_ros"
type="cartographer_node" args="
-configuration_directory $(find cartographer_ros)/configuration_files
-configuration_basename revo_lds.lua"
output="screen">
<remap from="scan" to="horizontal_laser_2d" />
</node>
<node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
type="cartographer_occupancy_grid_node" args="-resolution 0.05" />
<node name="rviz" pkg="rviz" type="rviz" required="true"
args="-d $(find cartographer_ros)/configuration_files/demo_2d.rviz" />
<!--
<node name="playbag" pkg="rosbag" type="play"
args="-clock $(arg bag_filename)" />
-->
</launch>
需要修改的是
<remap from="scan" to="horizontal_laser_2d" />这里的话题是雷达的话题必须与启动文件中的名字一致
以及 <node name="playbag" pkg="rosbag" type="play"
args="-clock $(arg bag_filename)" />
因为没有用到离线包所以将这一部分注释掉或者直接删掉
如果用到了imu还需配置一下urdf文件夹下的backpack_2d.urdf文件
<!--
Copyright 2016 The Cartographer Authors
Licensed under the Apache License, Version 2.0 (the "License");
<!--
Copyright 2016 The Cartographer Authors
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.
-->
<robot name="cartographer_backpack_2d">
<material name="orange">
<color rgba="1.0 0.5 0.2 1" />
</material>
<material name="gray">
<color rgba="0.2 0.2 0.2 1" />
</material>
<link name="imu_link">
<visual>
<origin xyz="0 0 0" />
<geometry>
<box size="0.06 0.04 0.02" />
</geometry>
<material name="orange" />
</visual>
</link>
<link name="horizontal_laser_link">
<visual>
<origin xyz="0 0 0" />
<geometry>
<cylinder length="0.05" radius="0.03" />
</geometry>
<material name="gray" />
</visual>
</link>
<link name="vertical_laser_link">
<visual>
<origin xyz="0 0 0" />
<geometry>
<cylinder length="0.05" radius="0.03" />
</geometry>
<material name="gray" />
</visual>
</link>
<link name="base_link" />
<joint name="imu_link_joint" type="fixed">
<parent link="base_link" />
<child link="imu_link" />
<origin xyz="0 0 0" />
</joint>
<joint name="horizontal_laser_link_joint" type="fixed">
<parent link="base_link" />
<child link="horizontal_laser_link" />
<origin xyz="0.1007 0 0.0558" />
</joint>
<joint name="vertical_laser_link_joint" type="fixed">
<parent link="base_link" />
<child link="vertical_laser_link" />
<origin rpy="0 -1.570796 3.141593" xyz="0.1007 0 0.1814" />
</joint>
</robot>
其实也就是根据代码将frame_id 和话题名称对应改进去就可以了。值得注意的是初始xyz对建图效果是有影响的,有时候可能影响比较大。另外frame_id对应不好很有可能造成坐标转换出现问题从而在建图过程中出现飞出去的现象。具体可参考另一位博主的文章。
五、2D-lidar+IMU完成cartographer建图(IMU天坑)
2.运行carto时报错找不到cartographyer_node节点
ERROR: cannot launch node of type [cartographer_ros/cartographer_node]: Cannot locate node of type [cartographer_node] in package [cartographer_ros]. Make sure file exists in package path and permission is set to executable (chmod +x)
解决办法:
在刷新环境时setup.bash的目录在 carto_ws/devel_isolated/setup.bash
source重新执行一下刷新环境命令即可
source devel_isolated/setup.bash
3.保存地图时停止数据的采集报错(无法保存地图)
ERROR: Unable to load type [cartographer_ros_msgs/FinishTrajectory].
Have you typed 'make' in [cartographer_ros_msgs]?
首先保存地图时打开一个新的终端进入工作空间,并刷新环境(注意与上一个问题相似,setup.bash在devel_isolated下面
这样就可以保存了
4.rviz运行无法显示雷达点云图
for frame [horizontal_laser_link]: frame [horizontal_laser_link] does not exist
解决办法:
将世界坐标该改成[horizontal_laser_link]就ok了
配置好雷达后运行启动文件后自动打开rivz
找到该启动文件(后缀为.launch)在其中找到
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find lsn10)/rviz/default.rviz" output="screen"/>
的字样然后删除就可以了