20220515
自己实际实现一下基于T265+D435i的octomap建图,NX上有让D435i发点云的launch文件也有octomap_server功能包。
发一个这样的tf把T265的坐标系和D435i的坐标系连起来就可以了。 realsense2_camera/launch/rs_d400_and_t265.launch · 马熙/realsense-ros - Gitee.com
D435i的坐标系是 camera_depth_optical_frame
T265的坐标系是 camera_odom_frame
<node pkg="tf" type="static_transform_publisher" name="t265_to_d400" args="0 0 0 0 0 0 /camera_odom_frame /camera_depth_optical_frame 100"/>
先运行 roslaunch realsense2_camera demo_pointcloud.launch
再运行 roslaunch octomap_server octomap_mapping_maxi.launch 并手动打开一个rviz添加话题
直接想强行显示彩色的/octomap_full有报错,不显示,看来还是得改下代码?
不改代码,直接在octomap_mapping_maxi.launch文件里面添加两句,再启动终端会有报错
把这句前面的注释去掉 ROS 八叉树地图构建 - 安装 octomap 和 octomap_server 建图包!-CSDN博客
改完再重新catkin_make编译
新建一个octomap_mapping_maxi1.launch 填入
<!-- Example launch file for octomap_server mapping: Listens to incoming PointCloud2 data and incrementally builds an octomap. The data is sent out in different representations. Copy this file into your workspace and adjust as needed, see www.ros.org/wiki/octomap_server for details --> <launch> <node pkg="octomap_server" type="octomap_server_node" name="octomap_server"> <param name="resolution" value="0.15" /> <!-- fixed map frame (set to 'map' if SLAM or localization running!) --> <param name="frame_id" type="string" value="camera_depth_optical_frame" /> <!-- maximum range to integrate (speedup!) --> <param name="sensor_model/max_range" value="10.0" /> <param name = "height_map" value = "false" /> <param name = "colored_map" value = "true" /> <!-- data source to integrate (PointCloud2) --> <remap from="cloud_in" to="/camera/depth/color/points" /> </node> </launch>
source后运行(就算有二进制安装的octomap_server功能包也没有关系) roslaunch octomap_server octomap_mapping_maxi1.launch 并手动打开一个rivz,添加话题/octomap_full,就可以正常显示彩色地图了,没有报错了
现在就有彩色图显示了
这个时候我移动无人机或者说是D435i建出的图是比较乱的
T265和D435i同时启动,会导致两个节点都挂掉
那我用这个launch来同时启动T265和D435i,注意红框处需要改成D435i,不然找不到,哪怕写成D435都不可以,原本是写的D4.5
launch文件里面加上这句后可以发出D435i的点云,T265和D435i也可以启动
rosrun rqt_tf_tree rqt_tf_tree
T265和D435i同时起起来的tf树是这样的
octomap_mapping_maxi1.launch里的坐标系改了下
<!-- Example launch file for octomap_server mapping: Listens to incoming PointCloud2 data and incrementally builds an octomap. The data is sent out in different representations. Copy this file into your workspace and adjust as needed, see www.ros.org/wiki/octomap_server for details --> <launch> <node pkg="octomap_server" type="octomap_server_node" name="octomap_server"> <param name="resolution" value="0.15" /> <!-- fixed map frame (set to 'map' if SLAM or localization running!) --> <param name="frame_id" type="string" value="t265_odom_frame" /> <!-- maximum range to integrate (speedup!) --> <param name="sensor_model/max_range" value="10.0" /> <param name = "height_map" value = "false" /> <param name = "colored_map" value = "true" /> <!-- data source to integrate (PointCloud2) --> <remap from="cloud_in" to="/camera/depth/color/points" /> </node> <node pkg="tf" type="static_transform_publisher" name="t265_to_d400" args="0 0 0 0 0 0 /camera_odom_frame /camera_depth_optical_frame 100"/> </launch>
因为现在启动rs_d400_t265.launch了,里面有tf发布了,所以octomap_mapping_maxi1.launch里的tf删掉,不然会冲突导致节点挂掉
rs_d400_and_t265.launch发布的D435i点云话题变为/d400/depth/color/points,现在把octomap_mapping_maxi1.launch里面订阅的点云话题名称改一下,不然打开rviz会发现没有octomap地图生成,因为订阅的点云话题不对。
octomap_mapping_maxi1.launch里的坐标系如果是camera_depth_optical_frame,也是/octomap_full话题没有内容,改为t265_odom_frame,rviz(主坐标系选择t265_odom_frame)查看/octomap_full就有东西了
这回我移动无人机,点云没有发生重叠了
我转动无人机,就差不多把整个办公室的地图给建出来了。
显示的这个报错不影响,因为我们没有用map坐标系
此时的octomap_mapping_maxi1.launch
<!-- Example launch file for octomap_server mapping: Listens to incoming PointCloud2 data and incrementally builds an octomap. The data is sent out in different representations. Copy this file into your workspace and adjust as needed, see www.ros.org/wiki/octomap_server for details --> <launch> <node pkg="octomap_server" type="octomap_server_node" name="octomap_server"> <param name="resolution" value="0.15" /> <!-- fixed map frame (set to 'map' if SLAM or localization running!) --> <param name="frame_id" type="string" value="t265_odom_frame" /> <!-- maximum range to integrate (speedup!) --> <param name="sensor_model/max_range" value="10.0" /> <param name = "height_map" value = "false" /> <param name = "colored_map" value = "true" /> <!-- data source to integrate (PointCloud2) --> <remap from="cloud_in" to="/d400/depth/color/points" /> </node> </launch>
此时的rs_d400_and_t265.launch
<launch> <arg name="device_type_camera1" default="t265"/> <arg name="device_type_camera2" default="d435i"/> <!-- Note: using regular expression. match D435, D435i, D415... --> <arg name="serial_no_camera1" default=""/> <arg name="serial_no_camera2" default=""/> <arg name="camera1" default="t265"/> <arg name="camera2" default="d400"/> <arg name="tf_prefix_camera1" default="$(arg camera1)"/> <arg name="tf_prefix_camera2" default="$(arg camera2)"/> <arg name="initial_reset" default="false"/> <arg name="enable_fisheye" default="false"/> <arg name="color_width" default="640"/> <arg name="color_height" default="480"/> <arg name="depth_width" default="640"/> <arg name="depth_height" default="480"/> <arg name="clip_distance" default="-2"/> <arg name="topic_odom_in" default="odom_in"/> <arg name="calib_odom_file" default=""/> <group ns="$(arg camera1)"> <include file="$(find realsense2_camera)/launch/includes/nodelet.launch.xml"> <arg name="device_type" value="$(arg device_type_camera1)"/> <arg name="serial_no" value="$(arg serial_no_camera1)"/> <arg name="tf_prefix" value="$(arg tf_prefix_camera1)"/> <arg name="initial_reset" value="$(arg initial_reset)"/> <arg name="enable_fisheye1" value="$(arg enable_fisheye)"/> <arg name="enable_fisheye2" value="$(arg enable_fisheye)"/> <arg name="topic_odom_in" value="$(arg topic_odom_in)"/> <arg name="calib_odom_file" value="$(arg calib_odom_file)"/> <arg name="enable_pose" value="true"/> </include> </group> <group ns="$(arg camera2)"> <include file="$(find realsense2_camera)/launch/includes/nodelet.launch.xml"> <arg name="device_type" value="$(arg device_type_camera2)"/> <arg name="serial_no" value="$(arg serial_no_camera2)"/> <arg name="tf_prefix" value="$(arg tf_prefix_camera2)"/> <arg name="initial_reset" value="$(arg initial_reset)"/> <arg name="align_depth" value="true"/> <arg name="filters" value="pointcloud"/> <arg name="color_width" value="$(arg color_width)"/> <arg name="color_height" value="$(arg color_height)"/> <arg name="depth_width" value="$(arg depth_width)"/> <arg name="depth_height" value="$(arg depth_height)"/> <arg name="clip_distance" value="$(arg clip_distance)"/> <arg name="enable_pointcloud" value="true"/> </include> </group> <node pkg="tf" type="static_transform_publisher" name="t265_to_d400" args="0 0 0 0 0 0 /$(arg tf_prefix_camera1)_link /$(arg tf_prefix_camera2)_link 100"/> </launch>
整体步骤我再梳理一下 ,首先需要两个功能包,realsense2_camera和octomap_server(这个需要源码编译安装,因为会涉及到修改代码) 对于realsense2_camera,是用到rs_d400_and_t265.launch这个launch文件了,因为需要把T265和D435i同时起起来,如果是想单独起demo_pointcloud.launch和rs_t265.launch会导致两个节点都挂掉。因为我们需要D435i发出点云话题,所以需要在rs_d400_and_t265.launch里D435i部分加上<arg name="enable_pointcloud" value="true"/> ,同时device_type_camera2参数原本是D4.5需要改成D435i,不然说找不到这个设备。这么改完rs_d400_and_t265.launch后,再运行命令roslaunch realsense2_camera rs_d400_and_t265.launch 就可以同时启动D435i和T265了,并且让D435i发出点云话题了,而且这个launch文件里面已经把D435i和T265的坐标系连到一起了,这样也省得自己再去单独发一个/tf了。 启动rs_d400_and_t265.launch的时候有这个显示没有关系,等几秒钟就也会把D435i启动起来,也就是D435i比T265会晚启动几秒钟
对于octomap_server,需要自己源码编译安装,因为需要改代码,这样才能发出彩色的octomap(参考这里的进行修改 ROS 八叉树地图构建 - 安装 octomap 和 octomap_server 建图包!-CSDN博客 ),同时复制octomap_mapping.launch并重名为octomap_mapping_maxi1.launch,octomap_mapping_maxi1.launch内需要修改的地方下面红线有标出,非常关键的一点是,如果要建增量式地图,那么需要frame_id是构建的全局地图的坐标系,这是非常关键的一点。后面自己打开rviz查看增量式地图的时候rviz的基坐标系也应该是这个frame_id,启动命令是 roslaunch octomap_server octomap_mapping_maxi1.launch
发出彩色octomap需要做的改动 ROS 八叉树地图构建 - 安装 octomap 和 octomap_server 建图包!-CSDN博客
实现octomap增量式建图,需要既有点云数据,也要有/tf
二维的占据栅格地图也可以显示出来,但是这么直接生成的二维占据栅格地图感觉占得有点多啊。感觉是把整个三维的都投影到二维上了,房顶投影上去也算是障碍物了。看来这样得到的栅格地图没法用,虽然也有octomap的拼接和叠加
装好fcl之后编译又说缺这个
忽然看到ompl可以二进制安装
我二进制安装了下,发现编译还是说缺ompl
还是通过源码的方式装完了ompl
但是catkin_make依旧找不到ompl
Could not find a package configuration file provided by "OMPL"...-CSDN博客
我这样依旧没有解决,看来你这个还有点折腾。
看了下这个视频
用命令 sudo apt-get install libompl-dev 装了之后再catkin_make,就没有报错说早不到OMPL了
现在是有了这个报错
我换下源
现在的源是
# deb cdrom:[Ubuntu 18.04.5 LTS _Bionic Beaver_ - Release amd64 (20200806.1)]/ bionic main restricted # See http://help.ubuntu.com/community/UpgradeNotes for how to upgrade to # newer versions of the distribution. deb http://cn.archive.ubuntu.com/ubuntu/ bionic main restricted # deb-src http://cn.archive.ubuntu.com/ubuntu/ bionic main restricted ## Major bug fix updates produced after the final release of the ## distribution. deb http://cn.archive.ubuntu.com/ubuntu/ bionic-updates main restricted # deb-src http://cn.archive.ubuntu.com/ubuntu/ bionic-updates main restricted ## N.B. software from this repository is ENTIRELY UNSUPPORTED by the Ubuntu ## team. Also, please note that software in universe WILL NOT receive any ## review or updates from the Ubuntu security team. deb http://cn.archive.ubuntu.com/ubuntu/ bionic universe # deb-src http://cn.archive.ubuntu.com/ubuntu/ bionic universe deb http://cn.archive.ubuntu.com/ubuntu/ bionic-updates universe # deb-src http://cn.archive.ubuntu.com/ubuntu/ bionic-updates universe ## N.B. software from this repository is ENTIRELY UNSUPPORTED by the Ubuntu ## team, and may not be under a free licence. Please satisfy yourself as to ## your rights to use the software. Also, please note that software in ## multiverse WILL NOT receive any review or updates from the Ubuntu ## security team. deb http://cn.archive.ubuntu.com/ubuntu/ bionic multiverse # deb-src http://cn.archive.ubuntu.com/ubuntu/ bionic multiverse deb http://cn.archive.ubuntu.com/ubuntu/ bionic-updates multiverse # deb-src http://cn.archive.ubuntu.com/ubuntu/ bionic-updates multiverse ## N.B. software from this repository may not have been tested as ## extensively as that contained in the main release, although it includes ## newer versions of some applications which may provide useful features. ## Also, please note that software in backports WILL NOT receive any review ## or updates from the Ubuntu security team. deb http://cn.archive.ubuntu.com/ubuntu/ bionic-backports main restricted universe multiverse # deb-src http://cn.archive.ubuntu.com/ubuntu/ bionic-backports main restricted universe multiverse ## Uncomment the following two lines to add software from Canonical's ## 'partner' repository. ## This software is not part of Ubuntu, but is offered by Canonical and the ## respective vendors as a service to Ubuntu users. # deb http://archive.canonical.com/ubuntu bionic partner # deb-src http://archive.canonical.com/ubuntu bionic partner deb http://security.ubuntu.com/ubuntu bionic-security main restricted # deb-src http://security.ubuntu.com/ubuntu bionic-security main restricted deb http://security.ubuntu.com/ubuntu bionic-security universe # deb-src http://security.ubuntu.com/ubuntu bionic-security universe deb http://security.ubuntu.com/ubuntu bionic-security multiverse # deb-src http://security.ubuntu.com/ubuntu bionic-security multiverse deb https://librealsense.intel.com/Debian/apt-repo bionic main # deb-src https://librealsense.intel.com/Debian/apt-repo bionic main
现在换成这样 ubuntu | 镜像站使用帮助 | 清华大学开源软件镜像站 | Tsinghua Open Source Mirror
换了清华源之后,sudo apt-get update之后,可以装 sudo apt-get install libfcl-dev 了 注释掉对应的,然后加上清华源给的
现在是有这个报错