├── .gitignore ├── LICENSE ├── README.md ├── ROS ├── .gitignore ├── build.sh ├── rename.py ├── run_camera.sh ├── run_collect.sh ├── run_lidar.sh └── src │ ├── CMakeLists.txt │ ├── calibration │ └── thomas_calibration │ │ ├── .gitignore │ │ ├── CMakeLists.txt │ │ ├── launch │ │ └── collect.launch │ │ ├── package.xml │ │ └── script │ │ ├── CLEmain.m │ │ ├── bag2kitti.m │ │ ├── calib2kitti.m │ │ ├── calibMat2CV.m │ │ ├── calibStereo.m │ │ ├── calibrator.py │ │ ├── cameracalibrator.py │ │ ├── checkRectify.m │ │ ├── compress.m │ │ ├── convert.m │ │ ├── costCLextrinsic.m │ │ ├── costReproj.m │ │ ├── demo.m │ │ ├── icp2kitti.m │ │ ├── readCalibrationFromOpenCV.m │ │ ├── testCalibration.m │ │ └── writeMat.m │ ├── camera │ └── mynteye_wrapper_d │ │ ├── CMakeLists.txt │ │ ├── launch │ │ ├── display.launch │ │ ├── mynteye.launch │ │ └── slam │ │ │ ├── orb_slam2.launch │ │ │ ├── vins_fusion.launch │ │ │ └── vins_mono.launch │ │ ├── mesh │ │ ├── D-0315.mtl │ │ └── D-0315.obj │ │ ├── msg │ │ └── Temp.msg │ │ ├── nodelet_plugins.xml │ │ ├── package.xml │ │ ├── rviz │ │ └── mynteye.rviz │ │ ├── src │ │ ├── configuru.hpp │ │ ├── mynteye_listener.cc │ │ ├── mynteye_wrapper_node.cc │ │ ├── mynteye_wrapper_nodelet.cc │ │ ├── pointcloud_generator.cc │ │ └── pointcloud_generator.h │ │ └── srv │ │ └── GetParams.srv │ ├── controller │ ├── CMakeLists.txt │ ├── msg │ │ └── Cmd.msg │ ├── package.xml │ └── scripts │ │ ├── can_bus.py │ │ ├── get_permission.sh │ │ ├── run.py │ │ └── xbox.py │ ├── gps │ ├── CMakeLists.txt │ ├── msg │ │ └── GPS.msg │ ├── package.xml │ └── scripts │ │ ├── get_permission.sh │ │ └── run_gps.py │ ├── imu │ ├── CMakeLists.txt │ ├── msg │ │ └── IMU.msg │ ├── package.xml │ └── scripts │ │ ├── mtdef.py │ │ ├── mtdevice.py │ │ └── mtnode.py │ ├── lidar │ ├── velodyne │ │ ├── CHANGELOG.rst │ │ ├── CMakeLists.txt │ │ └── package.xml │ ├── velodyne_driver │ │ ├── CHANGELOG.rst │ │ ├── CMakeLists.txt │ │ ├── cfg │ │ │ └── VelodyneNode.cfg │ │ ├── include │ │ │ └── velodyne_driver │ │ │ │ ├── driver.h │ │ │ │ ├── input.h │ │ │ │ ├── ring_sequence.h │ │ │ │ └── time_conversion.hpp │ │ ├── launch │ │ │ └── nodelet_manager.launch │ │ ├── nodelet_velodyne.xml │ │ ├── package.xml │ │ ├── src │ │ │ ├── driver │ │ │ │ ├── CMakeLists.txt │ │ │ │ ├── driver.cc │ │ │ │ ├── nodelet.cc │ │ │ │ └── velodyne_node.cc │ │ │ ├── lib │ │ │ │ ├── CMakeLists.txt │ │ │ │ └── input.cc │ │ │ └── vdump │ │ └── tests │ │ │ ├── diagnostic_agg.yaml │ │ │ ├── pcap_32e_node_hertz.test │ │ │ ├── pcap_32e_nodelet_hertz.test │ │ │ ├── pcap_node_hertz.test │ │ │ ├── pcap_nodelet_hertz.test │ │ │ ├── pcap_vlp16_node_hertz.test │ │ │ ├── pcap_vlp16_nodelet_hertz.test │ │ │ └── timeconversiontest.cpp │ ├── velodyne_laserscan │ │ ├── CHANGELOG.rst │ │ ├── CMakeLists.txt │ │ ├── cfg │ │ │ └── VelodyneLaserScan.cfg │ │ ├── include │ │ │ └── velodyne_laserscan │ │ │ │ └── velodyne_laserscan.h │ │ ├── nodelets.xml │ │ ├── package.xml │ │ ├── src │ │ │ ├── node.cpp │ │ │ ├── nodelet.cpp │ │ │ └── velodyne_laserscan.cpp │ │ └── tests │ │ │ ├── CMakeLists.txt │ │ │ ├── lazy_subscriber.cpp │ │ │ ├── lazy_subscriber_node.test │ │ │ ├── lazy_subscriber_nodelet.test │ │ │ ├── system.cpp │ │ │ ├── system_node.test │ │ │ └── system_nodelet.test │ ├── velodyne_msgs │ │ ├── CHANGELOG.rst │ │ ├── CMakeLists.txt │ │ ├── msg │ │ │ ├── VelodynePacket.msg │ │ │ └── VelodyneScan.msg │ │ └── package.xml │ └── velodyne_pointcloud │ │ ├── CHANGELOG.rst │ │ ├── CMakeLists.txt │ │ ├── cfg │ │ ├── CloudNode.cfg │ │ └── TransformNode.cfg │ │ ├── include │ │ └── velodyne_pointcloud │ │ │ ├── calibration.h │ │ │ ├── convert.h │ │ │ ├── datacontainerbase.h │ │ │ ├── organized_cloudXYZIR.h │ │ │ ├── pointcloudXYZIR.h │ │ │ ├── rawdata.h │ │ │ └── transform.h │ │ ├── launch │ │ ├── 32e_points.launch │ │ ├── 64e_S3.launch │ │ ├── VLP-32C_points.launch │ │ ├── VLP16_points.launch │ │ ├── cloud_nodelet.launch │ │ ├── laserscan_nodelet.launch │ │ └── transform_nodelet.launch │ │ ├── nodelets.xml │ │ ├── package.xml │ │ ├── params │ │ ├── 32db.yaml │ │ ├── 64e_s2.1-sztaki.yaml │ │ ├── 64e_s3-xiesc.yaml │ │ ├── 64e_utexas.yaml │ │ ├── VLP16_hires_db.yaml │ │ ├── VLP16db.yaml │ │ ├── VeloView-VLP-32C.yaml │ │ └── rviz_points.vcg │ │ ├── scripts │ │ └── gen_calibration.py │ │ ├── src │ │ ├── conversions │ │ │ ├── CMakeLists.txt │ │ │ ├── cloud_node.cc │ │ │ ├── cloud_nodelet.cc │ │ │ ├── convert.cc │ │ │ ├── organized_cloudXYZIR.cc │ │ │ ├── pointcloudXYZIR.cc │ │ │ ├── transform.cc │ │ │ ├── transform_node.cc │ │ │ └── transform_nodelet.cc │ │ └── lib │ │ │ ├── CMakeLists.txt │ │ │ ├── calibration.cc │ │ │ └── rawdata.cc │ │ └── tests │ │ ├── 32db.xml │ │ ├── 64e_s2.1-sztaki.xml │ │ ├── CMakeLists.txt │ │ ├── angles-calibrated.yaml │ │ ├── angles.yaml │ │ ├── cloud_node_32e_hz.test │ │ ├── cloud_node_64e_s2.1_hz.test │ │ ├── cloud_node_hz.test │ │ ├── cloud_node_vlp16_hz.test │ │ ├── cloud_nodelet_32e_hz.test │ │ ├── cloud_nodelet_64e_s2.1_hz.test │ │ ├── cloud_nodelet_hz.test │ │ ├── cloud_nodelet_vlp16_hz.test │ │ ├── empty.xml │ │ ├── issue_84_float_intensities.yaml │ │ ├── static_vehicle_tf.launch │ │ ├── test_calibration.cpp │ │ ├── test_db_without_intensities.xml │ │ ├── transform_node_hz.test │ │ ├── transform_nodelet_hz.test │ │ └── two_nodelet_managers.test │ └── tools │ └── save_img.py ├── ckpt └── .gitkeep ├── device ├── __init__.py ├── camera │ ├── CMakeLists.txt │ ├── build.sh │ ├── run.py │ └── src │ │ ├── main.cpp │ │ └── util │ │ ├── cam_utils.cc │ │ ├── cam_utils.h │ │ ├── mat_warper.cpp │ │ └── mat_warper.h ├── controller │ ├── __init__.py │ ├── controller.py │ ├── passive_xbox.py │ └── xbox.py ├── gps │ ├── __init__.py │ └── gps.py ├── imu │ ├── __init__.py │ ├── mtdef.py │ ├── mtdevice.py │ └── mtnode.py ├── lidar │ ├── __init__.py │ ├── lidar.py │ ├── open_port.sh │ └── velodyne-driver │ │ ├── CMakeLists.txt │ │ ├── build.sh │ │ ├── src │ │ └── velodyne.cpp │ │ └── test.py └── sensor_manager.py ├── doc ├── robocar.jpg └── video_link.png ├── learning ├── __init__.py ├── costmap_dataset.py ├── datasets.py ├── models.py └── path_model.py ├── scripts ├── .gitignore ├── carla │ ├── Town01 │ │ ├── navigation.csv │ │ ├── navigation_with_dynamic_obstacles.csv │ │ ├── one_turn.csv │ │ └── straight.csv │ ├── Town02 │ │ ├── navigation.csv │ │ ├── navigation_with_dynamic_obstacles.csv │ │ ├── one_turn.csv │ │ └── straight.csv │ ├── asynchronous_control_carla.py │ ├── collect_sim_data-2.py │ ├── collect_sim_data.py │ ├── train_GAN.py │ ├── train_GAN_unpaired.py │ └── train_path.py ├── dataset │ └── train_path.py ├── ff │ ├── bezier_curve.py │ ├── camera │ │ ├── basic_tools.py │ │ ├── coordinate_transformation.py │ │ └── parameters.py │ ├── capac_controller.py │ ├── carla_sensor.py │ ├── collect_ipm.py │ ├── collect_pm.py │ ├── collect_pm_real_time.py │ └── system │ │ ├── __init__.py │ │ ├── env_path.py │ │ └── load_carla.py ├── ff_collect_ipm_data.py ├── ff_collect_pm_data.py ├── generate_data │ ├── generate_image_semantic.py │ ├── manaul_collect_sim_data.py │ ├── manual_control_steeringwheel.py │ ├── visualizer.py │ └── wheel_config.ini ├── install.sh ├── other │ └── calibration.py ├── real │ ├── active-record.py │ ├── bag-record.py │ ├── calc_path_len.py │ ├── cat_img.py │ ├── draw_path.py │ ├── get_permission.sh │ ├── passive-record.py │ ├── run.py │ └── test.py ├── requirements.txt └── test │ ├── asynchronous_control_carla_img.py │ ├── manual_control.py │ ├── run_carla.py │ ├── spawn_npc.py │ ├── test_carla.py │ ├── test_carla_data.py │ ├── test_ipm.py │ ├── test_map.py │ ├── test_sim.py │ └── train_path_img.py ├── simulator ├── __init__.py ├── official_code.py ├── sensor_manager.py └── xbox.py ├── utils ├── __init__.py ├── camera_info.py ├── camera_info_sim.py ├── local_planner.py ├── local_planner_sim.py ├── manual_gps.py ├── map-mark.png ├── nav.png ├── navigator.py └── navigator_sim.py └── video └── README.md /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | *.egg-info/ 24 | .installed.cfg 25 | *.egg 26 | MANIFEST 27 | 28 | # PyInstaller 29 | # Usually these files are written by a python script from a template 30 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 31 | *.manifest 32 | *.spec 33 | 34 | # Installer logs 35 | pip-log.txt 36 | pip-delete-this-directory.txt 37 | 38 | # Unit test / coverage reports 39 | htmlcov/ 40 | .tox/ 41 | .coverage 42 | .coverage.* 43 | .cache 44 | nosetests.xml 45 | coverage.xml 46 | *.cover 47 | .hypothesis/ 48 | .pytest_cache/ 49 | 50 | # Translations 51 | *.mo 52 | *.pot 53 | 54 | # Django stuff: 55 | *.log 56 | local_settings.py 57 | db.sqlite3 58 | 59 | # Flask stuff: 60 | instance/ 61 | .webassets-cache 62 | 63 | # Scrapy stuff: 64 | .scrapy 65 | 66 | # Sphinx documentation 67 | docs/_build/ 68 | 69 | # PyBuilder 70 | target/ 71 | 72 | # Jupyter Notebook 73 | .ipynb_checkpoints 74 | 75 | # pyenv 76 | .python-version 77 | 78 | # celery beat schedule file 79 | celerybeat-schedule 80 | 81 | # SageMath parsed files 82 | *.sage.py 83 | 84 | # Environments 85 | .env 86 | .venv 87 | env/ 88 | venv/ 89 | ENV/ 90 | env.bak/ 91 | venv.bak/ 92 | 93 | # Spyder project settings 94 | .spyderproject 95 | .spyproject 96 | 97 | # Rope project settings 98 | .ropeproject 99 | 100 | # mkdocs documentation 101 | /site 102 | 103 | # mypy 104 | .mypy_cache/ 105 | ckpt/ 106 | record/ 107 | backup/ 108 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2019 Wang 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /ROS/.gitignore: -------------------------------------------------------------------------------- 1 | devel/ 2 | install/ 3 | logs/ 4 | build/ 5 | bin/ 6 | lib/ 7 | msg_gen/ 8 | srv_gen/ 9 | msg/*Action.msg 10 | msg/*ActionFeedback.msg 11 | msg/*ActionGoal.msg 12 | msg/*ActionResult.msg 13 | msg/*Feedback.msg 14 | msg/*Goal.msg 15 | msg/*Result.msg 16 | msg/_*.py 17 | build_isolated/ 18 | devel_isolated/ 19 | 20 | # Generated by dynamic reconfigure 21 | *.cfgc 22 | /cfg/cpp/ 23 | /cfg/*.py 24 | 25 | # Ignore generated docs 26 | *.dox 27 | *.wikidoc 28 | 29 | # eclipse stuff 30 | .project 31 | .cproject 32 | 33 | # qcreator stuff 34 | CMakeLists.txt.user 35 | 36 | srv/_*.py 37 | *.pcd 38 | *.pyc 39 | qtcreator-* 40 | *.user 41 | 42 | /planning/cfg 43 | /planning/docs 44 | /planning/src 45 | 46 | *~ 47 | 48 | # Emacs 49 | .#* 50 | 51 | # Catkin custom files 52 | CATKIN_IGNORE 53 | 54 | .catkin_workspace 55 | -------------------------------------------------------------------------------- /ROS/build.sh: -------------------------------------------------------------------------------- 1 | catkin_make --source src 2 | catkin_make install --source src 3 | -------------------------------------------------------------------------------- /ROS/rename.py: -------------------------------------------------------------------------------- 1 | import os 2 | import glob 3 | 4 | def read_pcd(): 5 | files = glob.glob('/home/wang/github/RoBoCar/ROS/pcd/*.pcd') 6 | file_path = [] 7 | for file in files: 8 | ts = file.split('/')[7][:-4] 9 | file_path.append(ts) 10 | 11 | file_path.sort() 12 | return file_path 13 | 14 | 15 | file_path = read_pcd() 16 | 17 | for i in range(len(file_path)): 18 | file_name = file_path[i] 19 | src = '/home/wang/github/RoBoCar/ROS/pcd/'+file_name+'.pcd' 20 | dst = '/home/wang/github/RoBoCar/ROS/out/'+str(i)+'.pcd' 21 | os.rename(src, dst) -------------------------------------------------------------------------------- /ROS/run_camera.sh: -------------------------------------------------------------------------------- 1 | source devel/setup.bash 2 | roslaunch mynteye_wrapper_d display.launch 3 | -------------------------------------------------------------------------------- /ROS/run_collect.sh: -------------------------------------------------------------------------------- 1 | source devel/setup.bash 2 | roslaunch thomas_calibration collect.launch 3 | -------------------------------------------------------------------------------- /ROS/run_lidar.sh: -------------------------------------------------------------------------------- 1 | source devel/setup.bash 2 | roslaunch velodyne_pointcloud VLP16_points.launch & rosrun rviz rviz -------------------------------------------------------------------------------- /ROS/src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | /opt/ros/melodic/share/catkin/cmake/toplevel.cmake -------------------------------------------------------------------------------- /ROS/src/calibration/thomas_calibration/.gitignore: -------------------------------------------------------------------------------- 1 | *.pyc 2 | *.yaml 3 | *.mat 4 | -------------------------------------------------------------------------------- /ROS/src/calibration/thomas_calibration/launch/collect.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /ROS/src/calibration/thomas_calibration/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | thomas_calibration 4 | 0.0.0 5 | The thomas_calibration package 6 | 7 | 8 | 9 | 10 | tangli 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | camera_calibration 44 | rospy 45 | camera_calibration 46 | rospy 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | -------------------------------------------------------------------------------- /ROS/src/calibration/thomas_calibration/script/bag2kitti.m: -------------------------------------------------------------------------------- 1 | function bag2kitti(bag, output_dir, calib) 2 | 3 | [map_l1, map_l2] = cv.initUndistortRectifyMap(... 4 | calib.K_{1}, calib.D_{1}, calib.P_rect_{1}, calib.S_rect_{1}, 'R', calib.R_rect_{1}); 5 | [map_r1, map_r2] = cv.initUndistortRectifyMap(... 6 | calib.K_{2}, calib.D_{2}, calib.P_rect_{2}, calib.S_rect_{2}, 'R', calib.R_rect_{2}); 7 | 8 | if exist(output_dir, 'dir') 9 | % fprintf('Ignoring %s\n', output_dir); 10 | % return; 11 | end 12 | 13 | mkdir(fullfile(output_dir, 'image_00', 'data')); 14 | mkdir(fullfile(output_dir, 'image_01', 'data')); 15 | mkdir(fullfile(output_dir, 'velodyne_points', 'data')); 16 | mkdir(fullfile(output_dir, 'icp', 'data')); 17 | mkdir(fullfile(output_dir, 'gps', 'data')); 18 | mkdir(fullfile(output_dir, 'imu', 'data')); 19 | 20 | if ischar(bag) 21 | fprintf('Reading ROS bag file...'); 22 | bag = rosbag(bag); 23 | fprintf('done\n'); 24 | end 25 | 26 | velo_msgs = bag.select('Topic', '/velodyne_points'); 27 | 28 | image00_msgs = bag.select('Topic', '/pointgrey/left/image_raw/compressed'); 29 | image01_msgs = bag.select('Topic', '/pointgrey/right/image_raw/compressed'); 30 | 31 | 32 | velo_ts_fid = fopen(fullfile(output_dir, 'velodyne_points', 'timestamps.txt'), 'w'); 33 | image00_ts_fid = fopen(fullfile(output_dir, 'image_00', 'timestamps.txt'), 'w'); 34 | image01_ts_fid = fopen(fullfile(output_dir, 'image_01', 'timestamps.txt'), 'w'); 35 | 36 | for i = 1:size(velo_msgs.MessageList,1) 37 | fprintf('\rHandling %d/%d...', i, size(velo_msgs.MessageList,1)); 38 | 39 | velo_msg = readMessages(velo_msgs, i); 40 | velo_msg = velo_msg{1}; 41 | 42 | [~, ind] = min(abs(velo_msgs.MessageList.Time(i) - image00_msgs.MessageList.Time)); 43 | image00_msg = readMessages(image00_msgs, ind); 44 | image00_msg = image00_msg{1}; 45 | % image00 = readImage(image00_msg{1}); 46 | image00_bayer = cv.imdecode(image00_msg.Data, 'Flags', -1); 47 | image00 = cv.cvtColor(image00_bayer, 'BayerBG2RGB'); 48 | 49 | [~, ind] = min(abs(velo_msgs.MessageList.Time(i) - image01_msgs.MessageList.Time)); 50 | image01_msg = readMessages(image01_msgs, ind); 51 | image01_msg = image01_msg{1}; 52 | image01_bayer = cv.imdecode(image01_msg.Data, 'Flags', -1); 53 | image01 = cv.cvtColor(image01_bayer, 'BayerBG2RGB'); 54 | 55 | image00 = cv.remap(image00, map_l1, map_l2, 'Interpolation', 'Cubic'); 56 | image01 = cv.remap(image01, map_r1, map_r2, 'Interpolation', 'Cubic'); 57 | 58 | 59 | fid = fopen(fullfile(output_dir, 'velodyne_points', 'data', [num2str(i-1, '%010d') '.bin']), 'w'); 60 | fwrite(fid, velo_msg.Data); 61 | fclose(fid); 62 | 63 | imwrite(image00, fullfile(output_dir, 'image_00', 'data', [num2str(i-1, '%010d') '.png'])); 64 | imwrite(image01, fullfile(output_dir, 'image_01', 'data', [num2str(i-1, '%010d') '.png'])); 65 | 66 | fprintf(velo_ts_fid, '%d.%d\n', velo_msg.Header.Stamp.Sec, velo_msg.Header.Stamp.Nsec); 67 | fprintf(image00_ts_fid, '%s\n', image00_msg.Header.Stamp.Sec, image00_msg.Header.Stamp.Nsec); 68 | fprintf(image01_ts_fid, '%s\n', image01_msg.Header.Stamp.Sec, image01_msg.Header.Stamp.Nsec); 69 | 70 | end 71 | 72 | fclose(velo_ts_fid); 73 | fclose(image00_ts_fid); 74 | fclose(image01_ts_fid); 75 | -------------------------------------------------------------------------------- /ROS/src/calibration/thomas_calibration/script/calib2kitti.m: -------------------------------------------------------------------------------- 1 | function calib2kitti() 2 | 3 | %% 4 | input_dir = '/mnt/DataBlock2/thomas/YQ-south'; 5 | output_dir = '/mnt/DataBlock2/thomas/YQ-south_RAW'; 6 | 7 | %% 8 | % CLEmain; 9 | fid = fopen(fullfile(input_dir, 'calibration', 'calib_velo_to_cam.txt'), 'w'); 10 | fprintf(fid, 'calib_time: %s\n', 'unknown'); 11 | T = [Pose_CL;0 0 0 1]^(-1); 12 | writeMat(fid, 'R', T(1:3,1:3)); 13 | writeMat(fid, 'T', T(1:3,4)); 14 | fclose(fid); 15 | 16 | %% 17 | calib = readCalibrationFromOpenCV(... 18 | fullfile(input_dir, 'calibration', 'left.yaml'), ... 19 | fullfile(input_dir, 'calibration', 'right.yaml')); 20 | 21 | fid = fopen(fullfile(input_dir, 'calibration', 'calib_cam_to_cam.txt'), 'w'); 22 | fprintf(fid, 'calib_time: %s\n', 'unknown'); 23 | fprintf(fid, 'corner_dist: %f\n', 0.1); 24 | writeMat(fid, 'S_00', calib.S_{1}); 25 | writeMat(fid, 'K_00', calib.K_{1}); 26 | writeMat(fid, 'D_00', calib.D_{1}); 27 | writeMat(fid, 'R_00', calib.R_{1}); 28 | writeMat(fid, 'T_00', calib.T_{1}); 29 | writeMat(fid, 'S_rect_00', calib.S_rect_{1}); 30 | writeMat(fid, 'R_rect_00', calib.R_rect_{1}); 31 | writeMat(fid, 'P_rect_00', calib.P_rect_{1}); 32 | writeMat(fid, 'S_01', calib.S_{2}); 33 | writeMat(fid, 'K_01', calib.K_{2}); 34 | writeMat(fid, 'D_01', calib.D_{2}); 35 | writeMat(fid, 'R_01', calib.R_{2}); 36 | writeMat(fid, 'T_01', calib.T_{2}); 37 | writeMat(fid, 'S_rect_01', calib.S_rect_{2}); 38 | writeMat(fid, 'R_rect_01', calib.R_rect_{2}); 39 | writeMat(fid, 'P_rect_01', calib.P_rect_{2}); 40 | fclose(fid); 41 | 42 | end 43 | 44 | -------------------------------------------------------------------------------- /ROS/src/calibration/thomas_calibration/script/calibMat2CV.m: -------------------------------------------------------------------------------- 1 | function calibMat2CV(stereoParams, filename1, filename2) 2 | 3 | %% 4 | cameraMatrix1 = stereoParams.CameraParameters1.IntrinsicMatrix'; 5 | distCoeffs1 = [stereoParams.CameraParameters1.RadialDistortion stereoParams.CameraParameters1.TangentialDistortion]; 6 | 7 | cameraMatrix2 = stereoParams.CameraParameters2.IntrinsicMatrix'; 8 | distCoeffs2 = [stereoParams.CameraParameters2.RadialDistortion stereoParams.CameraParameters2.TangentialDistortion]; 9 | 10 | imageSize = fliplr(stereoParams.CameraParameters1.ImageSize); 11 | R = stereoParams.RotationOfCamera2; 12 | T = stereoParams.TranslationOfCamera2'; 13 | 14 | SR = cv.stereoRectify(cameraMatrix1, distCoeffs1, ... 15 | cameraMatrix2, distCoeffs2, imageSize, R, T); 16 | 17 | %% 18 | S = struct(); 19 | S.image_width = stereoParams.CameraParameters1.ImageSize(2); 20 | S.image_height = stereoParams.CameraParameters1.ImageSize(1); 21 | 22 | %% 23 | S.camera_name = 'left'; 24 | S.camera_matrix = cameraMatrix1; 25 | S.distortion_model = 'plumb_bob'; 26 | S.distortion_coefficients = distCoeffs1; 27 | S.rectification_matrix = SR.R1; 28 | S.projection_matrix = SR.P1; 29 | cv.FileStorage(filename1, S); 30 | 31 | %% 32 | S.camera_name = 'right'; 33 | S.camera_matrix = cameraMatrix2; 34 | S.distortion_coefficients = distCoeffs2; 35 | S.rectification_matrix = SR.R2; 36 | S.projection_matrix = SR.P2; 37 | cv.FileStorage(filename2, S); 38 | -------------------------------------------------------------------------------- /ROS/src/calibration/thomas_calibration/script/checkRectify.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZJU-Robotics-Lab/CICT/ff873a03ab03d9113b8db96d26246939bb5da0d4/ROS/src/calibration/thomas_calibration/script/checkRectify.m -------------------------------------------------------------------------------- /ROS/src/calibration/thomas_calibration/script/compress.m: -------------------------------------------------------------------------------- 1 | 2 | source_dir = '/mnt/DataBlock2/thomas/YQ_RAW'; 3 | dest_dir = '/mnt/DataBlock2/thomas/YQ.compressed'; 4 | 5 | old_dir = pwd; 6 | cd(source_dir); 7 | 8 | date_dirs = lsdir('.'); 9 | 10 | for i=1:length(date_dirs) 11 | dirs = lsdir(fullfile(source_dir, date_dirs{i})); 12 | for j=1:length(dirs) 13 | fprintf('Compressing %s...\n', dirs{j}); 14 | zip([dest_dir '/' dirs{j} '.zip'], fullfile(date_dirs{i}, dirs{j})) 15 | end 16 | end 17 | 18 | cd(old_dir); 19 | -------------------------------------------------------------------------------- /ROS/src/calibration/thomas_calibration/script/convert.m: -------------------------------------------------------------------------------- 1 | 2 | %% 3 | addpath('~/Projects/YAMLMatlab/'); 4 | addpath(genpath('~/Projects/mexopencv/')); 5 | addpath('~/Projects/robot_toolbox/'); 6 | 7 | %% 8 | input_dir = '/mnt/DataBlock2/thomas/YQ-south'; 9 | output_dir = '/mnt/DataBlock2/thomas/YQ-south_RAW'; 10 | 11 | calib = readCalibrationFromOpenCV(... 12 | fullfile(input_dir, 'calibration', 'left.yaml'), ... 13 | fullfile(input_dir, 'calibration', 'right.yaml')); 14 | 15 | 16 | %% 17 | dirs = lsdir(input_dir); 18 | for i=1:length(dirs) 19 | 20 | % 21 | date_dir = [dirs{i}(1:4) '_' dirs{i}(6:7) '_' dirs{i}(9:10)]; 22 | if exist(fullfile(output_dir, date_dir), 'dir') 23 | fprintf('skipped: %s.\n', fullfile(output_dir, date_dir)); 24 | continue; 25 | end 26 | mkdir(fullfile(output_dir, date_dir)); 27 | 28 | % bags 29 | bagFileNames = dir(fullfile(input_dir, dirs{i}, '*.bag')); 30 | if ~exist('bags', 'var') 31 | bags = cell(length(bagFileNames), 1); 32 | end 33 | for j=1:length(bagFileNames) 34 | fprintf('Converting `%s` to `%s`...\n', ... 35 | fullfile(input_dir, dirs{i}, bagFileNames(j).name), ... 36 | fullfile(output_dir, date_dir, [date_dir '_drive_' num2str(j, '%04d') '_sync'])); 37 | if isempty(bags{j}) 38 | bags{j} = rosbag(fullfile(input_dir, dirs{i}, bagFileNames(j).name)); 39 | end 40 | bag2kitti(bags{j}, ... 41 | fullfile(output_dir, date_dir, [date_dir '_drive_' num2str(j, '%04d') '_sync']), calib); 42 | end 43 | 44 | % icp results 45 | icpFileNames = dir(fullfile(input_dir, dirs{i}, '*.txt')); 46 | for j=1:length(icpFileNames) 47 | fprintf('Converting `%s` to `%s`...\n', ... 48 | fullfile(input_dir, dirs{i}, icpFileNames(j).name), ... 49 | fullfile(output_dir, date_dir, [date_dir '_drive_' num2str(j, '%04d') '_sync'])); 50 | icp2kitti(... 51 | fullfile(input_dir, dirs{i}, icpFileNames(j).name), ... 52 | fullfile(output_dir, date_dir, [date_dir '_drive_' num2str(j, '%04d') '_sync'])); 53 | end 54 | end 55 | 56 | -------------------------------------------------------------------------------- /ROS/src/calibration/thomas_calibration/script/costCLextrinsic.m: -------------------------------------------------------------------------------- 1 | function cost = costCLextrinsic(theta,boardPlane,boardPC) 2 | cost = []; 3 | R = eul2rotm(theta(4:6).'); 4 | t = theta(1:3); 5 | for i = 1:size(boardPlane,2) 6 | pts = R*boardPC{i}+repmat(t,1,size(boardPC{i},2)); 7 | cost = [cost boardPlane(1:3,i).'*pts+boardPlane(4,i)]; 8 | end 9 | cost = cost*cost.'; 10 | end -------------------------------------------------------------------------------- /ROS/src/calibration/thomas_calibration/script/costReproj.m: -------------------------------------------------------------------------------- 1 | function cost = costReproj(theta,imagePoints,worldPoints,IntrinsicMatrix) 2 | R = eul2rotm(theta(4:6).'); 3 | t = theta(1:3); 4 | pts = R*worldPoints.'+repmat(t,1,size(worldPoints,1)); 5 | testimagePoints = IntrinsicMatrix*pts; 6 | testimagePoints = [testimagePoints(1,:)./testimagePoints(3,:);testimagePoints(2,:)./testimagePoints(3,:)]; 7 | residue = imagePoints- testimagePoints.'; 8 | cost = sum(residue(:).^2); 9 | end -------------------------------------------------------------------------------- /ROS/src/calibration/thomas_calibration/script/icp2kitti.m: -------------------------------------------------------------------------------- 1 | function icp2kitti(icpFile, outputDir) 2 | 3 | fid = fopen(icpFile); 4 | icp = fscanf(fid, '%s %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f', [(20+16) inf]); 5 | fclose(fid); 6 | timestamps = char(icp(1:16, :))'; 7 | poses = icp(21:end, :)'; 8 | fid = fopen(fullfile(outputDir, 'icp', 'timestamps.txt'), 'w'); 9 | fprintf(fid, '%c', [timestamps'; repmat(char(10), 1, size(timestamps,1))]); 10 | fclose(fid); 11 | fid = fopen(fullfile(outputDir, 'icp', 'icp.txt'), 'w'); 12 | fprintf(fid, '%f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\n', poses'); 13 | fclose(fid); -------------------------------------------------------------------------------- /ROS/src/calibration/thomas_calibration/script/readCalibrationFromOpenCV.m: -------------------------------------------------------------------------------- 1 | function calib = readCalibrationFromOpenCV(left, right) 2 | 3 | %% 4 | LY = ReadYaml(left); 5 | RY = ReadYaml(right); 6 | 7 | %% 8 | calib.S_{1} = [LY.image_width LY.image_height]; 9 | calib.K_{1} = reshape(cell2mat(LY.camera_matrix.data), 3, 3)'; 10 | calib.D_{1} = cell2mat(LY.distortion_coefficients.data); 11 | calib.R_{1} = reshape(cell2mat(LY.rotation_matrix.data), 3, 3)'; 12 | calib.T_{1} = cell2mat(LY.translation_matrix.data)'; 13 | 14 | calib.S_{2} = [RY.image_width RY.image_height]; 15 | calib.K_{2} = reshape(cell2mat(RY.camera_matrix.data), 3, 3)'; 16 | calib.D_{2} = cell2mat(RY.distortion_coefficients.data); 17 | calib.R_{2} = reshape(cell2mat(RY.rotation_matrix.data), 3, 3)'; 18 | calib.T_{2} = cell2mat(RY.translation_matrix.data)'; 19 | 20 | calib.S_rect_{1} = [LY.image_width LY.image_height]; 21 | calib.R_rect_{1} = reshape(cell2mat(LY.rectification_matrix.data), 3, 3)'; 22 | calib.P_rect_{1} = reshape(cell2mat(LY.projection_matrix.data), 4, 3)'; 23 | calib.S_rect_{2} = [RY.image_width RY.image_height]; 24 | calib.R_rect_{2} = reshape(cell2mat(RY.rectification_matrix.data), 3, 3)'; 25 | calib.P_rect_{2} = reshape(cell2mat(RY.projection_matrix.data), 4, 3)'; 26 | 27 | %% Determine rectified size 28 | %{ 29 | I0 = ones(LY.image_height, LY.image_width, 'uint8') * 255; 30 | need_update = true; 31 | while need_update 32 | need_update = false; 33 | S = cv.stereoRectify(calib.K_{1}, calib.D_{1}, calib.K_{2}, calib.D_{2}, ... 34 | calib.S_rect_{1}, calib.R_{2}, calib.T_{2}); 35 | calib.R_rect_{1} = S.R1; 36 | calib.P_rect_{1} = S.P1; 37 | calib.R_rect_{2} = S.R2; 38 | calib.P_rect_{2} = S.P2; 39 | [map_l1, map_l2] = cv.initUndistortRectifyMap(calib.K_{1}, calib.D_{1}, calib.P_rect_{1}, calib.S_rect_{1}, 'R', calib.R_rect_{1}); 40 | I1 = cv.remap(I0, map_l1, map_l2, 'Interpolation', 'Cubic'); 41 | [map_r1, map_r2] = cv.initUndistortRectifyMap(calib.K_{2}, calib.D_{2}, calib.P_rect_{2}, calib.S_rect_{2}, 'R', calib.R_rect_{2}); 42 | I2 = cv.remap(I0, map_r1, map_r2, 'Interpolation', 'Cubic'); 43 | 44 | figure(1); subplot(1,2,1); imagesc(I1); subplot(1,2,2); imagesc(I2); 45 | waitforbuttonpress; 46 | if ~isempty(find(~(I1&I2),1)) 47 | calib.S_rect_{1} = [min(S.roi1(3), S.roi2(3)) min(S.roi1(4), S.roi2(4))]; % calib.S_rect_{1} - 1; 48 | calib.S_rect_{2} = calib.S_rect_{1}; 49 | need_update = true; 50 | end 51 | end 52 | %} -------------------------------------------------------------------------------- /ROS/src/calibration/thomas_calibration/script/testCalibration.m: -------------------------------------------------------------------------------- 1 | 2 | %% 3 | addpath('~/Projects/mexopencv/'); 4 | 5 | %% 6 | % S = cv.stereoCalibrate(stereoParams.WorldPoints, imagePoints1, imagePoints2, imageSize); 7 | 8 | %% 9 | cameraMatrix1 = stereoParams.CameraParameters1.IntrinsicMatrix'; 10 | distCoeffs1 = [... 11 | stereoParams.CameraParameters1.RadialDistortion ... 12 | stereoParams.CameraParameters1.TangentialDistortion]; 13 | 14 | cameraMatrix2 = stereoParams.CameraParameters2.IntrinsicMatrix'; 15 | distCoeffs2 = [... 16 | stereoParams.CameraParameters2.RadialDistortion ... 17 | stereoParams.CameraParameters2.TangentialDistortion]; 18 | 19 | imageSize = fliplr(stereoParams.CameraParameters1.ImageSize); 20 | R = stereoParams.RotationOfCamera2' * [-1 0 0; 0 -1 0; 0 0 1]; 21 | T = -stereoParams.TranslationOfCamera2'/1000; 22 | 23 | S = cv.stereoRectify(cameraMatrix1, distCoeffs1, ... 24 | cameraMatrix2, distCoeffs2, imageSize, R, T); 25 | 26 | S.R2 = [-1 0 0; 0 -1 0; 0 0 1] * S.R2; 27 | 28 | [map_l1, map_l2] = cv.initUndistortRectifyMap(cameraMatrix1, ... 29 | distCoeffs1, imageSize, 'R', S.R1, 'NewCameraMatrix', S.P1); 30 | [map_r1, map_r2] = cv.initUndistortRectifyMap(cameraMatrix2, ... 31 | distCoeffs2, imageSize, 'R', S.R2, 'NewCameraMatrix', S.P2); 32 | 33 | % cvParams = S; 34 | %% 35 | Il = imread('/Users/tangli/Desktop/201807191600/left/left_50.png'); 36 | Ir = imread('/Users/tangli/Desktop/201807191600/right/right_50.png'); 37 | Ilrect = cv.remap(Il, map_l1, map_l2, 'Interpolation', 'Cubic'); 38 | Irrect = cv.remap(Ir, map_r1, map_r2, 'Interpolation', 'Cubic'); 39 | %figure(1); clf; imshow(stereoAnaglyph(Ilrect,Irrect)); 40 | % I = cat(2, Ilrect, imrotate(Irrect, 180)); 41 | I = cat(2, Ilrect, Irrect); 42 | 43 | figure(2); 44 | imshow(I); 45 | for i=1:50:size(Ilrect,1) 46 | line([0, 2* size(Ilrect,2)], [i, i]); 47 | end 48 | 49 | -------------------------------------------------------------------------------- /ROS/src/calibration/thomas_calibration/script/writeMat.m: -------------------------------------------------------------------------------- 1 | function writeMat(fid, name, mat) 2 | 3 | fprintf(fid, '%s:', name); 4 | fprintf(fid, ' %f', mat'); 5 | fprintf(fid, '\n'); 6 | -------------------------------------------------------------------------------- /ROS/src/camera/mynteye_wrapper_d/launch/display.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /ROS/src/camera/mynteye_wrapper_d/mesh/D-0315.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'None' 2 | # Material Count: 3 3 | 4 | newmtl Material 5 | Ns 96.078431 6 | Ka 1.000000 1.000000 1.000000 7 | Kd 0.640000 0.640000 0.640000 8 | Ks 0.500000 0.500000 0.500000 9 | Ke 0.000000 0.000000 0.000000 10 | Ni 1.000000 11 | d 1.000000 12 | illum 2 13 | 14 | newmtl Material.001 15 | Ns 96.078431 16 | Ka 1.000000 1.000000 1.000000 17 | Kd 0.640000 0.640000 0.640000 18 | Ks 0.500000 0.500000 0.500000 19 | Ke 0.000000 0.000000 0.000000 20 | Ni 1.000000 21 | d 1.000000 22 | illum 2 23 | 24 | newmtl None 25 | Ns 96.078431 26 | Ka 1.000000 1.000000 1.000000 27 | Kd 0.640000 0.640000 0.640000 28 | Ks 0.500000 0.500000 0.500000 29 | Ke 0.000000 0.000000 0.000000 30 | Ni 1.000000 31 | d 1.000000 32 | illum 2 33 | -------------------------------------------------------------------------------- /ROS/src/camera/mynteye_wrapper_d/msg/Temp.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | float32 data 3 | -------------------------------------------------------------------------------- /ROS/src/camera/mynteye_wrapper_d/nodelet_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | This is the nodelet of ROS interface for MYNT EYE camera. 6 | 7 | 8 | -------------------------------------------------------------------------------- /ROS/src/camera/mynteye_wrapper_d/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mynteye_wrapper_d 4 | 0.0.0 5 | The mynteye_wrapper_d package for the MYNT EYE Depth camera 6 | 7 | John Zhao 8 | 9 | Slightech License 10 | 11 | 12 | 13 | John Zhao 14 | 15 | catkin 16 | 17 | cv_bridge 18 | image_transport 19 | message_generation 20 | nodelet 21 | roscpp 22 | sensor_msgs 23 | std_msgs 24 | tf 25 | 26 | cv_bridge 27 | image_transport 28 | nodelet 29 | roscpp 30 | sensor_msgs 31 | std_msgs 32 | message_generation 33 | tf 34 | 35 | cv_bridge 36 | image_transport 37 | message_runtime 38 | nodelet 39 | roscpp 40 | sensor_msgs 41 | std_msgs 42 | tf 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | -------------------------------------------------------------------------------- /ROS/src/camera/mynteye_wrapper_d/src/mynteye_wrapper_node.cc: -------------------------------------------------------------------------------- 1 | // Copyright 2018 Slightech Co., Ltd. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #include 15 | #include 16 | 17 | int main(int argc, char* argv[]) { 18 | ros::init(argc, argv, "mynteye_wrapper_d_node"); 19 | 20 | nodelet::Loader nodelet; 21 | nodelet::M_string remap(ros::names::getRemappings()); 22 | nodelet::V_string nargv; 23 | nodelet.load(ros::this_node::getName(), 24 | "mynteye_wrapper_d/MYNTEYEWrapperNodelet", 25 | remap, nargv); 26 | 27 | ros::spin(); 28 | 29 | return 0; 30 | } 31 | -------------------------------------------------------------------------------- /ROS/src/camera/mynteye_wrapper_d/src/pointcloud_generator.h: -------------------------------------------------------------------------------- 1 | // Copyright 2018 Slightech Co., Ltd. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #ifndef MYNTEYE_WRAPPER_POINTCLOUD_GENERATOR_H_ 15 | #define MYNTEYE_WRAPPER_POINTCLOUD_GENERATOR_H_ 16 | #pragma once 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | #include 25 | 26 | #include 27 | #include 28 | 29 | #include "mynteyed/util/rate.h" 30 | #include "mynteyed/stubs/types_calib.h" 31 | 32 | MYNTEYE_BEGIN_NAMESPACE 33 | 34 | #define DEFAULT_POINTS_FREQUENCE (0) 35 | #define DEFAULT_POINTS_FACTOR (1000.0) 36 | 37 | class PointCloudGenerator { 38 | public: 39 | using Callback = std::function; 40 | 41 | PointCloudGenerator(CameraIntrinsics in, Callback callback, 42 | double factor = DEFAULT_POINTS_FACTOR, 43 | std::int32_t frequency = DEFAULT_POINTS_FREQUENCE); 44 | ~PointCloudGenerator(); 45 | 46 | bool Push(const cv::Mat& color, const cv::Mat& depth, ros::Time stamp); 47 | 48 | double factor() { return factor_; } 49 | void set_factor(double factor) { factor_ = factor; } 50 | 51 | private: 52 | void Start(); 53 | void Stop(); 54 | 55 | void Run(); 56 | 57 | CameraIntrinsics in_; 58 | Callback callback_; 59 | 60 | std::unique_ptr rate_; 61 | 62 | std::mutex mutex_; 63 | std::condition_variable condition_; 64 | 65 | bool running_; 66 | std::thread thread_; 67 | 68 | cv::Mat color_; 69 | cv::Mat depth_; 70 | ros::Time stamp_; 71 | 72 | double factor_; 73 | 74 | bool generating_; 75 | }; 76 | 77 | MYNTEYE_END_NAMESPACE 78 | 79 | #endif // MYNTEYE_WRAPPER_POINTCLOUD_GENERATOR_H_ 80 | -------------------------------------------------------------------------------- /ROS/src/camera/mynteye_wrapper_d/srv/GetParams.srv: -------------------------------------------------------------------------------- 1 | uint32 IMG_INTRINSICS=0 2 | uint32 IMG_EXTRINSICS_RTOL=1 3 | uint32 IMU_INTRINSICS=2 4 | uint32 IMU_EXTRINSICS=3 5 | uint32 key 6 | --- 7 | string value 8 | -------------------------------------------------------------------------------- /ROS/src/controller/msg/Cmd.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | string data -------------------------------------------------------------------------------- /ROS/src/controller/scripts/get_permission.sh: -------------------------------------------------------------------------------- 1 | sudo chmod 777 /dev/ttyUSB* 2 | sudo chmod 777 /dev/ttyS* 3 | -------------------------------------------------------------------------------- /ROS/src/controller/scripts/run.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | from __future__ import print_function 3 | from can_bus import Controller 4 | from xbox import scan_usb, JoyStick 5 | 6 | import rospy 7 | from std_msgs.msg import String, Header 8 | from controller.msg import Cmd 9 | 10 | 11 | if __name__ == '__main__': 12 | CAN_CHANNEL = scan_usb('CAN') 13 | print('CAN_CHANNEL', CAN_CHANNEL) 14 | ctrl = Controller(CAN_CHANNEL) 15 | ctrl.start() 16 | joystick = JoyStick(ctrl, verbose=False) 17 | joystick.start() 18 | pub = rospy.Publisher('cmd', Cmd, queue_size=1) 19 | rospy.init_node('talker', anonymous=True) 20 | r = rospy.Rate(60) 21 | frame_id = "cmd_data" 22 | while not rospy.is_shutdown(): 23 | v, w = joystick.get_data() 24 | data =str(v) + '\t' +str(w) + '\n' 25 | 26 | h = Header() 27 | h.stamp = rospy.Time.now() 28 | h.frame_id = frame_id 29 | cmd_msg = Cmd() 30 | cmd_msg.header = h 31 | cmd_msg.data = data 32 | if data == None: 33 | continue 34 | #print(data) 35 | pub.publish(cmd_msg) 36 | r.sleep() -------------------------------------------------------------------------------- /ROS/src/gps/msg/GPS.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | string data -------------------------------------------------------------------------------- /ROS/src/gps/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | gps 4 | 0.0.0 5 | The gps package 6 | 7 | 8 | 9 | 10 | wang 11 | 12 | 13 | 14 | 15 | 16 | MIT 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | roscpp 53 | rospy 54 | std_msgs 55 | message_generation 56 | roscpp 57 | rospy 58 | std_msgs 59 | message_generation 60 | roscpp 61 | rospy 62 | std_msgs 63 | message_runtime 64 | message_generation 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | -------------------------------------------------------------------------------- /ROS/src/gps/scripts/get_permission.sh: -------------------------------------------------------------------------------- 1 | sudo chmod 777 /dev/ttyUSB* 2 | -------------------------------------------------------------------------------- /ROS/src/imu/msg/IMU.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | string data -------------------------------------------------------------------------------- /ROS/src/imu/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | imu 4 | 0.0.0 5 | The imu package 6 | 7 | 8 | 9 | 10 | wang 11 | 12 | 13 | 14 | 15 | 16 | MIT 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | roscpp 53 | rospy 54 | std_msgs 55 | message_generation 56 | roscpp 57 | rospy 58 | std_msgs 59 | message_generation 60 | roscpp 61 | rospy 62 | std_msgs 63 | message_runtime 64 | message_generation 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | Change history 2 | ============== 3 | 4 | 1.5.2 (2019-01-28) 5 | ------------------ 6 | 7 | 1.5.1 (2018-12-10) 8 | ------------------ 9 | 10 | 1.5.0 (2018-10-19) 11 | ------------------ 12 | 13 | 1.4.0 (2018-09-19) 14 | ------------------ 15 | * Merge pull request `#160 `_ from ros-drivers/maint/updating_package_xml_to_v2 16 | * Updated all package.xmls to ver 2. Cleaned up catkin_lint errors. 17 | All package.xml files are now compatible with version 2 of the 18 | package.xml specification in REP 140. Removed some unnecessary 19 | execute permissions on a few files. Fixed a missing test_depend. 20 | * Contributors: Andre Volk, Joshua Whitley 21 | 22 | 1.3.0 (2017-11-10) 23 | ------------------ 24 | * Merge pull request `#110 `_ from kmhallen/master 25 | Added velodyne_laserscan package 26 | * Added velodyne_laserscan package and inserted into existing launch files 27 | * Contributors: Jack O'Quin, Joshua Whitley, Kevin Hallenbeck 28 | 29 | 1.2.0 (2014-08-06) 30 | ------------------ 31 | 32 | 1.1.2 (2013-11-05) 33 | ------------------- 34 | 35 | 1.1.1 (2013-07-30) 36 | ------------------ 37 | 38 | 1.1.0 (2013-07-16) 39 | ------------------ 40 | 41 | 1.0.1 (2013-06-15) 42 | ------------------ 43 | 44 | 1.0.0 (2013-06-14) 45 | ------------------ 46 | 47 | * Convert to catkin (`#1`_). 48 | * Release to Hydro. 49 | 50 | 0.9.2 (2013-07-08) 51 | ------------------ 52 | 53 | 0.9.1 (2012-06-05) 54 | ------------------ 55 | 56 | 0.9.0 (2012-04-03) 57 | ------------------ 58 | 59 | * Completely revised API, anticipating a 1.0.0 release. 60 | * New velodyne_driver and velodyne_pointcloud packages. 61 | * Old velodyne_common and velodyne_pcl packages no longer included. 62 | * Released to Electric, Fuerte and Groovy. 63 | 64 | 0.2.6 (2011-02-23) 65 | ------------------ 66 | 67 | 0.2.5 (2010-11-19) 68 | ------------------ 69 | 70 | * Initial implementation of new 0.3 interfaces. 71 | 72 | 0.2.0 (2010-08-17) 73 | ------------------ 74 | 75 | * Initial release to ROS C-turtle. 76 | 77 | .. _`#1`: https://github.com/ros-drivers/velodyne/issues/1 78 | .. _`#4`: https://github.com/ros-drivers/velodyne/issues/4 79 | .. _`#7`: https://github.com/ros-drivers/velodyne/issues/7 80 | .. _`#8`: https://github.com/ros-drivers/velodyne/pull/8 81 | .. _`#9`: https://github.com/ros-drivers/velodyne/issues/9 82 | .. _`#10`: https://github.com/ros-drivers/velodyne/issues/10 83 | .. _`#11`: https://github.com/ros-drivers/velodyne/issues/11 84 | .. _`#12`: https://github.com/ros-drivers/velodyne/pull/12 85 | .. _`#13`: https://github.com/ros-drivers/velodyne/issues/13 86 | .. _`#14`: https://github.com/ros-drivers/velodyne/pull/14 87 | .. _`#17`: https://github.com/ros-drivers/velodyne/issues/17 88 | .. _`#18`: https://github.com/ros-drivers/velodyne/issues/18 89 | .. _`#20`: https://github.com/ros-drivers/velodyne/issues/20 90 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(velodyne) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | velodyne 4 | 1.5.2 5 | 6 | Basic ROS support for the Velodyne 3D LIDARs. 7 | 8 | Josh Whitley 9 | Jack O'Quin 10 | BSD 11 | http://www.ros.org/wiki/velodyne 12 | https://github.com/ros-drivers/velodyne 13 | https://github.com/ros-drivers/velodyne/issues 14 | 15 | catkin 16 | 17 | velodyne_driver 18 | velodyne_laserscan 19 | velodyne_msgs 20 | velodyne_pointcloud 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_driver/cfg/VelodyneNode.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "velodyne_driver" 3 | NODE_NAME = "velodyne_node" 4 | PARAMS_NAME = "VelodyneNode" 5 | 6 | from math import pi 7 | from dynamic_reconfigure.parameter_generator_catkin import * 8 | 9 | gen = ParameterGenerator() 10 | 11 | gen.add("time_offset", double_t, 1, "A manually calibrated offset (in seconds) to add to the timestamp before publication of a message.", 12 | 0.0, -1.0, 1.0) 13 | gen.add("enabled", bool_t, 2, "Switch to enable and disable lidar packet consumption", True); 14 | 15 | exit(gen.generate(PACKAGE, NODE_NAME, PARAMS_NAME)) 16 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_driver/include/velodyne_driver/ring_sequence.h: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2010, 2019 Austin Robot Technology, Jack O'Quin, Joshua Whitley 2 | // All rights reserved. 3 | // 4 | // Software License Agreement (BSD License 2.0) 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions 8 | // are met: 9 | // 10 | // * Redistributions of source code must retain the above copyright 11 | // notice, this list of conditions and the following disclaimer. 12 | // * Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // * Neither the name of {copyright_holder} nor the names of its 17 | // contributors may be used to endorse or promote products derived 18 | // from this software without specific prior written permission. 19 | // 20 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | // COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | // INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | // BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | // LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | // POSSIBILITY OF SUCH DAMAGE. 32 | 33 | /** \file 34 | * 35 | * Velodyne HDL-64E 3D LIDAR laser ring sequence. 36 | * 37 | * \author Jack O'Quin 38 | */ 39 | 40 | #ifndef VELODYNE_DRIVER_RING_SEQUENCE_H 41 | #define VELODYNE_DRIVER_RING_SEQUENCE_H 42 | 43 | namespace velodyne 44 | { 45 | 46 | // number of lasers 47 | const int N_LASERS = 64; 48 | 49 | // ring sequence for device laser numbers 50 | const int LASER_SEQUENCE[N_LASERS] = 51 | { 52 | 6, 7, 10, 11, 0, 1, 4, 5, 53 | 8, 9, 14, 15, 18, 19, 22, 23, 54 | 12, 13, 16, 17, 20, 21, 26, 27, 55 | 30, 31, 2, 3, 24, 25, 28, 29, 56 | 38, 39, 42, 43, 32, 33, 36, 37, 57 | 40, 41, 46, 47, 50, 51, 54, 55, 58 | 44, 45, 48, 49, 52, 53, 58, 59, 59 | 62, 63, 34, 35, 56, 57, 60, 61 60 | }; 61 | 62 | // convert laser number to ring sequence (inverse of LASER_SEQUENCE) 63 | const int LASER_RING[N_LASERS] = 64 | { 65 | 4, 5, 26, 27, 6, 7, 0, 1, 66 | 8, 9, 2, 3, 16, 17, 10, 11, 67 | 18, 19, 12, 13, 20, 21, 14, 15, 68 | 28, 29, 22, 23, 30, 31, 24, 25, 69 | 36, 37, 58, 59, 38, 39, 32, 33, 70 | 40, 41, 34, 35, 48, 49, 42, 43, 71 | 50, 51, 44, 45, 52, 53, 46, 47, 72 | 60, 61, 54, 55, 62, 63, 56, 57 73 | }; 74 | 75 | } // namespace velodyne 76 | 77 | #endif // VELODYNE_DRIVER_RING_SEQUENCE_H 78 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_driver/launch/nodelet_manager.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_driver/nodelet_velodyne.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | Publish raw Velodyne data packets. 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_driver/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | velodyne_driver 4 | 1.5.2 5 | 6 | ROS device driver for Velodyne 3D LIDARs. 7 | 8 | Josh Whitley 9 | Brice Rebsamen 10 | Jack O'Quin 11 | Patrick Beeson 12 | Michael Quinlan 13 | Yaxin Liu 14 | BSD 15 | 16 | http://www.ros.org/wiki/velodyne_driver 17 | https://github.com/ros-drivers/velodyne 18 | https://github.com/ros-drivers/velodyne/issues 19 | 20 | catkin 21 | 22 | roslint 23 | 24 | diagnostic_updater 25 | dynamic_reconfigure 26 | libpcap 27 | nodelet 28 | roscpp 29 | tf 30 | velodyne_msgs 31 | 32 | roslaunch 33 | rostest 34 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_driver/src/driver/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # build the driver node 2 | add_executable(velodyne_node velodyne_node.cc driver.cc) 3 | add_dependencies(velodyne_node velodyne_driver_gencfg) 4 | target_link_libraries(velodyne_node 5 | velodyne_input 6 | ${catkin_LIBRARIES} 7 | ${libpcap_LIBRARIES} 8 | ) 9 | 10 | # build the nodelet version 11 | add_library(driver_nodelet nodelet.cc driver.cc) 12 | add_dependencies(driver_nodelet velodyne_driver_gencfg) 13 | target_link_libraries(driver_nodelet 14 | velodyne_input 15 | ${catkin_LIBRARIES} 16 | ${libpcap_LIBRARIES} 17 | ) 18 | 19 | # install runtime files 20 | install(TARGETS velodyne_node 21 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 22 | COMPONENT main 23 | ) 24 | install(TARGETS driver_nodelet 25 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 26 | ) 27 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_driver/src/driver/velodyne_node.cc: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2012 Austin Robot Technology, Jack O'Quin 2 | // All rights reserved. 3 | // 4 | // Software License Agreement (BSD License 2.0) 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions 8 | // are met: 9 | // 10 | // * Redistributions of source code must retain the above copyright 11 | // notice, this list of conditions and the following disclaimer. 12 | // * Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // * Neither the name of {copyright_holder} nor the names of its 17 | // contributors may be used to endorse or promote products derived 18 | // from this software without specific prior written permission. 19 | // 20 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | // COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | // INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | // BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | // LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | // POSSIBILITY OF SUCH DAMAGE. 32 | 33 | /** \file 34 | * 35 | * ROS driver node for the Velodyne 3D LIDARs. 36 | */ 37 | 38 | #include 39 | #include "velodyne_driver/driver.h" 40 | 41 | int main(int argc, char** argv) 42 | { 43 | ros::init(argc, argv, "velodyne_node"); 44 | ros::NodeHandle node; 45 | ros::NodeHandle private_nh("~"); 46 | 47 | // start the driver 48 | velodyne_driver::VelodyneDriver dvr(node, private_nh); 49 | 50 | // loop until shut down or end of file 51 | while(ros::ok()) 52 | { 53 | // poll device until end of file 54 | bool polled_ = dvr.poll(); 55 | if (!polled_) 56 | ROS_ERROR_THROTTLE(1.0, "Velodyne - Failed to poll device."); 57 | 58 | ros::spinOnce(); 59 | } 60 | 61 | return 0; 62 | } 63 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_driver/src/lib/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(velodyne_input input.cc) 2 | target_link_libraries(velodyne_input 3 | ${catkin_LIBRARIES} 4 | ${libpcap_LIBRARIES} 5 | ) 6 | if(catkin_EXPORTED_TARGETS) 7 | add_dependencies(velodyne_input ${catkin_EXPORTED_TARGETS}) 8 | endif() 9 | 10 | install(TARGETS velodyne_input 11 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 12 | ) 13 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_driver/src/vdump: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # dump velodyne packets 4 | # $Id: vdump 8892 2009-10-24 15:13:57Z joq $ 5 | 6 | if [ x$1 = x ] 7 | then echo -e "usage:\t`basename $0` file-prefix [ interface ]" 8 | echo -e "\n\tfile-prefix is completed with a three-digit number" 9 | echo -e "\tinterface default is 'eth0'\n" 10 | exit 9 11 | fi 12 | 13 | IF=${2:-eth0} 14 | UN=`id -un` 15 | ID=`id -u` 16 | 17 | echo "acquiring packets on $IF for user $UN; press ^C when done" 18 | 19 | if [ $ID = 0 ]; then 20 | /usr/sbin/tcpdump -i $IF -s 0 -C 100 -W 999 -w $1 21 | else 22 | sudo /usr/sbin/tcpdump -i $IF -Z $UN -s 0 -C 100 -W 999 -w $1 23 | fi 24 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_driver/tests/diagnostic_agg.yaml: -------------------------------------------------------------------------------- 1 | ## Diagnostic aggregator parameters for testing Velodyne diagnostics. 2 | # 3 | # $ rosparam load $(rospack find velodyne_driver)/tests/diagnostic_agg.yaml 4 | # $ rosrun diagnostic_aggregator aggregator_node 5 | # 6 | diagnostic_aggregator: 7 | analyzers: 8 | sensors: 9 | type: diagnostic_aggregator/AnalyzerGroup 10 | path: Sensors 11 | analyzers: 12 | velodyne: 13 | type: diagnostic_aggregator/GenericAnalyzer 14 | path: Velodyne HDL 15 | timeout: 5.0 16 | find_and_remove_prefix: velodyne_nodelet_manager 17 | num_items: 1 18 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_driver/tests/pcap_32e_node_hertz.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_driver/tests/pcap_32e_nodelet_hertz.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_driver/tests/pcap_node_hertz.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_driver/tests/pcap_nodelet_hertz.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_driver/tests/pcap_vlp16_node_hertz.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_driver/tests/pcap_vlp16_nodelet_hertz.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_driver/tests/timeconversiontest.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2019 Matthew Pitropov, Joshua Whitley 2 | // All rights reserved. 3 | // 4 | // Software License Agreement (BSD License 2.0) 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions 8 | // are met: 9 | // 10 | // * Redistributions of source code must retain the above copyright 11 | // notice, this list of conditions and the following disclaimer. 12 | // * Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // * Neither the name of {copyright_holder} nor the names of its 17 | // contributors may be used to endorse or promote products derived 18 | // from this software without specific prior written permission. 19 | // 20 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | // COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | // INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | // BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | // LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | // POSSIBILITY OF SUCH DAMAGE. 32 | 33 | #include "velodyne_driver/time_conversion.hpp" 34 | #include 35 | #include 36 | 37 | TEST(TimeConversion, BytesToTimestamp) 38 | { 39 | ros::Time::init(); 40 | ros::Time ros_stamp = ros::Time::now(); 41 | 42 | // get the seconds past the hour and multiply by 1million to convert to microseconds 43 | // divide nanoseconds by 1000 to convert to microseconds 44 | uint32_t since_the_hour = ((ros_stamp.sec % 3600) * 1000000) + ros_stamp.nsec / 1000; 45 | 46 | uint8_t native_format[4]; 47 | native_format[0] = 0xFF & since_the_hour; 48 | native_format[1] = 0xFF & (((uint32_t)since_the_hour) >> 8); 49 | native_format[2] = 0xFF & (((uint32_t)since_the_hour) >> 16); 50 | native_format[3] = 0xFF & (((uint32_t)since_the_hour) >> 24); 51 | 52 | ros::Time ros_stamp_converted = rosTimeFromGpsTimestamp(native_format); 53 | 54 | ASSERT_EQ(ros_stamp_converted.sec, ros_stamp.sec); 55 | ASSERT_NEAR(ros_stamp_converted.nsec, ros_stamp.nsec, 1000); 56 | } 57 | 58 | int main(int argc, char **argv) 59 | { 60 | ::testing::InitGoogleTest(&argc, argv); 61 | int ret = RUN_ALL_TESTS(); 62 | return ret; 63 | } 64 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_laserscan/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package velodyne_laserscan 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.5.2 (2019-01-28) 6 | ------------------ 7 | 8 | 1.5.1 (2018-12-10) 9 | ------------------ 10 | 11 | 1.5.0 (2018-10-19) 12 | ------------------ 13 | 14 | 1.4.0 (2018-09-19) 15 | ------------------ 16 | * Merge pull request `#170 `_ from ros-drivers/maint/move_header_files 17 | * Moving header files to traditional location inside include folders. 18 | * Merge pull request `#160 `_ from ros-drivers/maint/updating_package_xml_to_v2 19 | * Updated all package.xmls to ver 2. Cleaned up catkin_lint errors. 20 | All package.xml files are now compatible with version 2 of the 21 | package.xml specification in REP 140. Removed some unnecessary 22 | execute permissions on a few files. Fixed a missing test_depend. 23 | * Merge pull request `#146 `_ from stsundermann/patch-2 24 | Use std::abs instead of fabsf 25 | * Merge pull request `#150 `_ from ros-drivers/mikaelarguedas-patch-1 26 | * update to use non deprecated pluginlib macro 27 | * Use std::abs instead of fabsf 28 | cfg\_.resolution is double but fabsf takes a float which may cause truncation of value. 29 | * Contributors: Andre Volk, CNR, Joshua Whitley, Mikael Arguedas, Stephan Sundermann 30 | 31 | 1.3.0 (2017-11-10) 32 | ------------------ 33 | * Merge pull request `#110 `_ from kmhallen/master 34 | Added velodyne_laserscan package 35 | * Added tests for velodyne_laserscan 36 | * Fixed validating PointCloud2 field types 37 | * Package.xml format version 2 38 | * Merge pull request `#1 `_ from volkandre/master 39 | Fixed bug. Laserscans now cover full 360 degrees. 40 | * Fixed bug. Laserscans now cover full 360 degrees. 41 | * Added velodyne_laserscan package and inserted into existing launch files 42 | * Contributors: Joshua Whitley, Kevin Hallenbeck, kmhallen, volkandre 43 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_laserscan/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(velodyne_laserscan) 3 | 4 | # Set minimum C++ standard to C++11 5 | if (NOT "${CMAKE_CXX_STANDARD_COMPUTED_DEFAULT}") 6 | message(STATUS "Changing CXX_STANDARD from C++98 to C++11") 7 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 8 | elseif ("${CMAKE_CXX_STANDARD_COMPUTED_DEFAULT}" STREQUAL "98") 9 | message(STATUS "Changing CXX_STANDARD from C++98 to C++11") 10 | set(CMAKE_CXX_STANDARD 11) 11 | endif() 12 | 13 | find_package(catkin REQUIRED COMPONENTS 14 | roscpp 15 | nodelet 16 | sensor_msgs 17 | dynamic_reconfigure 18 | roslint 19 | ) 20 | 21 | generate_dynamic_reconfigure_options( 22 | cfg/VelodyneLaserScan.cfg 23 | ) 24 | 25 | catkin_package(CATKIN_DEPENDS 26 | sensor_msgs 27 | ) 28 | 29 | include_directories( 30 | include 31 | ${catkin_INCLUDE_DIRS} 32 | ) 33 | 34 | add_library(${PROJECT_NAME} 35 | src/velodyne_laserscan.cpp 36 | src/nodelet.cpp 37 | ) 38 | add_dependencies(${PROJECT_NAME} 39 | ${PROJECT_NAME}_gencfg 40 | ) 41 | target_link_libraries(${PROJECT_NAME} 42 | ${catkin_LIBRARIES} 43 | ) 44 | 45 | add_executable(${PROJECT_NAME}_node 46 | src/node.cpp 47 | ) 48 | target_link_libraries(${PROJECT_NAME}_node 49 | ${catkin_LIBRARIES} 50 | ${PROJECT_NAME} 51 | ) 52 | 53 | install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 54 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 55 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 56 | ) 57 | install(FILES nodelets.xml 58 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 59 | ) 60 | 61 | roslint_cpp() 62 | 63 | if (CATKIN_ENABLE_TESTING) 64 | add_subdirectory(tests) 65 | endif() 66 | 67 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_laserscan/cfg/VelodyneLaserScan.cfg: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | PACKAGE='velodyne_laserscan' 4 | 5 | from dynamic_reconfigure.parameter_generator_catkin import * 6 | 7 | gen = ParameterGenerator() 8 | 9 | # Name Type Lvl Description Default Min Max 10 | gen.add("ring", int_t, 0, "Ring to extract as laser scan (-1 default)", -1, -1, 31) 11 | gen.add("resolution", double_t, 0, "Laser scan angular resolution (rad)", 0.007, 0.001, 0.05) 12 | 13 | exit(gen.generate(PACKAGE, PACKAGE, "VelodyneLaserScan")) 14 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_laserscan/include/velodyne_laserscan/velodyne_laserscan.h: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2018, 2019 Kevin Hallenbeck, Joshua Whitley 2 | // All rights reserved. 3 | // 4 | // Software License Agreement (BSD License 2.0) 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions 8 | // are met: 9 | // 10 | // * Redistributions of source code must retain the above copyright 11 | // notice, this list of conditions and the following disclaimer. 12 | // * Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // * Neither the name of {copyright_holder} nor the names of its 17 | // contributors may be used to endorse or promote products derived 18 | // from this software without specific prior written permission. 19 | // 20 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | // COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | // INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | // BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | // LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | // POSSIBILITY OF SUCH DAMAGE. 32 | 33 | #ifndef VELODYNE_LASERSCAN_VELODYNE_LASERSCAN_H 34 | #define VELODYNE_LASERSCAN_VELODYNE_LASERSCAN_H 35 | 36 | #include 37 | #include 38 | #include 39 | 40 | #include 41 | #include 42 | 43 | #include 44 | #include 45 | 46 | namespace velodyne_laserscan 47 | { 48 | 49 | class VelodyneLaserScan 50 | { 51 | public: 52 | VelodyneLaserScan(ros::NodeHandle &nh, ros::NodeHandle &nh_priv); 53 | 54 | private: 55 | boost::mutex connect_mutex_; 56 | void connectCb(); 57 | void recvCallback(const sensor_msgs::PointCloud2ConstPtr& msg); 58 | 59 | ros::NodeHandle nh_; 60 | ros::Subscriber sub_; 61 | ros::Publisher pub_; 62 | 63 | VelodyneLaserScanConfig cfg_; 64 | dynamic_reconfigure::Server srv_; 65 | void reconfig(VelodyneLaserScanConfig& config, uint32_t level); 66 | 67 | unsigned int ring_count_; 68 | }; 69 | 70 | } // namespace velodyne_laserscan 71 | 72 | #endif // VELODYNE_LASERSCAN_VELODYNE_LASERSCAN_H 73 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_laserscan/nodelets.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | Extract a single ring from a Velodyne PointCloud2 and publish 7 | as a LaserScan. 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_laserscan/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | velodyne_laserscan 4 | 1.5.2 5 | 6 | Extract a single ring of a Velodyne PointCloud2 and publish it as a LaserScan message 7 | 8 | Josh Whitley 9 | Micho Radovnikovich 10 | Kevin Hallenbeck 11 | BSD 12 | 13 | http://ros.org/wiki/velodyne_laserscan 14 | https://github.com/ros-drivers/velodyne 15 | https://github.com/ros-drivers/velodyne/issues 16 | 17 | catkin 18 | 19 | roslint 20 | 21 | roscpp 22 | nodelet 23 | sensor_msgs 24 | dynamic_reconfigure 25 | 26 | roslaunch 27 | rostest 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_laserscan/src/node.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2018, 2019 Kevin Hallenbeck, Joshua Whitley 2 | // All rights reserved. 3 | // 4 | // Software License Agreement (BSD License 2.0) 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions 8 | // are met: 9 | // 10 | // * Redistributions of source code must retain the above copyright 11 | // notice, this list of conditions and the following disclaimer. 12 | // * Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // * Neither the name of {copyright_holder} nor the names of its 17 | // contributors may be used to endorse or promote products derived 18 | // from this software without specific prior written permission. 19 | // 20 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | // COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | // INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | // BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | // LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | // POSSIBILITY OF SUCH DAMAGE. 32 | 33 | #include 34 | #include "velodyne_laserscan/velodyne_laserscan.h" 35 | 36 | int main(int argc, char** argv) 37 | { 38 | ros::init(argc, argv, "velodyne_laserscan_node"); 39 | ros::NodeHandle nh; 40 | ros::NodeHandle nh_priv("~"); 41 | 42 | // create VelodyneLaserScan class 43 | velodyne_laserscan::VelodyneLaserScan n(nh, nh_priv); 44 | 45 | // handle callbacks until shut down 46 | ros::spin(); 47 | 48 | return 0; 49 | } 50 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_laserscan/src/nodelet.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2018, 2019 Kevin Hallenbeck, Joshua Whitley 2 | // All rights reserved. 3 | // 4 | // Software License Agreement (BSD License 2.0) 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions 8 | // are met: 9 | // 10 | // * Redistributions of source code must retain the above copyright 11 | // notice, this list of conditions and the following disclaimer. 12 | // * Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // * Neither the name of {copyright_holder} nor the names of its 17 | // contributors may be used to endorse or promote products derived 18 | // from this software without specific prior written permission. 19 | // 20 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | // COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | // INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | // BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | // LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | // POSSIBILITY OF SUCH DAMAGE. 32 | 33 | #include 34 | #include 35 | #include 36 | #include "velodyne_laserscan/velodyne_laserscan.h" 37 | 38 | namespace velodyne_laserscan 39 | { 40 | 41 | class LaserScanNodelet: public nodelet::Nodelet 42 | { 43 | public: 44 | LaserScanNodelet() {} 45 | ~LaserScanNodelet() {} 46 | 47 | private: 48 | virtual void onInit() 49 | { 50 | node_.reset(new VelodyneLaserScan(getNodeHandle(), getPrivateNodeHandle())); 51 | } 52 | 53 | boost::shared_ptr node_; 54 | }; 55 | 56 | } // namespace velodyne_laserscan 57 | 58 | PLUGINLIB_EXPORT_CLASS(velodyne_laserscan::LaserScanNodelet, nodelet::Nodelet); 59 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_laserscan/tests/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ### Unit tests 2 | # 3 | # Only configured when CATKIN_ENABLE_TESTING is true. 4 | 5 | # These dependencies are only needed for unit testing 6 | find_package(roslaunch REQUIRED) 7 | find_package(rostest REQUIRED) 8 | 9 | # C++ gtests 10 | #catkin_add_gtest(test_calibration test_calibration.cpp) 11 | #add_dependencies(test_calibration ${catkin_EXPORTED_TARGETS}) 12 | #target_link_libraries(test_calibration velodyne_rawdata ${catkin_LIBRARIES}) 13 | 14 | # ROS rostests 15 | add_rostest_gtest(test_lazy_subscriber_node lazy_subscriber_node.test lazy_subscriber.cpp) 16 | add_dependencies(test_lazy_subscriber_node ${catkin_EXPORTED_TARGETS}) 17 | target_link_libraries(test_lazy_subscriber_node ${catkin_LIBRARIES}) 18 | 19 | add_rostest_gtest(test_lazy_subscriber_nodelet lazy_subscriber_nodelet.test lazy_subscriber.cpp) 20 | add_dependencies(test_lazy_subscriber_nodelet ${catkin_EXPORTED_TARGETS}) 21 | target_link_libraries(test_lazy_subscriber_nodelet ${catkin_LIBRARIES}) 22 | 23 | add_rostest_gtest(test_system_node system_node.test system.cpp) 24 | add_dependencies(test_system_node ${catkin_EXPORTED_TARGETS}) 25 | target_link_libraries(test_system_node ${catkin_LIBRARIES}) 26 | 27 | add_rostest_gtest(test_system_nodelet system_nodelet.test system.cpp) 28 | add_dependencies(test_system_nodelet ${catkin_EXPORTED_TARGETS}) 29 | target_link_libraries(test_system_nodelet ${catkin_LIBRARIES}) 30 | 31 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_laserscan/tests/lazy_subscriber_node.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_laserscan/tests/lazy_subscriber_nodelet.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 14 | 15 | 16 | 17 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_laserscan/tests/system_node.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_laserscan/tests/system_nodelet.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 14 | 15 | 16 | 17 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_msgs/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | Change history 2 | ============== 3 | 4 | 1.5.2 (2019-01-28) 5 | ------------------ 6 | 7 | 1.5.1 (2018-12-10) 8 | ------------------ 9 | 10 | 1.5.0 (2018-10-19) 11 | ------------------ 12 | 13 | 1.4.0 (2018-09-19) 14 | ------------------ 15 | * Updated all package.xmls to ver 2. Cleaned up catkin_lint errors. 16 | All package.xml files are now compatible with version 2 of the 17 | package.xml specification in REP 140. Removed some unnecessary 18 | execute permissions on a few files. Fixed a missing test_depend. 19 | * Updated cut_at_specified_angle_feature with latest master version. 20 | * Contributors: Andre Volk, Joshua Whitley 21 | 22 | 1.3.0 (2017-11-10) 23 | ------------------ 24 | 25 | 1.2.0 (2014-08-06) 26 | ------------------ 27 | 28 | 1.1.2 (2013-11-05) 29 | ------------------- 30 | 31 | 1.1.1 (2013-07-30) 32 | ------------------ 33 | 34 | 1.1.0 (2013-07-16) 35 | ------------------ 36 | 37 | 1.0.1 (2013-06-15) 38 | ------------------ 39 | 40 | 1.0.0 (2013-06-14) 41 | ------------------ 42 | 43 | * Convert to catkin (`#1`_). 44 | * Release to Hydro. 45 | 46 | 0.9.2 (2013-07-08) 47 | ------------------ 48 | 49 | 0.9.1 (2012-06-05) 50 | ------------------ 51 | 52 | * Released to Electric, Fuerte and Groovy. 53 | 54 | 0.9.0 (2012-04-03) 55 | ------------------ 56 | 57 | * Completely revised API, anticipating a 1.0.0 release. 58 | * Released to Electric, Fuerte and Groovy. 59 | 60 | 0.2.6 (2011-02-23) 61 | ------------------ 62 | 63 | 0.2.5 (2010-11-19) 64 | ------------------ 65 | 66 | * Initial implementation of new 0.3 interfaces. 67 | * Support for ROS 1.3 `std_msgs::Header` changes. 68 | 69 | 0.2.0 (2010-08-17) 70 | ------------------ 71 | 72 | * Initial release to ROS C-turtle. 73 | 74 | .. _`#1`: https://github.com/ros-drivers/velodyne/issues/1 75 | .. _`#4`: https://github.com/ros-drivers/velodyne/issues/4 76 | .. _`#7`: https://github.com/ros-drivers/velodyne/issues/7 77 | .. _`#8`: https://github.com/ros-drivers/velodyne/pull/8 78 | .. _`#9`: https://github.com/ros-drivers/velodyne/issues/9 79 | .. _`#10`: https://github.com/ros-drivers/velodyne/issues/10 80 | .. _`#11`: https://github.com/ros-drivers/velodyne/issues/11 81 | .. _`#12`: https://github.com/ros-drivers/velodyne/pull/12 82 | .. _`#13`: https://github.com/ros-drivers/velodyne/issues/13 83 | .. _`#14`: https://github.com/ros-drivers/velodyne/pull/14 84 | .. _`#17`: https://github.com/ros-drivers/velodyne/issues/17 85 | .. _`#18`: https://github.com/ros-drivers/velodyne/issues/18 86 | .. _`#20`: https://github.com/ros-drivers/velodyne/issues/20 87 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(velodyne_msgs) 3 | 4 | find_package(catkin REQUIRED COMPONENTS message_generation std_msgs) 5 | 6 | add_message_files( 7 | DIRECTORY msg 8 | FILES 9 | VelodynePacket.msg 10 | VelodyneScan.msg 11 | ) 12 | generate_messages(DEPENDENCIES std_msgs) 13 | 14 | catkin_package( 15 | CATKIN_DEPENDS message_runtime std_msgs 16 | ) 17 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_msgs/msg/VelodynePacket.msg: -------------------------------------------------------------------------------- 1 | # Raw Velodyne LIDAR packet. 2 | 3 | time stamp # packet timestamp 4 | uint8[1206] data # packet contents 5 | 6 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_msgs/msg/VelodyneScan.msg: -------------------------------------------------------------------------------- 1 | # Velodyne LIDAR scan packets. 2 | 3 | Header header # standard ROS message header 4 | VelodynePacket[] packets # vector of raw packets 5 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | velodyne_msgs 4 | 1.5.2 5 | 6 | ROS message definitions for Velodyne 3D LIDARs. 7 | 8 | Josh Whitley 9 | Jack O'Quin 10 | BSD 11 | 12 | http://ros.org/wiki/velodyne_msgs 13 | https://github.com/ros-drivers/velodyne 14 | https://github.com/ros-drivers/velodyne/issues 15 | 16 | catkin 17 | 18 | message_generation 19 | 20 | std_msgs 21 | 22 | message_runtime 23 | 24 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_pointcloud/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(velodyne_pointcloud) 3 | 4 | # Set minimum C++ standard to C++11 5 | if (NOT "${CMAKE_CXX_STANDARD_COMPUTED_DEFAULT}") 6 | message(STATUS "Changing CXX_STANDARD from C++98 to C++11") 7 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 8 | elseif ("${CMAKE_CXX_STANDARD_COMPUTED_DEFAULT}" STREQUAL "98") 9 | message(STATUS "Changing CXX_STANDARD from C++98 to C++11") 10 | set(CMAKE_CXX_STANDARD 11) 11 | endif() 12 | 13 | set(${PROJECT_NAME}_CATKIN_DEPS 14 | angles 15 | nodelet 16 | roscpp 17 | roslib 18 | sensor_msgs 19 | tf 20 | velodyne_driver 21 | velodyne_msgs 22 | dynamic_reconfigure 23 | diagnostic_updater 24 | ) 25 | 26 | find_package(catkin REQUIRED COMPONENTS 27 | ${${PROJECT_NAME}_CATKIN_DEPS} 28 | roslint) 29 | 30 | find_package(Boost COMPONENTS signals) 31 | find_package(Eigen3 REQUIRED) 32 | # Resolve system dependency on yaml-cpp, which apparently does not 33 | # provide a CMake find_package() module. 34 | find_package(PkgConfig REQUIRED) 35 | pkg_check_modules(YAML_CPP REQUIRED yaml-cpp) 36 | find_path(YAML_CPP_INCLUDE_DIR 37 | NAMES yaml_cpp.h 38 | PATHS ${YAML_CPP_INCLUDE_DIRS}) 39 | find_library(YAML_CPP_LIBRARY 40 | NAMES YAML_CPP 41 | PATHS ${YAML_CPP_LIBRARY_DIRS}) 42 | 43 | link_directories(${YAML_CPP_LIBRARY_DIRS}) 44 | 45 | generate_dynamic_reconfigure_options( 46 | cfg/CloudNode.cfg cfg/TransformNode.cfg 47 | ) 48 | 49 | if(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5") 50 | add_definitions(-DHAVE_NEW_YAMLCPP) 51 | endif(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5") 52 | 53 | include_directories(include ${catkin_INCLUDE_DIRS} 54 | ${dynamic_reconfigure_PACKAGE_PATH}/cmake/cfgbuild.cmake 55 | ${EIGEN3_INCLUDE_DIR} 56 | ) 57 | 58 | catkin_package( 59 | CATKIN_DEPENDS ${${PROJECT_NAME}_CATKIN_DEPS} 60 | INCLUDE_DIRS include 61 | LIBRARIES velodyne_rawdata) 62 | 63 | #add_executable(dynamic_reconfigure_node src/dynamic_reconfigure_node.cpp) 64 | #target_link_libraries(dynamic_reconfigure_node 65 | # ${catkin_LIBRARIES} 66 | # ) 67 | 68 | add_subdirectory(src/lib) 69 | add_subdirectory(src/conversions) 70 | 71 | install(DIRECTORY include/${PROJECT_NAME}/ 72 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) 73 | install(FILES nodelets.xml 74 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 75 | install(DIRECTORY launch/ 76 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch) 77 | install(DIRECTORY params/ 78 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/params) 79 | install(PROGRAMS scripts/gen_calibration.py 80 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 81 | 82 | roslint_cpp() 83 | 84 | if (CATKIN_ENABLE_TESTING) 85 | add_subdirectory(tests) 86 | endif() 87 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_pointcloud/cfg/CloudNode.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "velodyne_pointcloud" 3 | 4 | from math import pi 5 | from dynamic_reconfigure.parameter_generator_catkin import * 6 | 7 | gen = ParameterGenerator() 8 | 9 | gen.add("fixed_frame", str_t, 0, "The desired input frame", "velodyne") 10 | gen.add("target_frame", str_t, 0, "The desired output frame", "velodyne") 11 | gen.add("min_range", double_t, 0, "min range to publish", 0.9, 0.1, 10.0) 12 | gen.add("max_range", double_t, 0, "max range to publish", 130, 0.1, 200) 13 | gen.add("view_direction", double_t, 0, "angle defining the center of view", 14 | 0.0, -pi, pi) 15 | gen.add("view_width", double_t, 0, "angle defining the view width", 16 | 2*pi, 0.0, 2*pi) 17 | gen.add("organize_cloud", bool_t, 0, "organized cloud", False) 18 | 19 | exit(gen.generate(PACKAGE, "cloud_node", "CloudNode")) 20 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_pointcloud/cfg/TransformNode.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "velodyne_pointcloud" 3 | 4 | from math import pi 5 | import dynamic_reconfigure.parameter_generator_catkin as pgc 6 | 7 | gen = pgc.ParameterGenerator() 8 | 9 | gen.add("min_range", 10 | pgc.double_t, 11 | 0, 12 | "min range to publish", 13 | 0.9, 0.1, 10.0) 14 | 15 | gen.add("max_range", 16 | pgc.double_t, 17 | 0, 18 | "max range to publish", 19 | 130, 0.1, 200) 20 | 21 | gen.add("view_direction", 22 | pgc.double_t, 23 | 0, 24 | "angle defining the center of view", 25 | 0.0, -pi, pi) 26 | 27 | gen.add("view_width", 28 | pgc.double_t, 29 | 0, 30 | "angle defining the view width", 31 | 2*pi, 0.0, 2*pi) 32 | 33 | gen.add("frame_id", 34 | pgc.str_t, 35 | 0, 36 | "fixed frame of reference for point clouds", 37 | "map") 38 | 39 | gen.add("organize_cloud", 40 | pgc.bool_t, 41 | 0, 42 | "organize cloud", 43 | False) 44 | 45 | exit(gen.generate(PACKAGE, "transform_node", "TransformNode")) 46 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_pointcloud/include/velodyne_pointcloud/organized_cloudXYZIR.h: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2012, 2019 Austin Robot Technology, Jack O'Quin, Joshua Whitley, Sebastian Pütz 2 | // All rights reserved. 3 | // 4 | // Software License Agreement (BSD License 2.0) 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions 8 | // are met: 9 | // 10 | // * Redistributions of source code must retain the above copyright 11 | // notice, this list of conditions and the following disclaimer. 12 | // * Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // * Neither the name of {copyright_holder} nor the names of its 17 | // contributors may be used to endorse or promote products derived 18 | // from this software without specific prior written permission. 19 | // 20 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | // COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | // INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | // BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | // LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | // POSSIBILITY OF SUCH DAMAGE. 32 | 33 | #ifndef VELODYNE_POINTCLOUD_ORGANIZED_CLOUDXYZIR_H 34 | #define VELODYNE_POINTCLOUD_ORGANIZED_CLOUDXYZIR_H 35 | 36 | #include 37 | #include 38 | #include 39 | 40 | namespace velodyne_pointcloud 41 | { 42 | class OrganizedCloudXYZIR : public velodyne_rawdata::DataContainerBase 43 | { 44 | public: 45 | OrganizedCloudXYZIR(const double max_range, const double min_range, const std::string& target_frame, 46 | const std::string& fixed_frame, const unsigned int num_lasers, const unsigned int scans_per_block, 47 | boost::shared_ptr tf_ptr = boost::shared_ptr()); 48 | 49 | virtual void newLine(); 50 | 51 | virtual void setup(const velodyne_msgs::VelodyneScan::ConstPtr& scan_msg); 52 | 53 | virtual void addPoint(float x, float y, float z, const uint16_t ring, const uint16_t azimuth, const float distance, 54 | const float intensity, const float time); 55 | 56 | private: 57 | sensor_msgs::PointCloud2Iterator iter_x, iter_y, iter_z, iter_intensity, iter_time; 58 | sensor_msgs::PointCloud2Iterator iter_ring; 59 | }; 60 | } /* namespace velodyne_pointcloud */ 61 | #endif // VELODYNE_POINTCLOUD_ORGANIZED_CLOUDXYZIR_H 62 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_pointcloud/include/velodyne_pointcloud/pointcloudXYZIR.h: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2012, 2019 Austin Robot Technology, Jack O'Quin, Joshua Whitley, Sebastian Pütz 2 | // All rights reserved. 3 | // 4 | // Software License Agreement (BSD License 2.0) 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions 8 | // are met: 9 | // 10 | // * Redistributions of source code must retain the above copyright 11 | // notice, this list of conditions and the following disclaimer. 12 | // * Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // * Neither the name of {copyright_holder} nor the names of its 17 | // contributors may be used to endorse or promote products derived 18 | // from this software without specific prior written permission. 19 | // 20 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | // COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | // INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | // BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | // LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | // POSSIBILITY OF SUCH DAMAGE. 32 | 33 | #ifndef VELODYNE_POINTCLOUD_POINTCLOUDXYZIR_H 34 | #define VELODYNE_POINTCLOUD_POINTCLOUDXYZIR_H 35 | 36 | #include 37 | #include 38 | 39 | namespace velodyne_pointcloud 40 | { 41 | class PointcloudXYZIR : public velodyne_rawdata::DataContainerBase 42 | { 43 | public: 44 | PointcloudXYZIR(const double max_range, const double min_range, const std::string& target_frame, 45 | const std::string& fixed_frame, const unsigned int scans_per_block, 46 | boost::shared_ptr tf_ptr = boost::shared_ptr()); 47 | 48 | virtual void newLine(); 49 | 50 | virtual void setup(const velodyne_msgs::VelodyneScan::ConstPtr& scan_msg); 51 | 52 | virtual void addPoint(float x, float y, float z, const uint16_t ring, const uint16_t azimuth, 53 | const float distance, const float intensity, const float time); 54 | 55 | sensor_msgs::PointCloud2Iterator iter_x, iter_y, iter_z, iter_intensity, iter_time; 56 | sensor_msgs::PointCloud2Iterator iter_ring; 57 | }; 58 | } // namespace velodyne_pointcloud 59 | 60 | #endif // VELODYNE_POINTCLOUD_POINTCLOUDXYZIR_H 61 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_pointcloud/launch/32e_points.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_pointcloud/launch/64e_S3.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_pointcloud/launch/VLP-32C_points.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_pointcloud/launch/VLP16_points.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_pointcloud/launch/cloud_nodelet.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_pointcloud/launch/laserscan_nodelet.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_pointcloud/launch/transform_nodelet.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_pointcloud/nodelets.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 7 | 8 | Aggregates points from multiple packets, publishing PointCloud2. 9 | 10 | 11 | 12 | 13 | 14 | 17 | 18 | Converts a Velodyne PointCloud2 to PointXYZRGB, assigning colors 19 | for visualization of the laser rings. 20 | 21 | 22 | 23 | 24 | 25 | 28 | 29 | Transforms packets into /map frame, publishing multiple packets 30 | as PointCloud2. 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_pointcloud/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | velodyne_pointcloud 4 | 1.5.2 5 | 6 | Point cloud conversions for Velodyne 3D LIDARs. 7 | 8 | Josh Whitley 9 | Jack O'Quin 10 | Piyush Khandelwal 11 | Jesse Vera 12 | Sebastian Pütz 13 | BSD 14 | http://ros.org/wiki/velodyne_pointcloud 15 | https://github.com/ros-drivers/velodyne 16 | https://github.com/ros-drivers/velodyne/issues 17 | 18 | catkin 19 | 20 | roslint 21 | 22 | angles 23 | nodelet 24 | roscpp 25 | roslib 26 | sensor_msgs 27 | tf 28 | velodyne_driver 29 | velodyne_msgs 30 | yaml-cpp 31 | dynamic_reconfigure 32 | diagnostic_updater 33 | eigen 34 | 35 | velodyne_laserscan 36 | 37 | rosunit 38 | roslaunch 39 | rostest 40 | tf2_ros 41 | 42 | 43 | 44 | 45 | 46 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_pointcloud/params/rviz_points.vcg: -------------------------------------------------------------------------------- 1 | Background\ ColorB=0 2 | Background\ ColorG=0 3 | Background\ ColorR=0 4 | Camera\ Config=0.384797 3.10041 41.6639 0 0 0 5 | Camera\ Type=rviz::OrbitViewController 6 | Fixed\ Frame=/velodyne 7 | Grid.Alpha=0.5 8 | Grid.Cell\ Size=1 9 | Grid.ColorB=0.5 10 | Grid.ColorG=0.5 11 | Grid.ColorR=0.5 12 | Grid.Enabled=1 13 | Grid.Line\ Style=0 14 | Grid.Line\ Width=0.03 15 | Grid.Normal\ Cell\ Count=0 16 | Grid.OffsetX=0 17 | Grid.OffsetY=0 18 | Grid.OffsetZ=0 19 | Grid.Plane=0 20 | Grid.Plane\ Cell\ Count=50 21 | Grid.Reference\ Frame= 22 | Property\ Grid\ Splitter=751,78 23 | Property\ Grid\ State=expanded=.Global Options,Grid.Enabled,Velodyne Cloud.Enabled;splitterratio=0.5 24 | QMainWindow=000000ff00000000fd00000003000000000000011d0000038ffc0200000001fb000000100044006900730070006c006100790073010000001d0000038f000000ee00ffffff00000001000001300000038ffc0200000003fb0000001e0054006f006f006c002000500072006f0070006500720074006900650073010000001d0000011f0000006700fffffffb0000000a00560069006500770073010000014200000145000000bb00fffffffb0000001200530065006c0065006300740069006f006e010000028d0000011f0000006700ffffff00000003000005d10000003efc0100000001fb0000000800540069006d00650100000000000005d1000002bf00ffffff000003780000038f00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 25 | TF.All\ Enabled=1 26 | TF.Enabled=1 27 | TF.Frame\ Timeout=15 28 | TF.Marker\ Scale=1 29 | TF.Show\ Arrows=1 30 | TF.Show\ Axes=1 31 | TF.Show\ Names=1 32 | TF.Update\ Interval=0 33 | Target\ Frame= 34 | Tool\ 2D\ Nav\ GoalTopic=move_base_simple/goal 35 | Tool\ 2D\ Pose\ EstimateTopic=initialpose 36 | Velodyne\ Cloud..AxisColorAutocompute\ Value\ Bounds=1 37 | Velodyne\ Cloud..AxisColorAxis=2 38 | Velodyne\ Cloud..AxisColorMax\ Value=10 39 | Velodyne\ Cloud..AxisColorMin\ Value=-10 40 | Velodyne\ Cloud..AxisColorUse\ Fixed\ Frame=1 41 | Velodyne\ Cloud..FlatColorColorB=1 42 | Velodyne\ Cloud..FlatColorColorG=1 43 | Velodyne\ Cloud..FlatColorColorR=1 44 | Velodyne\ Cloud..IntensityAutocompute\ Intensity\ Bounds=1 45 | Velodyne\ Cloud..IntensityChannel\ Name=intensity 46 | Velodyne\ Cloud..IntensityMax\ ColorB=1 47 | Velodyne\ Cloud..IntensityMax\ ColorG=0.776471 48 | Velodyne\ Cloud..IntensityMax\ ColorR=0.796078 49 | Velodyne\ Cloud..IntensityMax\ Intensity=255 50 | Velodyne\ Cloud..IntensityMin\ ColorB=1 51 | Velodyne\ Cloud..IntensityMin\ ColorG=0.0823529 52 | Velodyne\ Cloud..IntensityMin\ ColorR=0.141176 53 | Velodyne\ Cloud..IntensityMin\ Intensity=95 54 | Velodyne\ Cloud..IntensityUse\ full\ RGB\ spectrum=0 55 | Velodyne\ Cloud.Alpha=1 56 | Velodyne\ Cloud.Billboard\ Size=0.1 57 | Velodyne\ Cloud.Color\ Transformer=Intensity 58 | Velodyne\ Cloud.Decay\ Time=0 59 | Velodyne\ Cloud.Enabled=1 60 | Velodyne\ Cloud.Position\ Transformer=XYZ 61 | Velodyne\ Cloud.Queue\ Size=10 62 | Velodyne\ Cloud.Selectable=1 63 | Velodyne\ Cloud.Style=0 64 | Velodyne\ Cloud.Topic=/velodyne_points 65 | [Display0] 66 | ClassName=rviz::GridDisplay 67 | Name=Grid 68 | [Display1] 69 | ClassName=rviz::TFDisplay 70 | Name=TF 71 | [Display2] 72 | ClassName=rviz::PointCloud2Display 73 | Name=Velodyne Cloud 74 | [Window] 75 | Height=1045 76 | Width=1505 77 | X=-8 78 | Y=-29 79 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_pointcloud/src/conversions/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_executable(cloud_node cloud_node.cc convert.cc pointcloudXYZIR.cc organized_cloudXYZIR.cc) 2 | add_dependencies(cloud_node ${${PROJECT_NAME}_EXPORTED_TARGETS}) 3 | target_link_libraries(cloud_node velodyne_rawdata 4 | ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES}) 5 | install(TARGETS cloud_node 6 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 7 | 8 | add_library(cloud_nodelet cloud_nodelet.cc convert.cc pointcloudXYZIR.cc organized_cloudXYZIR.cc) 9 | add_dependencies(cloud_nodelet ${${PROJECT_NAME}_EXPORTED_TARGETS}) 10 | target_link_libraries(cloud_nodelet velodyne_rawdata 11 | ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES}) 12 | install(TARGETS cloud_nodelet 13 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 14 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 15 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) 16 | 17 | add_executable(transform_node transform_node.cc transform.cc pointcloudXYZIR.cc organized_cloudXYZIR.cc) 18 | add_dependencies(transform_node ${${PROJECT_NAME}_EXPORTED_TARGETS}) 19 | target_link_libraries(transform_node velodyne_rawdata 20 | ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES}) 21 | install(TARGETS transform_node 22 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 23 | 24 | add_library(transform_nodelet transform_nodelet.cc transform.cc pointcloudXYZIR.cc organized_cloudXYZIR.cc) 25 | add_dependencies(transform_nodelet ${${PROJECT_NAME}_EXPORTED_TARGETS}) 26 | target_link_libraries(transform_nodelet velodyne_rawdata 27 | ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES}) 28 | install(TARGETS transform_nodelet 29 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 30 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 31 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) 32 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_pointcloud/src/conversions/cloud_node.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2012 Austin Robot Technology, Jack O'Quin 3 | * License: Modified BSD Software License Agreement 4 | * 5 | * $Id$ 6 | */ 7 | 8 | /** \file 9 | 10 | This ROS node converts raw Velodyne LIDAR packets to PointCloud2. 11 | 12 | */ 13 | 14 | #include 15 | #include "velodyne_pointcloud/convert.h" 16 | 17 | /** Main node entry point. */ 18 | int main(int argc, char **argv) 19 | { 20 | ros::init(argc, argv, "cloud_node"); 21 | ros::NodeHandle node; 22 | ros::NodeHandle priv_nh("~"); 23 | 24 | // create conversion class, which subscribes to raw data 25 | velodyne_pointcloud::Convert conv(node, priv_nh); 26 | 27 | // handle callbacks until shut down 28 | ros::spin(); 29 | 30 | return 0; 31 | } 32 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_pointcloud/src/conversions/cloud_nodelet.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2012 Austin Robot Technology, Jack O'Quin 3 | * License: Modified BSD Software License Agreement 4 | * 5 | * $Id$ 6 | */ 7 | 8 | /** @file 9 | 10 | This ROS nodelet converts raw Velodyne 3D LIDAR packets to a 11 | PointCloud2. 12 | 13 | */ 14 | 15 | #include 16 | #include 17 | #include 18 | 19 | #include "velodyne_pointcloud/convert.h" 20 | 21 | namespace velodyne_pointcloud 22 | { 23 | class CloudNodelet: public nodelet::Nodelet 24 | { 25 | public: 26 | 27 | CloudNodelet() {} 28 | ~CloudNodelet() {} 29 | 30 | private: 31 | 32 | virtual void onInit(); 33 | boost::shared_ptr conv_; 34 | }; 35 | 36 | /** @brief Nodelet initialization. */ 37 | void CloudNodelet::onInit() 38 | { 39 | conv_.reset(new Convert(getNodeHandle(), getPrivateNodeHandle(), getName())); 40 | } 41 | 42 | } // namespace velodyne_pointcloud 43 | 44 | 45 | // Register this plugin with pluginlib. Names must match nodelets.xml. 46 | // 47 | // parameters: class type, base class type 48 | PLUGINLIB_EXPORT_CLASS(velodyne_pointcloud::CloudNodelet, nodelet::Nodelet) 49 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_pointcloud/src/conversions/organized_cloudXYZIR.cc: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | 4 | namespace velodyne_pointcloud 5 | { 6 | 7 | OrganizedCloudXYZIR::OrganizedCloudXYZIR( 8 | const double max_range, const double min_range, 9 | const std::string& target_frame, const std::string& fixed_frame, 10 | const unsigned int num_lasers, const unsigned int scans_per_block, 11 | boost::shared_ptr tf_ptr) 12 | : DataContainerBase( 13 | max_range, min_range, target_frame, fixed_frame, 14 | num_lasers, 0, false, scans_per_block, tf_ptr, 6, 15 | "x", 1, sensor_msgs::PointField::FLOAT32, 16 | "y", 1, sensor_msgs::PointField::FLOAT32, 17 | "z", 1, sensor_msgs::PointField::FLOAT32, 18 | "intensity", 1, sensor_msgs::PointField::FLOAT32, 19 | "ring", 1, sensor_msgs::PointField::UINT16, 20 | "time", 1, sensor_msgs::PointField::FLOAT32), 21 | iter_x(cloud, "x"), iter_y(cloud, "y"), iter_z(cloud, "z"), 22 | iter_intensity(cloud, "intensity"), iter_ring(cloud, "ring"), iter_time(cloud, "time") 23 | { 24 | } 25 | 26 | void OrganizedCloudXYZIR::newLine() 27 | { 28 | iter_x = iter_x + config_.init_width; 29 | iter_y = iter_y + config_.init_width; 30 | iter_z = iter_z + config_.init_width; 31 | iter_ring = iter_ring + config_.init_width; 32 | iter_intensity = iter_intensity + config_.init_width; 33 | iter_time = iter_time + config_.init_width; 34 | ++cloud.height; 35 | } 36 | 37 | void OrganizedCloudXYZIR::setup(const velodyne_msgs::VelodyneScan::ConstPtr& scan_msg){ 38 | DataContainerBase::setup(scan_msg); 39 | iter_x = sensor_msgs::PointCloud2Iterator(cloud, "x"); 40 | iter_y = sensor_msgs::PointCloud2Iterator(cloud, "y"); 41 | iter_z = sensor_msgs::PointCloud2Iterator(cloud, "z"); 42 | iter_intensity = sensor_msgs::PointCloud2Iterator(cloud, "intensity"); 43 | iter_ring = sensor_msgs::PointCloud2Iterator(cloud, "ring"); 44 | iter_time = sensor_msgs::PointCloud2Iterator(cloud, "time"); 45 | } 46 | 47 | 48 | void OrganizedCloudXYZIR::addPoint(float x, float y, float z, 49 | const uint16_t ring, const uint16_t /*azimuth*/, const float distance, const float intensity, const float time) 50 | { 51 | /** The laser values are not ordered, the organized structure 52 | * needs ordered neighbour points. The right order is defined 53 | * by the laser_ring value. 54 | * To keep the right ordering, the filtered values are set to 55 | * NaN. 56 | */ 57 | if (pointInRange(distance)) 58 | { 59 | if(config_.transform) 60 | transformPoint(x, y, z); 61 | 62 | *(iter_x+ring) = x; 63 | *(iter_y+ring) = y; 64 | *(iter_z+ring) = z; 65 | *(iter_intensity+ring) = intensity; 66 | *(iter_ring+ring) = ring; 67 | *(iter_time+time) = time; 68 | } 69 | else 70 | { 71 | *(iter_x+ring) = nanf(""); 72 | *(iter_y+ring) = nanf(""); 73 | *(iter_z+ring) = nanf(""); 74 | *(iter_intensity+ring) = nanf(""); 75 | *(iter_ring+ring) = ring; 76 | *(iter_time+time) = time; 77 | } 78 | } 79 | } 80 | 81 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_pointcloud/src/conversions/pointcloudXYZIR.cc: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | #include 5 | 6 | namespace velodyne_pointcloud 7 | { 8 | 9 | PointcloudXYZIR::PointcloudXYZIR( 10 | const double max_range, const double min_range, 11 | const std::string& target_frame, const std::string& fixed_frame, 12 | const unsigned int scans_per_block, boost::shared_ptr tf_ptr) 13 | : DataContainerBase( 14 | max_range, min_range, target_frame, fixed_frame, 15 | 0, 1, true, scans_per_block, tf_ptr, 6, 16 | "x", 1, sensor_msgs::PointField::FLOAT32, 17 | "y", 1, sensor_msgs::PointField::FLOAT32, 18 | "z", 1, sensor_msgs::PointField::FLOAT32, 19 | "intensity", 1, sensor_msgs::PointField::FLOAT32, 20 | "ring", 1, sensor_msgs::PointField::UINT16, 21 | "time", 1, sensor_msgs::PointField::FLOAT32), 22 | iter_x(cloud, "x"), iter_y(cloud, "y"), iter_z(cloud, "z"), 23 | iter_ring(cloud, "ring"), iter_intensity(cloud, "intensity"), iter_time(cloud, "time") 24 | {}; 25 | 26 | void PointcloudXYZIR::setup(const velodyne_msgs::VelodyneScan::ConstPtr& scan_msg){ 27 | DataContainerBase::setup(scan_msg); 28 | iter_x = sensor_msgs::PointCloud2Iterator(cloud, "x"); 29 | iter_y = sensor_msgs::PointCloud2Iterator(cloud, "y"); 30 | iter_z = sensor_msgs::PointCloud2Iterator(cloud, "z"); 31 | iter_intensity = sensor_msgs::PointCloud2Iterator(cloud, "intensity"); 32 | iter_ring = sensor_msgs::PointCloud2Iterator(cloud, "ring"); 33 | iter_time = sensor_msgs::PointCloud2Iterator(cloud, "time"); 34 | } 35 | 36 | void PointcloudXYZIR::newLine() 37 | {} 38 | 39 | void PointcloudXYZIR::addPoint(float x, float y, float z, uint16_t ring, uint16_t /*azimuth*/, float distance, float intensity, float time) 40 | { 41 | if(!pointInRange(distance)) return; 42 | 43 | // convert polar coordinates to Euclidean XYZ 44 | 45 | if(config_.transform) 46 | transformPoint(x, y, z); 47 | 48 | *iter_x = x; 49 | *iter_y = y; 50 | *iter_z = z; 51 | *iter_ring = ring; 52 | *iter_intensity = intensity; 53 | *iter_time = time; 54 | 55 | ++cloud.width; 56 | ++iter_x; 57 | ++iter_y; 58 | ++iter_z; 59 | ++iter_ring; 60 | ++iter_intensity; 61 | ++iter_time; 62 | } 63 | } 64 | 65 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_pointcloud/src/conversions/transform_node.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2012 Austin Robot Technology, Jack O'Quin 3 | * License: Modified BSD Software License Agreement 4 | * 5 | * $Id$ 6 | */ 7 | 8 | /** \file 9 | 10 | This ROS node transforms raw Velodyne LIDAR packets to PointCloud2 11 | in the /map frame of reference. 12 | 13 | */ 14 | 15 | #include 16 | #include "velodyne_pointcloud/transform.h" 17 | 18 | /** Main node entry point. */ 19 | int main(int argc, char **argv) 20 | { 21 | ros::init(argc, argv, "transform_node"); 22 | 23 | // create conversion class, which subscribes to raw data 24 | velodyne_pointcloud::Transform transform(ros::NodeHandle(), 25 | ros::NodeHandle("~")); 26 | 27 | // handle callbacks until shut down 28 | ros::spin(); 29 | 30 | return 0; 31 | } 32 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_pointcloud/src/conversions/transform_nodelet.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2012 Austin Robot Technology, Jack O'Quin 3 | * License: Modified BSD Software License Agreement 4 | * 5 | * $Id$ 6 | */ 7 | 8 | /** @file 9 | 10 | This ROS nodelet transforms raw Velodyne 3D LIDAR packets to a 11 | PointCloud2 in the /map frame. 12 | 13 | */ 14 | 15 | #include 16 | #include 17 | #include 18 | 19 | #include "velodyne_pointcloud/transform.h" 20 | 21 | namespace velodyne_pointcloud 22 | { 23 | class TransformNodelet: public nodelet::Nodelet 24 | { 25 | public: 26 | 27 | TransformNodelet() {} 28 | ~TransformNodelet() {} 29 | 30 | private: 31 | 32 | virtual void onInit(); 33 | boost::shared_ptr tf_; 34 | }; 35 | 36 | /** @brief Nodelet initialization. */ 37 | void TransformNodelet::onInit() 38 | { 39 | tf_.reset(new Transform(getNodeHandle(), getPrivateNodeHandle(), getName())); 40 | } 41 | 42 | } // namespace velodyne_pointcloud 43 | 44 | 45 | // Register this plugin with pluginlib. Names must match nodelets.xml. 46 | // 47 | // parameters: class type, base class type 48 | PLUGINLIB_EXPORT_CLASS(velodyne_pointcloud::TransformNodelet, nodelet::Nodelet) 49 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_pointcloud/src/lib/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(velodyne_rawdata rawdata.cc calibration.cc) 2 | target_link_libraries(velodyne_rawdata 3 | ${catkin_LIBRARIES} 4 | ${YAML_CPP_LIBRARIES}) 5 | install(TARGETS velodyne_rawdata 6 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 7 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 8 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) 9 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_pointcloud/tests/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ### Unit tests 2 | # 3 | # Only configured when CATKIN_ENABLE_TESTING is true. 4 | 5 | # these dependencies are only needed for unit testing 6 | find_package(roslaunch REQUIRED) 7 | find_package(rostest REQUIRED) 8 | find_package(tf2_ros REQUIRED) 9 | 10 | # C++ gtests 11 | catkin_add_gtest(test_calibration test_calibration.cpp) 12 | add_dependencies(test_calibration ${catkin_EXPORTED_TARGETS}) 13 | target_link_libraries(test_calibration velodyne_rawdata ${catkin_LIBRARIES}) 14 | 15 | # Download packet capture (PCAP) files containing test data. 16 | # Store them in devel-space, so rostest can easily find them. 17 | catkin_download_test_data( 18 | ${PROJECT_NAME}_tests_class.pcap 19 | http://download.ros.org/data/velodyne/class.pcap 20 | DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/tests 21 | MD5 65808d25772101358a3719b451b3d015) 22 | catkin_download_test_data( 23 | ${PROJECT_NAME}_tests_32e.pcap 24 | http://download.ros.org/data/velodyne/32e.pcap 25 | DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/tests 26 | MD5 e41d02aac34f0967c03a5597e1d554a9) 27 | catkin_download_test_data( 28 | ${PROJECT_NAME}_tests_64e_s2.1-300-sztaki.pcap 29 | http://download.ros.org/data/velodyne/64e_s2.1-300-sztaki.pcap 30 | DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/tests 31 | MD5 176c900ffb698f9b948a13e281ffc1a2) 32 | catkin_download_test_data( 33 | ${PROJECT_NAME}_tests_vlp16.pcap 34 | http://download.ros.org/data/velodyne/vlp16.pcap 35 | DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/tests 36 | MD5 f45c2bb1d7ee358274e423ea3b66fd73) 37 | 38 | # run rostests 39 | add_rostest(cloud_node_hz.test) 40 | add_rostest(cloud_nodelet_hz.test) 41 | add_rostest(cloud_node_32e_hz.test) 42 | add_rostest(cloud_nodelet_32e_hz.test) 43 | add_rostest(cloud_node_64e_s2.1_hz.test) 44 | add_rostest(cloud_nodelet_64e_s2.1_hz.test) 45 | add_rostest(cloud_node_vlp16_hz.test) 46 | add_rostest(cloud_nodelet_vlp16_hz.test) 47 | add_rostest(transform_node_hz.test) 48 | add_rostest(transform_nodelet_hz.test) 49 | add_rostest(two_nodelet_managers.test) 50 | 51 | # parse check all the launch/*.launch files 52 | roslaunch_add_file_check(../launch) 53 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_pointcloud/tests/cloud_node_32e_hz.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 16 | 17 | 18 | 19 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_pointcloud/tests/cloud_node_64e_s2.1_hz.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 17 | 18 | 19 | 20 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_pointcloud/tests/cloud_node_hz.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 15 | 16 | 17 | 18 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_pointcloud/tests/cloud_node_vlp16_hz.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 16 | 17 | 18 | 19 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_pointcloud/tests/cloud_nodelet_32e_hz.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_pointcloud/tests/cloud_nodelet_64e_s2.1_hz.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 17 | 18 | 19 | 20 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_pointcloud/tests/cloud_nodelet_hz.test: -------------------------------------------------------------------------------- 1 | 2 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 20 | 21 | 22 | 23 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_pointcloud/tests/cloud_nodelet_vlp16_hz.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_pointcloud/tests/empty.xml: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZJU-Robotics-Lab/CICT/ff873a03ab03d9113b8db96d26246939bb5da0d4/ROS/src/lidar/velodyne_pointcloud/tests/empty.xml -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_pointcloud/tests/static_vehicle_tf.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 10 | 11 | 13 | 16 | 17 | 19 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_pointcloud/tests/transform_node_hz.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 18 | 19 | 20 | 24 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_pointcloud/tests/transform_nodelet_hz.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 18 | 19 | 20 | 24 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /ROS/src/lidar/velodyne_pointcloud/tests/two_nodelet_managers.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | -------------------------------------------------------------------------------- /ROS/src/tools/save_img.py: -------------------------------------------------------------------------------- 1 | import rosbag 2 | import rospy 3 | import cv2 4 | from cv_bridge import CvBridge, CvBridgeError 5 | import argparse 6 | 7 | parser = argparse.ArgumentParser(description='Params') 8 | parser.add_argument('-d', '--data', type=int, default=3, help='data index') 9 | parser.add_argument('-b', '--bag', type=str, default="2020-10-10-16-58-22.bag", help='bag name') 10 | args = parser.parse_args() 11 | 12 | img_path = '/media/wang/Data1/images'+str(args.data)+'/' 13 | gps_path = '/media/wang/Data1/gps'+str(args.data)+'/' 14 | cmd_path = '/media/wang/Data1/cmd'+str(args.data)+'/' 15 | 16 | class ImageCreator(): 17 | def __init__(self): 18 | self.bridge = CvBridge() 19 | self.last_gps = "" 20 | self.last_cmd = "" 21 | 22 | self.gps_file = open(gps_path+'gps.txt', 'w') 23 | self.cmd_file = open(cmd_path+'cmd.txt', 'w') 24 | with rosbag.Bag('/media/wang/Data1/'+args.bag, 'r') as bag: 25 | for topic,msg,t in bag.read_messages(): 26 | if topic == "/mynteye/left/image_color": 27 | try: 28 | cv_image = self.bridge.imgmsg_to_cv2(msg,"bgr8") 29 | except CvBridgeError as e: 30 | print(e) 31 | timestr = "%.6f" % msg.header.stamp.to_sec() 32 | image_name = timestr+ ".png" 33 | cv2.imwrite(img_path + image_name, cv_image) 34 | 35 | self.gps_file.write(timestr + '\t' + self.last_gps) 36 | self.cmd_file.write(timestr + '\t' + self.last_cmd) 37 | elif topic == "/gps": 38 | message_string = str(msg.data) 39 | self.last_gps = message_string 40 | elif topic == "/cmd": 41 | message_string = str(msg.data) 42 | self.last_cmd = message_string 43 | 44 | if __name__ == '__main__': 45 | try: 46 | image_creator = ImageCreator() 47 | except rospy.ROSInterruptException: 48 | pass -------------------------------------------------------------------------------- /ckpt/.gitkeep: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZJU-Robotics-Lab/CICT/ff873a03ab03d9113b8db96d26246939bb5da0d4/ckpt/.gitkeep -------------------------------------------------------------------------------- /device/__init__.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | from .sensor_manager import * 4 | from .controller import controller 5 | from .gps import * 6 | from .imu import * 7 | from .lidar import * -------------------------------------------------------------------------------- /device/camera/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0) 2 | 3 | set(PROJ_NAME "robot_camera") 4 | project(${PROJ_NAME}) 5 | 6 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -O3") 7 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3") 8 | string(STRIP "${CMAKE_C_FLAGS}" CMAKE_C_FLAGS) 9 | string(STRIP "${CMAKE_CXX_FLAGS}" CMAKE_CXX_FLAGS) 10 | message(STATUS "C_FLAGS: ${CMAKE_C_FLAGS}") 11 | message(STATUS "CXX_FLAGS: ${CMAKE_CXX_FLAGS}") 12 | 13 | 14 | find_package(OpenCV REQUIRED) 15 | include_directories(${OpenCV_INCLUDE_DIRS}) 16 | link_directories(${OpenCV_LIBS}) 17 | 18 | add_definitions(-DLOG_TAG=MYNTEYE) 19 | find_package(mynteyed REQUIRED) 20 | message(STATUS "Found mynteye: ${mynteyed_VERSION}") 21 | 22 | include_directories( 23 | "/usr/local/include/mynteyed" 24 | "src/util" 25 | ${OpenCV_INCLUDE_DIRS} 26 | ${CMAKE_CURRENT_LIST_DIR}/src 27 | ) 28 | 29 | find_package(pybind11 REQUIRED) 30 | pybind11_add_module(${PROJ_NAME} src/main.cpp src/util/mat_warper.cpp src/util/mat_warper.h src/util/cam_utils.cc) 31 | 32 | target_link_libraries(${PROJ_NAME} PRIVATE pybind11::module mynteye_depth ${OpenCV_LIBS}) 33 | 34 | find_package(Python) 35 | find_path(PYTHON_SITE_PACKAGES site-packages ${PYTHON_INCLUDE_PATH}/..) 36 | 37 | install(TARGETS ${PROJ_NAME} RUNTIME DESTINATION ${PYTHON_SITE_PACKAGES} 38 | LIBRARY DESTINATION ${PYTHON_SITE_PACKAGES} 39 | ARCHIVE DESTINATION ${PYTHON_SITE_PACKAGES} 40 | ) -------------------------------------------------------------------------------- /device/camera/build.sh: -------------------------------------------------------------------------------- 1 | mkdir build 2 | cd build 3 | cmake .. 4 | make -j12 5 | sudo make install 6 | cd .. 7 | sudo ln -s /usr/local/lib/libmynteye_depth.so /usr/lib/libmynteye_depth.so.1 8 | -------------------------------------------------------------------------------- /device/camera/run.py: -------------------------------------------------------------------------------- 1 | import cv2 2 | from robot_camera import Camera 3 | 4 | camera = Camera() 5 | 6 | while True: 7 | img = camera.getImage() 8 | img = cv2.cvtColor(img, cv2.COLOR_RGB2BGR); 9 | cv2.imshow('input_image', img) 10 | cv2.waitKey(30) 11 | 12 | cv2.destroyAllWindows() 13 | # that will cause an error on Linux kernel version >= 4.16 in libeSPDI.so 14 | # issue: https://github.com/slightech/MYNT-EYE-D-SDK/issues/13 15 | camera.close() 16 | -------------------------------------------------------------------------------- /device/camera/src/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include "mat_warper.h" 8 | #include "mynteyed/camera.h" 9 | #include "mynteyed/utils.h" 10 | #include "util/cam_utils.h" 11 | 12 | #define FRAMERATE 30 13 | #define RESOLUTION StreamMode::STREAM_1280x720 14 | 15 | namespace py = pybind11; 16 | MYNTEYE_USE_NAMESPACE 17 | 18 | class MyCamera{ 19 | public: 20 | MyCamera(){ 21 | init(); 22 | } 23 | void init(){ 24 | cam = new Camera(); 25 | dev_info = new DeviceInfo(); 26 | OpenParams params(dev_info->index); 27 | params.framerate = FRAMERATE; 28 | params.color_mode = ColorMode::COLOR_RECTIFIED; 29 | params.dev_mode = DeviceMode::DEVICE_COLOR; 30 | params.stream_mode = RESOLUTION; 31 | cam->EnableImageInfo(true); 32 | cam->Open(params); 33 | cam->AutoWhiteBalanceControl(true); 34 | cam->AutoExposureControl(true); 35 | cam->SetExposureTime(1.0); 36 | cam->SetGlobalGain(1.0); 37 | bool is_left_ok = cam->IsStreamDataEnabled(ImageType::IMAGE_LEFT_COLOR); 38 | if (!cam->IsOpened() || !is_left_ok) { 39 | std::cerr << "Error: Open camera failed" << std::endl; 40 | return; 41 | } 42 | else{ 43 | std::cout << "Open device success" << std::endl << std::endl; 44 | } 45 | } 46 | void close(){ 47 | cam->DisableImageInfo(); 48 | cam->Close(); 49 | delete cam; 50 | delete dev_info; 51 | } 52 | py::array_t getImage(){ 53 | while(true){ 54 | cam->WaitForStream(); 55 | auto left_color = cam->GetStreamData(ImageType::IMAGE_LEFT_COLOR); 56 | if (left_color.img) { 57 | cv::Mat left = left_color.img->To(ImageFormat::COLOR_BGR)->ToMat(); 58 | cv::Mat dst; 59 | cv::cvtColor(left, dst, cv::COLOR_RGB2BGR); 60 | return cv_mat_uint8_3c_to_numpy(dst); 61 | } 62 | else{ 63 | continue; 64 | } 65 | } 66 | } 67 | private: 68 | Camera* cam; 69 | DeviceInfo *dev_info; 70 | }; 71 | 72 | PYBIND11_MODULE(robot_camera, m) { 73 | m.doc() = "Python warper for MYNT EYE D-1000-120 camera"; 74 | py::class_(m, "Camera") 75 | .def(py::init()) 76 | .def("close", &MyCamera::close) 77 | .def("getImage", &MyCamera::getImage); 78 | } -------------------------------------------------------------------------------- /device/camera/src/util/cam_utils.cc: -------------------------------------------------------------------------------- 1 | // Copyright 2018 Slightech Co., Ltd. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #include "util/cam_utils.h" 15 | 16 | #include 17 | 18 | MYNTEYE_BEGIN_NAMESPACE 19 | 20 | namespace util { 21 | 22 | std::shared_ptr new_format(int width, int prec, char fillch) { 23 | auto fmt = std::make_shared(nullptr); 24 | fmt->setf(std::ios::fixed); 25 | if (width > 0) 26 | fmt->width(std::move(width)); 27 | if (prec > 0) 28 | fmt->precision(std::move(prec)); 29 | fmt->fill(std::move(fillch)); 30 | return fmt; 31 | } 32 | 33 | } // namespace util 34 | 35 | MYNTEYE_END_NAMESPACE 36 | -------------------------------------------------------------------------------- /device/camera/src/util/cam_utils.h: -------------------------------------------------------------------------------- 1 | // Copyright 2018 Slightech Co., Ltd. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #ifndef MYNTEYE_SAMPLES_UTIL_CAM_UTILS_H_ 15 | #define MYNTEYE_SAMPLES_UTIL_CAM_UTILS_H_ 16 | #pragma once 17 | 18 | #include 19 | #ifdef MYNTEYE_OS_LINUX 20 | #include 21 | #endif 22 | #ifdef MYNTEYE_OS_WIN 23 | #include 24 | #endif 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | #include "mynteyed/stubs/global.h" 31 | 32 | MYNTEYE_BEGIN_NAMESPACE 33 | 34 | namespace util { 35 | 36 | inline 37 | std::ostringstream& clear(std::ostringstream& os) { 38 | os.str(""); 39 | os.clear(); 40 | return os; 41 | } 42 | 43 | std::shared_ptr new_format(int width, int prec, char fillch = ' '); 44 | 45 | template 46 | std::string to_string(T val) { 47 | std::ostringstream ss; 48 | ss << val; 49 | return ss.str(); 50 | } 51 | 52 | template 53 | std::string to_string(T val, const std::shared_ptr& fmt) { 54 | std::ostringstream ss; 55 | if (fmt) ss.copyfmt(*fmt); 56 | ss << val; 57 | return ss.str(); 58 | } 59 | 60 | template 61 | std::string to_string(T val, int width, int prec, char fillch = ' ') { 62 | auto fmt = new_format(std::move(width), std::move(prec), std::move(fillch)); 63 | return to_string(val, fmt); 64 | } 65 | 66 | } // namespace util 67 | 68 | MYNTEYE_END_NAMESPACE 69 | 70 | #endif // MYNTEYE_SAMPLES_UTIL_CAM_UTILS_H_ 71 | -------------------------------------------------------------------------------- /device/camera/src/util/mat_warper.cpp: -------------------------------------------------------------------------------- 1 | #include "mat_warper.h" 2 | #include 3 | 4 | cv::Mat numpy_uint8_1c_to_cv_mat(py::array_t& input) { 5 | 6 | if (input.ndim() != 2) 7 | throw std::runtime_error("1-channel image must be 2 dims "); 8 | 9 | py::buffer_info buf = input.request(); 10 | 11 | cv::Mat mat(buf.shape[0], buf.shape[1], CV_8UC1, (unsigned char*)buf.ptr); 12 | 13 | return mat; 14 | } 15 | 16 | 17 | cv::Mat numpy_uint8_3c_to_cv_mat(py::array_t& input) { 18 | 19 | if (input.ndim() != 3) 20 | throw std::runtime_error("3-channel image must be 3 dims "); 21 | 22 | py::buffer_info buf = input.request(); 23 | 24 | cv::Mat mat(buf.shape[0], buf.shape[1], CV_8UC3, (unsigned char*)buf.ptr); 25 | 26 | return mat; 27 | } 28 | 29 | 30 | /* 31 | C++ Mat ->numpy 32 | */ 33 | py::array_t cv_mat_uint8_1c_to_numpy(cv::Mat& input) { 34 | 35 | py::array_t dst = py::array_t({ input.rows,input.cols }, input.data); 36 | return dst; 37 | } 38 | 39 | py::array_t cv_mat_uint8_3c_to_numpy(cv::Mat& input) { 40 | 41 | py::array_t dst = py::array_t({ input.rows,input.cols,3}, input.data); 42 | return dst; 43 | } 44 | 45 | 46 | 47 | //PYBIND11_MODULE(cv_mat_warper, m) { 48 | // 49 | // m.doc() = "OpenCV Mat -> Numpy.ndarray warper"; 50 | // 51 | // m.def("numpy_uint8_1c_to_cv_mat", &numpy_uint8_1c_to_cv_mat); 52 | // m.def("numpy_uint8_1c_to_cv_mat", &numpy_uint8_1c_to_cv_mat); 53 | // 54 | // 55 | //} -------------------------------------------------------------------------------- /device/camera/src/util/mat_warper.h: -------------------------------------------------------------------------------- 1 | #ifndef MAT_WARPER_H_ 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace py = pybind11; 8 | 9 | cv::Mat numpy_uint8_1c_to_cv_mat(py::array_t& input); 10 | 11 | cv::Mat numpy_uint8_3c_to_cv_mat(py::array_t& input); 12 | 13 | py::array_t cv_mat_uint8_1c_to_numpy(cv::Mat & input); 14 | 15 | py::array_t cv_mat_uint8_3c_to_numpy(cv::Mat & input); 16 | 17 | #endif // !MAT_WARPER_H_ -------------------------------------------------------------------------------- /device/controller/__init__.py: -------------------------------------------------------------------------------- 1 | from .controller import * 2 | from .xbox import * 3 | from .passive_xbox import * -------------------------------------------------------------------------------- /device/gps/__init__.py: -------------------------------------------------------------------------------- 1 | from .gps import * -------------------------------------------------------------------------------- /device/imu/__init__.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | from .mtdevice import * 4 | from .mtdef import * 5 | from .mtnode import * -------------------------------------------------------------------------------- /device/lidar/__init__.py: -------------------------------------------------------------------------------- 1 | from .lidar import LiDAR, Visualizer -------------------------------------------------------------------------------- /device/lidar/open_port.sh: -------------------------------------------------------------------------------- 1 | sudo ufw enable 2 | sudo ufw allow 2368 3 | -------------------------------------------------------------------------------- /device/lidar/velodyne-driver/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0) 2 | 3 | set(PROJ_NAME "velodyne") 4 | project(${PROJ_NAME}) 5 | 6 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -O3") 7 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3") 8 | 9 | find_package(pybind11 REQUIRED) 10 | pybind11_add_module(${PROJ_NAME} src/velodyne.cpp) 11 | 12 | target_link_libraries(${PROJ_NAME} PRIVATE pybind11::module) 13 | 14 | find_package(Python) 15 | find_path(PYTHON_SITE_PACKAGES site-packages ${PYTHON_INCLUDE_PATH}/..) 16 | 17 | install(TARGETS ${PROJ_NAME} RUNTIME DESTINATION ${PYTHON_SITE_PACKAGES} 18 | LIBRARY DESTINATION ${PYTHON_SITE_PACKAGES} 19 | ARCHIVE DESTINATION ${PYTHON_SITE_PACKAGES} 20 | ) -------------------------------------------------------------------------------- /device/lidar/velodyne-driver/build.sh: -------------------------------------------------------------------------------- 1 | mkdir build 2 | cd build 3 | cmake .. 4 | make -j12 5 | sudo make install 6 | cd .. 7 | -------------------------------------------------------------------------------- /device/lidar/velodyne-driver/test.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | import time 4 | import numpy as np 5 | import velodyne 6 | 7 | data = b'6'*1200 8 | 9 | while True: 10 | result = velodyne.parse_data(data) 11 | print(result[:,:3].shape) 12 | #time.sleep(1/(36000/2)) 13 | break -------------------------------------------------------------------------------- /doc/robocar.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZJU-Robotics-Lab/CICT/ff873a03ab03d9113b8db96d26246939bb5da0d4/doc/robocar.jpg -------------------------------------------------------------------------------- /doc/video_link.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZJU-Robotics-Lab/CICT/ff873a03ab03d9113b8db96d26246939bb5da0d4/doc/video_link.png -------------------------------------------------------------------------------- /learning/__init__.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | from .models import * 4 | from .datasets import * -------------------------------------------------------------------------------- /scripts/.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | share/python-wheels/ 24 | *.egg-info/ 25 | .installed.cfg 26 | *.egg 27 | MANIFEST 28 | 29 | # PyInstaller 30 | # Usually these files are written by a python script from a template 31 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 32 | *.manifest 33 | *.spec 34 | 35 | # Installer logs 36 | pip-log.txt 37 | pip-delete-this-directory.txt 38 | 39 | # Unit test / coverage reports 40 | htmlcov/ 41 | .tox/ 42 | .nox/ 43 | .coverage 44 | .coverage.* 45 | .cache 46 | nosetests.xml 47 | coverage.xml 48 | *.cover 49 | *.py,cover 50 | .hypothesis/ 51 | .pytest_cache/ 52 | cover/ 53 | 54 | # Translations 55 | *.mo 56 | *.pot 57 | 58 | # Django stuff: 59 | *.log 60 | local_settings.py 61 | db.sqlite3 62 | db.sqlite3-journal 63 | 64 | # Flask stuff: 65 | instance/ 66 | .webassets-cache 67 | 68 | # Scrapy stuff: 69 | .scrapy 70 | 71 | # Sphinx documentation 72 | docs/_build/ 73 | 74 | # PyBuilder 75 | .pybuilder/ 76 | target/ 77 | 78 | # Jupyter Notebook 79 | .ipynb_checkpoints 80 | 81 | # IPython 82 | profile_default/ 83 | ipython_config.py 84 | 85 | # pyenv 86 | # For a library or package, you might want to ignore these files since the code is 87 | # intended to run in multiple environments; otherwise, check them in: 88 | # .python-version 89 | 90 | # pipenv 91 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 92 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 93 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 94 | # install all needed dependencies. 95 | #Pipfile.lock 96 | 97 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow 98 | __pypackages__/ 99 | 100 | # Celery stuff 101 | celerybeat-schedule 102 | celerybeat.pid 103 | 104 | # SageMath parsed files 105 | *.sage.py 106 | 107 | # Environments 108 | .env 109 | .venv 110 | env/ 111 | venv/ 112 | ENV/ 113 | env.bak/ 114 | venv.bak/ 115 | 116 | # Spyder project settings 117 | .spyderproject 118 | .spyproject 119 | 120 | # Rope project settings 121 | .ropeproject 122 | 123 | # mkdocs documentation 124 | /site 125 | 126 | # mypy 127 | .mypy_cache/ 128 | .dmypy.json 129 | dmypy.json 130 | 131 | # Pyre type checker 132 | .pyre/ 133 | 134 | # pytype static type analyzer 135 | .pytype/ 136 | 137 | # Cython debug symbols 138 | cython_debug/ 139 | 140 | result/ 141 | *.png 142 | *.jpg 143 | *.pdf 144 | *.pcd 145 | -------------------------------------------------------------------------------- /scripts/carla/Town01/navigation.csv: -------------------------------------------------------------------------------- 1 | 2 | 320.8699645996094, 327.04998779296875, 88.61998748779297, 169.84999084472656 3 | 88.61998748779297, 145.83999633789062, 256.3499755859375, 195.5699920654297 4 | 381.3399658203125, 330.53997802734375, -1.980019450187683, 249.42999267578125 5 | 299.39996337890625, 195.5699920654297, 88.61998748779297, 145.83999633789062 6 | 92.1099853515625, 113.05999755859375, 21.770000457763672, -1.9599987268447876 7 | 92.10997772216797, 308.2099914550781, 92.1099853515625, 159.9499969482422 8 | 1.5599901676177979, 149.83001708984375, 356.79998779296875, 2.0200390815734863 9 | 92.1099853515625, 176.88999938964844, 270.79998779296875, 133.43003845214844 10 | 92.1099853515625, 227.22000122070312, 158.0800018310547, 27.18000030517578 11 | 320.8699645996094, 327.04998779296875, 157.1899871826172, 133.24002075195312 12 | 299.3999938964844, 59.33003616333008, 88.61997985839844, 249.42999267578125 13 | 334.8299865722656, 217.0800018310547, 88.6199951171875, 15.279999732971191 14 | 335.489990234375, 298.80999755859375, 88.6199951171875, 26.559999465942383 15 | 299.3999938964844, 55.84000015258789, 46.14997863769531, 326.9700012207031 16 | 1.55999755859375, 22.440019607543945, 110.02999877929688, 2.02001953125 17 | -2.4200193881988525, 187.97000122070312, -2.420001268386841, 17.779998779296875 18 | 88.6199951171875, 15.279999732971191, 88.61997985839844, 295.32000732421875 19 | 378.17999267578125, 2.0200390815734863, 14.139999389648438, 2.0200109481811523 20 | 381.3399658203125, 330.53997802734375, 22.17997932434082, 330.4599914550781 21 | 88.6199951171875, 15.279999732971191, 272.2900085449219, 55.84000015258789 22 | 262.5999755859375, 330.53997802734375, 306.28997802734375, 2.02001953125 23 | 392.4700012207031, 308.2099914550781, 14.139999389648438, 2.0200109481811523 24 | 1.5099804401397705, 308.2099914550781, 378.17999267578125, 2.0200390815734863 25 | 392.4700012207031, 164.1699981689453, 1.5599803924560547, 187.9700164794922 26 | -2.4200048446655273, 79.31999969482422, 392.4700012207031, 249.42999267578125 27 | -------------------------------------------------------------------------------- /scripts/carla/Town01/navigation_with_dynamic_obstacles.csv: -------------------------------------------------------------------------------- 1 | 2 | 320.8699645996094, 327.04998779296875, 88.61998748779297, 169.84999084472656 3 | 88.61998748779297, 145.83999633789062, 256.3499755859375, 195.5699920654297 4 | 381.3399658203125, 330.53997802734375, -1.980019450187683, 249.42999267578125 5 | 299.39996337890625, 195.5699920654297, 88.61998748779297, 145.83999633789062 6 | 92.1099853515625, 113.05999755859375, 21.770000457763672, -1.9599987268447876 7 | 92.10997772216797, 308.2099914550781, 92.1099853515625, 159.9499969482422 8 | 1.5599901676177979, 149.83001708984375, 356.79998779296875, 2.0200390815734863 9 | 92.1099853515625, 176.88999938964844, 270.79998779296875, 133.43003845214844 10 | 92.1099853515625, 227.22000122070312, 158.0800018310547, 27.18000030517578 11 | 320.8699645996094, 327.04998779296875, 157.1899871826172, 133.24002075195312 12 | 299.3999938964844, 59.33003616333008, 88.61997985839844, 249.42999267578125 13 | 334.8299865722656, 217.0800018310547, 88.6199951171875, 15.279999732971191 14 | 335.489990234375, 298.80999755859375, 88.6199951171875, 26.559999465942383 15 | 299.3999938964844, 55.84000015258789, 46.14997863769531, 326.9700012207031 16 | 1.55999755859375, 22.440019607543945, 110.02999877929688, 2.02001953125 17 | -2.4200193881988525, 187.97000122070312, -2.420001268386841, 17.779998779296875 18 | 88.6199951171875, 15.279999732971191, 88.61997985839844, 295.32000732421875 19 | 378.17999267578125, 2.0200390815734863, 14.139999389648438, 2.0200109481811523 20 | 381.3399658203125, 330.53997802734375, 22.17997932434082, 330.4599914550781 21 | 88.6199951171875, 15.279999732971191, 272.2900085449219, 55.84000015258789 22 | 262.5999755859375, 330.53997802734375, 306.28997802734375, 2.02001953125 23 | 392.4700012207031, 308.2099914550781, 14.139999389648438, 2.0200109481811523 24 | 1.5099804401397705, 308.2099914550781, 378.17999267578125, 2.0200390815734863 25 | 392.4700012207031, 164.1699981689453, 1.5599803924560547, 187.9700164794922 26 | -2.4200048446655273, 79.31999969482422, 392.4700012207031, 249.42999267578125 27 | -------------------------------------------------------------------------------- /scripts/carla/Town01/one_turn.csv: -------------------------------------------------------------------------------- 1 | 2 | 191.0800018310547, 55.84000015258789, 92.11000061035156, 30.820009231567383 3 | 47.939998626708984, 2.020014524459839, 88.6199951171875, 26.559999465942383 4 | 92.1099853515625, 159.9499969482422, 157.1899871826172, 133.24002075195312 5 | 1.55999755859375, 22.440019607543945, 62.12999725341797, 2.020014524459839 6 | 92.1099853515625, 227.22000122070312, 142.91998291015625, 198.75999450683594 7 | -1.2800195217132568, 309.4599914550781, 46.14997863769531, 330.4599914550781 8 | 334.8299865722656, 217.0800018310547, 299.39996337890625, 199.05999755859375 9 | 153.75999450683594, 28.899999618530273, 110.02999877929688, 2.02001953125 10 | 395.9599914550781, 68.86003875732422, 363.0, -1.9599609375 11 | 47.939998626708984, -1.9599950313568115, -2.4200048446655273, 48.70000076293945 12 | 1.5599950551986694, 48.70001983642578, 244.09999084472656, 2.02001953125 13 | 271.0400085449219, 129.489990234375, 88.61998748779297, 169.84999084472656 14 | 216.26998901367188, 129.75, 88.61997985839844, 249.42999267578125 15 | 142.91998291015625, 326.9700012207031, 92.1099853515625, 227.22000122070312 16 | 237.6999969482422, 129.75, 88.61997985839844, 249.42999267578125 17 | 395.9599914550781, 308.2099914550781, 21.770000457763672, -1.9599987268447876 18 | 378.17999267578125, -1.9599609375, -1.2800195217132568, 309.4599914550781 19 | -2.420001268386841, 17.779998779296875, 381.3399658203125, 330.53997802734375 20 | 22.17997932434082, 330.4599914550781, 396.4499816894531, 19.9200382232666 21 | 378.17999267578125, -1.9599609375, 88.61997985839844, 249.42999267578125 22 | 1.5099804401397705, 308.2099914550781, 378.17999267578125, 2.0200390815734863 23 | 47.939998626708984, 2.020014524459839, 392.4700012207031, 308.2099914550781 24 | 262.5999755859375, 327.04998779296875, 92.11000061035156, 39.709999084472656 25 | 256.3499755859375, 195.5699920654297, 92.11000061035156, 30.820009231567383 26 | 271.0400085449219, 129.489990234375, 92.11000061035156, 30.820009231567383 27 | -------------------------------------------------------------------------------- /scripts/carla/Town01/straight.csv: -------------------------------------------------------------------------------- 1 | 2 | 1.5599901676177979, 120.02001953125, 1.5599950551986694, 48.70001983642578 3 | -2.4200048446655273, 48.70000076293945, -2.4200096130371094, 120.0199966430664 4 | 262.5999755859375, 327.04998779296875, 199.94998168945312, 326.9700012207031 5 | 191.3199920654297, 133.24002075195312, 237.6999969482422, 133.239990234375 6 | 271.0400085449219, 129.489990234375, 216.26998901367188, 129.75 7 | 378.17999267578125, -1.9599609375, 116.63999938964844, -1.95999014377594 8 | 278.80999755859375, -1.9599803686141968, 244.09999084472656, -1.9599803686141968 9 | 47.939998626708984, 2.020014524459839, 306.28997802734375, 2.02001953125 10 | 299.3999938964844, 55.84000015258789, 118.94999694824219, 55.84000015258789 11 | -2.4200096130371094, 149.8300018310547, -1.980019450187683, 249.42999267578125 12 | 92.1099853515625, 159.9499969482422, 92.11000061035156, 86.95999908447266 13 | 395.9599914550781, 249.42999267578125, 395.9599914550781, 68.86003875732422 14 | 14.139999389648438, 2.0200109481811523, 62.12999725341797, 2.020014524459839 15 | 185.55999755859375, -1.9599803686141968, 21.770000457763672, -1.9599987268447876 16 | 88.61998748779297, 169.84999084472656, 88.61997985839844, 295.32000732421875 17 | 22.17997932434082, 330.4599914550781, 358.39996337890625, 330.53997802734375 18 | 1.5099804401397705, 308.2099914550781, 1.5599901676177979, 149.83001708984375 19 | 126.38999938964844, 2.02001953125, 356.79998779296875, 2.0200390815734863 20 | 92.1099853515625, 95.44999694824219, 92.11000061035156, 30.820009231567383 21 | 111.56999969482422, 59.33001708984375, 299.3999938964844, 59.33003616333008 22 | 88.6199951171875, 72.6199951171875, 88.61997985839844, 295.32000732421875 23 | 395.9599914550781, 308.2099914550781, 396.4499816894531, 19.9200382232666 24 | 22.17997932434082, 330.4599914550781, 381.3399658203125, 330.53997802734375 25 | 378.17999267578125, -1.9599609375, 21.770000457763672, -1.9599987268447876 26 | 14.139999389648438, 2.0200109481811523, 378.17999267578125, 2.0200390815734863 27 | -------------------------------------------------------------------------------- /scripts/carla/Town02/navigation.csv: -------------------------------------------------------------------------------- 1 | 2 | 92.11000061035156, 86.95999908447266, 363.0, -1.9599609375 3 | 392.4700012207031, 308.2099914550781, 105.43998718261719, 133.24002075195312 4 | 92.11000061035156, 86.95999908447266, 209.5800018310547, -1.9599803686141968 5 | 88.61998748779297, 95.44999694824219, 270.79998779296875, 133.43003845214844 6 | 126.38999938964844, 2.02001953125, 395.9599914550781, 68.86003875732422 7 | 1.55999755859375, 22.440019607543945, 119.46998596191406, 129.75 8 | 1.5599803924560547, 187.9700164794922, 392.4700012207031, 19.9200382232666 9 | -2.4200096130371094, 149.8300018310547, 216.26998901367188, 133.239990234375 10 | 338.97998046875, 226.75, -2.4200193881988525, 187.97000122070312 11 | 338.97998046875, 301.2599792480469, 278.80999755859375, -1.9599803686141968 12 | 363.0, -1.9599609375, 237.6999969482422, 133.239990234375 13 | 88.61998748779297, 145.83999633789062, 128.94998168945312, 133.24002075195312 14 | 392.4700012207031, 308.2099914550781, 92.11000061035156, 86.95999908447266 15 | 237.6999969482422, 129.75, 88.61998748779297, 169.84999084472656 16 | 88.6199951171875, 26.559999465942383, 105.43998718261719, 133.24002075195312 17 | 216.26998901367188, 133.239990234375, 209.5800018310547, -1.9599803686141968 18 | 396.4499816894531, 19.9200382232666, 392.4700012207031, 164.1699981689453 19 | 47.939998626708984, -1.9599950313568115, 356.79998779296875, 2.0200390815734863 20 | 209.5800018310547, -1.9599803686141968, 116.63999938964844, -1.95999014377594 21 | 278.80999755859375, -1.9599803686141968, 62.12999725341797, 2.020014524459839 22 | 335.489990234375, 298.80999755859375, 128.94998168945312, 133.24002075195312 23 | 110.02999877929688, 2.02001953125, 392.4700012207031, 249.42999267578125 24 | 392.4700012207031, 68.86003875732422, 378.17999267578125, -1.9599609375 25 | 173.14999389648438, 2.02001953125, 334.8299865722656, 217.0800018310547 26 | 335.489990234375, 249.42999267578125, 338.97998046875, 226.75 27 | -------------------------------------------------------------------------------- /scripts/carla/Town02/navigation_with_dynamic_obstacles.csv: -------------------------------------------------------------------------------- 1 | 2 | 92.11000061035156, 86.95999908447266, 363.0, -1.9599609375 3 | 392.4700012207031, 308.2099914550781, 105.43998718261719, 133.24002075195312 4 | 92.11000061035156, 86.95999908447266, 209.5800018310547, -1.9599803686141968 5 | 88.61998748779297, 95.44999694824219, 270.79998779296875, 133.43003845214844 6 | 126.38999938964844, 2.02001953125, 395.9599914550781, 68.86003875732422 7 | 1.55999755859375, 22.440019607543945, 119.46998596191406, 129.75 8 | 1.5599803924560547, 187.9700164794922, 392.4700012207031, 19.9200382232666 9 | -2.4200096130371094, 149.8300018310547, 216.26998901367188, 133.239990234375 10 | 338.97998046875, 226.75, -2.4200193881988525, 187.97000122070312 11 | 338.97998046875, 301.2599792480469, 278.80999755859375, -1.9599803686141968 12 | 363.0, -1.9599609375, 237.6999969482422, 133.239990234375 13 | 88.61998748779297, 145.83999633789062, 128.94998168945312, 133.24002075195312 14 | 392.4700012207031, 308.2099914550781, 92.11000061035156, 86.95999908447266 15 | 237.6999969482422, 129.75, 88.61998748779297, 169.84999084472656 16 | 88.6199951171875, 26.559999465942383, 105.43998718261719, 133.24002075195312 17 | 216.26998901367188, 133.239990234375, 209.5800018310547, -1.9599803686141968 18 | 396.4499816894531, 19.9200382232666, 392.4700012207031, 164.1699981689453 19 | 47.939998626708984, -1.9599950313568115, 356.79998779296875, 2.0200390815734863 20 | 209.5800018310547, -1.9599803686141968, 116.63999938964844, -1.95999014377594 21 | 278.80999755859375, -1.9599803686141968, 62.12999725341797, 2.020014524459839 22 | 335.489990234375, 298.80999755859375, 128.94998168945312, 133.24002075195312 23 | 110.02999877929688, 2.02001953125, 392.4700012207031, 249.42999267578125 24 | 392.4700012207031, 68.86003875732422, 378.17999267578125, -1.9599609375 25 | 173.14999389648438, 2.02001953125, 334.8299865722656, 217.0800018310547 26 | 335.489990234375, 249.42999267578125, 338.97998046875, 226.75 27 | -------------------------------------------------------------------------------- /scripts/carla/Town02/one_turn.csv: -------------------------------------------------------------------------------- 1 | 2 | -2.4200048446655273, 79.31999969482422, 395.9599914550781, 68.86003875732422 3 | 157.1899871826172, 129.75, 92.1099853515625, 113.05999755859375 4 | 244.09999084472656, 2.02001953125, 378.17999267578125, 2.0200390815734863 5 | 1.5599950551986694, 79.32001495361328, 338.97998046875, 301.2599792480469 6 | 335.489990234375, 298.80999755859375, 270.79998779296875, 133.43003845214844 7 | 209.5800018310547, 2.02001953125, 392.4700012207031, 19.9200382232666 8 | 395.9599914550781, 105.38999938964844, 338.97998046875, 249.42999267578125 9 | 21.770000457763672, -1.9599987268447876, 271.0400085449219, 129.489990234375 10 | 392.4700012207031, 19.9200382232666, 88.6199951171875, 26.559999465942383 11 | 105.43998718261719, 133.24002075195312, 92.1099853515625, 113.05999755859375 12 | 1.5599901676177979, 149.83001708984375, 128.94998168945312, 129.75 13 | 335.489990234375, 249.42999267578125, 105.43998718261719, 133.24002075195312 14 | 392.4700012207031, 105.38999938964844, 88.6199951171875, 26.559999465942383 15 | 395.9599914550781, 249.42999267578125, 335.489990234375, 298.80999755859375 16 | 237.6999969482422, 133.239990234375, 88.61998748779297, 95.44999694824219 17 | 392.4700012207031, 105.38999938964844, 244.09999084472656, -1.9599803686141968 18 | 116.63999938964844, -1.95999014377594, 47.939998626708984, 2.020014524459839 19 | 128.94998168945312, 129.75, 92.11000061035156, 86.95999908447266 20 | 392.4700012207031, 68.86003875732422, 1.5599901676177979, 149.83001708984375 21 | 392.4700012207031, 308.2099914550781, 88.61998748779297, 103.37999725341797 22 | 1.5599950551986694, 48.70001983642578, 316.8500061035156, -1.9599803686141968 23 | 209.5800018310547, 2.02001953125, 395.9599914550781, 68.86003875732422 24 | 392.4700012207031, 308.2099914550781, 185.55999755859375, -1.9599803686141968 25 | 88.6199951171875, 26.559999465942383, 278.80999755859375, -1.9599803686141968 26 | 88.61998748779297, 145.83999633789062, 128.94998168945312, 129.75 27 | -------------------------------------------------------------------------------- /scripts/carla/Town02/straight.csv: -------------------------------------------------------------------------------- 1 | 2 | 1.5599950551986694, 79.32001495361328, 1.5599901676177979, 149.83001708984375 3 | 216.26998901367188, 129.75, 237.6999969482422, 129.75 4 | 128.94998168945312, 133.24002075195312, 338.97998046875, 301.2599792480469 5 | 278.80999755859375, 2.02001953125, 185.55999755859375, -1.9599803686141968 6 | 335.489990234375, 249.42999267578125, 47.939998626708984, 2.020014524459839 7 | 306.28997802734375, 2.02001953125, 363.0, -1.9599609375 8 | 395.9599914550781, 308.2099914550781, 395.9599914550781, 68.86003875732422 9 | 244.09999084472656, -1.9599803686141968, 209.5800018310547, -1.9599803686141968 10 | 278.80999755859375, -1.9599803686141968, 88.6199951171875, 15.279999732971191 11 | -2.4200096130371094, 120.0199966430664, -2.4200048446655273, 48.70000076293945 12 | 128.94998168945312, 133.24002075195312, 157.1899871826172, 129.75 13 | 271.0400085449219, 129.489990234375, 88.6199951171875, 15.279999732971191 14 | 392.4700012207031, 105.38999938964844, 378.17999267578125, -1.9599609375 15 | 338.97998046875, 226.75, 244.09999084472656, 2.02001953125 16 | 14.139999389648438, 2.0200109481811523, 62.12999725341797, 2.020014524459839 17 | 47.939998626708984, -1.9599950313568115, 1.55999755859375, 22.440019607543945 18 | 126.38999938964844, 2.02001953125, 47.939998626708984, -1.9599950313568115 19 | 395.9599914550781, 249.42999267578125, 88.61998748779297, 169.84999084472656 20 | 334.8299865722656, 217.0800018310547, 316.8500061035156, -1.9599803686141968 21 | 271.0400085449219, 129.489990234375, 392.4700012207031, 249.42999267578125 22 | 338.97998046875, 226.75, 316.8500061035156, -1.9599803686141968 23 | 110.02999877929688, 2.02001953125, 1.55999755859375, 22.440019607543945 24 | 88.6199951171875, 26.559999465942383, 92.11000061035156, 86.95999908447266 25 | 92.11000061035156, 30.820009231567383, 92.1099853515625, 159.9499969482422 26 | 392.4700012207031, 68.86003875732422, 378.17999267578125, -1.9599609375 27 | -------------------------------------------------------------------------------- /scripts/ff/camera/basic_tools.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import numpy as np 4 | 5 | def pi2pi(theta, theta0=0.0): 6 | # print('pi2pi: '+str(theta)+'----'+str(theta0)) 7 | while(theta > np.pi + theta0): 8 | theta = theta - 2.0 * np.pi 9 | while(theta < -np.pi + theta0): 10 | theta = theta + 2.0 * np.pi 11 | return theta 12 | 13 | 14 | # def np_dot(args): 15 | # res = args[0] 16 | # for arg in args[1:]: 17 | # res = np.dot(res, arg) 18 | # return res 19 | 20 | 21 | def np_dot(*args): 22 | res = args[0] 23 | for arg in args[1:]: 24 | res = np.dot(res, arg) 25 | return res 26 | 27 | 28 | 29 | 30 | # import rospy 31 | # import tf 32 | # from geometry_msgs.msg import PoseStamped 33 | # def waypoint_to_pose(waypoint): 34 | # # waypoint: carla.Waypoint 35 | # pose = PoseStamped() 36 | # pose.header.stamp = rospy.Time.now() 37 | # pose.header.frame_id = 'map' 38 | # pose.pose.position.x = waypoint.transform.location.x 39 | # pose.pose.position.y = waypoint.transform.location.y 40 | # pose.pose.position.z = waypoint.transform.location.z 41 | # pose.pose.position.z = 0 42 | 43 | # roll_rad = np.deg2rad(waypoint.transform.rotation.roll) 44 | # pitch_rad= np.deg2rad(waypoint.transform.rotation.pitch) 45 | # yaw_rad = np.deg2rad(waypoint.transform.rotation.yaw) 46 | # quaternion = tf.transformations.quaternion_from_euler( 47 | # roll_rad, pitch_rad, yaw_rad) 48 | # pose.pose.orientation.x = quaternion[0] 49 | # pose.pose.orientation.y = quaternion[1] 50 | # pose.pose.orientation.z = quaternion[2] 51 | # pose.pose.orientation.w = quaternion[3] 52 | # return pose 53 | 54 | 55 | # from carla_msgs.msg_wrap import CarlaWaypointWrap 56 | 57 | # def waypoint_to_msg(waypoint, reference=False, step=None): 58 | # # convert carla.Waypoint to carla_msgs/CarlaWaypoint 59 | # pose = waypoint_to_pose(waypoint) 60 | # return CarlaWaypointWrap(pose, reference=reference, step=step) 61 | 62 | 63 | 64 | # def pose_to_waypoint(pose, reference=False, step=None): 65 | # return CarlaWaypointWrap(pose, reference=reference, step=step) 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | import matplotlib.pyplot as plt 74 | 75 | def plot_arrow_2D(generalized_pose, length=1.0, width=0.5, fc="r", ec="k"): # pragma: no cover 76 | """ 77 | generalized_pose: include carla_msgs/CarlaWaypoint, carla_msgs/CarlaState 78 | """ 79 | x, y, theta = generalized_pose.x, generalized_pose.y, generalized_pose.theta 80 | plt.arrow(x, y, length * np.cos(theta), length * np.sin(theta), 81 | fc=fc, ec=ec, head_width=width, head_length=width) 82 | # plt.plot(x, y, linewidth=10, markersize=5) 83 | # plt.plot(0, 0) -------------------------------------------------------------------------------- /scripts/ff/camera/parameters.py: -------------------------------------------------------------------------------- 1 | 2 | import numpy as np 3 | np.set_printoptions(precision=4, suppress=True) 4 | 5 | # import sys 6 | # from os.path import join, dirname 7 | # sys.path.insert(0, join(dirname(__file__), '..')) 8 | 9 | from .coordinate_transformation import CoordinateTransformation, rotationMatrix3D, intrinsicMatrix 10 | 11 | 12 | class IntrinsicParams(object): 13 | def __init__(self, sensor): 14 | ''' 15 | Args: 16 | sensor: carla.Sensor 17 | ''' 18 | image_size_x = float(sensor.attributes['image_size_x']) 19 | image_size_y = float(sensor.attributes['image_size_y']) 20 | fov = eval(sensor.attributes['fov']) 21 | f = image_size_x /(2 * np.tan(fov * np.pi / 360)) 22 | 23 | # [px] 24 | self.fx = f 25 | self.fy = f 26 | self.u0 = image_size_x / 2 27 | self.v0 = image_size_y / 2 28 | 29 | 30 | class ExtrinsicParams(object): 31 | def __init__(self, sensor): 32 | ''' 33 | Args: 34 | sensor: carla.Sensor 35 | ''' 36 | 37 | # camera coordinate in world coordinate 38 | transform = sensor.get_transform() 39 | 40 | # [m] 41 | self.x = transform.location.x 42 | self.y = transform.location.y 43 | self.z = transform.location.z 44 | 45 | # [rad] 46 | self.roll = np.deg2rad(transform.rotation.roll) 47 | self.pitch = np.deg2rad(transform.rotation.pitch) 48 | self.yaw = np.deg2rad(transform.rotation.yaw) 49 | 50 | 51 | class CameraParams(object): 52 | def __init__(self, intrinsic_params, extrinsic_params): 53 | ''' 54 | Args: 55 | intrinsic_params: IntrinsicParams 56 | extrinsic_params: ExtrinsicParams 57 | ''' 58 | 59 | self.K = intrinsicMatrix(intrinsic_params.fx, intrinsic_params.fy, intrinsic_params.u0, intrinsic_params.v0) 60 | 61 | # (coordinate) t: camera in world, R: camera to world 62 | self.t = np.array([[extrinsic_params.x, extrinsic_params.y, extrinsic_params.z]]).T 63 | self.R = rotationMatrix3D(extrinsic_params.roll, extrinsic_params.pitch, extrinsic_params.yaw) 64 | 65 | -------------------------------------------------------------------------------- /scripts/ff/collect_ipm.py: -------------------------------------------------------------------------------- 1 | import cv2 2 | import numpy as np 3 | from .camera.parameters import CameraParams, IntrinsicParams, ExtrinsicParams 4 | from .camera.coordinate_transformation import CoordinateTransformation, rotationMatrix3D#, reverseX, reverseY 5 | from .camera import basic_tools 6 | 7 | class InversePerspectiveMapping(object): 8 | def __init__(self, param, sensor): 9 | self.sensor = sensor 10 | intrinsic_params = IntrinsicParams(sensor) 11 | extrinsic_params = ExtrinsicParams(sensor) 12 | self.camera_params = CameraParams(intrinsic_params, extrinsic_params) 13 | 14 | self.img_width = 400#eval(sensor.attributes['image_size_x']) 15 | self.img_height = 200#eval(sensor.attributes['image_size_y']) 16 | #self.max_pixel = np.array([self.img_height, self.img_width]).reshape(2,1) 17 | #self.min_pixel = np.array([0, 0]).reshape(2,1) 18 | 19 | self.empty_image = np.zeros((self.img_height, self.img_width), dtype=np.dtype("uint8")) 20 | 21 | self.longitudinal_length = param.longitudinal_length 22 | self.ksize = param.ksize 23 | 24 | f = float(self.img_height) / self.longitudinal_length 25 | self.pesudo_K = np.array([ [f, 0, self.img_width/2], 26 | [0, f, self.img_height], 27 | [0, 0, 1] ]) 28 | self.reverseXY = basic_tools.np_dot(rotationMatrix3D(0,0,-np.pi/2)) 29 | 30 | 31 | def getIPM(self, image): 32 | self.empty_image = np.zeros((self.img_height, self.img_width), dtype=np.dtype("uint8")) 33 | index_array = np.argwhere(image > 200) 34 | index_array = index_array[:,:2] 35 | index_array = np.unique(index_array, axis=0) 36 | index_array = np.array([index_array[:,1], index_array[:,0]]) 37 | vehicle_vec = CoordinateTransformation.image2DToWorld3D2(index_array, self.camera_params.K, self.camera_params.R, self.camera_params.t) 38 | 39 | vehicle_vec[:,2,0] = 1.0 40 | temp = np.dot(self.pesudo_K, self.reverseXY) 41 | vehicle_vec = np.squeeze(vehicle_vec, axis = 2) 42 | new_image_vec = np.dot(temp, vehicle_vec.T) 43 | new_image_vec = new_image_vec[:2,:] 44 | new_image_vec = new_image_vec[::-1,:] 45 | 46 | new_image_y_pixel = new_image_vec[0,:].astype(int) 47 | new_image_x_pixel = new_image_vec[1,:].astype(int) 48 | 49 | #self.empty_image[new_image_y_pixel, new_image_x_pixel] = 255 50 | 51 | mask = np.where((new_image_x_pixel >= 0)&(new_image_x_pixel < self.img_width))[0] 52 | new_image_x_pixel = new_image_x_pixel[mask] 53 | new_image_y_pixel = new_image_y_pixel[mask] 54 | 55 | mask = np.where((new_image_y_pixel >= 0)&(new_image_y_pixel < self.img_height))[0] 56 | new_image_x_pixel = new_image_x_pixel[mask] 57 | new_image_y_pixel = new_image_y_pixel[mask] 58 | self.empty_image[new_image_y_pixel, new_image_x_pixel] = 255 59 | 60 | self.empty_image[np.clip(new_image_y_pixel+1,0, self.img_height-1),new_image_x_pixel] = 255 61 | self.empty_image[np.clip(new_image_y_pixel-1,0, self.img_height-1),new_image_x_pixel] = 255 62 | 63 | #self.empty_image = cv2.GaussianBlur(self.empty_image, (self.ksize, self.ksize), 25) 64 | return self.empty_image -------------------------------------------------------------------------------- /scripts/ff/system/__init__.py: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | def printVariable(name, value): 5 | print(name + ': ' + str(value)) 6 | -------------------------------------------------------------------------------- /scripts/ff/system/env_path.py: -------------------------------------------------------------------------------- 1 | 2 | ros_path = '/opt/ros/kinetic/lib/python2.7/dist-packages' 3 | 4 | def remove_ros_path(sys): 5 | if ros_path in sys.path: 6 | sys.path.remove(ros_path) 7 | 8 | def append_ros_path(sys): 9 | if ros_path not in sys.path: 10 | sys.path.append(ros_path) 11 | 12 | python2_path = [] 13 | def remove_python2_path(sys): 14 | for i, p in enumerate(sys.path): 15 | if 'python2' in p: 16 | python2_path.append(p) 17 | del sys.path[i] 18 | 19 | def append_python2_path(sys): 20 | sys.path.extend(python2_path) 21 | 22 | 23 | def append(sys, path): 24 | sys.path.append(path) -------------------------------------------------------------------------------- /scripts/ff/system/load_carla.py: -------------------------------------------------------------------------------- 1 | 2 | import os 3 | import sys 4 | import glob 5 | 6 | def load(path): 7 | try: 8 | sys.path.append(path+'/PythonAPI') 9 | sys.path.append(glob.glob(path+'/PythonAPI/carla/dist/carla-*%d.%d-%s.egg' % ( 10 | sys.version_info.major, 11 | sys.version_info.minor, 12 | 'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0]) 13 | except: 14 | print('Fail to load carla library') -------------------------------------------------------------------------------- /scripts/generate_data/wheel_config.ini: -------------------------------------------------------------------------------- 1 | [G29 Racing Wheel] 2 | steering_wheel = 0 3 | clutch = 1 4 | throttle = 2 5 | brake = 3 6 | handbrake = 4 7 | reverse = 5 8 | -------------------------------------------------------------------------------- /scripts/install.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | OPTIONS="cmake git opencv pybind camera py-3rdparty quit" 3 | select opt in $OPTIONS; do 4 | if [ "$opt" = "cmake" ]; then 5 | sudo apt-get install cmake 6 | 7 | elif [ "$opt" = "git" ]; then 8 | sudo apt-get install git 9 | 10 | elif [ "$opt" = "opencv" ]; then 11 | sudo apt-get install libopencv-dev 12 | pip install opencv-python 13 | 14 | elif [ "$opt" = "pybind" ]; then 15 | if [ ! -d "3rdparty" ]; then 16 | mkdir 3rdparty 17 | fi 18 | cd 3rdparty 19 | pip install pytest 20 | git clone https://github.com/pybind/pybind11.git 21 | cd pybind11 22 | mkdir build 23 | cd build 24 | cmake .. 25 | make -j16 26 | sudo make install 27 | cd ../../../ 28 | 29 | elif [ "$opt" = "camera" ]; then 30 | if [ ! -d "3rdparty" ]; then 31 | mkdir 3rdparty 32 | fi 33 | cd 3rdparty 34 | git clone https://github.com/IamWangYunKai/MYNT-EYE-D-SDK.git 35 | cd MYNT-EYE-D-SDK 36 | make init 37 | make all -j16 38 | sudo make install 39 | cd .. 40 | 41 | elif [ "$opt" = "py-3rdparty" ]; then 42 | pip install -r requirements.txt 43 | 44 | elif [ "$opt" = "quit" ]; then 45 | exit 46 | 47 | else 48 | echo -e "\033[31mBad Option!!! Please enter 1-6\033[0m" 49 | fi 50 | done 51 | -------------------------------------------------------------------------------- /scripts/real/bag-record.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | import sys 4 | from os.path import join, dirname 5 | sys.path.insert(0, join(dirname(__file__), '..')) 6 | 7 | import cv2 8 | import time 9 | import argparse 10 | import numpy as np 11 | 12 | from device import SensorManager 13 | from utils.navigator import NavMaker 14 | 15 | parser = argparse.ArgumentParser() 16 | parser.add_argument('--img_height', type=int, default=128, help='size of image height') 17 | parser.add_argument('--img_width', type=int, default=256, help='size of image width') 18 | parser.add_argument('--show', type=bool, default=False, help='show image') 19 | opt = parser.parse_args() 20 | 21 | def get_nav(): 22 | nav = nav_maker.get() 23 | nav = cv2.cvtColor(np.asarray(nav),cv2.COLOR_RGB2BGR) 24 | return nav 25 | 26 | def get_img(nav): 27 | img = sm['camera'].getImage() 28 | return img 29 | 30 | if __name__ == '__main__': 31 | file = open('record/record.txt', 'a+') 32 | sensor_dict = { 33 | #'lidar':None, 34 | 'camera':None, 35 | 'gps':None, 36 | 'imu':None, 37 | } 38 | sm = SensorManager(sensor_dict) 39 | sm.init_all() 40 | nav_maker = NavMaker(sm['gps'], sm['imu']) 41 | nav_maker.start() 42 | time.sleep(1) 43 | while True: 44 | x,y,t = sm['gps'].get() 45 | nav = get_nav() 46 | ax, ay, az, yaw, pitch, roll, w = sm['imu'].get() 47 | img = get_img(nav) 48 | file_name = str(t) 49 | cv2.imwrite('record/'+file_name+'.png', img) 50 | cv2.imwrite('record/'+file_name+'_nav.png', nav) 51 | file.write(file_name+'\t'+str(yaw)+'\t'+str(x)+'\t'+str(y)+'\n') 52 | file.close() 53 | sm.close_all() -------------------------------------------------------------------------------- /scripts/real/calc_path_len.py: -------------------------------------------------------------------------------- 1 | 2 | import numpy as np 3 | import matplotlib.pyplot as plt 4 | 5 | def calc_len(index): 6 | file_name = '/home/wang/video/data'+str(index)+'/gps/gps.txt' 7 | with open(file_name, 'r') as file: 8 | lines = file.readlines() 9 | ts = [] 10 | xs = [] 11 | ys = [] 12 | yaws = [] 13 | for line in lines: 14 | sp_line = line.split() 15 | t = float(sp_line[0]) 16 | x = float(sp_line[1]) 17 | y = float(sp_line[2]) 18 | yaw = float(sp_line[3]) 19 | ts.append(t) 20 | xs.append(x) 21 | ys.append(y) 22 | yaws.append(yaw) 23 | 24 | xs = np.array(xs) 25 | ys = np.array(ys) 26 | 27 | xs = xs - xs[1] 28 | ys = ys - ys[1] 29 | mask = np.where(abs(xs) < 500)[0] 30 | xs = xs[mask] 31 | ys = ys[mask] 32 | mask = np.where(abs(ys) < 500)[0] 33 | xs = xs[mask] 34 | ys = ys[mask] 35 | 36 | 37 | dists = np.hypot(xs[:-1]-xs[1:], ys[:-1]-ys[1:]) 38 | mask = np.where(dists < 0.9)[0] 39 | xs = xs[mask] 40 | ys = ys[mask] 41 | dists = dists[mask] 42 | dist = np.sum(dists) 43 | print(dist) 44 | plt.figure(figsize=(5, 5)) 45 | 46 | plt.plot(xs, ys, color='red', label='Ours') 47 | 48 | # plt.title('Success Rate / %') 49 | # plt.xlabel('Delay Time / ms') 50 | # plt.ylabel('Success Rate / %') 51 | # plt.xlim(0, 1100) 52 | # plt.ylim(0, 100) 53 | plt.legend() 54 | plt.tight_layout() 55 | # plt.savefig('delay.png') 56 | plt.show() 57 | 58 | 59 | for i in range(2,6): 60 | calc_len(i) -------------------------------------------------------------------------------- /scripts/real/cat_img.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | import sys 4 | import glob 5 | from os.path import join, dirname 6 | sys.path.insert(0, join(dirname(__file__), '..')) 7 | sys.path.insert(0, join(dirname(__file__), '../..')) 8 | 9 | import cv2 10 | import numpy as np 11 | 12 | def read_files(index): 13 | file_path = '/media/wang/Data/video/data'+str(index) 14 | img_list = [] 15 | out_list = [] 16 | pcd_list = [] 17 | nav_list = [] 18 | cost_list = [] 19 | for file_name in glob.glob(file_path+'/img/'+'*.png'): 20 | img_list.append(file_name.split('/')[-1][:-4]) 21 | for file_name in glob.glob(file_path+'/output/'+'*.png'): 22 | out_list.append(file_name.split('/')[-1][:-4]) 23 | for file_name in glob.glob(file_path+'/lidar/'+'*.npy'): 24 | pcd_list.append(file_name.split('/')[-1][:-4]) 25 | for file_name in glob.glob(file_path+'/nav/'+'*.png'): 26 | nav_list.append(file_name.split('/')[-1][:-4]) 27 | for file_name in glob.glob(file_path+'/cost/'+'*.png'): 28 | cost_list.append(file_name.split('/')[-1][:-4]) 29 | img_list.sort(), pcd_list.sort(), nav_list.sort(), cost_list.sort(), out_list.sort() 30 | return img_list, pcd_list, nav_list, cost_list, out_list 31 | 32 | 33 | 34 | def find_nn(ts, ts_list, back=0): 35 | dt_list = list(map(lambda x: abs(float(x)-float(ts)), ts_list)) 36 | index = max(0, dt_list.index(min(dt_list)) - back) 37 | return ts_list[index] 38 | 39 | if __name__ == '__main__': 40 | rate = 1.25 41 | rate2 = 1.0 42 | dataset = {} 43 | 44 | 45 | fps = 30 46 | video_size = (1280, 720) 47 | 48 | videoWriter = cv2.VideoWriter("/media/wang/Data/video/first-person/2.mp4", cv2.VideoWriter_fourcc(*'MJPG'), fps, video_size) 49 | 50 | for index in [2,4,5]: 51 | img_list, pcd_list, nav_list, cost_list, out_list = read_files(index) 52 | dataset[index] = {'img_list':img_list, 'pcd_list':pcd_list, 'nav_list':nav_list, 'cost_list':cost_list, 'out_list':out_list} 53 | 54 | for index in [2]: 55 | choose_dataset = dataset[index] 56 | for ts in choose_dataset['img_list']: 57 | img = cv2.imread('/media/wang/Data/video/data'+str(index)+'/output/'+ts+'.png') 58 | #print(img.shape) #(720, 1280, 3) 59 | if img is None: continue 60 | #img = cv2.cvtColor(img,cv2.COLOR_BGR2RGB) 61 | nav_ts = find_nn(ts, choose_dataset['nav_list']) 62 | cost_ts = find_nn(ts, choose_dataset['cost_list']) 63 | nav = cv2.imread('/media/wang/Data/video/data'+str(index)+'/nav/'+nav_ts+'.png') 64 | costmap = cv2.imread('/media/wang/Data/video/data'+str(index)+'/cost/'+cost_ts+'.png') 65 | nav = cv2.cvtColor(nav, cv2.COLOR_BGR2RGB) #(160, 200, 3) 66 | #input_img = get_img(img, nav) 67 | nav = cv2.resize(nav, (int(200*rate), int(rate*160))) 68 | img[0:int(rate*160), -int(200*rate):] = nav 69 | img[0:int(rate2*200), 0:int(400*rate2)] = costmap 70 | cv2.imshow('img', img) 71 | videoWriter.write(img) 72 | #cv2.imshow('costmap', costmap) 73 | cv2.waitKey(1) 74 | 75 | cv2.destroyAllWindows() 76 | videoWriter.release() 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | -------------------------------------------------------------------------------- /scripts/real/get_permission.sh: -------------------------------------------------------------------------------- 1 | sudo chmod 777 /dev/ttyUSB* 2 | -------------------------------------------------------------------------------- /scripts/real/test.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | import sys 4 | from os.path import join, dirname 5 | sys.path.insert(0, join(dirname(__file__), '..')) 6 | sys.path.insert(0, join(dirname(__file__), '../..')) 7 | 8 | import cv2 9 | import time 10 | import random 11 | import argparse 12 | import numpy as np 13 | from PIL import Image 14 | from datetime import datetime 15 | 16 | import torch 17 | import torchvision.transforms as transforms 18 | from learning.models import GeneratorUNet 19 | 20 | from utils.local_planner import get_cost_map, get_cmd, read_pcd 21 | from utils.camera_info import camera2lidar 22 | 23 | parser = argparse.ArgumentParser() 24 | parser.add_argument('--img_height', type=int, default=128, help='size of image height') 25 | parser.add_argument('--img_width', type=int, default=256, help='size of image width') 26 | opt = parser.parse_args() 27 | 28 | random.seed(datetime.now()) 29 | torch.manual_seed(999) 30 | 31 | device = torch.device('cpu') 32 | generator = GeneratorUNet() 33 | 34 | generator = generator.to(device) 35 | generator.load_state_dict(torch.load('../../ckpt/g.pth', map_location=device)) 36 | generator.eval() 37 | 38 | img_trans_ = [ 39 | transforms.Resize((opt.img_height, opt.img_width), Image.BICUBIC), 40 | transforms.ToTensor(), 41 | transforms.Normalize((0.5, 0.5, 0.5), (0.5, 0.5, 0.5)), 42 | ] 43 | img_trans = transforms.Compose(img_trans_) 44 | 45 | def get_nav(): 46 | global nav_maker 47 | nav = nav_maker.get() 48 | return nav 49 | 50 | def get_img(): 51 | img = cv2.imread("img.png") 52 | img = Image.fromarray(cv2.cvtColor(img,cv2.COLOR_BGR2RGB)) 53 | 54 | nav = cv2.imread("nav.png") 55 | nav = Image.fromarray(cv2.cvtColor(nav,cv2.COLOR_BGR2RGB)) 56 | img = img_trans(img) 57 | nav = img_trans(nav) 58 | input_img = torch.cat((img, nav), 0).unsqueeze(0) 59 | return input_img 60 | 61 | 62 | def get_net_result(input_img): 63 | with torch.no_grad(): 64 | input_img = input_img#.to(device) 65 | result = generator(input_img) 66 | return result 67 | 68 | theta_y = 20.0*np.pi/180. 69 | pitch_rotationMat = np.array([ 70 | [np.cos(theta_y), 0., np.sin(theta_y)], 71 | [ 0., 1., 0. ], 72 | [-np.sin(theta_y), 0., np.cos(theta_y)], 73 | ]) 74 | 75 | def inverse_perspective_mapping(img): 76 | point_cloud, intensity = read_pcd('pcd.pcd') 77 | intensity = np.array(intensity) 78 | mask = np.where((intensity > 10))[0] 79 | point_cloud = point_cloud[:,mask] 80 | point_cloud = np.dot(pitch_rotationMat, point_cloud) 81 | img = cv2.resize(img, (1280, 720), interpolation=cv2.INTER_AREA) 82 | res = np.where(img > 100) 83 | image_uv = np.stack([res[1],res[0]]) 84 | trans_pc = camera2lidar(image_uv) 85 | img = get_cost_map(trans_pc, point_cloud, False) 86 | t1 = time.time() 87 | yaw = get_cmd(img, show=True) 88 | t2 = time.time() 89 | print(round(1000*(t2-t1), 2), 'ms') 90 | print(2.4*yaw) 91 | 92 | if __name__ == '__main__': 93 | while True: 94 | input_img = get_img() 95 | result = get_net_result(input_img)[0][0] 96 | result = result.data.numpy()*255+255 97 | inverse_perspective_mapping(result) 98 | break 99 | -------------------------------------------------------------------------------- /scripts/requirements.txt: -------------------------------------------------------------------------------- 1 | pyopengl 2 | pyqt5==5.14 3 | pyqtgraph 4 | numpy 5 | pyquery 6 | pyserial 7 | python-can 8 | -------------------------------------------------------------------------------- /scripts/test/test_ipm.py: -------------------------------------------------------------------------------- 1 | import cv2 2 | import glob 3 | import random 4 | import copy 5 | import numpy as np 6 | from PIL import Image 7 | 8 | dataset_path = '/media/wang/DATASET/CARLA_HUMAN/town01/1/' 9 | 10 | files = glob.glob(dataset_path+'/pm/*.png') 11 | file_names = [] 12 | for file in files: 13 | file_name = file.split('/')[-1][:-4] 14 | file_names.append(file_name) 15 | file_names.sort() 16 | 17 | def get_one(): 18 | file_name = random.sample(file_names, 1)[0] 19 | 20 | img_path = dataset_path +'/img/'+file_name+'.png' 21 | img = Image.open(img_path).convert("RGB") 22 | 23 | label_path = dataset_path + '/pm/'+file_name+'.png' 24 | label = Image.open(label_path).convert('L') 25 | 26 | _img = np.asarray(img) 27 | img = copy.deepcopy(_img) 28 | img.flags.writeable = True 29 | label = np.asarray(label) 30 | print(img.shape, label.shape) 31 | 32 | 33 | mask = np.where(label > 200) 34 | image_uv = np.stack([mask[1],mask[0]]) 35 | 36 | img[mask[0],mask[1]] = (255, 0, 0) 37 | 38 | img = Image.fromarray(img, 'RGB') 39 | img.show() 40 | 41 | for _ in range(10): 42 | get_one() -------------------------------------------------------------------------------- /simulator/__init__.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | 4 | import os 5 | import sys 6 | import glob 7 | import random 8 | 9 | config = { 10 | 'host': 'localhost', 11 | 'port': 2000, 12 | 'timeout': 5.0, 13 | 'camera':{ 14 | 'img_length': 640, 15 | 'img_width': 360, 16 | 'fov': 120, 17 | 'fps': 30, 18 | }, 19 | 'lidar':{ 20 | 'channels': 64, 21 | 'rpm': 30, 22 | 'sensor_tick':0.05, 23 | 'pps': 100000, 24 | 'range': 5000,# <= 0.9.5 25 | 'lower_fov': -30, 26 | 'upper_fov': 10, 27 | }, 28 | 'imu':{ 29 | 'fps': 400, 30 | }, 31 | 'gnss':{ 32 | 'fps': 20, 33 | }, 34 | } 35 | 36 | def load(path='/home/wang/CARLA_0.9.9.4'): 37 | try: 38 | sys.path.append(path+'/PythonAPI') 39 | sys.path.append(glob.glob(path+'/PythonAPI/carla/dist/carla-*%d.%d-%s.egg' % ( 40 | sys.version_info.major, 41 | sys.version_info.minor, 42 | 'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0]) 43 | except: 44 | print('Fail to load carla library') 45 | 46 | def set_weather(world, weather): 47 | world.set_weather(weather) 48 | return weather 49 | 50 | def add_vehicle(world, blueprint, vehicle_type='vehicle.bmw.grandtourer'): 51 | bp = random.choice(blueprint.filter(vehicle_type)) 52 | if bp.has_attribute('color'): 53 | color = random.choice(bp.get_attribute('color').recommended_values) 54 | bp.set_attribute('color', color) 55 | transform = random.choice(world.get_map().get_spawn_points()) 56 | vehicle = world.spawn_actor(bp, transform) 57 | return vehicle -------------------------------------------------------------------------------- /simulator/xbox.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | import sys 4 | from os.path import join, dirname 5 | sys.path.insert(0, join(dirname(__file__), '..')) 6 | 7 | import simulator 8 | simulator.load('/home/wang/CARLA_0.9.9.4') 9 | import carla 10 | 11 | import pygame 12 | import threading 13 | 14 | class JoyStick: 15 | def __init__(self, verbose=False, cloc_time=20): 16 | self.verbose = verbose 17 | self.cloc_time = cloc_time 18 | 19 | self.control = carla.VehicleControl() 20 | self.control.manual_gear_shift = False 21 | self.control.reverse = False 22 | self.control.gear = 1 23 | 24 | pygame.init() 25 | self.halt = False 26 | self.clock = pygame.time.Clock() 27 | pygame.joystick.init() 28 | 29 | self.joystick_count = pygame.joystick.get_count() 30 | if self.joystick_count == 0: 31 | print('No joystick found !') 32 | elif self.joystick_count > 1: 33 | print('Found joystick number:', self.joystick_count) 34 | self.joystick = pygame.joystick.Joystick(0) 35 | self.joystick.init() 36 | 37 | self.stop_read_event = threading.Event() 38 | self.parse_cyclic = threading.Thread( 39 | target=self.parse, args=() 40 | ) 41 | 42 | def start(self): 43 | self.stop_read_event.clear() 44 | self.parse_cyclic.start() 45 | 46 | def stop(self): 47 | self.stop_read_event.set() 48 | 49 | def parse(self): 50 | while not self.stop_read_event.is_set(): 51 | if pygame.QUIT in pygame.event.get(): 52 | self.stop_read_event.set() 53 | 54 | self.parse_axes() 55 | self.clock.tick(self.cloc_time) 56 | 57 | def parse_axes(self): 58 | speed = self.joystick.get_axis(4) 59 | speed = max(-speed, 0) 60 | if self.verbose: 61 | print('speed:',speed) 62 | if not self.halt: 63 | self.control.throttle = speed/2.0 64 | 65 | rotation_l = self.joystick.get_axis(2) 66 | rotation_r = self.joystick.get_axis(5) 67 | # fix some bug in xbox reading 68 | if abs(rotation_l-0)<0.0001: 69 | rotation_l = -1.0 70 | if abs(rotation_r-0)<0.0001: 71 | rotation_r = -1.0 72 | 73 | rotation = (rotation_l - rotation_r)/2. 74 | rotation = - rotation 75 | if self.verbose: 76 | print('rotation:', rotation) 77 | if not self.halt: 78 | self.control.steer = rotation 79 | 80 | def get(self): 81 | return self.control 82 | -------------------------------------------------------------------------------- /utils/__init__.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | import time 4 | import numpy as np 5 | import cv2 6 | from PIL import Image 7 | 8 | class Singleton(object): 9 | _instance = None 10 | 11 | def __new__(cls, *args, **kw): 12 | if not cls._instance: 13 | cls._instance = super(Singleton, cls).__new__(cls) 14 | return cls._instance 15 | 16 | def debug(info, info_type='debug'): 17 | if info_type == 'error': 18 | print('\033[1;31m ERROR:', info, '\033[0m') 19 | elif info_type == 'success': 20 | print('\033[1;32m SUCCESS:', info, '\033[0m') 21 | elif info_type == 'warning': 22 | print('\033[1;34m WARNING:', info, '\033[0m') 23 | elif info_type == 'debug': 24 | print('\033[1;35m DEBUG:', info, '\033[0m') 25 | else: 26 | print('\033[1;36m MESSAGE:', info, '\033[0m') 27 | 28 | def write_params(log_path, parser, description=None): 29 | opt = parser.parse_args() 30 | options = parser._optionals._actions 31 | with open(log_path+'params.md', 'w+') as file: 32 | file.write('# Params\n') 33 | file.write('********************************\n') 34 | file.write('Time: '+time.strftime("%Y-%m-%d %H:%M:%S", time.localtime())+'\n') 35 | if description is not None: 36 | file.write('**Description**: '+description+'\n') 37 | 38 | file.write('| Param | Value | Description |\n') 39 | file.write('| ----- | ----- | ----------- |\n') 40 | for i in range(len(parser._optionals._actions)): 41 | option = options[i] 42 | if option.dest != 'help': 43 | file.write('|**'+ option.dest+'**|'+str(opt.__dict__[option.dest])+'|'+option.help+'|\n') 44 | file.write('********************************\n\n') 45 | 46 | def fig2data(fig): 47 | # draw the renderer 48 | fig.canvas.draw() 49 | 50 | # Get the RGBA buffer from the figure 51 | w, h = fig.canvas.get_width_height() 52 | buf = np.frombuffer(fig.canvas.tostring_argb(), dtype=np.uint8) 53 | buf.shape = (w, h, 4) 54 | 55 | # canvas.tostring_argb give pixmap in ARGB mode. Roll the ALPHA channel to have it in RGBA mode 56 | buf = np.roll(buf, 3, axis=2) 57 | image = Image.frombytes("RGBA", (w, h), buf.tobytes()) 58 | image = np.asarray(image) 59 | image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB) 60 | return image 61 | 62 | def add_alpha_channel(img): 63 | b_channel, g_channel, r_channel = cv2.split(img) 64 | alpha_channel = np.ones(b_channel.shape, dtype=b_channel.dtype) * 255 65 | alpha_channel[:, :int(b_channel.shape[0] / 2)] = 100 66 | img_BGRA = cv2.merge((b_channel, g_channel, r_channel, alpha_channel)) 67 | return img_BGRA -------------------------------------------------------------------------------- /utils/manual_gps.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | import math 4 | 5 | manual_gps_y = [2400, 2300, 3815, 3630, 3325, 3240, 2855, 2570, 2420, 2465, 2115, 2260, 2095, 2075, 2200, 1655, 1820, 2605, 2460] 6 | manual_gps_x = [3200, 3430, 4000, 4520, 4400, 4600, 4665, 4555, 4240, 4090, 3970, 3555, 3490, 3395, 3055, 2820, 2335, 2615, 3045] 7 | 8 | def dist_p2p(x1, y1, x2, y2): 9 | return math.sqrt((x1-x2)**2 + (y1-y2)**2) 10 | 11 | def find_nn(x, y, x_list, y_list): 12 | assert len(x_list) == len(y_list) 13 | nn_dist = 9999999 14 | nn_x = None 15 | nn_y = None 16 | nn_index = 0 17 | for i in range(len(x_list)): 18 | dist = dist_p2p(x, y, x_list[i], y_list[i]) 19 | if dist < nn_dist: 20 | nn_dist = dist 21 | nn_x = x_list[i] 22 | nn_y = y_list[i] 23 | nn_index = i 24 | return nn_x, nn_y, nn_index 25 | 26 | def gen_manual_gps(x, y): 27 | step_len = 1.0 # 5.0 meters per setp 28 | gps_y = [] 29 | gps_x = [] 30 | for i in range(len(manual_gps_y)-1): 31 | x1 = manual_gps_x[i] 32 | y1 = manual_gps_y[i] 33 | x2 = manual_gps_x[i+1] 34 | y2 = manual_gps_y[i+1] 35 | dist = dist_p2p(x1, y1, x2, y2) 36 | n = int(dist/step_len) 37 | for j in range(n): 38 | gps_x.append(x1 + (x2-x1)*j/n) 39 | gps_y.append(y1 + (y2-y1)*j/n) 40 | 41 | return gps_x, gps_y -------------------------------------------------------------------------------- /utils/map-mark.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZJU-Robotics-Lab/CICT/ff873a03ab03d9113b8db96d26246939bb5da0d4/utils/map-mark.png -------------------------------------------------------------------------------- /utils/nav.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZJU-Robotics-Lab/CICT/ff873a03ab03d9113b8db96d26246939bb5da0d4/utils/nav.png -------------------------------------------------------------------------------- /video/README.md: -------------------------------------------------------------------------------- 1 | # Video Link 2 | 3 | [Video click here](https://youtu.be/0TL_mn4zHlY) 4 | --------------------------------------------------------------------------------