diff --git a/.gitignore b/.gitignore index 656301ab4f8d4f30cf20bdff1c12c2dd6e7aacec..0bcf22aaf389e1af17bc2954fb1508f48838615a 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,4 @@ .vscode/ + +result/* \ No newline at end of file diff --git a/Dockerfile b/Dockerfile new file mode 100644 index 0000000000000000000000000000000000000000..2ee53590e0ee6916b7d7570a9fc5828c51baa5c0 --- /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 diff --git a/README.md b/README.md index af460b61b6d5a9abc338553ddc96e6a9cdb202c5..a607dca6aad526c385bd686bbfec7b4be649a74e 100644 --- a/README.md +++ b/README.md @@ -1,15 +1,17 @@ # 简易版建图/定位 +!!! 请查看 refactor 分支，进行了一部分的重构 但是全部还没完成 还在todo中，比master更 简洁了一点 + 主要从[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) # 使用说明 @@ -18,32 +20,25 @@ ```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 ``` -相关依赖请遵循对应系列： - -```bash -# 请先遵循下列进行安装依赖 -catkin build -source ~/workspace/mapping_ws/devel/setup.zsh -``` - -### kinetic +安装相关依赖（一些ROS包和glog） ```bash -apt install ros-kinetic-roslint ros-kinetic-jsk-rviz-plugins ros-kinetic-velodyne-pointcloud ros-kinetic-tf2-geometry-msgs +cd src +./assets/setup_lib.sh ``` -### melodic +然后再编译 ```bash -apt install ros-melodic-roslint ros-melodic-jsk-rviz-plugins ros-melodic-velodyne-pointcloud ros-melodic-tf2-geometry-msgs +cd ~/workspace/mapping_ws +catkin build +source ~/workspace/mapping_ws/devel/setup.zsh ``` - - ## 调参 1. 首先检查数据包有激光雷达信息，`sensor_msgs/PointCloud2` 格式 @@ -97,7 +92,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 0000000000000000000000000000000000000000..9d7a40414c0ac2ef9ac7d32d5ba49706c7bcb564 --- /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 e7b3dcda2287827d70d179de72e2e071a91ca43f..00d6082dfe6f532f7c8e1e78c04f0d326665bcdb 100644 --- a/packages/lidar_localizer/CMakeLists.txt +++ b/packages/lidar_localizer/CMakeLists.txt @@ -15,7 +15,13 @@ endif() find_package(Eigen3 QUIET) find_package(autoware_build_flags REQUIRED) +find_package (glog 0.4.0 REQUIRED) +# AW_CHECK_CUDA() +if(USE_CUDA) + add_definitions(-DCUDA_FOUND) + list(APPEND PCL_OPENMP_PACKAGES ndt_gpu) +endif() if(NOT EIGEN3_FOUND) # Fallback to cmake_modules @@ -44,6 +50,8 @@ find_package(catkin REQUIRED COMPONENTS tf tf_conversions velodyne_pointcloud + rospy + glog ) catkin_package( @@ -56,6 +64,7 @@ catkin_package( ndt_cpu std_msgs velodyne_pointcloud + rospy DEPENDS PCL ) @@ -72,7 +81,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}) @@ -86,7 +95,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}) @@ -110,6 +119,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 @@ -127,4 +140,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/config/ndt_mapping.yaml b/packages/lidar_localizer/config/ndt_mapping.yaml index 3b2f71e3065bfca09fc01bb0853da9db7235273f..b9dfea866ded8945b085d82f6ed1571e000109c0 100644 --- a/packages/lidar_localizer/config/ndt_mapping.yaml +++ b/packages/lidar_localizer/config/ndt_mapping.yaml @@ -1,7 +1,12 @@ lidar_topic: "/velodyne_points" odom_topic: "/vehicle/odom" 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/ndt_mapping.launch b/packages/lidar_localizer/launch/ndt_mapping.launch index 70baea7d7db0c676bcc17d259fa7c9c37b07df6e..ad7fc31b9c7d4f6bbfabeff72b4703e7ab6f9ce7 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/launch/setup_tf.launch b/packages/lidar_localizer/launch/setup_tf.launch index f6119d76d7efd7dfdbb113b531693a7128c10a1c..50850d83cb0fa28b43d6dab4dca926e083f6490c 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 ec8931ea3db7da897a3c56acf92451937abb0e6b..17b3896c321e2b61c46c222b9e8aefea8467689f 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) { @@ -1150,16 +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); - 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 4c7d67ffe31de6ac33254768c0ef745fc1a0df55..6f6e11f563894ccfb5c0c6690b06894d945e431d 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 0000000000000000000000000000000000000000..9d5471e65ed623fcf31ff4cd57bcd656c3f4c9b7 --- /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 d13de6c32df8bb221901cca13099aecd254d5e25..fd1e4c6ccb56e5ee97f8d751abbc8fcd460264ad 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