From b676f926eabcfa8fee5e774332d083e13f33a71a Mon Sep 17 00:00:00 2001 From: Kin Date: Wed, 20 Apr 2022 18:06:59 +0800 Subject: [PATCH 1/7] update readme --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index af460b6..a94191f 100644 --- a/README.md +++ b/README.md @@ -18,7 +18,7 @@ ```bash mkdir -p ~/workspace/mapping_ws cd ~/workspace/mapping_ws -git clone git@bitbucket.org:kin_zhang/mapping_ws.git +git clone https://gitee.com/kin_zhang/mapping_ws.git mv mapping_ws src ``` -- Gitee From fd7e91b79adbcf297434049355307af9089289d8 Mon Sep 17 00:00:00 2001 From: KinZhang Date: Tue, 28 Jun 2022 23:14:38 +0800 Subject: [PATCH 2/7] =?UTF-8?q?add=20pose=20lidar=20point=20=E4=B8=BB?= =?UTF-8?q?=E8=A6=81=E6=98=AF=20=E4=B8=8Eodom=E4=B9=8B=E9=97=B4=E6=97=B6?= =?UTF-8?q?=E9=97=B4=E6=88=B3=E5=8C=B9=E9=85=8D?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- packages/lidar_localizer/config/ndt_mapping.yaml | 1 + packages/lidar_localizer/launch/ndt_mapping.launch | 2 +- packages/lidar_localizer/nodes/ndt_mapping/ndt_mapping.cpp | 7 ++++++- 3 files changed, 8 insertions(+), 2 deletions(-) diff --git a/packages/lidar_localizer/config/ndt_mapping.yaml b/packages/lidar_localizer/config/ndt_mapping.yaml index 3b2f71e..dc9b73e 100644 --- a/packages/lidar_localizer/config/ndt_mapping.yaml +++ b/packages/lidar_localizer/config/ndt_mapping.yaml @@ -1,5 +1,6 @@ lidar_topic: "/velodyne_points" odom_topic: "/vehicle/odom" +odom_lidar_topic: "/odom_lidar" imu_topic: "/imu/data" output_odom_topic: "/guess_pose_odom" # will republish point with now timestamp diff --git a/packages/lidar_localizer/launch/ndt_mapping.launch b/packages/lidar_localizer/launch/ndt_mapping.launch index 70baea7..ad7fc31 100644 --- a/packages/lidar_localizer/launch/ndt_mapping.launch +++ b/packages/lidar_localizer/launch/ndt_mapping.launch @@ -1,7 +1,7 @@ - + diff --git a/packages/lidar_localizer/nodes/ndt_mapping/ndt_mapping.cpp b/packages/lidar_localizer/nodes/ndt_mapping/ndt_mapping.cpp index ec8931e..3dda39a 100644 --- a/packages/lidar_localizer/nodes/ndt_mapping/ndt_mapping.cpp +++ b/packages/lidar_localizer/nodes/ndt_mapping/ndt_mapping.cpp @@ -127,7 +127,7 @@ static ros::Time callback_start, callback_end, t1_start, t1_end, t2_start, t2_en t5_start, t5_end; static ros::Duration d_callback, d1, d2, d3, d4, d5; -static ros::Publisher ndt_map_pub, current_pose_pub, current_odom_pub, path_viz_pub; +static ros::Publisher ndt_map_pub, current_pose_pub, current_odom_pub, path_viz_pub, odom_lidar_pub; static ros::Publisher guess_pose_linaer_pub; static geometry_msgs::PoseStamped current_pose_msg, guess_pose_msg; static nav_msgs::Path path_buffer; @@ -154,6 +154,7 @@ static std::string _imu_topic = "/imu_raw"; static std::string _lidar_topic = "points_raw"; static std::string _odom_topic = "/vehicle/odom"; static std::string _output_odom_topic = "/guess_pose_odom"; +static std::string _odom_lidar_topic = "/odom_lidar"; static bool replay_bag = false; static double fitness_score; @@ -804,6 +805,7 @@ static void points_callback(const sensor_msgs::PointCloud2::ConstPtr& input) current_pose_msg.pose.orientation.w = q.w(); current_pose_pub.publish(current_pose_msg); + odom_lidar_pub.publish(input); nav_msgs::Odometry odom_to_pub; odom_to_pub.header.stamp = current_scan_time; @@ -1021,6 +1023,8 @@ int main(int argc, char** argv) private_nh.getParam("replay_bag", replay_bag); private_nh.getParam("debug_print", _debug_print); private_nh.getParam("output_odom_topic", _output_odom_topic); + private_nh.getParam("odom_lidar_topic", _odom_lidar_topic); + if(replay_bag) { @@ -1152,6 +1156,7 @@ int main(int argc, char** argv) ndt_map_pub = nh.advertise("/ndt_map", 1000); current_pose_pub = nh.advertise("/current_pose", 1000); + odom_lidar_pub = nh.advertise(_odom_lidar_topic, 1000); current_odom_pub = nh.advertise(_output_odom_topic, 1000); path_viz_pub = nh.advertise("/path", 100000); // ros::Subscriber param_sub = nh.subscribe("config/ndt_mapping", 10, param_callback); -- Gitee From 20594aba39edea9d54929ac7e415733aed959d27 Mon Sep 17 00:00:00 2001 From: KinZhang Date: Tue, 5 Jul 2022 19:56:00 +0800 Subject: [PATCH 3/7] update for save pcd and LOG things --- .gitignore | 2 + README.md | 27 +++++----- {images => assets/readme}/example.png | Bin assets/scripts/setup_lib.sh | 17 +++++++ packages/lidar_localizer/CMakeLists.txt | 17 +++++-- .../lidar_localizer/launch/setup_tf.launch | 4 +- .../nodes/ndt_mapping/ndt_mapping.cpp | 18 +++---- .../nodes/queue_counter/queue_counter.cpp | 4 +- packages/lidar_localizer/nodes/save_pcd.py | 46 ++++++++++++++++++ packages/lidar_localizer/package.xml | 1 + 10 files changed, 102 insertions(+), 34 deletions(-) rename {images => assets/readme}/example.png (100%) create mode 100644 assets/scripts/setup_lib.sh create mode 100755 packages/lidar_localizer/nodes/save_pcd.py diff --git a/.gitignore b/.gitignore index 656301a..0bcf22a 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,4 @@ .vscode/ + +result/* \ No newline at end of file diff --git a/README.md b/README.md index a94191f..f1bfe2f 100644 --- a/README.md +++ b/README.md @@ -2,14 +2,14 @@ 主要从[autoware.ai 1.14版本core_perception](https://github.com/Autoware-AI/core_perception)抽取而来，仅留下与mapping相关代码 -测试系统： +测试系统：【注意Ubuntu 20.04 无法运行，如想在20.04上运行 请从docker里弄 将roscore映射好就行】 - Ubuntu 18.04 ROS melodic - Ubuntu 16.04 ROS kinetic 测试截图： -![](images/example.png) +![](assets/readme/example.png) # 使用说明 @@ -22,28 +22,19 @@ git clone https://gitee.com/kin_zhang/mapping_ws.git mv mapping_ws src ``` -相关依赖请遵循对应系列： +安装相关依赖（一些ROS包和glog） ```bash -# 请先遵循下列进行安装依赖 -catkin build -source ~/workspace/mapping_ws/devel/setup.zsh -``` - -### kinetic - -```bash -apt install ros-kinetic-roslint ros-kinetic-jsk-rviz-plugins ros-kinetic-velodyne-pointcloud ros-kinetic-tf2-geometry-msgs +./setup_lib.sh ``` -### melodic +然后再编译 ```bash -apt install ros-melodic-roslint ros-melodic-jsk-rviz-plugins ros-melodic-velodyne-pointcloud ros-melodic-tf2-geometry-msgs +catkin build +source ~/workspace/mapping_ws/devel/setup.zsh ``` - - ## 调参 1. 首先检查数据包有激光雷达信息，`sensor_msgs/PointCloud2` 格式 @@ -97,7 +88,11 @@ source ~/workspace/mapping_ws/devel/setup.zsh roslaunch lidar_localizer ndt_mapping.launch ``` +如需要保存图片请再开一个终端并运行： +```bash +rosrun lidar_localizer save_pcd.py +``` --- diff --git a/images/example.png b/assets/readme/example.png similarity index 100% rename from images/example.png rename to assets/readme/example.png diff --git a/assets/scripts/setup_lib.sh b/assets/scripts/setup_lib.sh new file mode 100644 index 0000000..9d7a404 --- /dev/null +++ b/assets/scripts/setup_lib.sh @@ -0,0 +1,17 @@ + +# ros package +apt install ros-$ROS_DISTRO-roslint ros-$ROS_DISTRO-jsk-rviz-plugins ros-$ROS_DISTRO-velodyne-pointcloud ros-$ROS_DISTRO-tf2-geometry-msgs + +# install glog +cd ~ +mkdir ndt_lib +cd ndt_lib +git clone https://github.com/google/glog.git +git fetch --all --tags +git checkout tags/v0.4.0 -b v0.4.0 +mkdir build +cmake .. +make +install make +cd ~ +rm -rf ndt_lib \ No newline at end of file diff --git a/packages/lidar_localizer/CMakeLists.txt b/packages/lidar_localizer/CMakeLists.txt index 9693faa..735897d 100644 --- a/packages/lidar_localizer/CMakeLists.txt +++ b/packages/lidar_localizer/CMakeLists.txt @@ -16,8 +16,8 @@ endif() find_package(CUDA) find_package(Eigen3 QUIET) find_package(autoware_build_flags REQUIRED) - -AW_CHECK_CUDA() +find_package (glog 0.4.0 REQUIRED) +# AW_CHECK_CUDA() if(USE_CUDA) add_definitions(-DCUDA_FOUND) @@ -52,6 +52,8 @@ find_package(catkin REQUIRED COMPONENTS tf tf_conversions velodyne_pointcloud + rospy + glog ) catkin_package( @@ -65,6 +67,7 @@ catkin_package( ndt_tku std_msgs velodyne_pointcloud + rospy DEPENDS PCL ) @@ -81,7 +84,7 @@ target_link_libraries(ndt_matching ${catkin_LIBRARIES}) add_dependencies(ndt_matching ${catkin_EXPORTED_TARGETS}) add_executable(ndt_mapping nodes/ndt_mapping/ndt_mapping.cpp) -target_link_libraries(ndt_mapping ${catkin_LIBRARIES}) +target_link_libraries(ndt_mapping ${catkin_LIBRARIES} glog) add_dependencies(ndt_mapping ${catkin_EXPORTED_TARGETS}) if(USE_CUDA) @@ -100,7 +103,7 @@ target_link_libraries(approximate_ndt_mapping ${catkin_LIBRARIES}) add_dependencies(approximate_ndt_mapping ${catkin_EXPORTED_TARGETS}) add_executable(queue_counter nodes/queue_counter/queue_counter.cpp) -target_link_libraries(queue_counter ${catkin_LIBRARIES}) +target_link_libraries(queue_counter ${catkin_LIBRARIES} glog) add_dependencies(queue_counter ${catkin_EXPORTED_TARGETS}) add_executable(ndt_matching_tku nodes/ndt_matching_tku/ndt_matching_tku.cpp) @@ -135,6 +138,10 @@ add_executable(icp_matching nodes/icp_matching/icp_matching.cpp) target_link_libraries(icp_matching ${catkin_LIBRARIES}) add_dependencies(icp_matching ${catkin_EXPORTED_TARGETS}) +catkin_install_python(PROGRAMS nodes/save_pcd.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + install( TARGETS approximate_ndt_mapping @@ -154,4 +161,4 @@ install( install(DIRECTORY launch/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch PATTERN ".svn" EXCLUDE -) +) \ No newline at end of file diff --git a/packages/lidar_localizer/launch/setup_tf.launch b/packages/lidar_localizer/launch/setup_tf.launch index f6119d7..b30f0a9 100644 --- a/packages/lidar_localizer/launch/setup_tf.launch +++ b/packages/lidar_localizer/launch/setup_tf.launch @@ -8,9 +8,9 @@ - + - + diff --git a/packages/lidar_localizer/nodes/ndt_mapping/ndt_mapping.cpp b/packages/lidar_localizer/nodes/ndt_mapping/ndt_mapping.cpp index 3dda39a..17b3896 100644 --- a/packages/lidar_localizer/nodes/ndt_mapping/ndt_mapping.cpp +++ b/packages/lidar_localizer/nodes/ndt_mapping/ndt_mapping.cpp @@ -1154,17 +1154,17 @@ int main(int argc, char** argv) map.header.frame_id = "map"; - ndt_map_pub = nh.advertise("/ndt_map", 1000); - current_pose_pub = nh.advertise("/current_pose", 1000); - odom_lidar_pub = nh.advertise(_odom_lidar_topic, 1000); - current_odom_pub = nh.advertise(_output_odom_topic, 1000); - path_viz_pub = nh.advertise("/path", 100000); + ndt_map_pub = nh.advertise("/ndt_map", 10); + current_pose_pub = nh.advertise("/current_pose", 10); + odom_lidar_pub = nh.advertise(_odom_lidar_topic, 10); + current_odom_pub = nh.advertise(_output_odom_topic, 10); + path_viz_pub = nh.advertise("/path", 10); // ros::Subscriber param_sub = nh.subscribe("config/ndt_mapping", 10, param_callback); ros::Subscriber output_sub = nh.subscribe("config/ndt_mapping_output", 10, output_callback); - ros::Subscriber points_sub = nh.subscribe(_lidar_topic, 100000, points_callback); - ros::Subscriber odom_sub = nh.subscribe(_odom_topic, 100000, odom_callback); - ros::Subscriber imu_sub = nh.subscribe(_imu_topic, 100000, imu_callback); - ros::Subscriber twist_sub = nh.subscribe("vehicle/twist", 100000, vehicle_twist_callback); + ros::Subscriber points_sub = nh.subscribe(_lidar_topic, 10, points_callback); + ros::Subscriber odom_sub = nh.subscribe(_odom_topic, 10, odom_callback); + ros::Subscriber imu_sub = nh.subscribe(_imu_topic, 10, imu_callback); + ros::Subscriber twist_sub = nh.subscribe("vehicle/twist", 10, vehicle_twist_callback); ros::spin(); diff --git a/packages/lidar_localizer/nodes/queue_counter/queue_counter.cpp b/packages/lidar_localizer/nodes/queue_counter/queue_counter.cpp index 4c7d67f..6f6e11f 100644 --- a/packages/lidar_localizer/nodes/queue_counter/queue_counter.cpp +++ b/packages/lidar_localizer/nodes/queue_counter/queue_counter.cpp @@ -36,6 +36,7 @@ #include #include #include +#include static int enqueue = 0; static int dequeue = 0; @@ -72,8 +73,7 @@ static void points_callback(const sensor_msgs::PointCloud2::ConstPtr& input) static void ndt_pose_callback(const geometry_msgs::PoseStamped& msg) { dequeue++; - if(_debug_print) - std::cout << "(Processed/Input): (" << dequeue << " / " << enqueue << ")" << std::endl; + LOG(INFO) << "(Processed/Input): (" << dequeue << " / " << enqueue << ")" << std::endl; } diff --git a/packages/lidar_localizer/nodes/save_pcd.py b/packages/lidar_localizer/nodes/save_pcd.py new file mode 100755 index 0000000..9d5471e --- /dev/null +++ b/packages/lidar_localizer/nodes/save_pcd.py @@ -0,0 +1,46 @@ +#!/usr/bin/env python +import rospy +import numpy as np +from autoware_config_msgs.msg import ConfigNDTMappingOutput +from datetime import datetime + + +class MyDialogNDTMapping(): + def __init__(self): + self.filter_rate = 0.0 + self.klass_msg = ConfigNDTMappingOutput + self.pub = rospy.Publisher('config/ndt_mapping_output', self.klass_msg, queue_size=10) + + def OnPcdOutput(self): + now = datetime.now() + string = 'pcd_save' + string += '_'.join(map(lambda x: '%02d' % x, (now.month, now.day, now.hour, now.minute, now.second))) + self.filename = '/root/workspace/mapping_ws/src/result/' + string + + msg = self.klass_msg() + msg.filename = self.filename + msg.filter_res = self.filter_rate + self.pub.publish(msg) + +if __name__ == '__main__': + rospy.init_node('save_pcd', anonymous=True) + diag = MyDialogNDTMapping() + rate = rospy.Rate(1) + stop = False + + while not rospy.is_shutdown() and not stop: + # find the sub node + cout_connect, search_cot = 0, 0 + while cout_connect == 0 and search_cot<1000: + cout_connect = diag.pub.get_num_connections() + search_cot+=1 + + if cout_connect > 0: + print("here is a sub node") + diag.OnPcdOutput() + stop = True + print("=======> pub the save command") + else: + print("cannot found the sub node") + rate.sleep() + diff --git a/packages/lidar_localizer/package.xml b/packages/lidar_localizer/package.xml index d13de6c..fd1e4c6 100644 --- a/packages/lidar_localizer/package.xml +++ b/packages/lidar_localizer/package.xml @@ -24,6 +24,7 @@ pcl_omp_registration pcl_ros roscpp + rospy sensor_msgs std_msgs tf -- Gitee From 0d8a1e384a349a413427f72e16b7fd0f35bd52e3 Mon Sep 17 00:00:00 2001 From: KinZhang Date: Tue, 5 Jul 2022 20:01:54 +0800 Subject: [PATCH 4/7] update readme --- README.md | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index f1bfe2f..d1ba040 100644 --- a/README.md +++ b/README.md @@ -25,12 +25,14 @@ mv mapping_ws src 安装相关依赖（一些ROS包和glog） ```bash -./setup_lib.sh +cd src +./assets/setup_lib.sh ``` 然后再编译 ```bash +cd ~/workspace/mapping_ws catkin build source ~/workspace/mapping_ws/devel/setup.zsh ``` -- Gitee From 07647959f59d8ba81cc40aca3aae81d4490bca96 Mon Sep 17 00:00:00 2001 From: KinZhang Date: Tue, 5 Jul 2022 20:10:31 +0800 Subject: [PATCH 5/7] =?UTF-8?q?fix:=20=E6=9C=89=E6=97=B6=E5=80=99tf?= =?UTF-8?q?=E4=BC=9A=E6=B2=A1=E6=9C=89=E6=94=B6=E5=88=B0=E6=8A=A5=E9=94=99?= =?UTF-8?q?=EF=BC=8C=E5=90=8C=E6=97=B6=E6=B7=BB=E5=8A=A0odom=E5=AF=B9?= =?UTF-8?q?=E5=BA=94=E7=9A=84=E6=97=B6=E9=97=B4=E6=88=B3=E7=9A=84lidar=20t?= =?UTF-8?q?opic=20=E4=BB=A5=E4=BE=BF=E5=BD=95=E5=8C=85=E4=BD=BF=E7=94=A8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- packages/lidar_localizer/config/ndt_mapping.yaml | 6 +++++- packages/lidar_localizer/launch/setup_tf.launch | 2 +- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/packages/lidar_localizer/config/ndt_mapping.yaml b/packages/lidar_localizer/config/ndt_mapping.yaml index dc9b73e..b9dfea8 100644 --- a/packages/lidar_localizer/config/ndt_mapping.yaml +++ b/packages/lidar_localizer/config/ndt_mapping.yaml @@ -1,8 +1,12 @@ lidar_topic: "/velodyne_points" odom_topic: "/vehicle/odom" -odom_lidar_topic: "/odom_lidar" imu_topic: "/imu/data" + + +# output things ================ > +odom_lidar_topic: "/odom_lidar" output_odom_topic: "/guess_pose_odom" + # will republish point with now timestamp replay_bag: true # pcl_generic=0, pcl_anh=1, pcl_anh_gpu=2, pcl_openmp=3 diff --git a/packages/lidar_localizer/launch/setup_tf.launch b/packages/lidar_localizer/launch/setup_tf.launch index b30f0a9..50850d8 100644 --- a/packages/lidar_localizer/launch/setup_tf.launch +++ b/packages/lidar_localizer/launch/setup_tf.launch @@ -8,7 +8,7 @@ - + -- Gitee From baa8b89499a5aa62bd1d57ce6ec98237430dbfc4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E5=BC=A0=E8=81=AA=E6=98=8E?= Date: Tue, 5 Jul 2022 12:15:42 +0000 Subject: [PATCH 6/7] add Dockerfile. TB Test --- Dockerfile | 48 ++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 48 insertions(+) create mode 100644 Dockerfile diff --git a/Dockerfile b/Dockerfile new file mode 100644 index 0000000..2ee5359 --- /dev/null +++ b/Dockerfile @@ -0,0 +1,48 @@ +# check more detail on: https://hub.docker.com/r/nvidia/cuda +FROM nvidia/cuda:10.2-devel-ubuntu18.04 + +# Just in case we need it +ENV DEBIAN_FRONTEND noninteractive + +# install zsh +RUN apt install -y wget git zsh tmux vim g++ +RUN sh -c "$(wget -O- https://github.com/deluan/zsh-in-docker/releases/download/v1.1.2/zsh-in-docker.sh)" -- \ + -t robbyrussell \ + -p git \ + -p ssh-agent \ + -p https://github.com/agkozak/zsh-z \ + -p https://github.com/zsh-users/zsh-autosuggestions \ + -p https://github.com/zsh-users/zsh-completions \ + -p https://github.com/zsh-users/zsh-syntax-highlighting + +# ==========> INSTALL ROS melodic <============= +RUN apt update && apt install -y curl lsb-release +RUN sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' +RUN curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | apt-key add - +RUN apt update && apt install -y ros-melodic-desktop-full +RUN apt-get install -y python-catkin-pkg \ + python-catkin-tools \ + python-empy \ + python-nose \ + python-pip \ + libgtest-dev \ + ros-melodic-catkin \ + python-pip \ + python3-pip \ + ros-melodic-grid-map + +RUN echo "source /opt/ros/melodic/setup.zsh" >> ~/.zshrc +RUN echo "source /opt/ros/melodic/setup.bashrc" >> ~/.bashrc + +# needs to be done before we can apply the patches +RUN git config --global user.email "xxx@163.com" +RUN git config --global user.name "kin-docker" + +RUN mkdir -p ~/workspace/mapping_ws +RUN cd ~/workspace/mapping_ws +RUN git clone https://gitee.com/kin_zhang/mapping_ws.git +RUN mv mapping_ws src + +RUN cd src +RUN ./assets/setup_lib.sh +WORKDIR ~/workspace/mapping_ws \ No newline at end of file -- Gitee From afd85bb6a15069200e45d984880ea1dc0a448ce4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E5=BC=A0=E8=81=AA=E6=98=8E?= Date: Thu, 15 Sep 2022 06:56:08 +0000 Subject: [PATCH 7/7] update README.md. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: 张聪明 --- README.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/README.md b/README.md index d1ba040..a607dca 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,7 @@ # 简易版建图/定位 +!!! 请查看 refactor 分支，进行了一部分的重构 但是全部还没完成 还在todo中，比master更 简洁了一点 + 主要从[autoware.ai 1.14版本core_perception](https://github.com/Autoware-AI/core_perception)抽取而来，仅留下与mapping相关代码 测试系统：【注意Ubuntu 20.04 无法运行，如想在20.04上运行 请从docker里弄 将roscore映射好就行】 -- Gitee