├── CMakeLists.txt ├── README.md ├── ackermann_msgs ├── .gitignore ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.rst ├── mainpage.dox ├── msg │ ├── AckermannDrive.msg │ └── AckermannDriveStamped.msg └── package.xml ├── darknet_ros ├── .gitmodules ├── LICENSE ├── README.md ├── darknet │ ├── .gitignore │ ├── LICENSE │ ├── LICENSE.fuck │ ├── LICENSE.gen │ ├── LICENSE.gpl │ ├── LICENSE.meta │ ├── LICENSE.mit │ ├── LICENSE.v1 │ ├── Makefile │ ├── README.md │ ├── examples │ │ ├── art.c │ │ ├── attention.c │ │ ├── captcha.c │ │ ├── cifar.c │ │ ├── classifier.c │ │ ├── coco.c │ │ ├── darknet.c │ │ ├── detector-scipy-opencv.py │ │ ├── detector.c │ │ ├── detector.py │ │ ├── dice.c │ │ ├── go.c │ │ ├── lsd.c │ │ ├── nightmare.c │ │ ├── regressor.c │ │ ├── rnn.c │ │ ├── rnn_vid.c │ │ ├── segmenter.c │ │ ├── super.c │ │ ├── swag.c │ │ ├── tag.c │ │ ├── voxel.c │ │ ├── writing.c │ │ └── yolo.c │ ├── include │ │ └── darknet.h │ ├── python │ │ ├── darknet.py │ │ └── proverbot.py │ ├── scripts │ │ ├── dice_label.sh │ │ ├── gen_tactic.sh │ │ ├── get_coco_dataset.sh │ │ ├── imagenet_label.sh │ │ └── voc_label.py │ └── src │ │ ├── activation_kernels.cu │ │ ├── activation_layer.c │ │ ├── activation_layer.h │ │ ├── activations.c │ │ ├── activations.h │ │ ├── avgpool_layer.c │ │ ├── avgpool_layer.h │ │ ├── avgpool_layer_kernels.cu │ │ ├── batchnorm_layer.c │ │ ├── batchnorm_layer.h │ │ ├── blas.c │ │ ├── blas.h │ │ ├── blas_kernels.cu │ │ ├── box.c │ │ ├── box.h │ │ ├── classifier.h │ │ ├── col2im.c │ │ ├── col2im.h │ │ ├── col2im_kernels.cu │ │ ├── compare.c │ │ ├── connected_layer.c │ │ ├── connected_layer.h │ │ ├── convolutional_kernels.cu │ │ ├── convolutional_layer.c │ │ ├── convolutional_layer.h │ │ ├── cost_layer.c │ │ ├── cost_layer.h │ │ ├── crnn_layer.c │ │ ├── crnn_layer.h │ │ ├── crop_layer.c │ │ ├── crop_layer.h │ │ ├── crop_layer_kernels.cu │ │ ├── cuda.c │ │ ├── cuda.h │ │ ├── data.c │ │ ├── data.h │ │ ├── deconvolutional_kernels.cu │ │ ├── deconvolutional_layer.c │ │ ├── deconvolutional_layer.h │ │ ├── demo.c │ │ ├── demo.h │ │ ├── detection_layer.c │ │ ├── detection_layer.h │ │ ├── dropout_layer.c │ │ ├── dropout_layer.h │ │ ├── dropout_layer_kernels.cu │ │ ├── gemm.c │ │ ├── gemm.h │ │ ├── gru_layer.c │ │ ├── gru_layer.h │ │ ├── im2col.c │ │ ├── im2col.h │ │ ├── im2col_kernels.cu │ │ ├── image.c │ │ ├── image.h │ │ ├── l2norm_layer.c │ │ ├── l2norm_layer.h │ │ ├── layer.c │ │ ├── layer.h │ │ ├── list.c │ │ ├── list.h │ │ ├── local_layer.c │ │ ├── local_layer.h │ │ ├── logistic_layer.c │ │ ├── logistic_layer.h │ │ ├── lstm_layer.c │ │ ├── lstm_layer.h │ │ ├── matrix.c │ │ ├── matrix.h │ │ ├── maxpool_layer.c │ │ ├── maxpool_layer.h │ │ ├── maxpool_layer_kernels.cu │ │ ├── network.c │ │ ├── network.h │ │ ├── normalization_layer.c │ │ ├── normalization_layer.h │ │ ├── option_list.c │ │ ├── option_list.h │ │ ├── parser.c │ │ ├── parser.h │ │ ├── region_layer.c │ │ ├── region_layer.h │ │ ├── reorg_layer.c │ │ ├── reorg_layer.h │ │ ├── rnn_layer.c │ │ ├── rnn_layer.h │ │ ├── route_layer.c │ │ ├── route_layer.h │ │ ├── shortcut_layer.c │ │ ├── shortcut_layer.h │ │ ├── softmax_layer.c │ │ ├── softmax_layer.h │ │ ├── stb_image.h │ │ ├── stb_image_write.h │ │ ├── tree.c │ │ ├── tree.h │ │ ├── upsample_layer.c │ │ ├── upsample_layer.h │ │ ├── utils.c │ │ ├── utils.h │ │ ├── yolo_layer.c │ │ └── yolo_layer.h ├── darknet_ros │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ ├── config │ │ ├── ros.yaml │ │ ├── yolov2-tiny-voc.yaml │ │ ├── yolov2-tiny.yaml │ │ ├── yolov2-voc.yaml │ │ ├── yolov2.yaml │ │ ├── yolov3-tiny.yaml │ │ ├── yolov3-voc.yaml │ │ └── yolov3.yaml │ ├── doc │ │ ├── quadruped_anymal_and_person.JPG │ │ ├── test_detection.png │ │ └── test_detection_anymal.png │ ├── include │ │ └── darknet_ros │ │ │ ├── YoloObjectDetector.hpp │ │ │ └── image_interface.h │ ├── launch │ │ ├── darknet_ros.launch │ │ ├── darknet_ros2.launch │ │ ├── darknet_ros_gdb.launch │ │ └── yolo_v3.launch │ ├── package.xml │ ├── src │ │ ├── YoloObjectDetector.cpp │ │ ├── image_interface.c │ │ └── yolo_object_detector_node.cpp │ ├── test │ │ ├── ObjectDetection.cpp │ │ ├── object_detection.test │ │ ├── test_main.cpp │ │ └── yolov2.yaml │ └── yolo_network_config │ │ ├── cfg │ │ ├── yolov2-tiny-voc.cfg │ │ ├── yolov2-tiny.cfg │ │ ├── yolov2-voc.cfg │ │ ├── yolov2.cfg │ │ ├── yolov3-tiny.cfg │ │ ├── yolov3-voc.cfg │ │ └── yolov3.cfg │ │ └── weights │ │ ├── .gitignore │ │ └── how_to_download_weights.txt ├── darknet_ros_msgs │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ ├── action │ │ └── CheckForObjects.action │ ├── msg │ │ ├── BoundingBox.msg │ │ ├── BoundingBoxes.msg │ │ └── ObjectCount.msg │ └── package.xml └── jenkins-pipeline ├── hector_slam ├── README.txt ├── hector_compressed_map_transport │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ ├── package.xml │ └── src │ │ └── map_to_image_node.cpp ├── hector_geotiff │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ ├── include │ │ └── hector_geotiff │ │ │ ├── geotiff_writer.h │ │ │ ├── map_writer_interface.h │ │ │ └── map_writer_plugin_interface.h │ ├── launch │ │ ├── geotiff_mapper.launch │ │ └── geotiff_mapper_only.launch │ ├── package.xml │ └── src │ │ ├── geotiff_node.cpp │ │ ├── geotiff_saver.cpp │ │ └── geotiff_writer │ │ └── geotiff_writer.cpp ├── hector_geotiff_plugins │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ ├── hector_geotiff_plugins.xml │ ├── package.xml │ └── src │ │ └── trajectory_geotiff_plugin.cpp ├── hector_imu_attitude_to_tf │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ ├── launch │ │ └── example.launch │ ├── package.xml │ └── src │ │ └── imu_attitude_to_tf_node.cpp ├── hector_imu_tools │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ ├── launch │ │ ├── bertl_robot.launch │ │ └── jasmine_robot.launch │ ├── package.xml │ └── src │ │ └── pose_and_orientation_to_imu_node.cpp ├── hector_map_server │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ ├── package.xml │ └── src │ │ └── hector_map_server.cpp ├── hector_map_tools │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ ├── include │ │ └── hector_map_tools │ │ │ └── HectorMapTools.h │ └── package.xml ├── hector_mapping │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ ├── include │ │ └── hector_slam_lib │ │ │ ├── map │ │ │ ├── GridMap.h │ │ │ ├── GridMapBase.h │ │ │ ├── GridMapCacheArray.h │ │ │ ├── GridMapLogOdds.h │ │ │ ├── GridMapReflectanceCount.h │ │ │ ├── GridMapSimpleCount.h │ │ │ ├── MapDimensionProperties.h │ │ │ ├── OccGridMapBase.h │ │ │ ├── OccGridMapUtil.h │ │ │ └── OccGridMapUtilConfig.h │ │ │ ├── matcher │ │ │ └── ScanMatcher.h │ │ │ ├── scan │ │ │ └── DataPointContainer.h │ │ │ ├── slam_main │ │ │ ├── HectorSlamProcessor.h │ │ │ ├── MapProcContainer.h │ │ │ ├── MapRepMultiMap.h │ │ │ ├── MapRepSingleMap.h │ │ │ └── MapRepresentationInterface.h │ │ │ └── util │ │ │ ├── DrawInterface.h │ │ │ ├── HectorDebugInfoInterface.h │ │ │ ├── MapLockerInterface.h │ │ │ └── UtilFunctions.h │ ├── launch │ │ ├── mapping_default.launch │ │ └── mapping_tif.launch │ ├── msg │ │ ├── HectorDebugInfo.msg │ │ └── HectorIterData.msg │ ├── package.xml │ └── src │ │ ├── HectorDebugInfoProvider.h │ │ ├── HectorDrawings.h │ │ ├── HectorMapMutex.h │ │ ├── HectorMappingRos.cpp │ │ ├── HectorMappingRos.h │ │ ├── PoseInfoContainer.cpp │ │ ├── PoseInfoContainer.h │ │ ├── main.cpp │ │ └── main_mapper.cpp ├── hector_marker_drawing │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ ├── include │ │ └── hector_marker_drawing │ │ │ ├── DrawInterface.h │ │ │ └── HectorDrawings.h │ └── package.xml ├── hector_nav_msgs │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ ├── package.xml │ └── srv │ │ ├── GetDistanceToObstacle.srv │ │ ├── GetNormal.srv │ │ ├── GetRecoveryInfo.srv │ │ ├── GetRobotTrajectory.srv │ │ └── GetSearchPosition.srv ├── hector_slam │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ └── package.xml ├── hector_slam_launch │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ ├── launch │ │ ├── cityflyer_logfile_processing.launch │ │ ├── hector_ugv.launch │ │ ├── height_mapping.launch │ │ ├── mapping_box.launch │ │ ├── mpo700_mapping.launch │ │ ├── postproc_data.launch │ │ ├── postproc_qut_logs.launch │ │ ├── pr2os.launch │ │ ├── tutorial.launch │ │ └── tutorial_tif.launch │ ├── package.xml │ └── rviz_cfg │ │ └── mapping_demo.rviz └── hector_trajectory_server │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ ├── package.xml │ └── src │ └── hector_trajectory_server.cpp ├── kinematics ├── CMakeLists.txt ├── package.xml └── scripts │ ├── kinematics_with_imu.py │ └── kinematics_with_imu_v2.py ├── lane_detection_example ├── CMakeLists.txt ├── launch │ ├── hanho_lane_detector.launch │ ├── lane_detection.launch │ └── traffic_perception.launch ├── package.xml ├── scripts │ ├── _backup_otsu.py │ ├── controller.py │ ├── hanho_lane_detector.py │ ├── hanho_utils.py │ ├── hanho_utils.pyc │ ├── img_parser.py │ ├── img_parser_lane.py │ ├── img_test.py │ ├── joycom.py │ ├── lane_detector.py │ ├── lane_detector_6_SteerControl2.py │ ├── otsu.py │ ├── otsu_record.py │ ├── stopline_detector.py │ ├── traffic_detector.py │ ├── utils.py │ ├── utils.pyc │ ├── utils_copy.py │ └── utils_copy.pyc └── sensor │ ├── LG │ └── sensor_params.json │ ├── MS │ └── sensor_params.json │ ├── sensor_params.json │ └── sensor_params_MS.json ├── maps ├── 01.pgm ├── 01.yaml ├── 02.pgm └── 02.yaml ├── openslam_gmapping ├── CHANGELOG.rst ├── CMakeLists.txt ├── Makefile ├── TODO.txt ├── build_tools │ ├── Makefile.app │ ├── Makefile.generic-shared-object │ ├── Makefile.subdirs │ ├── generate_shared_object │ ├── message │ ├── pretty_compiler │ └── testlib ├── carmenwrapper │ ├── Makefile │ └── carmenwrapper.cpp ├── configfile │ ├── Makefile │ ├── configfile.cpp │ ├── configfile_test.cpp │ ├── demo.cfg │ └── test.ini ├── configure ├── docs │ ├── Instructions.txt │ ├── scanmatcher.tex │ └── userver.txt ├── gfs-carmen │ ├── Makefile │ └── gfs-carmen.cpp ├── grid │ ├── Makefile │ ├── graphmap.cpp │ └── map_test.cpp ├── gridfastslam │ ├── Makefile │ ├── gfs2log.cpp │ ├── gfs2neff.cpp │ ├── gfs2rec.cpp │ ├── gfs2stat.cpp │ ├── gfs2stream.cpp │ ├── gfsreader.cpp │ ├── gridslamprocessor.cpp │ ├── gridslamprocessor_tree.cpp │ └── motionmodel.cpp ├── gui │ ├── Makefile │ ├── gfs2img.cpp │ ├── gfs_logplayer.cpp │ ├── gfs_nogui.cpp │ ├── gfs_simplegui.cpp │ ├── gsp_thread.cpp │ ├── qgraphpainter.cpp │ ├── qmappainter.cpp │ ├── qnavigatorwidget.cpp │ ├── qparticleviewer.cpp │ ├── qpixmapdumper.cpp │ └── qslamandnavwidget.cpp ├── include │ └── gmapping │ │ ├── carmenwrapper │ │ └── carmenwrapper.h │ │ ├── configfile │ │ └── configfile.h │ │ ├── grid │ │ ├── accessstate.h │ │ ├── array2d.h │ │ ├── harray2d.h │ │ └── map.h │ │ ├── gridfastslam │ │ ├── gfsreader.h │ │ ├── gridslamprocessor.h │ │ ├── gridslamprocessor.hxx │ │ └── motionmodel.h │ │ ├── gui │ │ ├── gsp_thread.h │ │ ├── qgraphpainter.h │ │ ├── qmappainter.h │ │ ├── qnavigatorwidget.h │ │ ├── qparticleviewer.h │ │ ├── qpixmapdumper.h │ │ └── qslamandnavwidget.h │ │ ├── log │ │ ├── carmenconfiguration.h │ │ ├── configuration.h │ │ ├── sensorlog.h │ │ └── sensorstream.h │ │ ├── particlefilter │ │ ├── particlefilter.h │ │ └── pf.h │ │ ├── scanmatcher │ │ ├── eig3.h │ │ ├── gridlinetraversal.h │ │ ├── icp.h │ │ ├── lumiles.h │ │ ├── scanmatcher.h │ │ ├── scanmatcherprocessor.h │ │ └── smmap.h │ │ ├── sensor │ │ ├── sensor_base │ │ │ ├── sensor.h │ │ │ ├── sensoreading.h │ │ │ └── sensorreading.h │ │ ├── sensor_odometry │ │ │ ├── odometryreading.h │ │ │ └── odometrysensor.h │ │ └── sensor_range │ │ │ ├── rangereading.h │ │ │ └── rangesensor.h │ │ └── utils │ │ ├── autoptr.h │ │ ├── commandline.h │ │ ├── datasmoother.h │ │ ├── dmatrix.h │ │ ├── gvalues.h │ │ ├── macro_params.h │ │ ├── movement.h │ │ ├── optimizer.h │ │ ├── orientedboundingbox.h │ │ ├── orientedboundingbox.hxx │ │ ├── point.h │ │ ├── printmemusage.h │ │ ├── printpgm.h │ │ └── stat.h ├── ini │ ├── gfs-LMS-10cm.ini │ ├── gfs-LMS-20cm.ini │ ├── gfs-LMS-5cm.ini │ ├── gfs-PLS-10cm.ini │ └── gfs-PLS-5cm.ini ├── log │ ├── Makefile │ ├── carmenconfiguration.cpp │ ├── configuration.cpp │ ├── log_plot.cpp │ ├── log_test.cpp │ ├── rdk2carmen.cpp │ ├── scanstudio2carmen.cpp │ ├── sensorlog.cpp │ └── sensorstream.cpp ├── manual.mk ├── manual.mk-template ├── package.xml ├── particlefilter │ ├── Makefile │ ├── particlefilter.cpp │ ├── particlefilter_test.cpp │ └── range_bearing.cpp ├── scanmatcher │ ├── Makefile │ ├── eig3.cpp │ ├── icptest.cpp │ ├── scanmatch_test.cpp │ ├── scanmatcher.cpp │ ├── scanmatcher.new.cpp │ ├── scanmatcherprocessor.cpp │ └── smmap.cpp ├── sensor │ ├── Makefile │ ├── sensor_base │ │ ├── Makefile │ │ ├── sensor.cpp │ │ └── sensorreading.cpp │ ├── sensor_odometry │ │ ├── Makefile │ │ ├── odometryreading.cpp │ │ └── odometrysensor.cpp │ └── sensor_range │ │ ├── Makefile │ │ ├── rangereading.cpp │ │ └── rangesensor.cpp ├── setlibpath └── utils │ ├── Makefile │ ├── autoptr_test.cpp │ ├── movement.cpp │ ├── printmemusage.cpp │ ├── stat.cpp │ └── stat_test.cpp ├── racecar ├── .gitignore ├── ackermann_cmd_mux │ ├── .cproject │ ├── .gitignore │ ├── .project │ ├── .pydevproject │ ├── .settings │ │ └── language.settings.xml │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ ├── LICENSE │ ├── cfg │ │ └── reload.cfg │ ├── include │ │ └── ackermann_cmd_mux │ │ │ ├── ackermann_cmd_mux_nodelet.hpp │ │ │ ├── ackermann_cmd_subscribers.hpp │ │ │ └── exceptions.hpp │ ├── launch │ │ ├── ackermann_cmd_mux.launch │ │ ├── reconfigure.launch │ │ └── standalone.launch │ ├── package.xml │ ├── param │ │ ├── example.yaml │ │ └── reconfigure.yaml │ ├── plugins │ │ └── nodelets.xml │ └── src │ │ ├── ackermann_cmd_mux_nodelet.cpp │ │ ├── ackermann_cmd_subscribers.cpp │ │ ├── throttle_interpolator (copy).py │ │ └── throttle_interpolator.py ├── racecar-vm.rosinstall ├── racecar.rosinstall └── racecar │ ├── CMakeLists.txt │ ├── LICENSE │ ├── config │ └── racecar-v2 │ │ ├── high_level_mux.yaml │ │ ├── joy_teleop.yaml │ │ ├── low_level_mux.yaml │ │ ├── sensors.yaml │ │ └── vesc.yaml │ ├── launch │ ├── includes │ │ ├── common │ │ │ ├── joy_teleop.launch.xml │ │ │ └── sensors.launch.xml │ │ ├── racecar-v2-teleop.launch.xml │ │ └── racecar-v2 │ │ │ ├── static_transforms.launch.xml │ │ │ └── vesc.launch.xml │ ├── keyboard_control.launch │ ├── known_map_localization.launch │ ├── mux.launch │ ├── replay_bag_file │ │ ├── replay_bag_file.launch │ │ ├── replay_bag_mapping.launch │ │ └── replay_bag_with_lidar_processing.launch │ └── teleop.launch │ ├── maps │ ├── basement_hallways_10cm.png │ ├── basement_hallways_10cm.yaml │ ├── basement_hallways_5cm.png │ ├── basement_hallways_5cm.yml │ ├── short-course-33.png │ └── short-course-33.yml │ ├── package.xml │ ├── rviz │ ├── known_map_localization.rviz │ ├── laser_scan_matcher.rviz │ └── mapping.rviz │ └── scripts │ ├── joy_teleop.py │ ├── keyboard_control.py │ ├── lane_detector_1_img_process.py │ ├── lane_detector_2_birdView.py │ ├── lane_detector_3_birdViewPointMak.py │ ├── lane_detector_4_SlidingWindow.py │ ├── lane_detector_5_CurvedProcess.py │ ├── lane_detector_6_SteerControl.py │ ├── odom_tf.py │ ├── utils.py │ └── utils.pyc ├── razor_imu_9dof ├── .gitignore ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── cfg │ └── imu.cfg ├── config │ └── razor_diags.yaml ├── launch │ ├── razor-display.launch │ ├── razor-pub-and-display.launch │ ├── razor-pub-diags.launch │ └── razor-pub.launch ├── magnetometer_calibration │ ├── Matlab │ │ └── magnetometer_calibration │ │ │ ├── ellipsoid_fit.m │ │ │ ├── ellipsoid_fit_license.txt │ │ │ └── magnetometer_calibration.m │ └── Processing │ │ └── Magnetometer_calibration │ │ ├── Magnetometer_calibration.pde │ │ └── data │ │ └── Univers-66.vlw ├── nodes │ ├── display_3D_visualization.py │ └── imu_node.py ├── package.xml └── src │ └── Razor_AHRS │ ├── Compass.ino │ ├── DCM.ino │ ├── Math.ino │ ├── Output.ino │ ├── Razor_AHRS.ino │ └── Sensors.ino ├── rf2o_laser_odometry ├── CMakeLists.txt ├── LICENSE ├── README.md ├── include │ └── rf2o_laser_odometry │ │ └── CLaserOdometry2D.h ├── launch │ └── rf2o_laser_odometry.launch ├── package.xml └── src │ ├── CLaserOdometry2D.cpp │ └── CLaserOdometry2DNode.cpp ├── rplidar_ros ├── CHANGELOG.rst ├── CMakeLists.txt ├── LICENSE ├── README.md ├── launch │ ├── rplidar.launch │ ├── rplidar_a3.launch │ ├── rplidar_s1.launch │ ├── rplidar_s1_tcp.launch │ ├── test_rplidar.launch │ ├── test_rplidar_a3.launch │ ├── view_rplidar.launch │ ├── view_rplidar_a3.launch │ ├── view_rplidar_s1.launch │ └── view_rplidar_s1_tcp.launch ├── package.xml ├── rplidar_A1.png ├── rplidar_A2.png ├── rviz │ └── rplidar.rviz ├── scripts │ ├── create_udev_rules.sh │ ├── delete_udev_rules.sh │ └── rplidar.rules ├── sdk │ ├── README.txt │ ├── include │ │ ├── rplidar.h │ │ ├── rplidar_cmd.h │ │ ├── rplidar_driver.h │ │ ├── rplidar_protocol.h │ │ └── rptypes.h │ └── src │ │ ├── arch │ │ ├── linux │ │ │ ├── arch_linux.h │ │ │ ├── net_serial.cpp │ │ │ ├── net_serial.h │ │ │ ├── net_socket.cpp │ │ │ ├── thread.hpp │ │ │ ├── timer.cpp │ │ │ └── timer.h │ │ ├── macOS │ │ │ ├── arch_macOS.h │ │ │ ├── net_serial.cpp │ │ │ ├── net_serial.h │ │ │ ├── net_socket.cpp │ │ │ ├── thread.hpp │ │ │ ├── timer.cpp │ │ │ └── timer.h │ │ └── win32 │ │ │ ├── arch_win32.h │ │ │ ├── net_serial.cpp │ │ │ ├── net_serial.h │ │ │ ├── net_socket.cpp │ │ │ ├── timer.cpp │ │ │ ├── timer.h │ │ │ └── winthread.hpp │ │ ├── hal │ │ ├── abs_rxtx.h │ │ ├── assert.h │ │ ├── byteops.h │ │ ├── event.h │ │ ├── locker.h │ │ ├── socket.h │ │ ├── thread.cpp │ │ ├── thread.h │ │ ├── types.h │ │ └── util.h │ │ ├── rplidar_driver.cpp │ │ ├── rplidar_driver_TCP.h │ │ ├── rplidar_driver_impl.h │ │ ├── rplidar_driver_serial.h │ │ └── sdkcommon.h └── src │ ├── client.cpp │ └── node.cpp ├── slam_gmapping ├── .gitignore ├── .travis.yml ├── gmapping │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ ├── launch │ │ └── slam_gmapping_pr2.launch │ ├── nodelet_plugins.xml │ ├── package.xml │ ├── src │ │ ├── main.cpp │ │ ├── nodelet.cpp │ │ ├── replay.cpp │ │ ├── slam_gmapping.cpp │ │ └── slam_gmapping.h │ └── test │ │ ├── basic_localization_laser_different_beamcount.test │ │ ├── basic_localization_stage.launch │ │ ├── basic_localization_stage_replay.launch │ │ ├── basic_localization_stage_replay2.launch │ │ ├── basic_localization_symmetry.launch │ │ ├── basic_localization_upside_down.launch │ │ ├── rtest.cpp │ │ └── test_map.py └── slam_gmapping │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ └── package.xml ├── usb_cam ├── .gitignore ├── .travis.yml ├── AUTHORS.md ├── CHANGELOG.rst ├── CMakeLists.txt ├── LICENSE ├── README.md ├── include │ └── usb_cam │ │ └── usb_cam.h ├── launch │ ├── usb_cam-test (original).launch │ └── usb_cam-test.launch ├── mainpage.dox ├── nodes │ ├── usb_cam_node (original).cpp │ └── usb_cam_node.cpp ├── package.xml └── src │ ├── LICENSE │ └── usb_cam.cpp └── vesc ├── .gitignore ├── vesc ├── CMakeLists.txt └── package.xml ├── vesc_ackermann ├── CMakeLists.txt ├── include │ └── vesc_ackermann │ │ ├── ackermann_to_vesc.h │ │ └── vesc_to_odom.h ├── launch │ ├── ackermann_to_vesc_node.launch │ ├── ackermann_to_vesc_nodelet.launch │ ├── vesc_to_odom_node.launch │ └── vesc_to_odom_nodelet.launch ├── package.xml ├── src │ ├── ackermann_to_vesc.cpp │ ├── ackermann_to_vesc_node.cpp │ ├── ackermann_to_vesc_nodelet.cpp │ ├── vesc_to_odom.cpp │ ├── vesc_to_odom_node.cpp │ └── vesc_to_odom_nodelet.cpp └── vesc_ackermann_nodelet.xml ├── vesc_driver ├── CMakeLists.txt ├── include │ └── vesc_driver │ │ ├── datatypes.h │ │ ├── v8stdint.h │ │ ├── vesc_driver.h │ │ ├── vesc_interface.h │ │ ├── vesc_packet.h │ │ └── vesc_packet_factory.h ├── launch │ ├── vesc_driver_node.launch │ └── vesc_driver_nodelet.launch ├── package.xml ├── src │ ├── vesc_driver (copy).cpp │ ├── vesc_driver.cpp │ ├── vesc_driver_node.cpp │ ├── vesc_driver_nodelet.cpp │ ├── vesc_interface.cpp │ ├── vesc_packet.cpp │ └── vesc_packet_factory.cpp └── vesc_driver_nodelet.xml └── vesc_msgs ├── CMakeLists.txt ├── msg ├── VescState.msg └── VescStateStamped.msg └── package.xml /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | /opt/ros/kinetic/share/catkin/cmake/toplevel.cmake -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ROS Lane Detection and Darknet_ros Object Detection 2 | 3 | 1. Object Detection 4 | - Recieve Video from Vehicle Camera(using usb_cam pkg) 5 | - using Darknet_ros, use networks yoloV3-tiny, for Bus Stop Detection 6 | - is Detected, Publish 'Speed 0' Command Wait 10sec Vehicle is Drive with lane detection 7 | 8 | 2. Lane Detection 9 | - Recieve Video from usb_cam pkg 10 | - Video Convert Bird Eyes View Image and Binary 11 | - using Sliding Windows and Calculate Vehicle's Steer and Publish Steer command 12 | - Speed is fixed 1000 13 | - subscribe message like 'Bus Stop Detect!' -> Speed is 0 and Wait 10sec 14 | - Over 10sec Drive using lane detection 15 | 16 | 17 | ## Command 18 | 19 | 1. run darknet_ros and usb_cam 20 | roslaunch darknet_ros darknet_ros2.launch 21 | 22 | 2. run lane detection 23 | rosrun lane_detection_example hanho_lane_detector.py 24 | 25 | 3. check binary image is right? 26 | 4. binary image is not right -> image shape is not right? 27 | - move 'control penal' top x,y bottom x,y point 28 | - this point is BEV matrix point. this change -> BEV image change 29 | 30 | 5. binary image is not right -> image binarization is not right? 31 | - in 'hanho_lane_detector.py' image_process_otsu function code change 32 | - i think change cv2.inRange(src, lower, upper) 33 | 34 | 6. all of image is right 35 | - run vesc_driver_node 36 | - this is vesc_node. if it is not running. vehicle don't Drive Anyway 37 | roslaunch vesc_driver vesc_driver_node.launch 38 | -------------------------------------------------------------------------------- /ackermann_msgs/.gitignore: -------------------------------------------------------------------------------- 1 | *.a 2 | *.o 3 | *.os 4 | *.pyc 5 | *.so 6 | *.tar.gz 7 | *~ 8 | TAGS 9 | build/ 10 | msg_gen/ 11 | src/ 12 | 13 | -------------------------------------------------------------------------------- /ackermann_msgs/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | Change history 2 | ============== 3 | 4 | 1.0.1 (2015-12-22) 5 | ------------------ 6 | * Removed Jim Rothrock from the maintainer list. Changed the version to 1.0.1. 7 | 8 | 1.0.0 (2015-08-28) 9 | ------------------ 10 | * Changed the version to 1.0.0. 11 | 12 | 0.9.1 (2014-04-30) 13 | ------------------ 14 | * Export architecture_independent flag in package.xml (`#3 15 | `_), thanks 16 | to Scott K Logan 17 | 18 | 0.9.0 (2013-03-17) 19 | ------------------ 20 | 21 | * convert to catkin 22 | * release to Hydro 23 | 24 | 0.4.0 (2012-03-01) 25 | ------------------ 26 | 27 | * initial release to both Fuerte and Electric following API review 28 | * later released with Groovy 29 | -------------------------------------------------------------------------------- /ackermann_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(ackermann_msgs) 3 | 4 | find_package(catkin REQUIRED COMPONENTS message_generation std_msgs) 5 | # We want boost/format.hpp, which isn't in its own component. 6 | find_package(Boost REQUIRED) 7 | 8 | add_message_files( 9 | DIRECTORY msg 10 | FILES AckermannDrive.msg AckermannDriveStamped.msg) 11 | 12 | generate_messages(DEPENDENCIES std_msgs) 13 | 14 | catkin_package( 15 | CATKIN_DEPENDS message_runtime std_msgs 16 | DEPENDS Boost) 17 | -------------------------------------------------------------------------------- /ackermann_msgs/README.rst: -------------------------------------------------------------------------------- 1 | ackermann_msgs 2 | ============== 3 | 4 | ROS messages for vehicles using front-wheel `Ackermann steering`_. It 5 | was defined by the ROS `Ackermann steering group`_. 6 | 7 | ROS documentation: http://www.ros.org/wiki/ackermann_msgs 8 | 9 | .. _Ackermann steering: http://en.wikipedia.org/wiki/Ackermann_steering_geometry 10 | .. _Ackermann steering group: http://www.ros.org/wiki/Ackermann%20Group 11 | -------------------------------------------------------------------------------- /ackermann_msgs/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | Messages for driving a vehicle using Ackermann steering. 6 | 7 | There are no nodes, scripts or other APIs in this package. 8 | 9 | */ 10 | -------------------------------------------------------------------------------- /ackermann_msgs/msg/AckermannDriveStamped.msg: -------------------------------------------------------------------------------- 1 | ## Time stamped drive command for robots with Ackermann steering. 2 | # $Id$ 3 | 4 | Header header 5 | AckermannDrive drive 6 | -------------------------------------------------------------------------------- /ackermann_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | ackermann_msgs 3 | 1.0.1 4 | 5 | ROS messages for robots using Ackermann steering. 6 | 7 | Jack O'Quin 8 | Jack O'Quin 9 | BSD 10 | 11 | http://ros.org/wiki/ackermann_msgs 12 | https://github.com/ros-drivers/ackermann_msgs.git 13 | https://github.com/ros-drivers/ackermann_msgs/issues 14 | 15 | catkin 16 | 17 | message_generation 18 | std_msgs 19 | 20 | message_runtime 21 | std_msgs 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /darknet_ros/.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "darknet"] 2 | path = darknet 3 | url = https://github.com/pjreddie/darknet 4 | -------------------------------------------------------------------------------- /darknet_ros/darknet/.gitignore: -------------------------------------------------------------------------------- 1 | *.o 2 | *.dSYM 3 | *.csv 4 | *.out 5 | *.png 6 | *.jpg 7 | *.pyc 8 | old/ 9 | mnist/ 10 | data/ 11 | caffe/ 12 | grasp/ 13 | images/ 14 | opencv/ 15 | convnet/ 16 | decaf/ 17 | submission/ 18 | cfg/ 19 | darknet 20 | .fuse* 21 | 22 | # OS Generated # 23 | .DS_Store* 24 | ehthumbs.db 25 | Icon? 26 | Thumbs.db 27 | *.swp 28 | -------------------------------------------------------------------------------- /darknet_ros/darknet/LICENSE: -------------------------------------------------------------------------------- 1 | YOLO LICENSE 2 | Version 2, July 29 2016 3 | 4 | THIS SOFTWARE LICENSE IS PROVIDED "ALL CAPS" SO THAT YOU KNOW IT IS SUPER 5 | SERIOUS AND YOU DON'T MESS AROUND WITH COPYRIGHT LAW BECAUSE YOU WILL GET IN 6 | TROUBLE HERE ARE SOME OTHER BUZZWORDS COMMONLY IN THESE THINGS WARRANTIES 7 | LIABILITY CONTRACT TORT LIABLE CLAIMS RESTRICTION MERCHANTABILITY. NOW HERE'S 8 | THE REAL LICENSE: 9 | 10 | 0. Darknet is public domain. 11 | 1. Do whatever you want with it. 12 | 2. Stop emailing me about it! 13 | -------------------------------------------------------------------------------- /darknet_ros/darknet/LICENSE.fuck: -------------------------------------------------------------------------------- 1 | DO WHAT THE FUCK YOU WANT TO PUBLIC LICENSE 2 | Version 2, December 2004 3 | 4 | Copyright (C) 2004 Sam Hocevar 5 | 6 | Everyone is permitted to copy and distribute verbatim or modified 7 | copies of this license document, and changing it is allowed as long 8 | as the name is changed. 9 | 10 | DO WHAT THE FUCK YOU WANT TO PUBLIC LICENSE 11 | TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION 12 | 13 | 0. You just DO WHAT THE FUCK YOU WANT TO. 14 | -------------------------------------------------------------------------------- /darknet_ros/darknet/LICENSE.meta: -------------------------------------------------------------------------------- 1 | META-LICENSE 2 | Version 1, June 21 2017 3 | 4 | Any and all licenses may be applied to the software either individually 5 | or in concert. Any issues, ambiguities, paradoxes, or metaphysical quandries 6 | arising from this combination should be discussed with a local faith leader, 7 | hermit, or guru. The Oxford comma shall be used. 8 | 9 | -------------------------------------------------------------------------------- /darknet_ros/darknet/LICENSE.mit: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2017 Joseph Redmon 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 | 23 | -------------------------------------------------------------------------------- /darknet_ros/darknet/LICENSE.v1: -------------------------------------------------------------------------------- 1 | YOLO LICENSE 2 | Version 1, July 10 2015 3 | 4 | THIS SOFTWARE LICENSE IS PROVIDED "ALL CAPS" SO THAT YOU KNOW IT IS SUPER 5 | SERIOUS AND YOU DON'T MESS AROUND WITH COPYRIGHT LAW BECAUSE YOU WILL GET IN 6 | TROUBLE HERE ARE SOME OTHER BUZZWORDS COMMONLY IN THESE THINGS WARRANTIES 7 | LIABILITY CONTRACT TORT LIABLE CLAIMS RESTRICTION MERCHANTABILITY SUBJECT TO 8 | THE FOLLOWING CONDITIONS: 9 | 10 | 1. #yolo 11 | 2. #swag 12 | 3. #blazeit 13 | 14 | -------------------------------------------------------------------------------- /darknet_ros/darknet/README.md: -------------------------------------------------------------------------------- 1 | ![Darknet Logo](http://pjreddie.com/media/files/darknet-black-small.png) 2 | 3 | # Darknet # 4 | Darknet is an open source neural network framework written in C and CUDA. It is fast, easy to install, and supports CPU and GPU computation. 5 | 6 | For more information see the [Darknet project website](http://pjreddie.com/darknet). 7 | 8 | For questions or issues please use the [Google Group](https://groups.google.com/forum/#!forum/darknet). 9 | -------------------------------------------------------------------------------- /darknet_ros/darknet/examples/detector.py: -------------------------------------------------------------------------------- 1 | # Stupid python path shit. 2 | # Instead just add darknet.py to somewhere in your python path 3 | # OK actually that might not be a great idea, idk, work in progress 4 | # Use at your own risk. or don't, i don't care 5 | 6 | import sys, os 7 | sys.path.append(os.path.join(os.getcwd(),'python/')) 8 | 9 | import darknet as dn 10 | import pdb 11 | 12 | dn.set_gpu(0) 13 | net = dn.load_net("cfg/yolo-thor.cfg", "/home/pjreddie/backup/yolo-thor_final.weights", 0) 14 | meta = dn.load_meta("cfg/thor.data") 15 | r = dn.detect(net, meta, "data/bedroom.jpg") 16 | print r 17 | 18 | # And then down here you could detect a lot more images like: 19 | r = dn.detect(net, meta, "data/eagle.jpg") 20 | print r 21 | r = dn.detect(net, meta, "data/giraffe.jpg") 22 | print r 23 | r = dn.detect(net, meta, "data/horses.jpg") 24 | print r 25 | r = dn.detect(net, meta, "data/person.jpg") 26 | print r 27 | 28 | -------------------------------------------------------------------------------- /darknet_ros/darknet/python/proverbot.py: -------------------------------------------------------------------------------- 1 | from darknet import * 2 | 3 | def predict_tactic(net, s): 4 | prob = 0 5 | d = c_array(c_float, [0.0]*256) 6 | tac = '' 7 | if not len(s): 8 | s = '\n' 9 | for c in s[:-1]: 10 | d[ord(c)] = 1 11 | pred = predict(net, d) 12 | d[ord(c)] = 0 13 | c = s[-1] 14 | while 1: 15 | d[ord(c)] = 1 16 | pred = predict(net, d) 17 | d[ord(c)] = 0 18 | pred = [pred[i] for i in range(256)] 19 | ind = sample(pred) 20 | c = chr(ind) 21 | prob += math.log(pred[ind]) 22 | if len(tac) and tac[-1] == '.': 23 | break 24 | tac = tac + c 25 | return (tac, prob) 26 | 27 | def predict_tactics(net, s, n): 28 | tacs = [] 29 | for i in range(n): 30 | reset_rnn(net) 31 | tacs.append(predict_tactic(net, s)) 32 | tacs = sorted(tacs, key=lambda x: -x[1]) 33 | return tacs 34 | 35 | net = load_net("cfg/coq.test.cfg", "/home/pjreddie/backup/coq.backup", 0) 36 | t = predict_tactics(net, "+++++\n", 10) 37 | print t 38 | -------------------------------------------------------------------------------- /darknet_ros/darknet/scripts/dice_label.sh: -------------------------------------------------------------------------------- 1 | mkdir -p images 2 | mkdir -p images/orig 3 | mkdir -p images/train 4 | mkdir -p images/val 5 | 6 | ffmpeg -i Face1.mp4 images/orig/face1_%6d.jpg 7 | ffmpeg -i Face2.mp4 images/orig/face2_%6d.jpg 8 | ffmpeg -i Face3.mp4 images/orig/face3_%6d.jpg 9 | ffmpeg -i Face4.mp4 images/orig/face4_%6d.jpg 10 | ffmpeg -i Face5.mp4 images/orig/face5_%6d.jpg 11 | ffmpeg -i Face6.mp4 images/orig/face6_%6d.jpg 12 | 13 | mogrify -resize 100x100^ -gravity center -crop 100x100+0+0 +repage images/orig/* 14 | 15 | ls images/orig/* | shuf | head -n 1000 | xargs mv -t images/val 16 | mv images/orig/* images/train 17 | 18 | find `pwd`/images/train > dice.train.list -name \*.jpg 19 | find `pwd`/images/val > dice.val.list -name \*.jpg 20 | 21 | -------------------------------------------------------------------------------- /darknet_ros/darknet/scripts/gen_tactic.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # Usage: 3 | # wget http://pjreddie.com/media/files/peek.weights 4 | # scripts/gen_tactic.sh < data/goal.txt 5 | ./darknet rnn generatetactic cfg/gru.cfg peek.weights 2>/dev/null 6 | -------------------------------------------------------------------------------- /darknet_ros/darknet/scripts/get_coco_dataset.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Clone COCO API 4 | git clone https://github.com/pdollar/coco 5 | cd coco 6 | 7 | mkdir images 8 | cd images 9 | 10 | # Download Images 11 | wget -c https://pjreddie.com/media/files/train2014.zip 12 | wget -c https://pjreddie.com/media/files/val2014.zip 13 | 14 | # Unzip 15 | unzip -q train2014.zip 16 | unzip -q val2014.zip 17 | 18 | cd .. 19 | 20 | # Download COCO Metadata 21 | wget -c https://pjreddie.com/media/files/instances_train-val2014.zip 22 | wget -c https://pjreddie.com/media/files/coco/5k.part 23 | wget -c https://pjreddie.com/media/files/coco/trainvalno5k.part 24 | wget -c https://pjreddie.com/media/files/coco/labels.tgz 25 | tar xzf labels.tgz 26 | unzip -q instances_train-val2014.zip 27 | 28 | # Set Up Image Lists 29 | paste <(awk "{print \"$PWD\"}" <5k.part) 5k.part | tr -d '\t' > 5k.txt 30 | paste <(awk "{print \"$PWD\"}" trainvalno5k.txt 31 | 32 | -------------------------------------------------------------------------------- /darknet_ros/darknet/scripts/imagenet_label.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | mkdir -p labelled 4 | wd=`pwd` 5 | 6 | for f in val/*.xml; 7 | do 8 | label=`grep -m1 "" $f | grep -oP '\K[^<]*'` 9 | im=`echo $f | sed 's/val/imgs/; s/xml/JPEG/'` 10 | out=`echo $im | sed 's/JPEG/'${label}'.JPEG/; s/imgs/labelled/'` 11 | ln -s ${wd}/$im ${wd}/$out 12 | done 13 | 14 | find ${wd}/labelled -name \*.JPEG > inet.val.list 15 | 16 | -------------------------------------------------------------------------------- /darknet_ros/darknet/src/activation_layer.h: -------------------------------------------------------------------------------- 1 | #ifndef ACTIVATION_LAYER_H 2 | #define ACTIVATION_LAYER_H 3 | 4 | #include "activations.h" 5 | #include "layer.h" 6 | #include "network.h" 7 | 8 | layer make_activation_layer(int batch, int inputs, ACTIVATION activation); 9 | 10 | void forward_activation_layer(layer l, network net); 11 | void backward_activation_layer(layer l, network net); 12 | 13 | #ifdef GPU 14 | void forward_activation_layer_gpu(layer l, network net); 15 | void backward_activation_layer_gpu(layer l, network net); 16 | #endif 17 | 18 | #endif 19 | 20 | -------------------------------------------------------------------------------- /darknet_ros/darknet/src/avgpool_layer.h: -------------------------------------------------------------------------------- 1 | #ifndef AVGPOOL_LAYER_H 2 | #define AVGPOOL_LAYER_H 3 | 4 | #include "image.h" 5 | #include "cuda.h" 6 | #include "layer.h" 7 | #include "network.h" 8 | 9 | typedef layer avgpool_layer; 10 | 11 | image get_avgpool_image(avgpool_layer l); 12 | avgpool_layer make_avgpool_layer(int batch, int w, int h, int c); 13 | void resize_avgpool_layer(avgpool_layer *l, int w, int h); 14 | void forward_avgpool_layer(const avgpool_layer l, network net); 15 | void backward_avgpool_layer(const avgpool_layer l, network net); 16 | 17 | #ifdef GPU 18 | void forward_avgpool_layer_gpu(avgpool_layer l, network net); 19 | void backward_avgpool_layer_gpu(avgpool_layer l, network net); 20 | #endif 21 | 22 | #endif 23 | 24 | -------------------------------------------------------------------------------- /darknet_ros/darknet/src/batchnorm_layer.h: -------------------------------------------------------------------------------- 1 | #ifndef BATCHNORM_LAYER_H 2 | #define BATCHNORM_LAYER_H 3 | 4 | #include "image.h" 5 | #include "layer.h" 6 | #include "network.h" 7 | 8 | layer make_batchnorm_layer(int batch, int w, int h, int c); 9 | void forward_batchnorm_layer(layer l, network net); 10 | void backward_batchnorm_layer(layer l, network net); 11 | 12 | #ifdef GPU 13 | void forward_batchnorm_layer_gpu(layer l, network net); 14 | void backward_batchnorm_layer_gpu(layer l, network net); 15 | void pull_batchnorm_layer(layer l); 16 | void push_batchnorm_layer(layer l); 17 | #endif 18 | 19 | #endif 20 | -------------------------------------------------------------------------------- /darknet_ros/darknet/src/box.h: -------------------------------------------------------------------------------- 1 | #ifndef BOX_H 2 | #define BOX_H 3 | #include "darknet.h" 4 | 5 | typedef struct{ 6 | float dx, dy, dw, dh; 7 | } dbox; 8 | 9 | float box_rmse(box a, box b); 10 | dbox diou(box a, box b); 11 | box decode_box(box b, box anchor); 12 | box encode_box(box b, box anchor); 13 | 14 | #endif 15 | -------------------------------------------------------------------------------- /darknet_ros/darknet/src/classifier.h: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /darknet_ros/darknet/src/col2im.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | void col2im_add_pixel(float *im, int height, int width, int channels, 4 | int row, int col, int channel, int pad, float val) 5 | { 6 | row -= pad; 7 | col -= pad; 8 | 9 | if (row < 0 || col < 0 || 10 | row >= height || col >= width) return; 11 | im[col + width*(row + height*channel)] += val; 12 | } 13 | //This one might be too, can't remember. 14 | void col2im_cpu(float* data_col, 15 | int channels, int height, int width, 16 | int ksize, int stride, int pad, float* data_im) 17 | { 18 | int c,h,w; 19 | int height_col = (height + 2*pad - ksize) / stride + 1; 20 | int width_col = (width + 2*pad - ksize) / stride + 1; 21 | 22 | int channels_col = channels * ksize * ksize; 23 | for (c = 0; c < channels_col; ++c) { 24 | int w_offset = c % ksize; 25 | int h_offset = (c / ksize) % ksize; 26 | int c_im = c / ksize / ksize; 27 | for (h = 0; h < height_col; ++h) { 28 | for (w = 0; w < width_col; ++w) { 29 | int im_row = h_offset + h * stride; 30 | int im_col = w_offset + w * stride; 31 | int col_index = (c * height_col + h) * width_col + w; 32 | double val = data_col[col_index]; 33 | col2im_add_pixel(data_im, height, width, channels, 34 | im_row, im_col, c_im, pad, val); 35 | } 36 | } 37 | } 38 | } 39 | 40 | -------------------------------------------------------------------------------- /darknet_ros/darknet/src/col2im.h: -------------------------------------------------------------------------------- 1 | #ifndef COL2IM_H 2 | #define COL2IM_H 3 | 4 | void col2im_cpu(float* data_col, 5 | int channels, int height, int width, 6 | int ksize, int stride, int pad, float* data_im); 7 | 8 | #ifdef GPU 9 | void col2im_gpu(float *data_col, 10 | int channels, int height, int width, 11 | int ksize, int stride, int pad, float *data_im); 12 | #endif 13 | #endif 14 | -------------------------------------------------------------------------------- /darknet_ros/darknet/src/connected_layer.h: -------------------------------------------------------------------------------- 1 | #ifndef CONNECTED_LAYER_H 2 | #define CONNECTED_LAYER_H 3 | 4 | #include "activations.h" 5 | #include "layer.h" 6 | #include "network.h" 7 | 8 | layer make_connected_layer(int batch, int inputs, int outputs, ACTIVATION activation, int batch_normalize, int adam); 9 | 10 | void forward_connected_layer(layer l, network net); 11 | void backward_connected_layer(layer l, network net); 12 | void update_connected_layer(layer l, update_args a); 13 | 14 | #ifdef GPU 15 | void forward_connected_layer_gpu(layer l, network net); 16 | void backward_connected_layer_gpu(layer l, network net); 17 | void update_connected_layer_gpu(layer l, update_args a); 18 | void push_connected_layer(layer l); 19 | void pull_connected_layer(layer l); 20 | #endif 21 | 22 | #endif 23 | 24 | -------------------------------------------------------------------------------- /darknet_ros/darknet/src/cost_layer.h: -------------------------------------------------------------------------------- 1 | #ifndef COST_LAYER_H 2 | #define COST_LAYER_H 3 | #include "layer.h" 4 | #include "network.h" 5 | 6 | typedef layer cost_layer; 7 | 8 | COST_TYPE get_cost_type(char *s); 9 | char *get_cost_string(COST_TYPE a); 10 | cost_layer make_cost_layer(int batch, int inputs, COST_TYPE type, float scale); 11 | void forward_cost_layer(const cost_layer l, network net); 12 | void backward_cost_layer(const cost_layer l, network net); 13 | void resize_cost_layer(cost_layer *l, int inputs); 14 | 15 | #ifdef GPU 16 | void forward_cost_layer_gpu(cost_layer l, network net); 17 | void backward_cost_layer_gpu(const cost_layer l, network net); 18 | #endif 19 | 20 | #endif 21 | -------------------------------------------------------------------------------- /darknet_ros/darknet/src/crnn_layer.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef CRNN_LAYER_H 3 | #define CRNN_LAYER_H 4 | 5 | #include "activations.h" 6 | #include "layer.h" 7 | #include "network.h" 8 | 9 | layer make_crnn_layer(int batch, int h, int w, int c, int hidden_filters, int output_filters, int steps, ACTIVATION activation, int batch_normalize); 10 | 11 | void forward_crnn_layer(layer l, network net); 12 | void backward_crnn_layer(layer l, network net); 13 | void update_crnn_layer(layer l, update_args a); 14 | 15 | #ifdef GPU 16 | void forward_crnn_layer_gpu(layer l, network net); 17 | void backward_crnn_layer_gpu(layer l, network net); 18 | void update_crnn_layer_gpu(layer l, update_args a); 19 | void push_crnn_layer(layer l); 20 | void pull_crnn_layer(layer l); 21 | #endif 22 | 23 | #endif 24 | 25 | -------------------------------------------------------------------------------- /darknet_ros/darknet/src/crop_layer.h: -------------------------------------------------------------------------------- 1 | #ifndef CROP_LAYER_H 2 | #define CROP_LAYER_H 3 | 4 | #include "image.h" 5 | #include "layer.h" 6 | #include "network.h" 7 | 8 | typedef layer crop_layer; 9 | 10 | image get_crop_image(crop_layer l); 11 | crop_layer make_crop_layer(int batch, int h, int w, int c, int crop_height, int crop_width, int flip, float angle, float saturation, float exposure); 12 | void forward_crop_layer(const crop_layer l, network net); 13 | void resize_crop_layer(layer *l, int w, int h); 14 | 15 | #ifdef GPU 16 | void forward_crop_layer_gpu(crop_layer l, network net); 17 | #endif 18 | 19 | #endif 20 | 21 | -------------------------------------------------------------------------------- /darknet_ros/darknet/src/cuda.h: -------------------------------------------------------------------------------- 1 | #ifndef CUDA_H 2 | #define CUDA_H 3 | 4 | #include "darknet.h" 5 | 6 | #ifdef GPU 7 | 8 | void check_error(cudaError_t status); 9 | cublasHandle_t blas_handle(); 10 | int *cuda_make_int_array(int *x, size_t n); 11 | void cuda_random(float *x_gpu, size_t n); 12 | float cuda_compare(float *x_gpu, float *x, size_t n, char *s); 13 | dim3 cuda_gridsize(size_t n); 14 | 15 | #ifdef CUDNN 16 | cudnnHandle_t cudnn_handle(); 17 | #endif 18 | 19 | #endif 20 | #endif 21 | -------------------------------------------------------------------------------- /darknet_ros/darknet/src/deconvolutional_layer.h: -------------------------------------------------------------------------------- 1 | #ifndef DECONVOLUTIONAL_LAYER_H 2 | #define DECONVOLUTIONAL_LAYER_H 3 | 4 | #include "cuda.h" 5 | #include "image.h" 6 | #include "activations.h" 7 | #include "layer.h" 8 | #include "network.h" 9 | 10 | #ifdef GPU 11 | void forward_deconvolutional_layer_gpu(layer l, network net); 12 | void backward_deconvolutional_layer_gpu(layer l, network net); 13 | void update_deconvolutional_layer_gpu(layer l, update_args a); 14 | void push_deconvolutional_layer(layer l); 15 | void pull_deconvolutional_layer(layer l); 16 | #endif 17 | 18 | layer make_deconvolutional_layer(int batch, int h, int w, int c, int n, int size, int stride, int padding, ACTIVATION activation, int batch_normalize, int adam); 19 | void resize_deconvolutional_layer(layer *l, int h, int w); 20 | void forward_deconvolutional_layer(const layer l, network net); 21 | void update_deconvolutional_layer(layer l, update_args a); 22 | void backward_deconvolutional_layer(layer l, network net); 23 | 24 | #endif 25 | 26 | -------------------------------------------------------------------------------- /darknet_ros/darknet/src/demo.h: -------------------------------------------------------------------------------- 1 | #ifndef DEMO_H 2 | #define DEMO_H 3 | 4 | #include "image.h" 5 | 6 | #endif 7 | -------------------------------------------------------------------------------- /darknet_ros/darknet/src/detection_layer.h: -------------------------------------------------------------------------------- 1 | #ifndef DETECTION_LAYER_H 2 | #define DETECTION_LAYER_H 3 | 4 | #include "layer.h" 5 | #include "network.h" 6 | 7 | typedef layer detection_layer; 8 | 9 | detection_layer make_detection_layer(int batch, int inputs, int n, int size, int classes, int coords, int rescore); 10 | void forward_detection_layer(const detection_layer l, network net); 11 | void backward_detection_layer(const detection_layer l, network net); 12 | 13 | #ifdef GPU 14 | void forward_detection_layer_gpu(const detection_layer l, network net); 15 | void backward_detection_layer_gpu(detection_layer l, network net); 16 | #endif 17 | 18 | #endif 19 | -------------------------------------------------------------------------------- /darknet_ros/darknet/src/dropout_layer.h: -------------------------------------------------------------------------------- 1 | #ifndef DROPOUT_LAYER_H 2 | #define DROPOUT_LAYER_H 3 | 4 | #include "layer.h" 5 | #include "network.h" 6 | 7 | typedef layer dropout_layer; 8 | 9 | dropout_layer make_dropout_layer(int batch, int inputs, float probability); 10 | 11 | void forward_dropout_layer(dropout_layer l, network net); 12 | void backward_dropout_layer(dropout_layer l, network net); 13 | void resize_dropout_layer(dropout_layer *l, int inputs); 14 | 15 | #ifdef GPU 16 | void forward_dropout_layer_gpu(dropout_layer l, network net); 17 | void backward_dropout_layer_gpu(dropout_layer l, network net); 18 | 19 | #endif 20 | #endif 21 | -------------------------------------------------------------------------------- /darknet_ros/darknet/src/dropout_layer_kernels.cu: -------------------------------------------------------------------------------- 1 | #include "cuda_runtime.h" 2 | #include "curand.h" 3 | #include "cublas_v2.h" 4 | 5 | extern "C" { 6 | #include "dropout_layer.h" 7 | #include "cuda.h" 8 | #include "utils.h" 9 | } 10 | 11 | __global__ void yoloswag420blazeit360noscope(float *input, int size, float *rand, float prob, float scale) 12 | { 13 | int id = (blockIdx.x + blockIdx.y*gridDim.x) * blockDim.x + threadIdx.x; 14 | if(id < size) input[id] = (rand[id] < prob) ? 0 : input[id]*scale; 15 | } 16 | 17 | void forward_dropout_layer_gpu(dropout_layer layer, network net) 18 | { 19 | if (!net.train) return; 20 | int size = layer.inputs*layer.batch; 21 | cuda_random(layer.rand_gpu, size); 22 | /* 23 | int i; 24 | for(i = 0; i < size; ++i){ 25 | layer.rand[i] = rand_uniform(); 26 | } 27 | cuda_push_array(layer.rand_gpu, layer.rand, size); 28 | */ 29 | 30 | yoloswag420blazeit360noscope<<>>(net.input_gpu, size, layer.rand_gpu, layer.probability, layer.scale); 31 | check_error(cudaPeekAtLastError()); 32 | } 33 | 34 | void backward_dropout_layer_gpu(dropout_layer layer, network net) 35 | { 36 | if(!net.delta_gpu) return; 37 | int size = layer.inputs*layer.batch; 38 | 39 | yoloswag420blazeit360noscope<<>>(net.delta_gpu, size, layer.rand_gpu, layer.probability, layer.scale); 40 | check_error(cudaPeekAtLastError()); 41 | } 42 | -------------------------------------------------------------------------------- /darknet_ros/darknet/src/gemm.h: -------------------------------------------------------------------------------- 1 | #ifndef GEMM_H 2 | #define GEMM_H 3 | 4 | void gemm_bin(int M, int N, int K, float ALPHA, 5 | char *A, int lda, 6 | float *B, int ldb, 7 | float *C, int ldc); 8 | 9 | void gemm(int TA, int TB, int M, int N, int K, float ALPHA, 10 | float *A, int lda, 11 | float *B, int ldb, 12 | float BETA, 13 | float *C, int ldc); 14 | 15 | void gemm_cpu(int TA, int TB, int M, int N, int K, float ALPHA, 16 | float *A, int lda, 17 | float *B, int ldb, 18 | float BETA, 19 | float *C, int ldc); 20 | 21 | #ifdef GPU 22 | void gemm_gpu(int TA, int TB, int M, int N, int K, float ALPHA, 23 | float *A_gpu, int lda, 24 | float *B_gpu, int ldb, 25 | float BETA, 26 | float *C_gpu, int ldc); 27 | 28 | void gemm_gpu(int TA, int TB, int M, int N, int K, float ALPHA, 29 | float *A, int lda, 30 | float *B, int ldb, 31 | float BETA, 32 | float *C, int ldc); 33 | #endif 34 | #endif 35 | -------------------------------------------------------------------------------- /darknet_ros/darknet/src/gru_layer.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef GRU_LAYER_H 3 | #define GRU_LAYER_H 4 | 5 | #include "activations.h" 6 | #include "layer.h" 7 | #include "network.h" 8 | 9 | layer make_gru_layer(int batch, int inputs, int outputs, int steps, int batch_normalize, int adam); 10 | 11 | void forward_gru_layer(layer l, network state); 12 | void backward_gru_layer(layer l, network state); 13 | void update_gru_layer(layer l, update_args a); 14 | 15 | #ifdef GPU 16 | void forward_gru_layer_gpu(layer l, network state); 17 | void backward_gru_layer_gpu(layer l, network state); 18 | void update_gru_layer_gpu(layer l, update_args a); 19 | void push_gru_layer(layer l); 20 | void pull_gru_layer(layer l); 21 | #endif 22 | 23 | #endif 24 | 25 | -------------------------------------------------------------------------------- /darknet_ros/darknet/src/im2col.c: -------------------------------------------------------------------------------- 1 | #include "im2col.h" 2 | #include 3 | float im2col_get_pixel(float *im, int height, int width, int channels, 4 | int row, int col, int channel, int pad) 5 | { 6 | row -= pad; 7 | col -= pad; 8 | 9 | if (row < 0 || col < 0 || 10 | row >= height || col >= width) return 0; 11 | return im[col + width*(row + height*channel)]; 12 | } 13 | 14 | //From Berkeley Vision's Caffe! 15 | //https://github.com/BVLC/caffe/blob/master/LICENSE 16 | void im2col_cpu(float* data_im, 17 | int channels, int height, int width, 18 | int ksize, int stride, int pad, float* data_col) 19 | { 20 | int c,h,w; 21 | int height_col = (height + 2*pad - ksize) / stride + 1; 22 | int width_col = (width + 2*pad - ksize) / stride + 1; 23 | 24 | int channels_col = channels * ksize * ksize; 25 | for (c = 0; c < channels_col; ++c) { 26 | int w_offset = c % ksize; 27 | int h_offset = (c / ksize) % ksize; 28 | int c_im = c / ksize / ksize; 29 | for (h = 0; h < height_col; ++h) { 30 | for (w = 0; w < width_col; ++w) { 31 | int im_row = h_offset + h * stride; 32 | int im_col = w_offset + w * stride; 33 | int col_index = (c * height_col + h) * width_col + w; 34 | data_col[col_index] = im2col_get_pixel(data_im, height, width, channels, 35 | im_row, im_col, c_im, pad); 36 | } 37 | } 38 | } 39 | } 40 | 41 | -------------------------------------------------------------------------------- /darknet_ros/darknet/src/im2col.h: -------------------------------------------------------------------------------- 1 | #ifndef IM2COL_H 2 | #define IM2COL_H 3 | 4 | void im2col_cpu(float* data_im, 5 | int channels, int height, int width, 6 | int ksize, int stride, int pad, float* data_col); 7 | 8 | #ifdef GPU 9 | 10 | void im2col_gpu(float *im, 11 | int channels, int height, int width, 12 | int ksize, int stride, int pad,float *data_col); 13 | 14 | #endif 15 | #endif 16 | -------------------------------------------------------------------------------- /darknet_ros/darknet/src/l2norm_layer.h: -------------------------------------------------------------------------------- 1 | #ifndef L2NORM_LAYER_H 2 | #define L2NORM_LAYER_H 3 | #include "layer.h" 4 | #include "network.h" 5 | 6 | layer make_l2norm_layer(int batch, int inputs); 7 | void forward_l2norm_layer(const layer l, network net); 8 | void backward_l2norm_layer(const layer l, network net); 9 | 10 | #ifdef GPU 11 | void forward_l2norm_layer_gpu(const layer l, network net); 12 | void backward_l2norm_layer_gpu(const layer l, network net); 13 | #endif 14 | 15 | #endif 16 | -------------------------------------------------------------------------------- /darknet_ros/darknet/src/layer.h: -------------------------------------------------------------------------------- 1 | #include "darknet.h" 2 | -------------------------------------------------------------------------------- /darknet_ros/darknet/src/list.h: -------------------------------------------------------------------------------- 1 | #ifndef LIST_H 2 | #define LIST_H 3 | #include "darknet.h" 4 | 5 | list *make_list(); 6 | int list_find(list *l, void *val); 7 | 8 | void list_insert(list *, void *); 9 | 10 | 11 | void free_list_contents(list *l); 12 | 13 | #endif 14 | -------------------------------------------------------------------------------- /darknet_ros/darknet/src/local_layer.h: -------------------------------------------------------------------------------- 1 | #ifndef LOCAL_LAYER_H 2 | #define LOCAL_LAYER_H 3 | 4 | #include "cuda.h" 5 | #include "image.h" 6 | #include "activations.h" 7 | #include "layer.h" 8 | #include "network.h" 9 | 10 | typedef layer local_layer; 11 | 12 | #ifdef GPU 13 | void forward_local_layer_gpu(local_layer layer, network net); 14 | void backward_local_layer_gpu(local_layer layer, network net); 15 | void update_local_layer_gpu(local_layer layer, update_args a); 16 | 17 | void push_local_layer(local_layer layer); 18 | void pull_local_layer(local_layer layer); 19 | #endif 20 | 21 | local_layer make_local_layer(int batch, int h, int w, int c, int n, int size, int stride, int pad, ACTIVATION activation); 22 | 23 | void forward_local_layer(const local_layer layer, network net); 24 | void backward_local_layer(local_layer layer, network net); 25 | void update_local_layer(local_layer layer, update_args a); 26 | 27 | void bias_output(float *output, float *biases, int batch, int n, int size); 28 | void backward_bias(float *bias_updates, float *delta, int batch, int n, int size); 29 | 30 | #endif 31 | 32 | -------------------------------------------------------------------------------- /darknet_ros/darknet/src/logistic_layer.h: -------------------------------------------------------------------------------- 1 | #ifndef LOGISTIC_LAYER_H 2 | #define LOGISTIC_LAYER_H 3 | #include "layer.h" 4 | #include "network.h" 5 | 6 | layer make_logistic_layer(int batch, int inputs); 7 | void forward_logistic_layer(const layer l, network net); 8 | void backward_logistic_layer(const layer l, network net); 9 | 10 | #ifdef GPU 11 | void forward_logistic_layer_gpu(const layer l, network net); 12 | void backward_logistic_layer_gpu(const layer l, network net); 13 | #endif 14 | 15 | #endif 16 | -------------------------------------------------------------------------------- /darknet_ros/darknet/src/lstm_layer.h: -------------------------------------------------------------------------------- 1 | #ifndef LSTM_LAYER_H 2 | #define LSTM_LAYER_H 3 | 4 | #include "activations.h" 5 | #include "layer.h" 6 | #include "network.h" 7 | #define USET 8 | 9 | layer make_lstm_layer(int batch, int inputs, int outputs, int steps, int batch_normalize, int adam); 10 | 11 | void forward_lstm_layer(layer l, network net); 12 | void update_lstm_layer(layer l, update_args a); 13 | 14 | #ifdef GPU 15 | void forward_lstm_layer_gpu(layer l, network net); 16 | void backward_lstm_layer_gpu(layer l, network net); 17 | void update_lstm_layer_gpu(layer l, update_args a); 18 | 19 | #endif 20 | #endif 21 | -------------------------------------------------------------------------------- /darknet_ros/darknet/src/matrix.h: -------------------------------------------------------------------------------- 1 | #ifndef MATRIX_H 2 | #define MATRIX_H 3 | #include "darknet.h" 4 | 5 | matrix copy_matrix(matrix m); 6 | void print_matrix(matrix m); 7 | 8 | matrix hold_out_matrix(matrix *m, int n); 9 | matrix resize_matrix(matrix m, int size); 10 | 11 | float *pop_column(matrix *m, int c); 12 | 13 | #endif 14 | -------------------------------------------------------------------------------- /darknet_ros/darknet/src/maxpool_layer.h: -------------------------------------------------------------------------------- 1 | #ifndef MAXPOOL_LAYER_H 2 | #define MAXPOOL_LAYER_H 3 | 4 | #include "image.h" 5 | #include "cuda.h" 6 | #include "layer.h" 7 | #include "network.h" 8 | 9 | typedef layer maxpool_layer; 10 | 11 | image get_maxpool_image(maxpool_layer l); 12 | maxpool_layer make_maxpool_layer(int batch, int h, int w, int c, int size, int stride, int padding); 13 | void resize_maxpool_layer(maxpool_layer *l, int w, int h); 14 | void forward_maxpool_layer(const maxpool_layer l, network net); 15 | void backward_maxpool_layer(const maxpool_layer l, network net); 16 | 17 | #ifdef GPU 18 | void forward_maxpool_layer_gpu(maxpool_layer l, network net); 19 | void backward_maxpool_layer_gpu(maxpool_layer l, network net); 20 | #endif 21 | 22 | #endif 23 | 24 | -------------------------------------------------------------------------------- /darknet_ros/darknet/src/network.h: -------------------------------------------------------------------------------- 1 | // Oh boy, why am I about to do this.... 2 | #ifndef NETWORK_H 3 | #define NETWORK_H 4 | #include "darknet.h" 5 | 6 | #include "image.h" 7 | #include "layer.h" 8 | #include "data.h" 9 | #include "tree.h" 10 | 11 | 12 | #ifdef GPU 13 | void pull_network_output(network *net); 14 | #endif 15 | 16 | void compare_networks(network *n1, network *n2, data d); 17 | char *get_layer_string(LAYER_TYPE a); 18 | 19 | network *make_network(int n); 20 | 21 | 22 | float network_accuracy_multi(network *net, data d, int n); 23 | int get_predicted_class_network(network *net); 24 | void print_network(network *net); 25 | int resize_network(network *net, int w, int h); 26 | void calc_network_cost(network *net); 27 | 28 | #endif 29 | 30 | -------------------------------------------------------------------------------- /darknet_ros/darknet/src/normalization_layer.h: -------------------------------------------------------------------------------- 1 | #ifndef NORMALIZATION_LAYER_H 2 | #define NORMALIZATION_LAYER_H 3 | 4 | #include "image.h" 5 | #include "layer.h" 6 | #include "network.h" 7 | 8 | layer make_normalization_layer(int batch, int w, int h, int c, int size, float alpha, float beta, float kappa); 9 | void resize_normalization_layer(layer *layer, int h, int w); 10 | void forward_normalization_layer(const layer layer, network net); 11 | void backward_normalization_layer(const layer layer, network net); 12 | void visualize_normalization_layer(layer layer, char *window); 13 | 14 | #ifdef GPU 15 | void forward_normalization_layer_gpu(const layer layer, network net); 16 | void backward_normalization_layer_gpu(const layer layer, network net); 17 | #endif 18 | 19 | #endif 20 | -------------------------------------------------------------------------------- /darknet_ros/darknet/src/option_list.h: -------------------------------------------------------------------------------- 1 | #ifndef OPTION_LIST_H 2 | #define OPTION_LIST_H 3 | #include "list.h" 4 | 5 | typedef struct{ 6 | char *key; 7 | char *val; 8 | int used; 9 | } kvp; 10 | 11 | 12 | int read_option(char *s, list *options); 13 | void option_insert(list *l, char *key, char *val); 14 | char *option_find(list *l, char *key); 15 | float option_find_float(list *l, char *key, float def); 16 | float option_find_float_quiet(list *l, char *key, float def); 17 | void option_unused(list *l); 18 | 19 | #endif 20 | -------------------------------------------------------------------------------- /darknet_ros/darknet/src/parser.h: -------------------------------------------------------------------------------- 1 | #ifndef PARSER_H 2 | #define PARSER_H 3 | #include "darknet.h" 4 | #include "network.h" 5 | 6 | void save_network(network net, char *filename); 7 | void save_weights_double(network net, char *filename); 8 | 9 | #endif 10 | -------------------------------------------------------------------------------- /darknet_ros/darknet/src/region_layer.h: -------------------------------------------------------------------------------- 1 | #ifndef REGION_LAYER_H 2 | #define REGION_LAYER_H 3 | 4 | #include "darknet.h" 5 | #include "layer.h" 6 | #include "network.h" 7 | 8 | layer make_region_layer(int batch, int w, int h, int n, int classes, int coords); 9 | void forward_region_layer(const layer l, network net); 10 | void backward_region_layer(const layer l, network net); 11 | void resize_region_layer(layer *l, int w, int h); 12 | 13 | #ifdef GPU 14 | void forward_region_layer_gpu(const layer l, network net); 15 | void backward_region_layer_gpu(layer l, network net); 16 | #endif 17 | 18 | #endif 19 | -------------------------------------------------------------------------------- /darknet_ros/darknet/src/reorg_layer.h: -------------------------------------------------------------------------------- 1 | #ifndef REORG_LAYER_H 2 | #define REORG_LAYER_H 3 | 4 | #include "image.h" 5 | #include "cuda.h" 6 | #include "layer.h" 7 | #include "network.h" 8 | 9 | layer make_reorg_layer(int batch, int w, int h, int c, int stride, int reverse, int flatten, int extra); 10 | void resize_reorg_layer(layer *l, int w, int h); 11 | void forward_reorg_layer(const layer l, network net); 12 | void backward_reorg_layer(const layer l, network net); 13 | 14 | #ifdef GPU 15 | void forward_reorg_layer_gpu(layer l, network net); 16 | void backward_reorg_layer_gpu(layer l, network net); 17 | #endif 18 | 19 | #endif 20 | 21 | -------------------------------------------------------------------------------- /darknet_ros/darknet/src/rnn_layer.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef RNN_LAYER_H 3 | #define RNN_LAYER_H 4 | 5 | #include "activations.h" 6 | #include "layer.h" 7 | #include "network.h" 8 | #define USET 9 | 10 | layer make_rnn_layer(int batch, int inputs, int outputs, int steps, ACTIVATION activation, int batch_normalize, int adam); 11 | 12 | void forward_rnn_layer(layer l, network net); 13 | void backward_rnn_layer(layer l, network net); 14 | void update_rnn_layer(layer l, update_args a); 15 | 16 | #ifdef GPU 17 | void forward_rnn_layer_gpu(layer l, network net); 18 | void backward_rnn_layer_gpu(layer l, network net); 19 | void update_rnn_layer_gpu(layer l, update_args a); 20 | void push_rnn_layer(layer l); 21 | void pull_rnn_layer(layer l); 22 | #endif 23 | 24 | #endif 25 | 26 | -------------------------------------------------------------------------------- /darknet_ros/darknet/src/route_layer.h: -------------------------------------------------------------------------------- 1 | #ifndef ROUTE_LAYER_H 2 | #define ROUTE_LAYER_H 3 | #include "network.h" 4 | #include "layer.h" 5 | 6 | typedef layer route_layer; 7 | 8 | route_layer make_route_layer(int batch, int n, int *input_layers, int *input_size); 9 | void forward_route_layer(const route_layer l, network net); 10 | void backward_route_layer(const route_layer l, network net); 11 | void resize_route_layer(route_layer *l, network *net); 12 | 13 | #ifdef GPU 14 | void forward_route_layer_gpu(const route_layer l, network net); 15 | void backward_route_layer_gpu(const route_layer l, network net); 16 | #endif 17 | 18 | #endif 19 | -------------------------------------------------------------------------------- /darknet_ros/darknet/src/shortcut_layer.h: -------------------------------------------------------------------------------- 1 | #ifndef SHORTCUT_LAYER_H 2 | #define SHORTCUT_LAYER_H 3 | 4 | #include "layer.h" 5 | #include "network.h" 6 | 7 | layer make_shortcut_layer(int batch, int index, int w, int h, int c, int w2, int h2, int c2); 8 | void forward_shortcut_layer(const layer l, network net); 9 | void backward_shortcut_layer(const layer l, network net); 10 | void resize_shortcut_layer(layer *l, int w, int h); 11 | 12 | #ifdef GPU 13 | void forward_shortcut_layer_gpu(const layer l, network net); 14 | void backward_shortcut_layer_gpu(const layer l, network net); 15 | #endif 16 | 17 | #endif 18 | -------------------------------------------------------------------------------- /darknet_ros/darknet/src/softmax_layer.h: -------------------------------------------------------------------------------- 1 | #ifndef SOFTMAX_LAYER_H 2 | #define SOFTMAX_LAYER_H 3 | #include "layer.h" 4 | #include "network.h" 5 | 6 | typedef layer softmax_layer; 7 | 8 | void softmax_array(float *input, int n, float temp, float *output); 9 | softmax_layer make_softmax_layer(int batch, int inputs, int groups); 10 | void forward_softmax_layer(const softmax_layer l, network net); 11 | void backward_softmax_layer(const softmax_layer l, network net); 12 | 13 | #ifdef GPU 14 | void pull_softmax_layer_output(const softmax_layer l); 15 | void forward_softmax_layer_gpu(const softmax_layer l, network net); 16 | void backward_softmax_layer_gpu(const softmax_layer l, network net); 17 | #endif 18 | 19 | #endif 20 | -------------------------------------------------------------------------------- /darknet_ros/darknet/src/tree.h: -------------------------------------------------------------------------------- 1 | #ifndef TREE_H 2 | #define TREE_H 3 | #include "darknet.h" 4 | 5 | int hierarchy_top_prediction(float *predictions, tree *hier, float thresh, int stride); 6 | float get_hierarchy_probability(float *x, tree *hier, int c, int stride); 7 | 8 | #endif 9 | -------------------------------------------------------------------------------- /darknet_ros/darknet/src/upsample_layer.h: -------------------------------------------------------------------------------- 1 | #ifndef UPSAMPLE_LAYER_H 2 | #define UPSAMPLE_LAYER_H 3 | #include "darknet.h" 4 | 5 | layer make_upsample_layer(int batch, int w, int h, int c, int stride); 6 | void forward_upsample_layer(const layer l, network net); 7 | void backward_upsample_layer(const layer l, network net); 8 | void resize_upsample_layer(layer *l, int w, int h); 9 | 10 | #ifdef GPU 11 | void forward_upsample_layer_gpu(const layer l, network net); 12 | void backward_upsample_layer_gpu(const layer l, network net); 13 | #endif 14 | 15 | #endif 16 | -------------------------------------------------------------------------------- /darknet_ros/darknet/src/yolo_layer.h: -------------------------------------------------------------------------------- 1 | #ifndef YOLO_LAYER_H 2 | #define YOLO_LAYER_H 3 | 4 | #include "darknet.h" 5 | #include "layer.h" 6 | #include "network.h" 7 | 8 | layer make_yolo_layer(int batch, int w, int h, int n, int total, int *mask, int classes); 9 | void forward_yolo_layer(const layer l, network net); 10 | void backward_yolo_layer(const layer l, network net); 11 | void resize_yolo_layer(layer *l, int w, int h); 12 | int yolo_num_detections(layer l, float thresh); 13 | 14 | #ifdef GPU 15 | void forward_yolo_layer_gpu(const layer l, network net); 16 | void backward_yolo_layer_gpu(layer l, network net); 17 | #endif 18 | 19 | #endif 20 | -------------------------------------------------------------------------------- /darknet_ros/darknet_ros/config/ros.yaml: -------------------------------------------------------------------------------- 1 | subscribers: 2 | 3 | camera_reading: 4 | topic: /usb_cam/image_raw 5 | queue_size: 1 6 | 7 | # default topic /camera/rgb/image_raw, /usb_cam/image_raw,/detector_img 8 | 9 | 10 | actions: 11 | 12 | camera_reading: 13 | name: /darknet_ros/check_for_objects 14 | 15 | publishers: 16 | 17 | object_detector: 18 | topic: /darknet_ros/found_object 19 | queue_size: 1 20 | latch: false 21 | 22 | bounding_boxes: 23 | topic: /darknet_ros/bounding_boxes 24 | queue_size: 1 25 | latch: false 26 | 27 | detection_image: 28 | topic: /darknet_ros/detection_image 29 | queue_size: 1 30 | latch: true 31 | 32 | image_view: 33 | 34 | enable_opencv: true 35 | wait_key_delay: 1 36 | enable_console_output: true 37 | -------------------------------------------------------------------------------- /darknet_ros/darknet_ros/config/yolov2-tiny-voc.yaml: -------------------------------------------------------------------------------- 1 | yolo_model: 2 | 3 | config_file: 4 | name: yolov2-tiny-voc.cfg 5 | weight_file: 6 | name: yolov2-tiny-voc.weights 7 | threshold: 8 | value: 0.3 9 | detection_classes: 10 | names: 11 | - aeroplane 12 | - bicycle 13 | - bird 14 | - boat 15 | - bottle 16 | - bus 17 | - car 18 | - cat 19 | - chair 20 | - cow 21 | - diningtable 22 | - dog 23 | - horse 24 | - motorbike 25 | - person 26 | - pottedplant 27 | - sheep 28 | - sofa 29 | - train 30 | - tvmonitor 31 | -------------------------------------------------------------------------------- /darknet_ros/darknet_ros/config/yolov2-voc.yaml: -------------------------------------------------------------------------------- 1 | yolo_model: 2 | 3 | config_file: 4 | name: yolov2-voc.cfg 5 | weight_file: 6 | name: yolov2-voc.weights 7 | threshold: 8 | value: 0.3 9 | detection_classes: 10 | names: 11 | - aeroplane 12 | - bicycle 13 | - bird 14 | - boat 15 | - bottle 16 | - bus 17 | - car 18 | - cat 19 | - chair 20 | - cow 21 | - diningtable 22 | - dog 23 | - horse 24 | - motorbike 25 | - person 26 | - pottedplant 27 | - sheep 28 | - sofa 29 | - train 30 | - tvmonitor 31 | -------------------------------------------------------------------------------- /darknet_ros/darknet_ros/config/yolov3-tiny.yaml: -------------------------------------------------------------------------------- 1 | yolo_model: 2 | 3 | config_file: 4 | name: yolov3-tiny.cfg 5 | weight_file: 6 | name: yolov3-tiny_final.weights 7 | threshold: 8 | value: 0.3 9 | detection_classes: 10 | names: 11 | - busstop 12 | -------------------------------------------------------------------------------- /darknet_ros/darknet_ros/config/yolov3-voc.yaml: -------------------------------------------------------------------------------- 1 | yolo_model: 2 | 3 | config_file: 4 | name: yolov3-voc.cfg 5 | weight_file: 6 | name: yolov3-voc.weights 7 | threshold: 8 | value: 0.3 9 | detection_classes: 10 | names: 11 | - aeroplane 12 | - bicycle 13 | - bird 14 | - boat 15 | - bottle 16 | - bus 17 | - car 18 | - cat 19 | - chair 20 | - cow 21 | - diningtable 22 | - dog 23 | - horse 24 | - motorbike 25 | - person 26 | - pottedplant 27 | - sheep 28 | - sofa 29 | - train 30 | - tvmonitor 31 | -------------------------------------------------------------------------------- /darknet_ros/darknet_ros/doc/quadruped_anymal_and_person.JPG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kyou2016/ROS_LaneDetectionWithBEV/fe8b086f84c3ee3b6017c5c70805886e68967e2a/darknet_ros/darknet_ros/doc/quadruped_anymal_and_person.JPG -------------------------------------------------------------------------------- /darknet_ros/darknet_ros/doc/test_detection.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kyou2016/ROS_LaneDetectionWithBEV/fe8b086f84c3ee3b6017c5c70805886e68967e2a/darknet_ros/darknet_ros/doc/test_detection.png -------------------------------------------------------------------------------- /darknet_ros/darknet_ros/doc/test_detection_anymal.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kyou2016/ROS_LaneDetectionWithBEV/fe8b086f84c3ee3b6017c5c70805886e68967e2a/darknet_ros/darknet_ros/doc/test_detection_anymal.png -------------------------------------------------------------------------------- /darknet_ros/darknet_ros/include/darknet_ros/image_interface.h: -------------------------------------------------------------------------------- 1 | /* 2 | * image_interface.h 3 | * 4 | * Created on: Dec 19, 2016 5 | * Author: Marko Bjelonic 6 | * Institute: ETH Zurich, Robotic Systems Lab 7 | */ 8 | 9 | #ifndef IMAGE_INTERFACE_H 10 | #define IMAGE_INTERFACE_H 11 | 12 | #include "image.h" 13 | 14 | static float get_pixel(image m, int x, int y, int c); 15 | image** load_alphabet_with_file(char* datafile); 16 | void generate_image(image p, IplImage* disp); 17 | 18 | #endif 19 | -------------------------------------------------------------------------------- /darknet_ros/darknet_ros/launch/darknet_ros.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 | -------------------------------------------------------------------------------- /darknet_ros/darknet_ros/launch/darknet_ros_gdb.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /darknet_ros/darknet_ros/launch/yolo_v3.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /darknet_ros/darknet_ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | darknet_ros 4 | 1.1.4 5 | Darknet is an open source neural network framework that runs on CPU and GPU. You only look once (YOLO) is a state-of-the-art, real-time object detection system. 6 | Marko Bjelonic 7 | BSD 8 | https://github.com/leggedrobotics/darknet_ros 9 | Marko Bjelonic 10 | 11 | catkin 12 | boost 13 | libopencv-dev 14 | libx11 15 | libxt-dev 16 | libxext 17 | 18 | roscpp 19 | rospy 20 | std_msgs 21 | image_transport 22 | cv_bridge 23 | sensor_msgs 24 | message_generation 25 | darknet_ros_msgs 26 | actionlib 27 | 28 | 29 | rostest 30 | wget 31 | 32 | -------------------------------------------------------------------------------- /darknet_ros/darknet_ros/src/image_interface.c: -------------------------------------------------------------------------------- 1 | /* 2 | * image_interface.c 3 | * 4 | * Created on: Dec 19, 2016 5 | * Author: Marko Bjelonic 6 | * Institute: ETH Zurich, Robotic Systems Lab 7 | */ 8 | 9 | #include "darknet_ros/image_interface.h" 10 | 11 | static float get_pixel(image m, int x, int y, int c) { 12 | assert(x < m.w && y < m.h && c < m.c); 13 | return m.data[c * m.h * m.w + y * m.w + x]; 14 | } 15 | 16 | image** load_alphabet_with_file(char* datafile) { 17 | int i, j; 18 | const int nsize = 8; 19 | image** alphabets = calloc(nsize, sizeof(image)); 20 | char* labels = "/labels/%d_%d.png"; 21 | char* files = (char*)malloc(1 + strlen(datafile) + strlen(labels)); 22 | strcpy(files, datafile); 23 | strcat(files, labels); 24 | for (j = 0; j < nsize; ++j) { 25 | alphabets[j] = calloc(128, sizeof(image)); 26 | for (i = 32; i < 127; ++i) { 27 | char buff[256]; 28 | sprintf(buff, files, i, j); 29 | alphabets[j][i] = load_image_color(buff, 0, 0); 30 | } 31 | } 32 | return alphabets; 33 | } 34 | 35 | #ifdef OPENCV 36 | void generate_image(image p, IplImage* disp) { 37 | int x, y, k; 38 | if (p.c == 3) rgbgr_image(p); 39 | // normalize_image(copy); 40 | 41 | int step = disp->widthStep; 42 | for (y = 0; y < p.h; ++y) { 43 | for (x = 0; x < p.w; ++x) { 44 | for (k = 0; k < p.c; ++k) { 45 | disp->imageData[y * step + x * p.c + k] = (unsigned char)(get_pixel(p, x, y, k) * 255); 46 | } 47 | } 48 | } 49 | } 50 | #endif 51 | -------------------------------------------------------------------------------- /darknet_ros/darknet_ros/src/yolo_object_detector_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * yolo_obstacle_detector_node.cpp 3 | * 4 | * Created on: Dec 19, 2016 5 | * Author: Marko Bjelonic 6 | * Institute: ETH Zurich, Robotic Systems Lab 7 | */ 8 | 9 | #include 10 | #include 11 | 12 | int main(int argc, char** argv) { 13 | ros::init(argc, argv, "darknet_ros"); 14 | ros::NodeHandle nodeHandle("~"); 15 | darknet_ros::YoloObjectDetector yoloObjectDetector(nodeHandle); 16 | 17 | ros::spin(); 18 | return 0; 19 | } 20 | -------------------------------------------------------------------------------- /darknet_ros/darknet_ros/test/object_detection.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /darknet_ros/darknet_ros/test/test_main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | // ROS 4 | #include 5 | 6 | int main(int argc, char** argv) { 7 | ros::init(argc, argv, "darknet_ros_test"); 8 | testing::InitGoogleTest(&argc, argv); 9 | return RUN_ALL_TESTS(); 10 | } 11 | -------------------------------------------------------------------------------- /darknet_ros/darknet_ros/yolo_network_config/weights/.gitignore: -------------------------------------------------------------------------------- 1 | *.weights 2 | -------------------------------------------------------------------------------- /darknet_ros/darknet_ros/yolo_network_config/weights/how_to_download_weights.txt: -------------------------------------------------------------------------------- 1 | cd catkin_workspace/src/darknet_ros/darknet_ros/yolo_network_config/weights/ 2 | 3 | COCO data set (Yolo v2): 4 | wget http://pjreddie.com/media/files/yolov2.weights 5 | wget http://pjreddie.com/media/files/yolov2-tiny.weights 6 | 7 | VOC data set (Yolo v2): 8 | wget http://pjreddie.com/media/files/yolov2-voc.weights 9 | wget http://pjreddie.com/media/files/yolov2-tiny-voc.weights 10 | 11 | Yolo v3: 12 | wget http://pjreddie.com/media/files/yolov3.weights 13 | wget http://pjreddie.com/media/files/yolov3-voc.weights 14 | -------------------------------------------------------------------------------- /darknet_ros/darknet_ros_msgs/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package darknet_ros_msgs 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.1.4 (2019-03-03) 6 | ------------------ 7 | 8 | 1.1.3 (2018-04-26) 9 | ------------------ 10 | * Fixed formatting part 2. 11 | * Merge branch 'firephinx-master' 12 | * Merge branch 'master' of https://github.com/firephinx/darknet_ros into firephinx-master 13 | * Added rgb_image_header to BoundingBoxes msg. 14 | * Merge pull request `#57 `_ from leggedrobotics/devel/threads 15 | Devel/threads 16 | * Adapted package description. 17 | * Merge branch 'master' into devel/threads 18 | * Update package.xml 19 | * Contributors: Kevin Zhang, Marko Bjelonic 20 | 21 | 1.1.2 (2018-01-06) 22 | ------------------ 23 | * First release of darknet_ros_msgs. 24 | -------------------------------------------------------------------------------- /darknet_ros/darknet_ros_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.12) 2 | 3 | project(darknet_ros_msgs) 4 | 5 | set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}") 6 | 7 | find_package(catkin REQUIRED 8 | COMPONENTS 9 | actionlib_msgs 10 | geometry_msgs 11 | sensor_msgs 12 | std_msgs 13 | message_generation 14 | ) 15 | 16 | add_message_files( 17 | FILES 18 | BoundingBox.msg 19 | BoundingBoxes.msg 20 | ObjectCount.msg 21 | ) 22 | 23 | add_action_files( 24 | FILES 25 | CheckForObjects.action 26 | ) 27 | 28 | generate_messages( 29 | DEPENDENCIES 30 | actionlib_msgs 31 | geometry_msgs 32 | sensor_msgs 33 | std_msgs 34 | ) 35 | 36 | catkin_package( 37 | CATKIN_DEPENDS 38 | actionlib_msgs 39 | geometry_msgs 40 | sensor_msgs 41 | message_runtime 42 | std_msgs 43 | ) 44 | -------------------------------------------------------------------------------- /darknet_ros/darknet_ros_msgs/action/CheckForObjects.action: -------------------------------------------------------------------------------- 1 | # Check if objects in image 2 | 3 | # Goal definition 4 | int16 id 5 | sensor_msgs/Image image 6 | 7 | --- 8 | # Result definition 9 | int16 id 10 | darknet_ros_msgs/BoundingBoxes bounding_boxes 11 | 12 | --- 13 | # Feedback definition 14 | -------------------------------------------------------------------------------- /darknet_ros/darknet_ros_msgs/msg/BoundingBox.msg: -------------------------------------------------------------------------------- 1 | float64 probability 2 | int64 xmin 3 | int64 ymin 4 | int64 xmax 5 | int64 ymax 6 | int16 id 7 | string Class 8 | -------------------------------------------------------------------------------- /darknet_ros/darknet_ros_msgs/msg/BoundingBoxes.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | Header image_header 3 | BoundingBox[] bounding_boxes 4 | -------------------------------------------------------------------------------- /darknet_ros/darknet_ros_msgs/msg/ObjectCount.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | int8 count 3 | -------------------------------------------------------------------------------- /darknet_ros/darknet_ros_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | darknet_ros_msgs 4 | 1.1.4 5 | Darknet is an open source neural network framework that runs on CPU and GPU. You only look once (YOLO) is a state-of-the-art, real-time object detection system. 6 | Marko Bjelonic 7 | BSD 8 | https://github.com/leggedrobotics/darknet_ros 9 | Marko Bjelonic 10 | 11 | catkin 12 | 13 | actionlib_msgs 14 | geometry_msgs 15 | sensor_msgs 16 | message_generation 17 | std_msgs 18 | 19 | actionlib_msgs 20 | geometry_msgs 21 | sensor_msgs 22 | message_runtime 23 | std_msgs 24 | 25 | -------------------------------------------------------------------------------- /darknet_ros/jenkins-pipeline: -------------------------------------------------------------------------------- 1 | library 'continuous_integration_pipeline' 2 | ciPipeline("") 3 | -------------------------------------------------------------------------------- /hector_slam/README.txt: -------------------------------------------------------------------------------- 1 | # hector_slam 2 | 3 | See the ROS Wiki for documentation: http://wiki.ros.org/hector_slam 4 | -------------------------------------------------------------------------------- /hector_slam/hector_compressed_map_transport/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package hector_compressed_map_transport 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.3.5 (2016-06-24) 6 | ------------------ 7 | * Use the FindEigen3.cmake module provided by Eigen 8 | * Contributors: Johannes Meyer 9 | 10 | 0.3.4 (2015-11-07) 11 | ------------------ 12 | 13 | 0.3.3 (2014-06-15) 14 | ------------------ 15 | * fixed cmake find for eigen in indigo 16 | * fixed libopencv-dev rosdep key 17 | * Contributors: Alexander Stumpf, Johannes Meyer 18 | 19 | 0.3.2 (2014-03-30) 20 | ------------------ 21 | 22 | 0.3.1 (2013-10-09) 23 | ------------------ 24 | * added changelogs 25 | 26 | 0.3.0 (2013-08-08) 27 | ------------------ 28 | * catkinized hector_slam 29 | -------------------------------------------------------------------------------- /hector_slam/hector_geotiff/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package hector_geotiff 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.3.5 (2016-06-24) 6 | ------------------ 7 | * Use the FindEigen3.cmake module provided by Eigen 8 | * hector_geotiff/hector_geotiff_plugins: added possibility to specify Color of robot path in the geotiff file in order to allow multiple color robot paths 9 | * Contributors: Dorothea Koert, Johannes Meyer 10 | 11 | 0.3.4 (2015-11-07) 12 | ------------------ 13 | * Removes trailing spaces and fixes indents 14 | * Contributors: YuK_Ota 15 | 16 | 0.3.3 (2014-06-15) 17 | ------------------ 18 | * fixed cmake find for eigen in indigo 19 | * added launchfile to restart geotiff node 20 | * Contributors: Alexander Stumpf 21 | 22 | 0.3.2 (2014-03-30) 23 | ------------------ 24 | * Add TrajectoryMapWriter to geotiff_mapper.launch per default 25 | * Add arguments to launch files for specifying geotiff file path 26 | * Contributors: Stefan Kohlbrecher 27 | 28 | 0.3.1 (2013-10-09) 29 | ------------------ 30 | * added missing install rule for launch files 31 | * moved header files from include/geotiff_writer to include/hector_geotiff 32 | * fixed warnings for deprecated pluginlib method/macro calls 33 | * added changelogs 34 | 35 | 0.3.0 (2013-08-08) 36 | ------------------ 37 | * catkinized hector_slam 38 | -------------------------------------------------------------------------------- /hector_slam/hector_geotiff/launch/geotiff_mapper_only.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /hector_slam/hector_geotiff_plugins/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package hector_geotiff_plugins 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.3.5 (2016-06-24) 6 | ------------------ 7 | * hector_geotiff/hector_geotiff_plugins: added possibility to specify Color of robot path in the geotiff file in order to allow multiple color robot paths 8 | * Contributors: Dorothea Koert 9 | 10 | 0.3.4 (2015-11-07) 11 | ------------------ 12 | 13 | 0.3.3 (2014-06-15) 14 | ------------------ 15 | 16 | 0.3.2 (2014-03-30) 17 | ------------------ 18 | 19 | 0.3.1 (2013-10-09) 20 | ------------------ 21 | * readded PLUGINLIB_DECLARE_CLASS macro for fuerte compatibility 22 | * use hector_geotiff_plugins/TrajectoryMapWriter as legacy lookup name for the trajectory geotiff plugin to not break old launch files 23 | * fixed warnings for deprecated pluginlib method/macro calls 24 | * added changelogs 25 | * added cmake dependencies to catkin exported targets to force message targets to be built before any cpp target using them 26 | 27 | 0.3.0 (2013-08-08) 28 | ------------------ 29 | * catkinized hector_slam 30 | -------------------------------------------------------------------------------- /hector_slam/hector_geotiff_plugins/hector_geotiff_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | This is a MapWriter plugin that draws the robot's trajectory in the map. 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /hector_slam/hector_imu_attitude_to_tf/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package hector_imu_attitude_to_tf 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.3.5 (2016-06-24) 6 | ------------------ 7 | 8 | 0.3.4 (2015-11-07) 9 | ------------------ 10 | * hector_imu_attitude_to_tf: fixed default values of the base_frame and base_stabilized_frame parameters (fix #20) 11 | * Contributors: Johannes Meyer 12 | 13 | 0.3.3 (2014-06-15) 14 | ------------------ 15 | 16 | 0.3.2 (2014-03-30) 17 | ------------------ 18 | 19 | 0.3.1 (2013-10-09) 20 | ------------------ 21 | * added missing install rule for launch files 22 | * added changelogs 23 | 24 | 0.3.0 (2013-08-08) 25 | ------------------ 26 | * catkinized hector_slam 27 | -------------------------------------------------------------------------------- /hector_slam/hector_imu_attitude_to_tf/launch/example.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /hector_slam/hector_imu_tools/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package hector_imu_tools 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.3.5 (2016-06-24) 6 | ------------------ 7 | 8 | 0.3.4 (2015-11-07) 9 | ------------------ 10 | * Fix sim setup 11 | * remap for bertl setup 12 | * Contributors: Florian Kunz, kohlbrecher 13 | 14 | 0.3.3 (2014-06-15) 15 | ------------------ 16 | * hector_imu_tools: Basics of simple height etimation 17 | * hector_imu_tools: Add tf publishers in hector_imu_tools 18 | * hector_imu_tools: Also write out fake odometry 19 | * hector_imu_tools: Fix typo 20 | * hector_imu_tools: Prevent race conditions in slam, formatting 21 | * hector_imu_tools: Small executable for generating a IMU message out of a (2d) pose and roll/pitch imu message 22 | * Contributors: Stefan Kohlbrecher 23 | -------------------------------------------------------------------------------- /hector_slam/hector_imu_tools/launch/bertl_robot.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /hector_slam/hector_imu_tools/launch/jasmine_robot.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /hector_slam/hector_map_server/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package hector_map_server 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.3.5 (2016-06-24) 6 | ------------------ 7 | 8 | 0.3.4 (2015-11-07) 9 | ------------------ 10 | 11 | 0.3.3 (2014-06-15) 12 | ------------------ 13 | 14 | 0.3.2 (2014-03-30) 15 | ------------------ 16 | 17 | 0.3.1 (2013-10-09) 18 | ------------------ 19 | * added changelogs 20 | * added cmake dependencies to catkin exported targets to force message targets to be built before any cpp target using them 21 | 22 | 0.3.0 (2013-08-08) 23 | ------------------ 24 | * catkinized hector_slam 25 | -------------------------------------------------------------------------------- /hector_slam/hector_map_tools/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package hector_map_tools 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.3.5 (2016-06-24) 6 | ------------------ 7 | 8 | 0.3.4 (2015-11-07) 9 | ------------------ 10 | * -Fix severe bug when not using square size maps (would results in completely wrong obstacle distances and coordinates) 11 | * Contributors: Stefan Kohlbrecher 12 | 13 | 0.3.3 (2014-06-15) 14 | ------------------ 15 | 16 | 0.3.2 (2014-03-30) 17 | ------------------ 18 | 19 | 0.3.1 (2013-10-09) 20 | ------------------ 21 | * added changelogs 22 | 23 | 0.3.0 (2013-08-08) 24 | ------------------ 25 | * catkinized hector_slam 26 | -------------------------------------------------------------------------------- /hector_slam/hector_mapping/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package hector_mapping 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.3.5 (2016-06-24) 6 | ------------------ 7 | * Use the FindEigen3.cmake module provided by Eigen 8 | * Contributors: Johannes Meyer 9 | 10 | 0.3.4 (2015-11-07) 11 | ------------------ 12 | 13 | 0.3.3 (2014-06-15) 14 | ------------------ 15 | 16 | 0.3.2 (2014-03-30) 17 | ------------------ 18 | 19 | 0.3.1 (2013-10-09) 20 | ------------------ 21 | * respect ``p_map_frame_`` 22 | * added changelogs 23 | 24 | 0.3.0 (2013-08-08) 25 | ------------------ 26 | * catkinized hector_slam 27 | * hector_mapping: fixed multi-resolution map scan matching index bug in MapRepMultiMap.h 28 | -------------------------------------------------------------------------------- /hector_slam/hector_mapping/msg/HectorDebugInfo.msg: -------------------------------------------------------------------------------- 1 | HectorIterData[] iterData -------------------------------------------------------------------------------- /hector_slam/hector_mapping/msg/HectorIterData.msg: -------------------------------------------------------------------------------- 1 | float64[9] hessian 2 | float64 conditionNum 3 | float64 determinant 4 | float64 conditionNum2d 5 | float64 determinant2d 6 | -------------------------------------------------------------------------------- /hector_slam/hector_mapping/src/HectorMapMutex.h: -------------------------------------------------------------------------------- 1 | #ifndef hectormapmutex_h__ 2 | #define hectormapmutex_h__ 3 | 4 | #include "util/MapLockerInterface.h" 5 | 6 | #include 7 | 8 | class HectorMapMutex : public MapLockerInterface 9 | { 10 | public: 11 | virtual void lockMap() 12 | { 13 | mapModifyMutex_.lock(); 14 | } 15 | 16 | virtual void unlockMap() 17 | { 18 | mapModifyMutex_.unlock(); 19 | } 20 | 21 | boost::mutex mapModifyMutex_; 22 | }; 23 | 24 | #endif 25 | -------------------------------------------------------------------------------- /hector_slam/hector_marker_drawing/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package hector_marker_drawing 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.3.5 (2016-06-24) 6 | ------------------ 7 | * Use the FindEigen3.cmake module provided by Eigen 8 | * Contributors: Johannes Meyer 9 | 10 | 0.3.4 (2015-11-07) 11 | ------------------ 12 | 13 | 0.3.3 (2014-06-15) 14 | ------------------ 15 | * fixed cmake find for eigen in indigo 16 | * Contributors: Alexander Stumpf 17 | 18 | 0.3.2 (2014-03-30) 19 | ------------------ 20 | 21 | 0.3.1 (2013-10-09) 22 | ------------------ 23 | * added changelogs 24 | 25 | 0.3.0 (2013-08-08) 26 | ------------------ 27 | * catkinized hector_slam 28 | -------------------------------------------------------------------------------- /hector_slam/hector_nav_msgs/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package hector_nav_msgs 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.3.5 (2016-06-24) 6 | ------------------ 7 | 8 | 0.3.4 (2015-11-07) 9 | ------------------ 10 | * hector_nav_msgs: removed yaw member from GetNormal response 11 | yaw is implicitly given by the normal vector 12 | * Contributors: Dorothea Koert 13 | 14 | 0.3.3 (2014-06-15) 15 | ------------------ 16 | * added GetNormal service, that returns normal and orientation of an occupied cell 17 | * Contributors: Dorothea Koert 18 | 19 | 0.3.2 (2014-03-30) 20 | ------------------ 21 | 22 | 0.3.1 (2013-10-09) 23 | ------------------ 24 | * added changelogs 25 | 26 | 0.3.0 (2013-08-08) 27 | ------------------ 28 | * catkinized hector_slam 29 | -------------------------------------------------------------------------------- /hector_slam/hector_nav_msgs/srv/GetDistanceToObstacle.srv: -------------------------------------------------------------------------------- 1 | # Returns the distance to the next obstacle from the origin of frame point.header.frame_id 2 | # in the direction of the point 3 | # 4 | # All units are meters. 5 | 6 | geometry_msgs/PointStamped point 7 | --- 8 | float32 distance 9 | geometry_msgs/PointStamped end_point 10 | 11 | 12 | -------------------------------------------------------------------------------- /hector_slam/hector_nav_msgs/srv/GetNormal.srv: -------------------------------------------------------------------------------- 1 | geometry_msgs/PointStamped point 2 | --- 3 | geometry_msgs/Vector3 normal 4 | -------------------------------------------------------------------------------- /hector_slam/hector_nav_msgs/srv/GetRecoveryInfo.srv: -------------------------------------------------------------------------------- 1 | # Returns the path travelled to get to req_pose (pose determined by request_time) 2 | # up to request_radius away from req_pose. 3 | # 4 | 5 | time request_time 6 | float64 request_radius 7 | --- 8 | nav_msgs/Path trajectory_radius_entry_pose_to_req_pose 9 | geometry_msgs/PoseStamped radius_entry_pose 10 | geometry_msgs/PoseStamped req_pose 11 | 12 | -------------------------------------------------------------------------------- /hector_slam/hector_nav_msgs/srv/GetRobotTrajectory.srv: -------------------------------------------------------------------------------- 1 | # Returns the distance to the next obstacle from the origin of frame point.header.frame_id 2 | # in the direction of the point 3 | # 4 | # All units are meters. 5 | 6 | --- 7 | nav_msgs/Path trajectory 8 | 9 | -------------------------------------------------------------------------------- /hector_slam/hector_nav_msgs/srv/GetSearchPosition.srv: -------------------------------------------------------------------------------- 1 | #Returns a suggested search/observation position for an object of interest located at ooi_pose 2 | 3 | geometry_msgs/PoseStamped ooi_pose 4 | float32 distance 5 | --- 6 | geometry_msgs/PoseStamped search_pose 7 | 8 | -------------------------------------------------------------------------------- /hector_slam/hector_slam/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package hector_slam 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.3.5 (2016-06-24) 6 | ------------------ 7 | 8 | 0.3.4 (2015-11-07) 9 | ------------------ 10 | 11 | 0.3.3 (2014-06-15) 12 | ------------------ 13 | 14 | 0.3.2 (2014-03-30) 15 | ------------------ 16 | 17 | 0.3.1 (2013-10-09) 18 | ------------------ 19 | * added changelogs 20 | 21 | 0.3.0 (2013-08-08) 22 | ------------------ 23 | * catkinized hector_slam 24 | -------------------------------------------------------------------------------- /hector_slam/hector_slam/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(hector_slam) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /hector_slam/hector_slam_launch/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package hector_slam_launch 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.3.5 (2016-06-24) 6 | ------------------ 7 | 8 | 0.3.4 (2015-11-07) 9 | ------------------ 10 | * Removes trailing spaces and fixes indents 11 | * "rviz_cfg" directory in the repository version. 12 | * Contributors: YuK_Ota, dani-carbonell 13 | 14 | 0.3.3 (2014-06-15) 15 | ------------------ 16 | 17 | 0.3.2 (2014-03-30) 18 | ------------------ 19 | * Add arguments to launch files for specifying geotiff file path 20 | * Update rviz config to have proper default displays 21 | * deleted quadrotor_uav.launch (moved to hector_quadrotor) 22 | * Contributors: Johannes Meyer, Stefan Kohlbrecher 23 | 24 | 0.3.1 (2013-10-09) 25 | ------------------ 26 | * updated rviz config to the new .rviz format 27 | * added changelogs 28 | 29 | 0.3.0 (2013-08-08) 30 | ------------------ 31 | * catkinized hector_slam 32 | -------------------------------------------------------------------------------- /hector_slam/hector_slam_launch/launch/hector_ugv.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 | -------------------------------------------------------------------------------- /hector_slam/hector_slam_launch/launch/mpo700_mapping.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 | -------------------------------------------------------------------------------- /hector_slam/hector_slam_launch/launch/postproc_data.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /hector_slam/hector_slam_launch/launch/postproc_qut_logs.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /hector_slam/hector_slam_launch/launch/tutorial.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /hector_slam/hector_slam_launch/launch/tutorial_tif.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /hector_slam/hector_trajectory_server/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package hector_trajectory_server 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.3.5 (2016-06-24) 6 | ------------------ 7 | * Changed from ros::WallTime to ros::Time in trajectory server 8 | * hector_trajectory_server: removed bug leading to potential infinite loop 9 | * Contributors: Andreas Lindahl Flåten, Paul Manns 10 | 11 | 0.3.4 (2015-11-07) 12 | ------------------ 13 | 14 | 0.3.3 (2014-06-15) 15 | ------------------ 16 | 17 | 0.3.2 (2014-03-30) 18 | ------------------ 19 | * wait based on WallTime and check ros::ok() in waitForTf() 20 | * Print out tf availability warning only once. 21 | * Wait for tf become available to suppress warnings on startup 22 | * Create a warning instead of an error for TransformExceptions 23 | * Contributors: Johannes Meyer, Stefan Kohlbrecher, wachaja 24 | 25 | 0.3.1 (2013-10-09) 26 | ------------------ 27 | * added changelogs 28 | * added cmake dependencies to catkin exported targets to force message targets to be built before any cpp target using them 29 | 30 | 0.3.0 (2013-08-08) 31 | ------------------ 32 | * catkinized hector_slam 33 | -------------------------------------------------------------------------------- /lane_detection_example/launch/hanho_lane_detector.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /lane_detection_example/launch/lane_detection.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /lane_detection_example/launch/traffic_perception.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /lane_detection_example/scripts/hanho_utils.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kyou2016/ROS_LaneDetectionWithBEV/fe8b086f84c3ee3b6017c5c70805886e68967e2a/lane_detection_example/scripts/hanho_utils.pyc -------------------------------------------------------------------------------- /lane_detection_example/scripts/joycom.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | import numpy as np 5 | 6 | from std_msgs.msg import Float64, String 7 | 8 | class joycom: 9 | def __init__(self): 10 | self.speed_sub = rospy.Subscriber('/vesc/commands/motor/speed',Float64,self.speed_callback) 11 | self.steer_sub = rospy.Subscriber('/vesc/commands/servo/position',Float64,self.steer_callback) 12 | 13 | self.speed_pub = rospy.Publisher('/commands/motor/speed', Float64, queue_size=1) 14 | self.steer_pub = rospy.Publisher('/commands/servo/position',Float64, queue_size=1) 15 | 16 | self.speed = Float64() 17 | self.steer = Float64() 18 | 19 | def steer_callback(self,msg): 20 | self.steer = msg 21 | print(msg) 22 | 23 | def speed_callback(self,msg): 24 | self.speed = msg 25 | print(msg) 26 | 27 | def rospub(self): 28 | self.speed_pub.publish(self.speed) 29 | self.steer_pub.publish(self.steer) 30 | 31 | if __name__ == '__main__': 32 | rospy.init_node('joycom', anonymous=True) 33 | 34 | wecar = joycom() 35 | 36 | rate = rospy.Rate(20) 37 | 38 | while not rospy.is_shutdown(): 39 | wecar.rospub() 40 | rate.sleep() 41 | -------------------------------------------------------------------------------- /lane_detection_example/scripts/utils.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kyou2016/ROS_LaneDetectionWithBEV/fe8b086f84c3ee3b6017c5c70805886e68967e2a/lane_detection_example/scripts/utils.pyc -------------------------------------------------------------------------------- /lane_detection_example/scripts/utils_copy.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kyou2016/ROS_LaneDetectionWithBEV/fe8b086f84c3ee3b6017c5c70805886e68967e2a/lane_detection_example/scripts/utils_copy.pyc -------------------------------------------------------------------------------- /lane_detection_example/sensor/LG/sensor_params.json: -------------------------------------------------------------------------------- 1 | { 2 | "params_cam": 3 | { 4 | "ENGINE":"LOGITECH", 5 | "TYPE":"JPG", 6 | "WIDTH":640, 7 | "HEIGHT":480, 8 | "FOV":72, 9 | "X":0.20, 10 | "Y":0, 11 | "Z":0.20, 12 | "YAW":0, 13 | "PITCH":5.0, 14 | "ROLL":0 15 | } 16 | } 17 | -------------------------------------------------------------------------------- /lane_detection_example/sensor/MS/sensor_params.json: -------------------------------------------------------------------------------- 1 | { 2 | "params_cam": 3 | { 4 | "ENGINE": "UNITY" , 5 | "TYPE": "JPG" , 6 | "WIDTH": 640 , 7 | "HEIGHT": 480 , 8 | "FOV": 78 , 9 | "X": 0.00 , 10 | "Y": 0.00 , 11 | "Z": 0.00 , 12 | "YAW": 0 , 13 | "PITCH": 0.0 , 14 | "ROLL": 0 15 | } 16 | } 17 | -------------------------------------------------------------------------------- /lane_detection_example/sensor/sensor_params.json: -------------------------------------------------------------------------------- 1 | { 2 | "params_cam": 3 | { 4 | "ENGINE":"LOGITECH", 5 | "TYPE":"JPG", 6 | "WIDTH":640, 7 | "HEIGHT":480, 8 | "FOV":72, 9 | "X":0.20, 10 | "Y":0, 11 | "Z":0.20, 12 | "YAW":0, 13 | "PITCH":5.0, 14 | "ROLL":0 15 | } 16 | } 17 | -------------------------------------------------------------------------------- /lane_detection_example/sensor/sensor_params_MS.json: -------------------------------------------------------------------------------- 1 | { 2 | "params_cam": 3 | { 4 | "ENGINE": "UNITY" , 5 | "TYPE": "JPG" , 6 | "WIDTH": 640 , 7 | "HEIGHT": 480 , 8 | "FOV": 78 , 9 | "X": 0.00 , 10 | "Y": 0.00 , 11 | "Z": 0.00 , 12 | "YAW": 0 , 13 | "PITCH": 0.0 , 14 | "ROLL": 0 15 | } 16 | } 17 | -------------------------------------------------------------------------------- /maps/01.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kyou2016/ROS_LaneDetectionWithBEV/fe8b086f84c3ee3b6017c5c70805886e68967e2a/maps/01.pgm -------------------------------------------------------------------------------- /maps/01.yaml: -------------------------------------------------------------------------------- 1 | image: map.pgm 2 | resolution: 0.050000 3 | origin: [-50.000000, -50.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /maps/02.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kyou2016/ROS_LaneDetectionWithBEV/fe8b086f84c3ee3b6017c5c70805886e68967e2a/maps/02.pgm -------------------------------------------------------------------------------- /maps/02.yaml: -------------------------------------------------------------------------------- 1 | image: map.pgm 2 | resolution: 0.050000 3 | origin: [-50.000000, -50.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /openslam_gmapping/Makefile: -------------------------------------------------------------------------------- 1 | -include ./global.mk 2 | 3 | ifeq ($(CARMENSUPPORT),1) 4 | SUBDIRS=utils sensor log configfile scanmatcher carmenwrapper gridfastslam gui gfs-carmen 5 | else 6 | ifeq ($(MACOSX),1) 7 | SUBDIRS=utils sensor log configfile scanmatcher gridfastslam 8 | else 9 | SUBDIRS=utils sensor log configfile scanmatcher gridfastslam gui 10 | endif 11 | endif 12 | 13 | LDFLAGS+= 14 | CPPFLAGS+= -I../sensor 15 | 16 | -include ./build_tools/Makefile.subdirs 17 | 18 | -------------------------------------------------------------------------------- /openslam_gmapping/TODO.txt: -------------------------------------------------------------------------------- 1 | TODO-List for gmapping (and partly explore) 2 | -------------------------------------------- 3 | 4 | open: 5 | ----- 6 | 7 | 1. implement a working(!) ancestry tree 8 | 9 | 2. compute trajectory uncertainty based on the 10 | ancestry tree formula 11 | 12 | 3. possibility to choose the way the pose uncertainty 13 | for a particle set is computed (grid vs set of 14 | gaussians) 15 | 16 | 4. Fix the NAN Problem with the pose uncertainty with gaussians 17 | 18 | done: 19 | ----- 20 | 21 | (move the done stuff down here) -------------------------------------------------------------------------------- /openslam_gmapping/build_tools/Makefile.subdirs: -------------------------------------------------------------------------------- 1 | export VERBOSE 2 | 3 | .PHONY: clean, all 4 | 5 | ifeq ($(VERBOSE), 0) 6 | QUIET=--no-print-directory 7 | endif 8 | 9 | all: 10 | @for subdir in $(SUBDIRS); do $(MESSAGE) "Entering $$subdir."; if ! $(MAKE) $(QUIET) -C $$subdir; then $(MESSAGE) "Compilation in $$subdir failed."; exit 1; fi; done 11 | 12 | clean: 13 | @for subdir in $(SUBDIRS); do $(MESSAGE) "Entering $$subdir."; $(MAKE) $(QUIET) -C $$subdir clean; done 14 | 15 | dep: 16 | @for subdir in $(SUBDIRS); do $(MESSAGE) "Entering $$subdir."; $(MAKE) $(QUIET) -C $$subdir dep; done 17 | -------------------------------------------------------------------------------- /openslam_gmapping/build_tools/generate_shared_object: -------------------------------------------------------------------------------- 1 | #!/bin/tcsh 2 | 3 | echo decompressing file $1 4 | 5 | set FILELIST=`ar -t $1` 6 | echo "Object files:" 7 | foreach i ($FILELIST) 8 | echo $i 9 | end 10 | 11 | echo generating $1:r.so 12 | 13 | ar -x $1 14 | ld -shared -o $1:r.so $FILELIST 15 | 16 | rm $FILELIST 17 | -------------------------------------------------------------------------------- /openslam_gmapping/build_tools/message: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | #echo "message: verbose = $VERBOSE" 4 | 5 | if ($VERBOSE) 6 | then 7 | exit 0; 8 | fi 9 | 10 | a=$MAKELEVEL 11 | 12 | while ((0<$a)); do echo -n " "; let "a = $a - 1";done 13 | 14 | echo $1 15 | -------------------------------------------------------------------------------- /openslam_gmapping/build_tools/pretty_compiler: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | 4 | #echo "pretty: verbose = $VERBOSE" 5 | 6 | if ($VERBOSE) 7 | then 8 | echo $1; 9 | if ! eval $1 10 | then 11 | echo "Failed command was:" 12 | echo $1 13 | echo "in directory " `pwd` 14 | exit 1 15 | fi 16 | else 17 | if ! eval $1 18 | then 19 | echo "Failed command was:" 20 | echo $1 21 | echo "in directory " `pwd` 22 | exit 1 23 | fi 24 | fi 25 | 26 | exit 0 27 | -------------------------------------------------------------------------------- /openslam_gmapping/build_tools/testlib: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | if [ -z "$1" ]; then 3 | echo "Syntax: rtestlib " 4 | exit 1 5 | fi 6 | 7 | exit 0 8 | 9 | FNAME=`mktemp rtestlibXXXXXX` 10 | echo "int main() { return 0; }" > $FNAME.cpp 11 | 12 | g++ $1 $FNAME.cpp -o $FNAME 13 | result=$? 14 | rm -f $FNAME.cpp $FNAME 15 | 16 | exit $result 17 | 18 | #if g++ $1 $FNAME.cpp -o $FNAME 19 | #then# 20 | # rm -f $FNAME.cpp $FNAME 21 | # exit 1 22 | #else 23 | # rm -f $FNAME.cpp $FNAME 24 | # exit 0 25 | #fi 26 | 27 | -------------------------------------------------------------------------------- /openslam_gmapping/carmenwrapper/Makefile: -------------------------------------------------------------------------------- 1 | OBJS= carmenwrapper.o 2 | APPS= 3 | 4 | LIBS+= -L$(CARMEN_HOME)/lib -lnavigator_interface -lsimulator_interface -lrobot_interface -llaser_interface -lparam_interface -lglobal -lipc -lm -lutils -lsensor_range -llog -lscanmatcher -lpthread -lz 5 | CPPFLAGS+=-I../sensor -I$(CARMEN_HOME)/include 6 | 7 | -include ../global.mk 8 | 9 | ifeq ($(CARMENSUPPORT), 0) 10 | OBJS= 11 | -include ../build_tools/Makefile.app 12 | else 13 | -include ../build_tools/Makefile.generic-shared-object 14 | endif 15 | -------------------------------------------------------------------------------- /openslam_gmapping/configfile/Makefile: -------------------------------------------------------------------------------- 1 | OBJS= configfile.o 2 | APPS= configfile_test 3 | 4 | -include ../global.mk 5 | -include ../build_tools/Makefile.generic-shared-object 6 | -------------------------------------------------------------------------------- /openslam_gmapping/configfile/demo.cfg: -------------------------------------------------------------------------------- 1 | [mysection] 2 | 3 | mykey1 string_value_1 # comment 4 | mykey2 string value 2 5 | 6 | mykey3 string value 3 7 | 8 | # mykey4 string value 3 9 | 10 | # mykey5 string value 3 11 | 12 | mykey6 1 13 | mykey7 1.7 14 | 15 | mykey8 On 16 | 17 | mykey9 off 18 | 19 | 20 | -------------------------------------------------------------------------------- /openslam_gmapping/configfile/test.ini: -------------------------------------------------------------------------------- 1 | 2 | [test] 3 | 4 | particles 30 5 | 6 | onLine on 7 | generateMap off 8 | 9 | teststr Hallo 10 | 11 | -------------------------------------------------------------------------------- /openslam_gmapping/gfs-carmen/Makefile: -------------------------------------------------------------------------------- 1 | OBJS= 2 | APPS= gfs-carmen 3 | 4 | LIBS+= -lcarmenwrapper -lgridfastslam -lconfigfile 5 | CPPFLAGS+= -I ../sensor -I$(CARMEN_HOME)/include 6 | 7 | -include ../global.mk 8 | ifeq ($(CARMENSUPPORT), 0) 9 | APPS= 10 | .PHONY: clean all 11 | 12 | all: 13 | 14 | clean: 15 | 16 | else 17 | -include ../build_tools/Makefile.app 18 | endif 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /openslam_gmapping/grid/Makefile: -------------------------------------------------------------------------------- 1 | OBJS= 2 | APPS= map_test 3 | 4 | LDFLAGS+= 5 | CPPFLAGS+= -DNDEBUG 6 | 7 | -include ../global.mk 8 | -include ../build_tools/Makefile.app 9 | 10 | -------------------------------------------------------------------------------- /openslam_gmapping/gridfastslam/Makefile: -------------------------------------------------------------------------------- 1 | OBJS= gridslamprocessor_tree.o motionmodel.o gridslamprocessor.o gfsreader.o 2 | APPS= gfs2log gfs2rec gfs2neff #gfs2stat 3 | 4 | #LDFLAGS+= -lutils -lsensor_range -llog -lscanmatcher -lsensor_base -lsensor_odometry $(GSL_LIB) 5 | LDFLAGS+= -lscanmatcher -llog -lsensor_range -lsensor_odometry -lsensor_base -lutils 6 | #CPPFLAGS+=-I../sensor $(GSL_INCLUDE) 7 | CPPFLAGS+=-I../sensor 8 | 9 | -include ../global.mk 10 | -include ../build_tools/Makefile.generic-shared-object 11 | -------------------------------------------------------------------------------- /openslam_gmapping/gridfastslam/gfs2neff.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | using namespace std; 7 | 8 | int main(int argc, char**argv){ 9 | if (argc<3){ 10 | cout << "usage gfs2neff " << endl; 11 | return -1; 12 | } 13 | ifstream is(argv[1]); 14 | if (!is){ 15 | cout << "could read file "<< endl; 16 | return -1; 17 | } 18 | ofstream os(argv[2]); 19 | if (! os){ 20 | cout << "could write file "<< endl; 21 | return -1; 22 | } 23 | unsigned int frame=0; 24 | double neff=0; 25 | while(is){ 26 | char buf[8192]; 27 | is.getline(buf, 8192); 28 | istringstream lineStream(buf); 29 | string recordType; 30 | lineStream >> recordType; 31 | if (recordType=="FRAME"){ 32 | lineStream>> frame; 33 | } 34 | if (recordType=="NEFF"){ 35 | lineStream>> neff; 36 | os << frame << " " << neff << endl; 37 | } 38 | } 39 | os.close(); 40 | } 41 | -------------------------------------------------------------------------------- /openslam_gmapping/gui/Makefile: -------------------------------------------------------------------------------- 1 | -include ../global.mk 2 | 3 | OBJS= gsp_thread.o qparticleviewer.o qgraphpainter.o qmappainter.o 4 | 5 | APPS= gfs_nogui gfs_simplegui gfs2img 6 | LDFLAGS+= $(QT_LIB) $(KDE_LIB) -lgridfastslam -lscanmatcher -llog -lsensor_range -lsensor_odometry -lsensor_base -lconfigfile -lutils -lpthread 7 | 8 | ifeq ($(CARMENSUPPORT),1) 9 | LDFLAGS+= -lcarmenwrapper 10 | endif 11 | 12 | CPPFLAGS+= -I../sensor $(QT_INCLUDE) $(KDE_INCLUDE) -I$(CARMEN_HOME)/include 13 | 14 | 15 | -include ../build_tools/Makefile.generic-shared-object 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /openslam_gmapping/gui/gfs_logplayer.cpp: -------------------------------------------------------------------------------- 1 | /***************************************************************** 2 | * 3 | * This file is part of the GMAPPING project 4 | * 5 | * GMAPPING Copyright (c) 2004 Giorgio Grisetti, 6 | * Cyrill Stachniss, and Wolfram Burgard 7 | * 8 | * This software is licensed under the "Creative Commons 9 | * License (Attribution-NonCommercial-ShareAlike 2.0)" 10 | * and is copyrighted by Giorgio Grisetti, Cyrill Stachniss, 11 | * and Wolfram Burgard. 12 | * 13 | * Further information on this license can be found at: 14 | * http://creativecommons.org/licenses/by-nc-sa/2.0/ 15 | * 16 | * GMAPPING is distributed in the hope that it will be useful, 17 | * but WITHOUT ANY WARRANTY; without even the implied 18 | * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR 19 | * PURPOSE. 20 | * 21 | *****************************************************************/ 22 | 23 | 24 | #include 25 | #include "gmapping/gui/qparticleviewer.h" 26 | 27 | int main (int argc, char ** argv){ 28 | QApplication app(argc, argv); 29 | QParticleViewer * pviewer=new QParticleViewer(0); 30 | app.setMainWidget(pviewer); 31 | pviewer->show(); 32 | FILE* f=fopen(argv[1], "r"); 33 | if (!f) 34 | return -1; 35 | QTextIStream is(f); 36 | pviewer->tis=&is; 37 | pviewer->start(10); 38 | return app.exec(); 39 | std::cout << "DONE: " << argv[1] <fill(Qt::white); 8 | } 9 | 10 | void QMapPainter::resizeEvent(QResizeEvent * sizeev){ 11 | m_pixmap->resize(sizeev->size()); 12 | } 13 | 14 | QMapPainter::~QMapPainter(){ 15 | delete m_pixmap; 16 | } 17 | 18 | 19 | void QMapPainter::timerEvent(QTimerEvent * te) { 20 | if (te->timerId()==timer) 21 | update(); 22 | } 23 | 24 | void QMapPainter::start(int period){ 25 | timer=startTimer(period); 26 | } 27 | 28 | 29 | void QMapPainter::paintEvent ( QPaintEvent * ){ 30 | bitBlt(this,0,0,m_pixmap,0,0,m_pixmap->width(),m_pixmap->height(),CopyROP); 31 | } 32 | 33 | -------------------------------------------------------------------------------- /openslam_gmapping/gui/qpixmapdumper.cpp: -------------------------------------------------------------------------------- 1 | #include "gmapping/gui/qpixmapdumper.h" 2 | #include 3 | #include 4 | 5 | QPixmapDumper::QPixmapDumper(std::string p, int c){ 6 | format="PNG"; 7 | prefix=p; 8 | reset(); 9 | cycles=c; 10 | } 11 | 12 | void QPixmapDumper::reset(){ 13 | cycles=0; 14 | frame=0; 15 | counter=0; 16 | } 17 | 18 | #define filename_bufsize 1024 19 | 20 | bool QPixmapDumper::dump(const QPixmap& pixmap){ 21 | bool processed=false; 22 | if (!(counter%cycles)){ 23 | char buf[filename_bufsize]; 24 | sprintf(buf,"%s-%05d.%s",prefix.c_str(), frame, format.c_str()); 25 | QImage image=pixmap.convertToImage(); 26 | image.save(QString(buf), format.c_str(),0); 27 | frame ++; 28 | } 29 | counter++; 30 | return processed; 31 | } 32 | 33 | 34 | -------------------------------------------------------------------------------- /openslam_gmapping/include/gmapping/grid/accessstate.h: -------------------------------------------------------------------------------- 1 | #ifndef ACCESSTATE_H 2 | #define ACCESSTATE_H 3 | 4 | namespace GMapping { 5 | enum AccessibilityState{Outside=0x0, Inside=0x1, Allocated=0x2}; 6 | }; 7 | 8 | #endif 9 | 10 | -------------------------------------------------------------------------------- /openslam_gmapping/include/gmapping/gridfastslam/motionmodel.h: -------------------------------------------------------------------------------- 1 | #ifndef MOTIONMODEL_H 2 | #define MOTIONMODEL_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace GMapping { 9 | 10 | struct MotionModel{ 11 | OrientedPoint drawFromMotion(const OrientedPoint& p, double linearMove, double angularMove) const; 12 | OrientedPoint drawFromMotion(const OrientedPoint& p, const OrientedPoint& pnew, const OrientedPoint& pold) const; 13 | Covariance3 gaussianApproximation(const OrientedPoint& pnew, const OrientedPoint& pold) const; 14 | double srr, str, srt, stt; 15 | }; 16 | 17 | }; 18 | 19 | #endif 20 | -------------------------------------------------------------------------------- /openslam_gmapping/include/gmapping/gui/qnavigatorwidget.h: -------------------------------------------------------------------------------- 1 | #ifndef _QNAVIGATOR_WIDGET_H 2 | #define _QNAVIGATOR_WIDGET_H 3 | 4 | #include "gmapping/gui/qmappainter.h" 5 | #include "gmapping/gui/qpixmapdumper.h" 6 | #include 7 | #include 8 | 9 | class QNavigatorWidget : public QMapPainter{ 10 | public: 11 | QNavigatorWidget( QWidget * parent = 0, const char * name = 0, WFlags f = 0); 12 | virtual ~QNavigatorWidget(); 13 | std::list trajectoryPoints; 14 | bool repositionRobot; 15 | GMapping::IntPoint robotPose; 16 | double robotHeading; 17 | bool confirmLocalization; 18 | bool enableMotion; 19 | bool startWalker; 20 | bool startGlobalLocalization; 21 | bool trajectorySent; 22 | bool goHome; 23 | bool wantsQuit; 24 | bool writeImages; 25 | QPixmapDumper dumper; 26 | bool drawRobot; 27 | 28 | protected: 29 | virtual void paintEvent ( QPaintEvent *paintevent ); 30 | virtual void mousePressEvent ( QMouseEvent * e ); 31 | virtual void keyPressEvent ( QKeyEvent * e ); 32 | }; 33 | 34 | #endif 35 | 36 | -------------------------------------------------------------------------------- /openslam_gmapping/include/gmapping/gui/qpixmapdumper.h: -------------------------------------------------------------------------------- 1 | #ifndef _QPIXMAPDUMPER_H_ 2 | #define _QPIXMAPDUMPER_H_ 3 | #include 4 | #include 5 | #include 6 | 7 | 8 | struct QPixmapDumper{ 9 | QPixmapDumper(std::string prefix, int cycles); 10 | void reset(); 11 | std::string prefix; 12 | std::string format; 13 | bool dump(const QPixmap& pixmap); 14 | int counter; 15 | int cycles; 16 | int frame; 17 | }; 18 | 19 | #endif 20 | -------------------------------------------------------------------------------- /openslam_gmapping/include/gmapping/gui/qslamandnavwidget.h: -------------------------------------------------------------------------------- 1 | #ifndef _QSLAMANDNAV_WIDGET_H 2 | #define _QSLAMANDNAV_WIDGET_H 3 | 4 | #include "gmapping/gui/qmappainter.h" 5 | #include "gmapping/gui/qpixmapdumper.h" 6 | #include 7 | #include 8 | 9 | class QSLAMandNavWidget : public QMapPainter{ 10 | public: 11 | QSLAMandNavWidget( QWidget * parent = 0, const char * name = 0, WFlags f = 0); 12 | virtual ~QSLAMandNavWidget(); 13 | std::list trajectoryPoints; 14 | GMapping::IntPoint robotPose; 15 | double robotHeading; 16 | 17 | bool slamRestart; 18 | bool slamFinished; 19 | bool enableMotion; 20 | bool startWalker; 21 | bool trajectorySent; 22 | bool goHome; 23 | bool wantsQuit; 24 | bool printHelp; 25 | bool saveGoalPoints; 26 | bool writeImages; 27 | bool drawRobot; 28 | QPixmapDumper dumper; 29 | 30 | 31 | protected: 32 | virtual void paintEvent ( QPaintEvent *paintevent ); 33 | virtual void mousePressEvent ( QMouseEvent * e ); 34 | virtual void keyPressEvent ( QKeyEvent * e ); 35 | }; 36 | 37 | #endif 38 | 39 | -------------------------------------------------------------------------------- /openslam_gmapping/include/gmapping/log/carmenconfiguration.h: -------------------------------------------------------------------------------- 1 | #ifndef CARMENCONFIGURATION_H 2 | #define CARMENCONFIGURATION_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include "gmapping/log/configuration.h" 10 | 11 | namespace GMapping { 12 | 13 | class CarmenConfiguration: public Configuration, public std::map >{ 14 | public: 15 | virtual std::istream& load(std::istream& is); 16 | virtual SensorMap computeSensorMap() const; 17 | }; 18 | 19 | }; 20 | 21 | #endif 22 | 23 | -------------------------------------------------------------------------------- /openslam_gmapping/include/gmapping/log/configuration.h: -------------------------------------------------------------------------------- 1 | #ifndef CONFIGURATION_H 2 | #define CONFIGURATION_H 3 | 4 | #include 5 | #include 6 | 7 | namespace GMapping { 8 | 9 | class Configuration{ 10 | public: 11 | virtual ~Configuration(); 12 | virtual SensorMap computeSensorMap() const=0; 13 | }; 14 | 15 | }; 16 | #endif 17 | 18 | -------------------------------------------------------------------------------- /openslam_gmapping/include/gmapping/log/sensorlog.h: -------------------------------------------------------------------------------- 1 | #ifndef SENSORLOG_H 2 | #define SENSORLOG_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include "gmapping/log/configuration.h" 12 | 13 | namespace GMapping { 14 | 15 | class SensorLog : public std::list{ 16 | public: 17 | SensorLog(const SensorMap&); 18 | ~SensorLog(); 19 | std::istream& load(std::istream& is); 20 | OrientedPoint boundingBox(double& xmin, double& ymin, double& xmax, double& ymax) const; 21 | protected: 22 | const SensorMap& m_sensorMap; 23 | OdometryReading* parseOdometry(std::istream& is, const OdometrySensor* ) const; 24 | RangeReading* parseRange(std::istream& is, const RangeSensor* ) const; 25 | }; 26 | 27 | }; 28 | 29 | #endif 30 | -------------------------------------------------------------------------------- /openslam_gmapping/include/gmapping/scanmatcher/eig3.h: -------------------------------------------------------------------------------- 1 | 2 | /* Eigen-decomposition for symmetric 3x3 real matrices. 3 | Public domain, copied from the public domain Java library JAMA. */ 4 | 5 | #ifndef _eig_h 6 | 7 | /* Symmetric matrix A => eigenvectors in columns of V, corresponding 8 | eigenvalues in d. */ 9 | void eigen_decomposition(double A[3][3], double V[3][3], double d[3]); 10 | 11 | #endif 12 | -------------------------------------------------------------------------------- /openslam_gmapping/include/gmapping/scanmatcher/lumiles.h: -------------------------------------------------------------------------------- 1 | #ifndef LUMILESPROCESSOR 2 | #define LUMILESPROCESSOR 3 | 4 | namespace GMapping{ 5 | 6 | class LuMilesProcessor{ 7 | typedef std:vector PointVector; 8 | static OrientedPoint step(const PointVector& src, const PointVector& dest); 9 | }; 10 | 11 | OrientedPoint LuMilesProcessors::step(const PointVector& src, const PointVector& dest){ 12 | assert(src.size()==dest.size()); 13 | unsigned int size=dest.size(); 14 | double smx=0, smy=0, dmx=0, dmy=0; 15 | for (PointVector::const_iterator it=src.begin(); it!=src.end(); it++){ 16 | smx+=it->x; 17 | smy+=it->y; 18 | } 19 | smx/=src.size(); 20 | smy/=src.size(); 21 | 22 | for (PointVector::const_iterator it=dest.begin(); it!=dest.end(); it++){ 23 | dmx+=it->x; 24 | dmy+=it->y; 25 | } 26 | dmx/=src.size(); 27 | dmy/=src.size(); 28 | 29 | double sxx=0, sxy=0; 30 | double syx=0, syy=0; 31 | for (unsigned int i=0; i 5 | #include 6 | 7 | namespace GMapping{ 8 | 9 | class Sensor{ 10 | public: 11 | Sensor(const std::string& name=""); 12 | virtual ~Sensor(); 13 | inline std::string getName() const {return m_name;} 14 | inline void setName(const std::string& name) {m_name=name;} 15 | protected: 16 | std::string m_name; 17 | }; 18 | 19 | typedef std::map SensorMap; 20 | 21 | }; //end namespace 22 | 23 | #endif 24 | 25 | -------------------------------------------------------------------------------- /openslam_gmapping/include/gmapping/sensor/sensor_base/sensoreading.h: -------------------------------------------------------------------------------- 1 | #ifndef SENSORREADING_H 2 | #define SENSORREADING_H 3 | 4 | #include "gmapping/sensor/sensor_base/sensor.h" 5 | namespace GMapping{ 6 | 7 | class SensorReading{ 8 | public: 9 | SensorReading(const Sensor* s=0, double time=0); 10 | inline double getTime() const {return m_time;} 11 | inline const Sensor* getSensor() const {return m_sensor;} 12 | protected: 13 | double m_time; 14 | const Sensor* m_sensor; 15 | }; 16 | 17 | }; //end namespace 18 | #endif 19 | -------------------------------------------------------------------------------- /openslam_gmapping/include/gmapping/sensor/sensor_base/sensorreading.h: -------------------------------------------------------------------------------- 1 | #ifndef SENSORREADING_H 2 | #define SENSORREADING_H 3 | 4 | #include "gmapping/sensor/sensor_base/sensor.h" 5 | namespace GMapping{ 6 | 7 | class SensorReading{ 8 | public: 9 | SensorReading(const Sensor* s=0, double time=0); 10 | virtual ~SensorReading(); 11 | inline double getTime() const {return m_time;} 12 | inline void setTime(double t) {m_time=t;} 13 | inline const Sensor* getSensor() const {return m_sensor;} 14 | protected: 15 | double m_time; 16 | const Sensor* m_sensor; 17 | 18 | }; 19 | 20 | }; //end namespace 21 | #endif 22 | 23 | 24 | -------------------------------------------------------------------------------- /openslam_gmapping/include/gmapping/sensor/sensor_odometry/odometryreading.h: -------------------------------------------------------------------------------- 1 | #ifndef ODOMETRYREADING_H 2 | #define ODOMETRYREADING_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include "gmapping/sensor/sensor_odometry/odometrysensor.h" 8 | 9 | namespace GMapping{ 10 | 11 | class OdometryReading: public SensorReading{ 12 | public: 13 | OdometryReading(const OdometrySensor* odo, double time=0); 14 | inline const OrientedPoint& getPose() const {return m_pose;} 15 | inline const OrientedPoint& getSpeed() const {return m_speed;} 16 | inline const OrientedPoint& getAcceleration() const {return m_acceleration;} 17 | inline void setPose(const OrientedPoint& pose) {m_pose=pose;} 18 | inline void setSpeed(const OrientedPoint& speed) {m_speed=speed;} 19 | inline void setAcceleration(const OrientedPoint& acceleration) {m_acceleration=acceleration;} 20 | 21 | protected: 22 | OrientedPoint m_pose; 23 | OrientedPoint m_speed; 24 | OrientedPoint m_acceleration; 25 | }; 26 | 27 | }; 28 | #endif 29 | 30 | -------------------------------------------------------------------------------- /openslam_gmapping/include/gmapping/sensor/sensor_odometry/odometrysensor.h: -------------------------------------------------------------------------------- 1 | #ifndef ODOMETRYSENSOR_H 2 | #define ODOMETRYSENSOR_H 3 | 4 | #include 5 | #include 6 | 7 | namespace GMapping{ 8 | 9 | class OdometrySensor: public Sensor{ 10 | public: 11 | OdometrySensor(const std::string& name, bool ideal=false); 12 | inline bool isIdeal() const { return m_ideal; } 13 | protected: 14 | bool m_ideal; 15 | }; 16 | 17 | }; 18 | 19 | #endif 20 | 21 | -------------------------------------------------------------------------------- /openslam_gmapping/include/gmapping/sensor/sensor_range/rangereading.h: -------------------------------------------------------------------------------- 1 | #ifndef RANGEREADING_H 2 | #define RANGEREADING_H 3 | 4 | #include 5 | #include 6 | #include "gmapping/sensor/sensor_range/rangesensor.h" 7 | 8 | namespace GMapping{ 9 | 10 | class RangeReading: public SensorReading, public std::vector{ 11 | public: 12 | RangeReading(const RangeSensor* rs, double time=0); 13 | RangeReading(unsigned int n_beams, const double* d, const RangeSensor* rs, double time=0); 14 | virtual ~RangeReading(); 15 | inline const OrientedPoint& getPose() const {return m_pose;} 16 | inline void setPose(const OrientedPoint& pose) {m_pose=pose;} 17 | unsigned int rawView(double* v, double density=0.) const; 18 | std::vector cartesianForm(double maxRange=1e6) const; 19 | unsigned int activeBeams(double density=0.) const; 20 | protected: 21 | OrientedPoint m_pose; 22 | }; 23 | 24 | }; 25 | 26 | #endif 27 | -------------------------------------------------------------------------------- /openslam_gmapping/include/gmapping/sensor/sensor_range/rangesensor.h: -------------------------------------------------------------------------------- 1 | #ifndef RANGESENSOR_H 2 | #define RANGESENSOR_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace GMapping{ 9 | 10 | class RangeSensor: public Sensor{ 11 | friend class Configuration; 12 | friend class CarmenConfiguration; 13 | friend class CarmenWrapper; 14 | public: 15 | struct Beam{ 16 | OrientedPoint pose; //pose relative to the center of the sensor 17 | double span; //spam=0 indicates a line-like beam 18 | double maxRange; //maximum range of the sensor 19 | double s,c; //sinus and cosinus of the beam (optimization); 20 | }; 21 | RangeSensor(std::string name); 22 | RangeSensor(std::string name, unsigned int beams, double res, const OrientedPoint& position=OrientedPoint(0,0,0), double span=0, double maxrange=89.0); 23 | inline const std::vector& beams() const {return m_beams;} 24 | inline std::vector& beams() {return m_beams;} 25 | inline OrientedPoint getPose() const {return m_pose;} 26 | void updateBeamsLookup(); 27 | bool newFormat; 28 | protected: 29 | OrientedPoint m_pose; 30 | std::vector m_beams; 31 | }; 32 | 33 | }; 34 | 35 | #endif 36 | -------------------------------------------------------------------------------- /openslam_gmapping/include/gmapping/utils/gvalues.h: -------------------------------------------------------------------------------- 1 | #ifndef _GVALUES_H_ 2 | #define _GVALUES_H_ 3 | 4 | #ifdef LINUX 5 | #include 6 | #endif 7 | #ifdef MACOSX 8 | #include 9 | #include 10 | #define MAXDOUBLE 1e1000 11 | //#define isnan(x) (x==FP_NAN) 12 | #endif 13 | #ifdef _WIN32 14 | #include 15 | #ifndef __DRAND48_DEFINED__ 16 | #define __DRAND48_DEFINED__ 17 | inline double drand48() { return double(rand()) / RAND_MAX;} 18 | #endif 19 | #ifndef M_PI 20 | #define M_PI 3.1415926535897932384626433832795 21 | #endif 22 | #define round(d) (floor((d) + 0.5)) 23 | typedef unsigned int uint; 24 | #define isnan(x) (_isnan(x)) 25 | #endif 26 | 27 | #endif 28 | 29 | -------------------------------------------------------------------------------- /openslam_gmapping/include/gmapping/utils/movement.h: -------------------------------------------------------------------------------- 1 | #ifndef FSRMOVEMENT_H 2 | #define FSRMOVEMENT_H 3 | 4 | #include "gmapping/utils/point.h" 5 | 6 | namespace GMapping { 7 | 8 | /** fsr-movement (forward, sideward, rotate) **/ 9 | class FSRMovement { 10 | public: 11 | FSRMovement(double f=0.0, double s=0.0, double r=0.0); 12 | FSRMovement(const FSRMovement& src); 13 | FSRMovement(const OrientedPoint& pt1, const OrientedPoint& pt2); 14 | FSRMovement(const FSRMovement& move1, const FSRMovement& move2); 15 | 16 | 17 | void normalize(); 18 | void invert(); 19 | void compose(const FSRMovement& move2); 20 | OrientedPoint move(const OrientedPoint& pt) const; 21 | 22 | 23 | /* static members */ 24 | 25 | static OrientedPoint movePoint(const OrientedPoint& pt, const FSRMovement& move1); 26 | 27 | static FSRMovement composeMoves(const FSRMovement& move1, 28 | const FSRMovement& move2); 29 | 30 | static FSRMovement moveBetweenPoints(const OrientedPoint& pt1, 31 | const OrientedPoint& pt2); 32 | 33 | static FSRMovement invertMove(const FSRMovement& move1); 34 | 35 | static OrientedPoint frameTransformation(const OrientedPoint& reference_pt_frame1, 36 | const OrientedPoint& reference_pt_frame2, 37 | const OrientedPoint& pt_frame1); 38 | 39 | public: 40 | double f; 41 | double s; 42 | double r; 43 | }; 44 | } 45 | #endif 46 | -------------------------------------------------------------------------------- /openslam_gmapping/include/gmapping/utils/orientedboundingbox.h: -------------------------------------------------------------------------------- 1 | #ifndef ORIENTENDBOUNDINGBOX_H 2 | #define ORIENTENDBOUNDINGBOX_H 3 | 4 | 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | namespace GMapping{ 11 | 12 | template 13 | class OrientedBoundingBox { 14 | 15 | public: 16 | OrientedBoundingBox(std::vector< point > p); 17 | double area(); 18 | 19 | protected: 20 | Point ul; 21 | Point ur; 22 | Point ll; 23 | Point lr; 24 | }; 25 | 26 | #include "gmapping/utils/orientedboundingbox.hxx" 27 | 28 | };// end namespace 29 | 30 | #endif 31 | 32 | -------------------------------------------------------------------------------- /openslam_gmapping/include/gmapping/utils/printmemusage.h: -------------------------------------------------------------------------------- 1 | #ifndef PRINTMEMUSAGE_H 2 | #define PRINTMEMUSAGE_H 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | namespace GMapping{ 10 | void printmemusage(); 11 | }; 12 | 13 | #endif 14 | -------------------------------------------------------------------------------- /openslam_gmapping/include/gmapping/utils/printpgm.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | 7 | using namespace std; 8 | ostream& printpgm(ostream& os, int xsize, int ysize, const double * const * matrix){ 9 | if (!os) 10 | return os; 11 | os<< "P5" << endl << xsize << endl << ysize << endl << 255 << endl; 12 | for (int y=ysize-1; y>=0; y--){ 13 | for (int x=0;x 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | 8 | using namespace std; 9 | using namespace GMapping; 10 | 11 | int main(int argc, char ** argv){ 12 | if (argc<2){ 13 | cout << "usage log_test " << endl; 14 | exit (-1); 15 | } 16 | ifstream is(argv[1]); 17 | if (! is){ 18 | cout << "no file " << argv[1] << " found" << endl; 19 | exit (-1); 20 | } 21 | CarmenConfiguration conf; 22 | conf.load(is); 23 | 24 | SensorMap m=conf.computeSensorMap(); 25 | 26 | //for (SensorMap::const_iterator it=m.begin(); it!=m.end(); it++) 27 | // cout << it->first << " " << it->second->getName() << endl; 28 | 29 | SensorLog log(m); 30 | is.close(); 31 | 32 | ifstream ls(argv[1]); 33 | log.load(ls); 34 | ls.close(); 35 | cerr << "log size" << log.size() << endl; 36 | for (SensorLog::iterator it=log.begin(); it!=log.end(); it++){ 37 | RangeReading* rr=dynamic_cast(*it); 38 | if (rr){ 39 | //cerr << rr->getSensor()->getName() << " "; 40 | //cerr << rr->size()<< " "; 41 | //for (RangeReading::const_iterator it=rr->begin(); it!=rr->end(); it++){ 42 | // cerr << *it << " "; 43 | //} 44 | cout<< rr->getPose().x << " " << rr->getPose().y << " " << rr->getPose().theta << " " << rr->getTime() << endl; 45 | } 46 | } 47 | } 48 | -------------------------------------------------------------------------------- /openslam_gmapping/manual.mk: -------------------------------------------------------------------------------- 1 | #CPPFLAGS+= -DNDEBUG 2 | CXXFLAGS+= -O3 -Wall -ffast-math 3 | #CXXFLAGS+= -g -O0 -Wall 4 | PROFILE= false 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /openslam_gmapping/manual.mk-template: -------------------------------------------------------------------------------- 1 | #CPPFLAGS+= -DNDEBUG 2 | #CXXFLAGS+= -O3 -Wall 3 | CXXFLAGS+= -g -O0 -Wall 4 | PROFILE= false 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /openslam_gmapping/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | openslam_gmapping 4 | 0.2.1 5 | The catkinized verseion of openslam_gmapping package (https://github.com/OpenSLAM-org/openslam_gmapping/tree/79ef0b0e6d9a12d6390ae64c4c00d37d776abefb) 6 | 7 | ROS Orphaned Package Maintainers 8 | BSD 9 | 10 | http://openslam.org/gmapping 11 | https://github.com/ros-perception/openslam_gmapping 12 | https://github.com/ros-perception/openslam_gmapping/issues 13 | 14 | Cyrill Stachniss 15 | Udo Frese 16 | Giorgio Grisetti 17 | Wolfram Burgard 18 | 19 | catkin 20 | 21 | 22 | -------------------------------------------------------------------------------- /openslam_gmapping/particlefilter/Makefile: -------------------------------------------------------------------------------- 1 | OBJS= 2 | APPS= range_bearing particlefilter_test 3 | 4 | LDFLAGS+= 5 | CPPFLAGS+= 6 | 7 | -include ../global.mk 8 | -include ../build_tools/Makefile.app 9 | 10 | -------------------------------------------------------------------------------- /openslam_gmapping/scanmatcher/Makefile: -------------------------------------------------------------------------------- 1 | OBJS= smmap.o scanmatcher.o scanmatcherprocessor.o eig3.o 2 | APPS= scanmatch_test icptest 3 | 4 | #LDFLAGS+= $(GSL_LIB) -lutils -lsensor_range -llog 5 | LDFLAGS+= -llog -lsensor_range -lsensor_odometry -lsensor_base -lutils 6 | #CPPFLAGS+=-I../sensor $(GSL_INCLUDE) 7 | CPPFLAGS+=-I../sensor 8 | 9 | -include ../global.mk 10 | -include ../build_tools/Makefile.generic-shared-object 11 | 12 | -------------------------------------------------------------------------------- /openslam_gmapping/scanmatcher/smmap.cpp: -------------------------------------------------------------------------------- 1 | #include "gmapping/scanmatcher/smmap.h" 2 | 3 | namespace GMapping { 4 | 5 | const PointAccumulator& PointAccumulator::Unknown(){ 6 | if (! unknown_ptr) 7 | unknown_ptr=new PointAccumulator; 8 | return *unknown_ptr; 9 | } 10 | 11 | PointAccumulator* PointAccumulator::unknown_ptr=0; 12 | 13 | }; 14 | 15 | 16 | -------------------------------------------------------------------------------- /openslam_gmapping/sensor/Makefile: -------------------------------------------------------------------------------- 1 | -include ../global.mk 2 | 3 | SUBDIRS=sensor_base sensor_odometry sensor_range 4 | 5 | -include ../build_tools/Makefile.subdirs 6 | -------------------------------------------------------------------------------- /openslam_gmapping/sensor/sensor_base/Makefile: -------------------------------------------------------------------------------- 1 | -include ../../global.mk 2 | 3 | OBJS= sensor.o sensorreading.o 4 | 5 | -include ../../build_tools/Makefile.generic-shared-object 6 | 7 | -------------------------------------------------------------------------------- /openslam_gmapping/sensor/sensor_base/sensor.cpp: -------------------------------------------------------------------------------- 1 | #include "gmapping/sensor/sensor_base/sensor.h" 2 | 3 | namespace GMapping{ 4 | 5 | Sensor::Sensor(const std::string& name){ 6 | m_name=name; 7 | } 8 | 9 | Sensor::~Sensor(){ 10 | } 11 | 12 | };// end namespace 13 | -------------------------------------------------------------------------------- /openslam_gmapping/sensor/sensor_base/sensorreading.cpp: -------------------------------------------------------------------------------- 1 | #include "gmapping/sensor/sensor_base/sensorreading.h" 2 | 3 | namespace GMapping{ 4 | 5 | SensorReading::SensorReading(const Sensor* s, double t){ 6 | m_sensor=s; 7 | m_time=t; 8 | } 9 | 10 | 11 | SensorReading::~SensorReading(){ 12 | } 13 | 14 | }; 15 | 16 | -------------------------------------------------------------------------------- /openslam_gmapping/sensor/sensor_odometry/Makefile: -------------------------------------------------------------------------------- 1 | -include ../../global.mk 2 | 3 | #CPPFLAGS+= -I.. 4 | LDFLAGS+= -lsensor_base 5 | 6 | OBJS= odometrysensor.o odometryreading.o 7 | 8 | -include ../../build_tools/Makefile.generic-shared-object 9 | 10 | -------------------------------------------------------------------------------- /openslam_gmapping/sensor/sensor_odometry/odometryreading.cpp: -------------------------------------------------------------------------------- 1 | #include "gmapping/sensor/sensor_odometry/odometryreading.h" 2 | 3 | namespace GMapping{ 4 | 5 | OdometryReading::OdometryReading(const OdometrySensor* odo, double time): 6 | SensorReading(odo,time){} 7 | 8 | }; 9 | 10 | -------------------------------------------------------------------------------- /openslam_gmapping/sensor/sensor_odometry/odometrysensor.cpp: -------------------------------------------------------------------------------- 1 | #include "gmapping/sensor/sensor_odometry/odometrysensor.h" 2 | 3 | namespace GMapping{ 4 | 5 | OdometrySensor::OdometrySensor(const std::string& name, bool ideal): Sensor(name){ m_ideal=ideal;} 6 | 7 | 8 | }; 9 | 10 | -------------------------------------------------------------------------------- /openslam_gmapping/sensor/sensor_range/Makefile: -------------------------------------------------------------------------------- 1 | -include ../../global.mk 2 | 3 | CPPFLAGS+= -I../ 4 | LDFLAGS+= -lsensor_base 5 | 6 | OBJS= rangesensor.o rangereading.o 7 | 8 | -include ../../build_tools/Makefile.generic-shared-object 9 | -------------------------------------------------------------------------------- /openslam_gmapping/sensor/sensor_range/rangesensor.cpp: -------------------------------------------------------------------------------- 1 | #include "gmapping/sensor/sensor_range/rangesensor.h" 2 | 3 | namespace GMapping{ 4 | 5 | RangeSensor::RangeSensor(std::string name): Sensor(name){} 6 | 7 | RangeSensor::RangeSensor(std::string name, unsigned int beams_num, double res, const OrientedPoint& position, double span, double maxrange):Sensor(name), 8 | m_pose(position), m_beams(beams_num){ 9 | double angle=-.5*res*beams_num; 10 | for (unsigned int i=0; i 2 | #include "gmapping/utils/autoptr.h" 3 | 4 | using namespace std; 5 | using namespace GMapping; 6 | 7 | typedef autoptr DoubleAutoPtr; 8 | 9 | int main(int argc, const char * const * argv){ 10 | double* d1=new double(10.); 11 | double* d2=new double(20.); 12 | cout << "Construction test" << endl; 13 | DoubleAutoPtr pd1(d1); 14 | DoubleAutoPtr pd2(d2); 15 | cout << *pd1 << " " << *pd2 << endl; 16 | cout << "Copy Construction" << endl; 17 | DoubleAutoPtr pd3(pd1); 18 | cout << *pd3 << endl; 19 | cout << "assignment" << endl; 20 | pd3=pd2; 21 | pd1=pd2; 22 | cout << *pd1 << " " << *pd2 << " " << *pd3 << " " << endl; 23 | cout << "conversion operator" << endl; 24 | DoubleAutoPtr nullPtr; 25 | cout << "conversion operator " << !(nullPtr) << endl; 26 | cout << "neg conversion operator " << nullPtr << endl; 27 | cout << "conversion operator " << (int)pd1 << endl; 28 | cout << "neg conversion operator " << !(pd1) << endl; 29 | } 30 | -------------------------------------------------------------------------------- /openslam_gmapping/utils/printmemusage.cpp: -------------------------------------------------------------------------------- 1 | #include "gmapping/utils/printmemusage.h" 2 | 3 | namespace GMapping{ 4 | 5 | using namespace std; 6 | void printmemusage(){ 7 | pid_t pid=getpid(); 8 | char procfilename[1000]; 9 | sprintf(procfilename, "/proc/%d/status", pid); 10 | ifstream is(procfilename); 11 | string line; 12 | while (is){ 13 | is >> line; 14 | if (line=="VmData:"){ 15 | is >> line; 16 | cerr << "#VmData:\t" << line << endl; 17 | } 18 | if (line=="VmSize:"){ 19 | is >> line; 20 | cerr << "#VmSize:\t" << line << endl; 21 | } 22 | 23 | } 24 | } 25 | 26 | }; 27 | 28 | -------------------------------------------------------------------------------- /racecar/.gitignore: -------------------------------------------------------------------------------- 1 | # -*- mode: gitignore; -*- 2 | *~ 3 | \#*\# 4 | -------------------------------------------------------------------------------- /racecar/ackermann_cmd_mux/.gitignore: -------------------------------------------------------------------------------- 1 | /bin 2 | /build 3 | /lib 4 | -------------------------------------------------------------------------------- /racecar/ackermann_cmd_mux/.pydevproject: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | Default 6 | python 2.7 7 | 8 | -------------------------------------------------------------------------------- /racecar/ackermann_cmd_mux/.settings/language.settings.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /racecar/ackermann_cmd_mux/cfg/reload.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python2 2 | 3 | PACKAGE = "ackermann_cmd_mux" 4 | 5 | from dynamic_reconfigure.parameter_generator_catkin import * 6 | 7 | gen = ParameterGenerator() 8 | 9 | gen.add("yaml_cfg_file", str_t, 0, "Pathname to a yaml file for re-configuration of the mux", "") 10 | 11 | # Second arg is node name it will run in (doc purposes only), third is generated filename prefix 12 | exit(gen.generate(PACKAGE, "ackermann_cmd_mux_reload", "reload")) 13 | -------------------------------------------------------------------------------- /racecar/ackermann_cmd_mux/launch/ackermann_cmd_mux.launch: -------------------------------------------------------------------------------- 1 | 4 | 5 | 6 | 7 | 8 | 9 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /racecar/ackermann_cmd_mux/launch/reconfigure.launch: -------------------------------------------------------------------------------- 1 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /racecar/ackermann_cmd_mux/launch/standalone.launch: -------------------------------------------------------------------------------- 1 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /racecar/ackermann_cmd_mux/param/reconfigure.yaml: -------------------------------------------------------------------------------- 1 | # Another example configuration file, though this one is used to test the dynamic reconfiguration 2 | # of the ackermann_cmd_mux. Used with reconfigure.launch. 3 | 4 | subscribers: 5 | - name: "Default input" 6 | topic: "input/default" 7 | timeout: 0.1 8 | priority: 0 9 | short_desc: "The default ackermann_cmd, controllers unaware that we are multiplexing ackermann_cmd should come here" 10 | - name: "Navigation stack" 11 | topic: "input/navigation" 12 | timeout: 0.5 13 | priority: 1 14 | short_desc: "Navigation stack controller" 15 | - name: "Safety controller" 16 | topic: "input/safety" 17 | timeout: 0.1 18 | priority: 10 19 | short_desc: "Used with the reactive velocity control provided by the bumper/cliff sensor safety controller" 20 | -------------------------------------------------------------------------------- /racecar/ackermann_cmd_mux/plugins/nodelets.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | Implementation for the ackermann command multiplexer as a nodelet. 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /racecar/racecar-vm.rosinstall: -------------------------------------------------------------------------------- 1 | - setup-file: {local-name: /opt/ros/kinetic/setup.sh} 2 | - git: {local-name: src/racecar, version: master, uri: 'https://github.com/mit-racecar/racecar.git'} 3 | - git: {local-name: src/vesc, version: master, uri: 'https://github.com/mit-racecar/vesc.git'} 4 | - git: {local-name: src/racecar-simulator, version: master, uri: 'https://github.com/mit-racecar/racecar-simulator.git'} 5 | -------------------------------------------------------------------------------- /racecar/racecar.rosinstall: -------------------------------------------------------------------------------- 1 | - setup-file: {local-name: /opt/ros/kinetic/setup.sh} 2 | - git: {local-name: src/racecar, version: master, uri: 'https://github.com/mit-racecar/racecar.git'} 3 | - git: {local-name: src/vesc, version: master, uri: 'https://github.com/mit-racecar/vesc.git'} 4 | - git: {local-name: src/zed_wrapper, version: 6c06bb0a3c32aa2dec4a539e296f6ee1d6937518, uri: 'https://github.com/stereolabs/zed-ros-wrapper.git'} 5 | -------------------------------------------------------------------------------- /racecar/racecar/config/racecar-v2/high_level_mux.yaml: -------------------------------------------------------------------------------- 1 | # Individual subscriber configuration: 2 | # name: Source name 3 | # topic: The topic that provides ackermann_cmd messages 4 | # timeout: Time in seconds without incoming messages to consider this topic inactive 5 | # priority: Priority: an UNIQUE unsigned integer from 0 (lowest) to MAX_INT 6 | # short_desc: Short description (optional) 7 | 8 | subscribers: 9 | - name: "Nav_0" 10 | topic: "input/nav_0" 11 | timeout: 0.2 12 | priority: 20 13 | short_desc: "Input for autonomous navigation, highest priority" 14 | 15 | - name: "Nav_1" 16 | topic: "input/nav_1" 17 | timeout: 0.2 18 | priority: 15 19 | short_desc: "Input for autonomous navigation" 20 | 21 | - name: "Nav_2" 22 | topic: "input/nav_2" 23 | timeout: 0.2 24 | priority: 10 25 | short_desc: "Input for autonomous navigation" 26 | 27 | - name: "Nav_3" 28 | topic: "input/nav_3" 29 | timeout: 0.2 30 | priority: 5 31 | short_desc: "Input for autonomous navigation" 32 | 33 | - name: "Default" 34 | topic: "input/default" 35 | timeout: 1.0 36 | priority: 0 37 | short_desc: "Default input, lowest priority" 38 | publisher: "output" -------------------------------------------------------------------------------- /racecar/racecar/config/racecar-v2/low_level_mux.yaml: -------------------------------------------------------------------------------- 1 | # Individual subscriber configuration: 2 | # name: Source name 3 | # topic: The topic that provides ackermann_cmd messages 4 | # timeout: Time in seconds without incoming messages to consider this topic inactive 5 | # priority: Priority: an UNIQUE unsigned integer from 0 (lowest) to MAX_INT 6 | # short_desc: Short description (optional) 7 | 8 | subscribers: 9 | - name: "Teleoperation" 10 | topic: "input/teleop" 11 | timeout: 0.2 12 | priority: 10 13 | short_desc: "Input for human teleoperation (joystick). Highest priority." 14 | 15 | - name: "Safety" 16 | topic: "input/safety" 17 | timeout: 0.2 18 | priority: 5 19 | short_desc: "Input for safety monitor." 20 | 21 | - name: "Navigation" 22 | topic: "input/navigation" 23 | timeout: 0.2 24 | priority: 0 25 | short_desc: "Input for autonomous navigation" 26 | 27 | publisher: "output" -------------------------------------------------------------------------------- /racecar/racecar/config/racecar-v2/sensors.yaml: -------------------------------------------------------------------------------- 1 | 2 | laser_node: 3 | device: /dev/ttyRplidar 4 | 5 | imu_node: 6 | device: /dev/ttyImu 7 | -------------------------------------------------------------------------------- /racecar/racecar/config/racecar-v2/vesc.yaml: -------------------------------------------------------------------------------- 1 | 2 | # erpm (electrical rpm) = speed_to_erpm_gain * speed (meters / second) + speed_to_erpm_offset 3 | # for offset=0. speed_to_erpm_gain = num_motor_poles*60/circumference_wheel_in_meters 4 | speed_to_erpm_gain: 4614 5 | speed_to_erpm_offset: 0.0 6 | 7 | tachometer_ticks_to_meters_gain: 0.00225 8 | # servo smoother - limits rotation speed and smooths anything above limit 9 | max_servo_speed: 3.2 # radians/second 10 | servo_smoother_rate: 75.0 # messages/sec 11 | 12 | # servo smoother - limits acceleration and smooths anything above limit 13 | max_acceleration: 2.5 # meters/second^2 14 | throttle_smoother_rate: 75.0 # messages/sec 15 | 16 | # servo value (0 to 1) = steering_angle_to_servo_gain * steering angle (radians) + steering_angle_to_servo_offset 17 | steering_angle_to_servo_gain: -1.2135 18 | steering_angle_to_servo_offset: 0.5304 19 | 20 | # publish odom to base link tf 21 | vesc_to_odom/publish_tf: false 22 | 23 | # car wheelbase is about 25cm 24 | wheelbase: .25 25 | 26 | vesc_driver: 27 | port: /dev/ttyVesc 28 | duty_cycle_min: 0.0 29 | duty_cycle_max: 0.0 30 | current_min: 0.0 31 | current_max: 20.0 32 | brake_min: -20000.0 33 | brake_max: 200000.0 34 | speed_min: -1200 35 | speed_max: 1200 36 | position_min: 0.0 37 | position_max: 0.0 38 | servo_min: 0.15 39 | servo_max: 0.85 40 | -------------------------------------------------------------------------------- /racecar/racecar/launch/includes/common/joy_teleop.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /racecar/racecar/launch/includes/common/sensors.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /racecar/racecar/launch/includes/racecar-v2-teleop.launch.xml: -------------------------------------------------------------------------------- 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 | -------------------------------------------------------------------------------- /racecar/racecar/launch/includes/racecar-v2/static_transforms.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 6 | 7 | 9 | 10 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /racecar/racecar/launch/includes/racecar-v2/vesc.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /racecar/racecar/launch/keyboard_control.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /racecar/racecar/launch/replay_bag_file/replay_bag_file.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 12 | 13 | -------------------------------------------------------------------------------- /racecar/racecar/launch/replay_bag_file/replay_bag_with_lidar_processing.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 33 | 35 | 36 | -------------------------------------------------------------------------------- /racecar/racecar/launch/teleop.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /racecar/racecar/maps/basement_hallways_10cm.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kyou2016/ROS_LaneDetectionWithBEV/fe8b086f84c3ee3b6017c5c70805886e68967e2a/racecar/racecar/maps/basement_hallways_10cm.png -------------------------------------------------------------------------------- /racecar/racecar/maps/basement_hallways_10cm.yaml: -------------------------------------------------------------------------------- 1 | image: basement_hallways_10cm.png 2 | resolution: 0.100000 3 | origin: [-30.000000, -30.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /racecar/racecar/maps/basement_hallways_5cm.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kyou2016/ROS_LaneDetectionWithBEV/fe8b086f84c3ee3b6017c5c70805886e68967e2a/racecar/racecar/maps/basement_hallways_5cm.png -------------------------------------------------------------------------------- /racecar/racecar/maps/basement_hallways_5cm.yml: -------------------------------------------------------------------------------- 1 | image: basement_hallways_5cm.png 2 | resolution: 0.050000 3 | origin: [-30.000000, -30.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /racecar/racecar/maps/short-course-33.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kyou2016/ROS_LaneDetectionWithBEV/fe8b086f84c3ee3b6017c5c70805886e68967e2a/racecar/racecar/maps/short-course-33.png -------------------------------------------------------------------------------- /racecar/racecar/maps/short-course-33.yml: -------------------------------------------------------------------------------- 1 | image: short-course-33.png 2 | resolution: 0.025000 3 | origin: [-11.50000, -5.000000, -0.090000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /racecar/racecar/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | racecar 4 | 0.0.1 5 | RACECAR launch scripts 6 | 7 | Michael Boulet 8 | Michael Boulet 9 | BSD 10 | 11 | 12 | 13 | 14 | 15 | catkin 16 | 17 | 18 | 19 | razor_imu_9dof 20 | tf 21 | tf2_ros 22 | urg_node 23 | joy 24 | rosbag 25 | rostopic 26 | rviz 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | gmapping 37 | hector_mapping 38 | robot_pose_ekf 39 | 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /racecar/racecar/scripts/utils.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kyou2016/ROS_LaneDetectionWithBEV/fe8b086f84c3ee3b6017c5c70805886e68967e2a/racecar/racecar/scripts/utils.pyc -------------------------------------------------------------------------------- /razor_imu_9dof/.gitignore: -------------------------------------------------------------------------------- 1 | *~ 2 | config/my_razor.yaml 3 | -------------------------------------------------------------------------------- /razor_imu_9dof/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(razor_imu_9dof) 3 | 4 | find_package(catkin REQUIRED COMPONENTS dynamic_reconfigure) 5 | 6 | generate_dynamic_reconfigure_options(cfg/imu.cfg) 7 | 8 | catkin_package(DEPENDS CATKIN DEPENDS dynamic_reconfigure) 9 | 10 | install(DIRECTORY launch 11 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 12 | ) 13 | 14 | install(DIRECTORY config 15 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 16 | ) 17 | 18 | install(DIRECTORY cfg 19 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 20 | ) 21 | 22 | install(DIRECTORY src 23 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 24 | ) 25 | 26 | install(DIRECTORY magnetometer_calibration 27 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 28 | ) 29 | 30 | catkin_install_python(PROGRAMS 31 | nodes/imu_node.py 32 | nodes/display_3D_visualization.py 33 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/nodes 34 | ) 35 | -------------------------------------------------------------------------------- /razor_imu_9dof/cfg/imu.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "razor_imu_9dof" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | gen.add("yaw_calibration", double_t, 0, "Yaw Calibration", 0, -10, 10) 8 | 9 | exit(gen.generate(PACKAGE, "razor_imu_9dof", "imu")) 10 | -------------------------------------------------------------------------------- /razor_imu_9dof/config/razor_diags.yaml: -------------------------------------------------------------------------------- 1 | pub_rate: 1.0 2 | analyzers: 3 | Razor9DofImu: 4 | type: diagnostic_aggregator/GenericAnalyzer 5 | path: 'Imu' 6 | timeout: 5.0 7 | contains: ['Razor_Imu'] 8 | -------------------------------------------------------------------------------- /razor_imu_9dof/launch/razor-display.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /razor_imu_9dof/launch/razor-pub-and-display.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /razor_imu_9dof/launch/razor-pub-diags.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /razor_imu_9dof/launch/razor-pub.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /razor_imu_9dof/magnetometer_calibration/Matlab/magnetometer_calibration/ellipsoid_fit_license.txt: -------------------------------------------------------------------------------- 1 | Copyright (c) 2009, Yury Petrov 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are 6 | met: 7 | 8 | * Redistributions of source code must retain the above copyright 9 | notice, this list of conditions and the following disclaimer. 10 | * Redistributions in binary form must reproduce the above copyright 11 | notice, this list of conditions and the following disclaimer in 12 | the documentation and/or other materials provided with the distribution 13 | 14 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 15 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 16 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 17 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 18 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 19 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 20 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 21 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 22 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 23 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 24 | POSSIBILITY OF SUCH DAMAGE. 25 | -------------------------------------------------------------------------------- /razor_imu_9dof/magnetometer_calibration/Processing/Magnetometer_calibration/data/Univers-66.vlw: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kyou2016/ROS_LaneDetectionWithBEV/fe8b086f84c3ee3b6017c5c70805886e68967e2a/razor_imu_9dof/magnetometer_calibration/Processing/Magnetometer_calibration/data/Univers-66.vlw -------------------------------------------------------------------------------- /razor_imu_9dof/package.xml: -------------------------------------------------------------------------------- 1 | 2 | razor_imu_9dof 3 | 1.2.0 4 | 5 | razor_imu_9dof is a package that provides a ROS driver 6 | for the Sparkfun Razor IMU 9DOF. It also provides Arduino 7 | firmware that runs on the Razor board, and which must be 8 | installed on the Razor board for the system to work. A node 9 | which displays the attitude (roll, pitch and yaw) of the Razor board 10 | (or any IMU) is provided for testing. 11 | 12 | 13 | Kristof Robot 14 | 15 | BSD 16 | 17 | Tang Tiong Yew 18 | Kristof Robot 19 | Paul Bouchier 20 | Peter Bartz 21 | 22 | http://ros.org/wiki/razor_imu_9dof 23 | 24 | catkin 25 | 26 | dynamic_reconfigure 27 | 28 | rospy 29 | tf 30 | sensor_msgs 31 | python-serial 32 | dynamic_reconfigure 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /razor_imu_9dof/src/Razor_AHRS/Compass.ino: -------------------------------------------------------------------------------- 1 | /* This file is part of the Razor AHRS Firmware */ 2 | 3 | void Compass_Heading() 4 | { 5 | float mag_x = 0; 6 | float mag_y = 0; 7 | float cos_roll = 0; 8 | float sin_roll = 0; 9 | float cos_pitch = 0; 10 | float sin_pitch = 0; 11 | 12 | cos_roll = cos(roll); 13 | sin_roll = sin(roll); 14 | cos_pitch = cos(pitch); 15 | sin_pitch = sin(pitch); 16 | 17 | // Tilt compensated magnetic field X 18 | mag_x = magnetom[0] * cos_pitch + magnetom[1] * sin_roll * sin_pitch + magnetom[2] * cos_roll * sin_pitch; 19 | // Tilt compensated magnetic field Y 20 | mag_y = magnetom[1] * cos_roll - magnetom[2] * sin_roll; 21 | // Magnetic Heading 22 | MAG_Heading = atan2(-mag_y, mag_x); 23 | } 24 | -------------------------------------------------------------------------------- /rf2o_laser_odometry/README.md: -------------------------------------------------------------------------------- 1 | # rf2o_laser_odometry 2 | Estimation of 2D odometry based on planar laser scans. Useful for mobile robots with innacurate base odometry. 3 | 4 | RF2O is a fast and precise method to estimate the planar motion of a lidar from consecutive range scans. For every scanned point we formulate the range flow constraint equation in terms of the sensor velocity, and minimize a robust function of the resulting geometric constraints to obtain the motion estimate. Conversely to traditional approaches, this method does not search for correspondences but performs dense scan alignment based on the scan gradients, in the fashion of dense 3D visual odometry. 5 | 6 | Its very low computational cost (0.9 milliseconds on a single CPU core) together whit its high precission, makes RF2O a suitable method for those robotic applications that require planar odometry. 7 | 8 | For full description of the algorithm, please refer to: **Planar Odometry from a Radial Laser Scanner. A Range Flow-based Approach. ICRA 2016** Available at: http://mapir.isa.uma.es/work/rf2o -------------------------------------------------------------------------------- /rplidar_ros/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(rplidar_ros) 3 | 4 | set(RPLIDAR_SDK_PATH "./sdk/") 5 | 6 | FILE(GLOB RPLIDAR_SDK_SRC 7 | "${RPLIDAR_SDK_PATH}/src/arch/linux/*.cpp" 8 | "${RPLIDAR_SDK_PATH}/src/hal/*.cpp" 9 | "${RPLIDAR_SDK_PATH}/src/*.cpp" 10 | ) 11 | 12 | find_package(catkin REQUIRED COMPONENTS 13 | roscpp 14 | rosconsole 15 | sensor_msgs 16 | ) 17 | 18 | include_directories( 19 | ${RPLIDAR_SDK_PATH}/include 20 | ${RPLIDAR_SDK_PATH}/src 21 | ${catkin_INCLUDE_DIRS} 22 | ) 23 | 24 | catkin_package() 25 | 26 | add_executable(rplidarNode src/node.cpp ${RPLIDAR_SDK_SRC}) 27 | target_link_libraries(rplidarNode ${catkin_LIBRARIES}) 28 | 29 | add_executable(rplidarNodeClient src/client.cpp) 30 | target_link_libraries(rplidarNodeClient ${catkin_LIBRARIES}) 31 | 32 | install(TARGETS rplidarNode rplidarNodeClient 33 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 34 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 35 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 36 | ) 37 | 38 | install(DIRECTORY launch rviz sdk 39 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 40 | USE_SOURCE_PERMISSIONS 41 | ) 42 | -------------------------------------------------------------------------------- /rplidar_ros/LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2009 - 2014 RoboPeak Team 2 | Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd. 3 | All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | * Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | * Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 19 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 20 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 21 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 22 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 23 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 24 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | -------------------------------------------------------------------------------- /rplidar_ros/launch/rplidar.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /rplidar_ros/launch/rplidar_a3.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /rplidar_ros/launch/rplidar_s1.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /rplidar_ros/launch/rplidar_s1_tcp.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /rplidar_ros/launch/test_rplidar.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /rplidar_ros/launch/test_rplidar_a3.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /rplidar_ros/launch/view_rplidar.launch: -------------------------------------------------------------------------------- 1 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /rplidar_ros/launch/view_rplidar_a3.launch: -------------------------------------------------------------------------------- 1 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /rplidar_ros/launch/view_rplidar_s1.launch: -------------------------------------------------------------------------------- 1 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /rplidar_ros/launch/view_rplidar_s1_tcp.launch: -------------------------------------------------------------------------------- 1 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /rplidar_ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rplidar_ros 4 | 1.10.0 5 | The rplidar ros package, support rplidar A2/A1 and A3/S1 6 | 7 | Slamtec ROS Maintainer 8 | BSD 9 | 10 | catkin 11 | roscpp 12 | rosconsole 13 | sensor_msgs 14 | std_srvs 15 | roscpp 16 | rosconsole 17 | sensor_msgs 18 | std_srvs 19 | 20 | 21 | -------------------------------------------------------------------------------- /rplidar_ros/rplidar_A1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kyou2016/ROS_LaneDetectionWithBEV/fe8b086f84c3ee3b6017c5c70805886e68967e2a/rplidar_ros/rplidar_A1.png -------------------------------------------------------------------------------- /rplidar_ros/rplidar_A2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kyou2016/ROS_LaneDetectionWithBEV/fe8b086f84c3ee3b6017c5c70805886e68967e2a/rplidar_ros/rplidar_A2.png -------------------------------------------------------------------------------- /rplidar_ros/scripts/create_udev_rules.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | echo "remap the device serial port(ttyUSBX) to rplidar" 4 | echo "rplidar usb connection as /dev/rplidar , check it using the command : ls -l /dev|grep ttyUSB" 5 | echo "start copy rplidar.rules to /etc/udev/rules.d/" 6 | echo "`rospack find rplidar_ros`/scripts/rplidar.rules" 7 | sudo cp `rospack find rplidar_ros`/scripts/rplidar.rules /etc/udev/rules.d 8 | echo " " 9 | echo "Restarting udev" 10 | echo "" 11 | sudo service udev reload 12 | sudo service udev restart 13 | echo "finish " 14 | -------------------------------------------------------------------------------- /rplidar_ros/scripts/delete_udev_rules.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | echo "delete remap the device serial port(ttyUSBX) to rplidar" 4 | echo "sudo rm /etc/udev/rules.d/rplidar.rules" 5 | sudo rm /etc/udev/rules.d/rplidar.rules 6 | echo " " 7 | echo "Restarting udev" 8 | echo "" 9 | sudo service udev reload 10 | sudo service udev restart 11 | echo "finish delete" 12 | -------------------------------------------------------------------------------- /rplidar_ros/scripts/rplidar.rules: -------------------------------------------------------------------------------- 1 | # set the udev rule , make the device_port be fixed by rplidar 2 | # 3 | KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", MODE:="0777", SYMLINK+="rplidar" 4 | 5 | -------------------------------------------------------------------------------- /rplidar_ros/sdk/src/hal/assert.h: -------------------------------------------------------------------------------- 1 | #ifndef _INFRA_HAL_ASSERT_H 2 | #define _INFRA_HAL_ASSERT_H 3 | 4 | #ifdef WIN32 5 | #include 6 | #ifndef assert 7 | #define assert(x) _ASSERT(x) 8 | #endif 9 | #elif defined(_AVR_) 10 | #define assert(x) 11 | #elif defined(__GNUC__) 12 | #ifndef assert 13 | #define assert(x) 14 | #endif 15 | #else 16 | #error assert.h cannot identify your platform 17 | #endif 18 | #endif 19 | -------------------------------------------------------------------------------- /slam_gmapping/.gitignore: -------------------------------------------------------------------------------- 1 | .cproject 2 | .project 3 | .pydevproject 4 | cmake_install.cmake 5 | build/ 6 | bin/ 7 | lib/ 8 | msg_gen/ 9 | srv_gen/ 10 | *.pyc 11 | *~ 12 | -------------------------------------------------------------------------------- /slam_gmapping/.travis.yml: -------------------------------------------------------------------------------- 1 | language: cpp 2 | compiler: 3 | - gcc 4 | - clang 5 | install: 6 | - export CI_ROS_DISTRO=hydro 7 | - echo $CI_ROS_DISTRO 8 | # Add ROS repositories 9 | - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu precise main" > /etc/apt/sources.list.d/ros-latest.list' 10 | - wget http://packages.ros.org/ros.key -O - | sudo apt-key add - 11 | - sudo apt-get update 12 | # Install and initialize rosdep 13 | - sudo apt-get install python-rosdep 14 | - sudo `which rosdep` init 15 | - rosdep update 16 | # Use rosdep to install rviz's dependencies 17 | - rosdep install --default-yes --from-paths ./ --rosdistro $CI_ROS_DISTRO 18 | script: 19 | - source /opt/ros/$CI_ROS_DISTRO/setup.bash 20 | - mkdir build 21 | - cd build 22 | - cmake .. -DCMAKE_INSTALL_PREFIX=./install 23 | - make -j1 24 | - make -j1 tests 25 | - make -j1 run_tests 26 | - catkin_test_results . 27 | - make -j1 install 28 | notifications: 29 | email: false 30 | -------------------------------------------------------------------------------- /slam_gmapping/gmapping/nodelet_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Nodelet ROS wrapper for OpenSlam's Gmapping. 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /slam_gmapping/gmapping/package.xml: -------------------------------------------------------------------------------- 1 | 2 | gmapping 3 | 1.4.1 4 | This package contains a ROS wrapper for OpenSlam's Gmapping. 5 | The gmapping package provides laser-based SLAM (Simultaneous Localization and Mapping), 6 | as a ROS node called slam_gmapping. Using slam_gmapping, you can create a 2-D occupancy 7 | grid map (like a building floorplan) from laser and pose data collected by a mobile robot. 8 | 9 | Brian Gerkey 10 | ROS Orphaned Package Maintainers 11 | BSD 12 | Apache 2.0 13 | 14 | http://ros.org/wiki/gmapping 15 | 16 | catkin 17 | 18 | nav_msgs 19 | openslam_gmapping 20 | roscpp 21 | rostest 22 | tf 23 | nodelet 24 | 25 | nav_msgs 26 | openslam_gmapping 27 | roscpp 28 | tf 29 | nodelet 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /slam_gmapping/gmapping/test/basic_localization_laser_different_beamcount.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 8 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /slam_gmapping/gmapping/test/basic_localization_stage.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 8 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /slam_gmapping/gmapping/test/basic_localization_stage_replay.launch: -------------------------------------------------------------------------------- 1 | 2 | 4 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /slam_gmapping/gmapping/test/basic_localization_stage_replay2.launch: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /slam_gmapping/gmapping/test/basic_localization_symmetry.launch: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /slam_gmapping/gmapping/test/basic_localization_upside_down.launch: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /slam_gmapping/slam_gmapping/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package slam_gmapping 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.4.1 (2020-03-16) 6 | ------------------ 7 | 8 | 1.4.0 (2019-07-12) 9 | ------------------ 10 | * update license to BSD and maintainer to ros-orphaned-packages@googlegroups.com 11 | since original gmapping source and ROS openslam_gmapping package has been updated to the BSD-3 license, I think we have no reason to use CC for slam_gmapping package 12 | * Contributors: Kei Okada 13 | 14 | 1.3.10 (2018-01-23) 15 | ------------------- 16 | 17 | 1.3.9 (2017-10-22) 18 | ------------------ 19 | 20 | 1.3.8 (2015-07-31) 21 | ------------------ 22 | 23 | 1.3.7 (2015-07-04) 24 | ------------------ 25 | 26 | 1.3.6 (2015-06-26) 27 | ------------------ 28 | 29 | 1.3.5 (2014-08-28) 30 | ------------------ 31 | 32 | 1.3.4 (2014-08-07) 33 | ------------------ 34 | 35 | 1.3.3 (2014-06-23) 36 | ------------------ 37 | 38 | 1.3.2 (2014-01-14) 39 | ------------------ 40 | 41 | 1.3.1 (2014-01-13) 42 | ------------------ 43 | 44 | 1.3.0 (2013-06-28) 45 | ------------------ 46 | * Renamed to gmapping, adding metapackage for slam_gmapping 47 | * Contributors: Mike Ferguson 48 | -------------------------------------------------------------------------------- /slam_gmapping/slam_gmapping/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(slam_gmapping) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /slam_gmapping/slam_gmapping/package.xml: -------------------------------------------------------------------------------- 1 | 2 | slam_gmapping 3 | 1.4.1 4 | slam_gmapping contains a wrapper around gmapping which provides SLAM capabilities. 5 | Brian Gerkey 6 | ROS Orphaned Package Maintainers 7 | BSD 8 | Apache 2.0 9 | 10 | http://ros.org/wiki/slam_gmapping 11 | 12 | catkin 13 | 14 | openslam_gmapping 15 | gmapping 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /usb_cam/.gitignore: -------------------------------------------------------------------------------- 1 | .cproject 2 | .project 3 | .settings 4 | -------------------------------------------------------------------------------- /usb_cam/.travis.yml: -------------------------------------------------------------------------------- 1 | sudo: required 2 | dist: trusty 3 | language: generic 4 | compiler: 5 | - gcc 6 | 7 | branches: 8 | only: 9 | - master 10 | - develop 11 | 12 | install: 13 | - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu trusty main" > /etc/apt/sources.list.d/ros-latest.list' 14 | - wget http://packages.ros.org/ros.key -O - | sudo apt-key add - 15 | - sudo apt-get update -qq 16 | - sudo apt-get install python-catkin-pkg python-rosdep ros-indigo-catkin -qq 17 | - sudo rosdep init 18 | - rosdep update 19 | - mkdir -p /tmp/ws/src 20 | - ln -s `pwd` /tmp/ws/src/package 21 | - cd /tmp/ws 22 | - rosdep install --from-paths src --ignore-src --rosdistro indigo -y 23 | 24 | script: 25 | - source /opt/ros/indigo/setup.bash 26 | - catkin_make 27 | - catkin_make install 28 | -------------------------------------------------------------------------------- /usb_cam/AUTHORS.md: -------------------------------------------------------------------------------- 1 | Original Authors 2 | ---------------- 3 | 4 | * [Benjamin Pitzer] (benjamin.pitzer@bosch.com) 5 | 6 | Contributors 7 | ------------ 8 | 9 | * [Russell Toris](http://users.wpi.edu/~rctoris/) (rctoris@wpi.edu) 10 | -------------------------------------------------------------------------------- /usb_cam/README.md: -------------------------------------------------------------------------------- 1 | usb_cam [![Build Status](https://api.travis-ci.org/bosch-ros-pkg/usb_cam.png)](https://travis-ci.org/bosch-ros-pkg/usb_cam) 2 | ======= 3 | 4 | #### A ROS Driver for V4L USB Cameras 5 | This package is based off of V4L devices specifically instead of just UVC. 6 | 7 | For full documentation, see [the ROS wiki](http://ros.org/wiki/usb_cam). 8 | 9 | [Doxygen](http://docs.ros.org/indigo/api/usb_cam/html/) files can be found on the ROS wiki. 10 | 11 | ### License 12 | usb_cam is released with a BSD license. For full terms and conditions, see the [LICENSE](LICENSE) file. 13 | 14 | ### Authors 15 | See the [AUTHORS](AUTHORS.md) file for a full list of contributors. 16 | -------------------------------------------------------------------------------- /usb_cam/launch/usb_cam-test (original).launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /usb_cam/launch/usb_cam-test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /usb_cam/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | 4 | \b usb_cam is a ROS driver for V4L USB cameras. 5 | */ 6 | -------------------------------------------------------------------------------- /usb_cam/package.xml: -------------------------------------------------------------------------------- 1 | 2 | usb_cam 3 | 0.3.6 4 | A ROS Driver for V4L USB Cameras 5 | 6 | Russell Toris 7 | ROS Orphaned Package Maintainers 8 | Benjamin Pitzer 9 | 10 | BSD 11 | 12 | http://wiki.ros.org/usb_cam 13 | https://github.com/bosch-ros-pkg/usb_cam/issues 14 | https://github.com/bosch-ros-pkg/usb_cam 15 | 16 | catkin 17 | 18 | image_transport 19 | roscpp 20 | std_msgs 21 | std_srvs 22 | sensor_msgs 23 | ffmpeg 24 | camera_info_manager 25 | 26 | image_transport 27 | roscpp 28 | std_msgs 29 | std_srvs 30 | sensor_msgs 31 | ffmpeg 32 | camera_info_manager 33 | v4l-utils 34 | 35 | -------------------------------------------------------------------------------- /usb_cam/src/LICENSE: -------------------------------------------------------------------------------- 1 | Video for Linux Two API Specification 2 | Revision 0.24 3 | Michael H Schimek 4 | Bill Dirks 5 | Hans Verkuil 6 | Martin Rubli 7 | 8 | Copyright © 1999, 2000, 2001, 2002, 2003, 2004, 2005, 2006, 2007, 2008 Bill Dirks, Michael H. Schimek, Hans Verkuil, Martin Rubli 9 | 10 | This document is copyrighted © 1999-2008 by Bill Dirks, Michael H. Schimek, Hans Verkuil and Martin Rubli. 11 | 12 | Permission is granted to copy, distribute and/or modify this document under the terms of the GNU Free Documentation License, Version 1.1 or any later version published by the Free Software Foundation; with no Invariant Sections, with no Front-Cover Texts, and with no Back-Cover Texts. A copy of the license is included in the appendix entitled "GNU Free Documentation License". 13 | 14 | Programming examples can be used and distributed without restrictions. 15 | -------------------------------------------------------------------------------- /vesc/.gitignore: -------------------------------------------------------------------------------- 1 | # -*- mode: gitignore; -*- 2 | *~ 3 | \#*\# 4 | 5 | -------------------------------------------------------------------------------- /vesc/vesc/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(vesc) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /vesc/vesc/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | vesc 4 | 0.0.1 5 | 6 | Metapackage for ROS interface to the Vedder VESC open source motor controller. 7 | 8 | 9 | Michael T. Boulet 10 | Michael T. Boulet 11 | BSD 12 | 13 | http://www.ros.org/wiki/vesc 14 | https://github.mit.edu/racecar/vesc 15 | https://github.mit.edu/racecar/vesc/issues 16 | 17 | catkin 18 | 19 | vesc_driver 20 | vesc_msgs 21 | vesc_ackermann 22 | 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /vesc/vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.h: -------------------------------------------------------------------------------- 1 | // -*- mode:c++; fill-column: 100; -*- 2 | 3 | #ifndef VESC_ACKERMANN_ACKERMANN_TO_VESC_H_ 4 | #define VESC_ACKERMANN_ACKERMANN_TO_VESC_H_ 5 | 6 | #include 7 | #include 8 | 9 | namespace vesc_ackermann 10 | { 11 | 12 | class AckermannToVesc 13 | { 14 | public: 15 | 16 | AckermannToVesc(ros::NodeHandle nh, ros::NodeHandle private_nh); 17 | 18 | private: 19 | // ROS parameters 20 | // conversion gain and offset 21 | double speed_to_erpm_gain_, speed_to_erpm_offset_; 22 | double steering_to_servo_gain_, steering_to_servo_offset_; 23 | 24 | /** @todo consider also providing an interpolated look-up table conversion */ 25 | 26 | // ROS services 27 | ros::Publisher erpm_pub_; 28 | ros::Publisher servo_pub_; 29 | ros::Subscriber ackermann_sub_; 30 | 31 | // ROS callbacks 32 | void ackermannCmdCallback(const ackermann_msgs::AckermannDriveStamped::ConstPtr& cmd); 33 | }; 34 | 35 | } // namespace vesc_ackermann 36 | 37 | #endif // VESC_ACKERMANN_ACKERMANN_TO_VESC_H_ 38 | -------------------------------------------------------------------------------- /vesc/vesc_ackermann/launch/ackermann_to_vesc_node.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /vesc/vesc_ackermann/launch/ackermann_to_vesc_nodelet.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 16 | 17 | 18 | 19 | 21 | 22 | 23 | 24 | 25 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /vesc/vesc_ackermann/launch/vesc_to_odom_node.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /vesc/vesc_ackermann/launch/vesc_to_odom_nodelet.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 16 | 17 | 18 | 19 | 21 | 22 | 23 | 24 | 25 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /vesc/vesc_ackermann/src/ackermann_to_vesc_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "vesc_ackermann/ackermann_to_vesc.h" 4 | 5 | int main(int argc, char** argv) 6 | { 7 | ros::init(argc, argv, "ackermann_to_vesc_node"); 8 | ros::NodeHandle nh; 9 | ros::NodeHandle private_nh("~"); 10 | 11 | vesc_ackermann::AckermannToVesc ackermann_to_vesc(nh, private_nh); 12 | 13 | ros::spin(); 14 | 15 | return 0; 16 | } 17 | -------------------------------------------------------------------------------- /vesc/vesc_ackermann/src/ackermann_to_vesc_nodelet.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include "vesc_ackermann/ackermann_to_vesc.h" 7 | 8 | namespace vesc_ackermann 9 | { 10 | 11 | class AckermannToVescNodelet: public nodelet::Nodelet 12 | { 13 | public: 14 | 15 | AckermannToVescNodelet() {} 16 | 17 | private: 18 | 19 | virtual void onInit(void); 20 | 21 | boost::shared_ptr ackermann_to_vesc_; 22 | 23 | }; // class AckermannToVescNodelet 24 | 25 | void AckermannToVescNodelet::onInit() 26 | { 27 | NODELET_DEBUG("Initializing ackermann to VESC nodelet"); 28 | ackermann_to_vesc_.reset(new AckermannToVesc(getNodeHandle(), getPrivateNodeHandle())); 29 | } 30 | 31 | } // namespace vesc_ackermann 32 | 33 | PLUGINLIB_EXPORT_CLASS(vesc_ackermann::AckermannToVescNodelet, nodelet::Nodelet); 34 | -------------------------------------------------------------------------------- /vesc/vesc_ackermann/src/vesc_to_odom_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "vesc_ackermann/vesc_to_odom.h" 4 | 5 | int main(int argc, char** argv) 6 | { 7 | ros::init(argc, argv, "vesc_to_odom_node"); 8 | ros::NodeHandle nh; 9 | ros::NodeHandle private_nh("~"); 10 | 11 | vesc_ackermann::VescToOdom vesc_to_odom(nh, private_nh); 12 | 13 | ros::spin(); 14 | 15 | return 0; 16 | } 17 | -------------------------------------------------------------------------------- /vesc/vesc_ackermann/src/vesc_to_odom_nodelet.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include "vesc_ackermann/vesc_to_odom.h" 7 | 8 | namespace vesc_ackermann 9 | { 10 | 11 | class VescToOdomNodelet: public nodelet::Nodelet 12 | { 13 | public: 14 | 15 | VescToOdomNodelet() {} 16 | 17 | private: 18 | 19 | virtual void onInit(void); 20 | 21 | boost::shared_ptr vesc_to_odom_; 22 | 23 | }; // class VescToOdomNodelet 24 | 25 | void VescToOdomNodelet::onInit() 26 | { 27 | NODELET_DEBUG("Initializing RACECAR VESC odometry estimator nodelet"); 28 | vesc_to_odom_.reset(new VescToOdom(getNodeHandle(), getPrivateNodeHandle())); 29 | } 30 | 31 | } // namespace vesc_ackermann 32 | 33 | PLUGINLIB_EXPORT_CLASS(vesc_ackermann::VescToOdomNodelet, nodelet::Nodelet); 34 | -------------------------------------------------------------------------------- /vesc/vesc_ackermann/vesc_ackermann_nodelet.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | Nodelet for translating ackermann messages to VESC motor controller commands. 4 | 5 | 6 | Nodelet for translating VESC motor controller state to odometry messages. 7 | 8 | 9 | -------------------------------------------------------------------------------- /vesc/vesc_driver/launch/vesc_driver_node.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /vesc/vesc_driver/launch/vesc_driver_nodelet.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 20 | 21 | 22 | 23 | 25 | 26 | 27 | 28 | 29 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /vesc/vesc_driver/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | vesc_driver 4 | 0.0.1 5 | 6 | ROS device driver for the Vedder VESC open source motor driver. 7 | 8 | 9 | Michael T. Boulet 10 | Michael T. Boulet 11 | BSD 12 | 13 | http://www.ros.org/wiki/vesc_driver 14 | https://github.mit.edu/racecar/vesc 15 | https://github.mit.edu/racecar/vesc/issues 16 | 17 | catkin 18 | 19 | nodelet 20 | pluginlib 21 | roscpp 22 | std_msgs 23 | vesc_msgs 24 | serial 25 | 26 | nodelet 27 | pluginlib 28 | roscpp 29 | std_msgs 30 | vesc_msgs 31 | serial 32 | 33 | 34 | 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /vesc/vesc_driver/src/vesc_driver_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "vesc_driver/vesc_driver.h" 4 | 5 | int main(int argc, char** argv) 6 | { 7 | ros::init(argc, argv, "vesc_driver_node"); 8 | ros::NodeHandle nh; 9 | ros::NodeHandle private_nh("~"); 10 | 11 | vesc_driver::VescDriver vesc_driver(nh, private_nh); 12 | 13 | ros::spin(); 14 | 15 | return 0; 16 | } 17 | -------------------------------------------------------------------------------- /vesc/vesc_driver/src/vesc_driver_nodelet.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include "vesc_driver/vesc_driver.h" 7 | 8 | namespace vesc_driver 9 | { 10 | 11 | class VescDriverNodelet: public nodelet::Nodelet 12 | { 13 | public: 14 | 15 | VescDriverNodelet() {} 16 | 17 | private: 18 | 19 | virtual void onInit(void); 20 | 21 | boost::shared_ptr vesc_driver_; 22 | 23 | }; // class VescDriverNodelet 24 | 25 | void VescDriverNodelet::onInit() 26 | { 27 | NODELET_DEBUG("Initializing VESC driver nodelet"); 28 | vesc_driver_.reset(new VescDriver(getNodeHandle(), getPrivateNodeHandle())); 29 | } 30 | 31 | } // namespace vesc_driver 32 | 33 | PLUGINLIB_EXPORT_CLASS(vesc_driver::VescDriverNodelet, nodelet::Nodelet); 34 | -------------------------------------------------------------------------------- /vesc/vesc_driver/vesc_driver_nodelet.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | Vedder VESC motor controller interface nodelet. 4 | 5 | 6 | -------------------------------------------------------------------------------- /vesc/vesc_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(vesc_msgs) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | std_msgs 6 | message_generation 7 | ) 8 | 9 | add_message_files( 10 | DIRECTORY msg 11 | FILES 12 | VescState.msg 13 | VescStateStamped.msg 14 | ) 15 | generate_messages( 16 | DEPENDENCIES 17 | std_msgs 18 | ) 19 | 20 | catkin_package( 21 | CATKIN_DEPENDS std_msgs message_runtime 22 | ) 23 | -------------------------------------------------------------------------------- /vesc/vesc_msgs/msg/VescState.msg: -------------------------------------------------------------------------------- 1 | # Vedder VESC open source motor controller state (telemetry) 2 | 3 | # fault codes 4 | int32 FAULT_CODE_NONE=0 5 | int32 FAULT_CODE_OVER_VOLTAGE=1 6 | int32 FAULT_CODE_UNDER_VOLTAGE=2 7 | int32 FAULT_CODE_DRV8302=3 8 | int32 FAULT_CODE_ABS_OVER_CURRENT=4 9 | int32 FAULT_CODE_OVER_TEMP_FET=5 10 | int32 FAULT_CODE_OVER_TEMP_MOTOR=6 11 | 12 | float64 voltage_input # input voltage (volt) 13 | float64 temperature_pcb # temperature of printed circuit board (degrees Celsius) 14 | float64 current_motor # motor current (ampere) 15 | float64 current_input # input current (ampere) 16 | float64 speed # motor electrical speed (revolutions per minute) 17 | float64 duty_cycle # duty cycle (0 to 1) 18 | float64 charge_drawn # electric charge drawn from input (ampere-hour) 19 | float64 charge_regen # electric charge regenerated to input (ampere-hour) 20 | float64 energy_drawn # energy drawn from input (watt-hour) 21 | float64 energy_regen # energy regenerated to input (watt-hour) 22 | float64 displacement # net tachometer (counts) 23 | float64 distance_traveled # total tachnometer (counts) 24 | int32 fault_code 25 | -------------------------------------------------------------------------------- /vesc/vesc_msgs/msg/VescStateStamped.msg: -------------------------------------------------------------------------------- 1 | # Timestamped VESC open source motor controller state (telemetry) 2 | 3 | Header header 4 | VescState state -------------------------------------------------------------------------------- /vesc/vesc_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | vesc_msgs 4 | 0.0.1 5 | 6 | ROS message definitions for the Vedder VESC open source motor controller. 7 | 8 | 9 | Michael T. Boulet 10 | Michael T. Boulet 11 | BSD 12 | 13 | http://ros.org/wiki/vesc_msgs 14 | https://github.mit.edu/racecar/vesc 15 | https://github.mit.edu/racecar/vesc/issues 16 | 17 | catkin 18 | 19 | std_msgs 20 | message_generation 21 | 22 | std_msgs 23 | message_runtime 24 | 25 | 26 | --------------------------------------------------------------------------------