├── .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 |
--------------------------------------------------------------------------------