├── .vscode ├── c_cpp_properties.json └── settings.json ├── CMakeLists.txt ├── README.md ├── data ├── Tf_real.jpg ├── Tf_simulation.png ├── battery.png ├── buck.png ├── final.png ├── jetson.png ├── lidar.png ├── mega.png ├── motordriver.png ├── motors.png ├── motortest.gif ├── mq1.jpg ├── nano.jpg ├── robot.jpg ├── robot1.jpg ├── robot2.jpg ├── robot3.jpg ├── robot4.jpg ├── robot5.jpg ├── robot7.jpg ├── robot8.jpg ├── video.png └── wheels.png ├── differential_drive ├── CMakeLists.txt ├── images │ └── crosshair.jpg ├── launch │ ├── motor_sim.launch │ ├── odom_ekf.launch │ └── tf_controller.launch ├── manifest.xml ├── package.xml └── scripts │ ├── diff_tf.py │ ├── pid_velocity.py │ ├── twist_to_motors.py │ ├── virtual_joystick.py │ ├── wheel_loopback.py │ └── wheel_scaler.py ├── firmware └── ARC_2.0 │ └── ARC_2.0.ino ├── navbot_description ├── CMakeLists.txt ├── LICENSE ├── launch │ ├── controller.launch │ ├── controller.yaml │ ├── display.launch │ ├── gazebo.launch │ ├── urdf.rviz │ └── world.launch ├── maps │ ├── mymap.pgm │ └── mymap.yaml ├── meshes │ ├── back_left_1.stl │ ├── back_right_1.stl │ ├── base_link.stl │ ├── camera_link_1.stl │ ├── camera_link_v1_1.stl │ ├── front_left_1.stl │ ├── front_right_1.stl │ ├── laser_link_1.stl │ └── laser_link_v1_1.stl ├── package.xml ├── urdf │ ├── materials.xacro │ ├── navbot.gazebo │ ├── navbot.trans │ ├── navbot.xacro │ └── navbot_gazebo_plugins.xacro └── worlds │ ├── home.world │ └── myhome.world ├── navbot_navigation ├── CMakeLists.txt ├── launch │ ├── amcl.launch │ ├── move_base.launch │ ├── t265_odom.launch │ └── tf.launch ├── maps │ ├── hall.pgm │ ├── hall.yaml │ ├── outdooe.pgm │ └── outdooe.yaml ├── package.xml └── param │ ├── base_local_planner_params.yaml │ ├── base_local_planner_params.yaml.save │ ├── base_local_planner_params.yaml.save.1 │ ├── costmap_common_params.yaml │ ├── global_costmap_params.yaml │ ├── local_costmap_params.yaml │ └── move_base_params.yaml ├── reconf_params ├── final_reconf_params_real.yaml └── final_reconf_params_sim.yaml ├── rviz_config └── Navbot.rviz ├── slam_gmapping ├── README.md ├── gmapping │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ ├── launch │ │ ├── mapping.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 └── ydlidar_ros ├── CMakeLists.txt ├── docs ├── paramters.md └── ydlidar.md ├── launch ├── G1.launch ├── G2.launch ├── G2C.launch ├── G4.launch ├── G6.launch ├── R2.launch ├── S2.launch ├── S4.launch ├── TG.launch ├── TX20.launch ├── TX8.launch ├── X2L.launch ├── X4.launch ├── display.launch ├── gazebo.launch ├── laser.launch ├── lidar.launch ├── lidar.rviz ├── lidar_view.launch └── yd_lidar.launch ├── meshes ├── ydlidar.dae └── ydlidar.png ├── package.xml ├── sdk ├── CMakeLists.txt ├── Doxyfile ├── README.md ├── doc │ ├── API.pdf │ └── README.pdf ├── image │ ├── EAI.png │ ├── FlowChart.png │ ├── G1.png │ ├── G2.png │ ├── G4.png │ ├── G6.png │ ├── T15.jpg │ ├── TG30.png │ ├── TOF.png │ ├── TS4.jpg │ ├── TX20.png │ ├── YDLidar.jpg │ ├── image.png │ ├── index-TX8.jpg │ ├── index-X2.jpg │ ├── index-X4.jpg │ ├── sdk_architecture.png │ ├── sdk_init.png │ ├── sdk_scanning.png │ ├── step1.png │ ├── step2.png │ ├── step3.png │ ├── step4.png │ ├── step5.png │ └── step6.png ├── include │ ├── CYdLidar.h │ ├── angles.h │ ├── help_info.h │ ├── lock.h │ ├── locker.h │ ├── serial.h │ ├── thread.h │ ├── timer.h │ ├── utils.h │ ├── v8stdint.h │ ├── ydlidar_driver.h │ └── ydlidar_protocol.h ├── license ├── samples │ ├── CMakeLists.txt │ └── main.cpp └── src │ ├── CYdLidar.cpp │ ├── common.h │ ├── impl │ ├── list_ports │ │ ├── list_ports_linux.cpp │ │ ├── list_ports_osx.cpp │ │ └── list_ports_win.cpp │ ├── unix │ │ ├── list_ports_linux.cpp │ │ ├── unix.h │ │ ├── unix_serial.cpp │ │ ├── unix_serial.h │ │ └── unix_timer.cpp │ └── windows │ │ ├── list_ports_win.cpp │ │ ├── win.h │ │ ├── win_serial.cpp │ │ ├── win_serial.h │ │ └── win_timer.cpp │ ├── lock.c │ ├── serial.cpp │ └── ydlidar_driver.cpp ├── src ├── .vscode │ └── settings.json ├── ydlidar_client.cpp └── ydlidar_node.cpp ├── startup └── initenv.sh ├── urdf └── ydlidar.urdf └── ydlidar.rviz /.vscode/c_cpp_properties.json: -------------------------------------------------------------------------------- 1 | { 2 | "configurations": [ 3 | { 4 | "browse": { 5 | "databaseFilename": "", 6 | "limitSymbolsToIncludedHeaders": true 7 | }, 8 | "includePath": [ 9 | "/home/jerinpeter/catkin_ws/devel/include/**", 10 | "/opt/ros/noetic/include/**", 11 | "/home/jerinpeter/catkin_ws/src/navigation/amcl/include/**", 12 | "/home/jerinpeter/catkin_ws/src/navigation/base_local_planner/include/**", 13 | "/home/jerinpeter/catkin_ws/src/navigation/carrot_planner/include/**", 14 | "/home/jerinpeter/catkin_ws/src/navigation/clear_costmap_recovery/include/**", 15 | "/home/jerinpeter/catkin_ws/src/navigation/costmap_2d/include/**", 16 | "/home/jerinpeter/catkin_ws/src/depthimage_to_laserscan/include/**", 17 | "/home/jerinpeter/catkin_ws/src/navigation/dwa_local_planner/include/**", 18 | "/home/jerinpeter/catkin_ws/src/freenect_stack/freenect_camera/include/**", 19 | "/home/jerinpeter/catkin_ws/src/navigation/global_planner/include/**", 20 | "/home/jerinpeter/catkin_ws/src/navigation/map_server/include/**", 21 | "/home/jerinpeter/catkin_ws/src/navigation/move_base/include/**", 22 | "/home/jerinpeter/catkin_ws/src/navigation/move_slow_and_clear/include/**", 23 | "/home/jerinpeter/catkin_ws/src/navigation/nav_core/include/**", 24 | "/home/jerinpeter/catkin_ws/src/navbot_navigation/include/**", 25 | "/home/jerinpeter/catkin_ws/src/navigation/navfn/include/**", 26 | "/home/jerinpeter/catkin_ws/src/opencv_apps/include/**", 27 | "/home/jerinpeter/catkin_ws/src/rosserial/rosserial_server/include/**", 28 | "/home/jerinpeter/catkin_ws/src/rosserial/rosserial_test/include/**", 29 | "/home/jerinpeter/catkin_ws/src/navigation/rotate_recovery/include/**", 30 | "/home/jerinpeter/catkin_ws/src/turtlebot3_simulations/turtlebot3_fake/include/**", 31 | "/home/jerinpeter/catkin_ws/src/turtlebot3_simulations/turtlebot3_gazebo/include/**", 32 | "/home/jerinpeter/catkin_ws/src/navigation/voxel_grid/include/**", 33 | "/usr/include/**" 34 | ], 35 | "name": "ROS" 36 | } 37 | ], 38 | "version": 4 39 | } -------------------------------------------------------------------------------- /.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "python.autoComplete.extraPaths": [ 3 | "/home/jerinpeter/catkin_ws/devel/lib/python3/dist-packages", 4 | "/opt/ros/noetic/lib/python3/dist-packages" 5 | ], 6 | "python.analysis.extraPaths": [ 7 | "/home/jerinpeter/catkin_ws/devel/lib/python3/dist-packages", 8 | "/opt/ros/noetic/lib/python3/dist-packages" 9 | ] 10 | } -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | /opt/ros/melodic/share/catkin/cmake/toplevel.cmake -------------------------------------------------------------------------------- /data/Tf_real.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jerinpeter/4wdNavbot/0962ac854c9dd4a1dd54da58aef3486182f03057/data/Tf_real.jpg -------------------------------------------------------------------------------- /data/Tf_simulation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jerinpeter/4wdNavbot/0962ac854c9dd4a1dd54da58aef3486182f03057/data/Tf_simulation.png -------------------------------------------------------------------------------- /data/battery.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jerinpeter/4wdNavbot/0962ac854c9dd4a1dd54da58aef3486182f03057/data/battery.png -------------------------------------------------------------------------------- /data/buck.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jerinpeter/4wdNavbot/0962ac854c9dd4a1dd54da58aef3486182f03057/data/buck.png -------------------------------------------------------------------------------- /data/final.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jerinpeter/4wdNavbot/0962ac854c9dd4a1dd54da58aef3486182f03057/data/final.png -------------------------------------------------------------------------------- /data/jetson.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jerinpeter/4wdNavbot/0962ac854c9dd4a1dd54da58aef3486182f03057/data/jetson.png -------------------------------------------------------------------------------- /data/lidar.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jerinpeter/4wdNavbot/0962ac854c9dd4a1dd54da58aef3486182f03057/data/lidar.png -------------------------------------------------------------------------------- /data/mega.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jerinpeter/4wdNavbot/0962ac854c9dd4a1dd54da58aef3486182f03057/data/mega.png -------------------------------------------------------------------------------- /data/motordriver.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jerinpeter/4wdNavbot/0962ac854c9dd4a1dd54da58aef3486182f03057/data/motordriver.png -------------------------------------------------------------------------------- /data/motors.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jerinpeter/4wdNavbot/0962ac854c9dd4a1dd54da58aef3486182f03057/data/motors.png -------------------------------------------------------------------------------- /data/motortest.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jerinpeter/4wdNavbot/0962ac854c9dd4a1dd54da58aef3486182f03057/data/motortest.gif -------------------------------------------------------------------------------- /data/mq1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jerinpeter/4wdNavbot/0962ac854c9dd4a1dd54da58aef3486182f03057/data/mq1.jpg -------------------------------------------------------------------------------- /data/nano.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jerinpeter/4wdNavbot/0962ac854c9dd4a1dd54da58aef3486182f03057/data/nano.jpg -------------------------------------------------------------------------------- /data/robot.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jerinpeter/4wdNavbot/0962ac854c9dd4a1dd54da58aef3486182f03057/data/robot.jpg -------------------------------------------------------------------------------- /data/robot1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jerinpeter/4wdNavbot/0962ac854c9dd4a1dd54da58aef3486182f03057/data/robot1.jpg -------------------------------------------------------------------------------- /data/robot2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jerinpeter/4wdNavbot/0962ac854c9dd4a1dd54da58aef3486182f03057/data/robot2.jpg -------------------------------------------------------------------------------- /data/robot3.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jerinpeter/4wdNavbot/0962ac854c9dd4a1dd54da58aef3486182f03057/data/robot3.jpg -------------------------------------------------------------------------------- /data/robot4.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jerinpeter/4wdNavbot/0962ac854c9dd4a1dd54da58aef3486182f03057/data/robot4.jpg -------------------------------------------------------------------------------- /data/robot5.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jerinpeter/4wdNavbot/0962ac854c9dd4a1dd54da58aef3486182f03057/data/robot5.jpg -------------------------------------------------------------------------------- /data/robot7.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jerinpeter/4wdNavbot/0962ac854c9dd4a1dd54da58aef3486182f03057/data/robot7.jpg -------------------------------------------------------------------------------- /data/robot8.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jerinpeter/4wdNavbot/0962ac854c9dd4a1dd54da58aef3486182f03057/data/robot8.jpg -------------------------------------------------------------------------------- /data/video.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jerinpeter/4wdNavbot/0962ac854c9dd4a1dd54da58aef3486182f03057/data/video.png -------------------------------------------------------------------------------- /data/wheels.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jerinpeter/4wdNavbot/0962ac854c9dd4a1dd54da58aef3486182f03057/data/wheels.png -------------------------------------------------------------------------------- /differential_drive/images/crosshair.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jerinpeter/4wdNavbot/0962ac854c9dd4a1dd54da58aef3486182f03057/differential_drive/images/crosshair.jpg -------------------------------------------------------------------------------- /differential_drive/launch/motor_sim.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /differential_drive/launch/odom_ekf.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /differential_drive/launch/tf_controller.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /differential_drive/manifest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Provides some basic tools for interfacing a differential-drive 5 | robot with the ROS navigation stack. The intent is to make this 6 | independent of specific robot implementation. 7 | 8 | 9 | Jon Stephan 10 | GNU GPL3 11 | 12 | http://ros.org/wiki/differential_drive 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /differential_drive/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | differential_drive 4 | 0.0.0 5 | The differential_drive package 6 | 7 | 8 | 9 | 10 | imesh 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | rospy 53 | std_msgs 54 | tf 55 | rospy 56 | std_msgs 57 | tf 58 | rospy 59 | std_msgs 60 | tf 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | -------------------------------------------------------------------------------- /differential_drive/scripts/twist_to_motors.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | """ 3 | twist_to_motors - converts a twist message to motor commands. Needed for navigation stack 4 | 5 | 6 | Copyright (C) 2012 Jon Stephan. 7 | 8 | This program is free software: you can redistribute it and/or modify 9 | it under the terms of the GNU General Public License as published by 10 | the Free Software Foundation, either version 3 of the License, or 11 | (at your option) any later version. 12 | 13 | This program is distributed in the hope that it will be useful, 14 | but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | GNU General Public License for more details. 17 | 18 | You should have received a copy of the GNU General Public License 19 | along with this program. If not, see . 20 | """ 21 | 22 | import rospy 23 | import roslib 24 | from std_msgs.msg import Float32 25 | from geometry_msgs.msg import Twist 26 | 27 | ############################################################# 28 | ############################################################# 29 | class TwistToMotors(): 30 | ############################################################# 31 | ############################################################# 32 | 33 | ############################################################# 34 | def __init__(self): 35 | ############################################################# 36 | rospy.init_node("twist_to_motors") 37 | nodename = rospy.get_name() 38 | rospy.loginfo("%s started" % nodename) 39 | 40 | self.w = rospy.get_param("~base_width", 0.2) 41 | 42 | self.pub_lmotor = rospy.Publisher('lwheel_tangent_vel_target', Float32, queue_size=10) 43 | self.pub_rmotor = rospy.Publisher('rwheel_tangent_vel_target', Float32, queue_size=10) 44 | rospy.Subscriber('cmd_vel', Twist, self.twistCallback) 45 | 46 | 47 | self.rate = rospy.get_param("~rate", 50) 48 | self.timeout_ticks = rospy.get_param("~timeout_ticks", 2) 49 | self.left = 0 50 | self.right = 0 51 | 52 | ############################################################# 53 | def spin(self): 54 | ############################################################# 55 | 56 | r = rospy.Rate(self.rate) 57 | idle = rospy.Rate(10) 58 | then = rospy.Time.now() 59 | self.ticks_since_target = self.timeout_ticks 60 | 61 | ###### main loop ###### 62 | while not rospy.is_shutdown(): 63 | 64 | while not rospy.is_shutdown() and self.ticks_since_target < self.timeout_ticks: 65 | self.spinOnce() 66 | r.sleep() 67 | idle.sleep() 68 | 69 | ############################################################# 70 | def spinOnce(self): 71 | ############################################################# 72 | 73 | # dx = (l + r) / 2 74 | # dr = (r - l) / w 75 | 76 | self.right = 1.0 * self.dx + self.dr * self.w / 2 77 | self.left = 1.0 * self.dx - self.dr * self.w / 2 78 | # rospy.loginfo("publishing: (%d, %d)", left, right) 79 | 80 | self.pub_lmotor.publish(self.left) 81 | self.pub_rmotor.publish(self.right) 82 | 83 | self.ticks_since_target += 1 84 | 85 | ############################################################# 86 | def twistCallback(self,msg): 87 | ############################################################# 88 | # rospy.loginfo("-D- twistCallback: %s" % str(msg)) 89 | self.ticks_since_target = 0 90 | self.dx = msg.linear.x /30 #added dev by 40 91 | self.dr = msg.angular.z /30 #added dev by 40 92 | self.dy = msg.linear.y 93 | 94 | ############################################################# 95 | ############################################################# 96 | if __name__ == '__main__': 97 | """ main """ 98 | twistToMotors = TwistToMotors() 99 | twistToMotors.spin() 100 | -------------------------------------------------------------------------------- /differential_drive/scripts/virtual_joystick.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """ 4 | 5 | Copyright (C) 2012 Jon Stephan. 6 | 7 | This program is free software: you can redistribute it and/or modify 8 | it under the terms of the GNU General Public License as published by 9 | the Free Software Foundation, either version 3 of the License, or 10 | (at your option) any later version. 11 | 12 | This program is distributed in the hope that it will be useful, 13 | but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | GNU General Public License for more details. 16 | 17 | You should have received a copy of the GNU General Public License 18 | along with this program. If not, see . 19 | 20 | """ 21 | import sys 22 | 23 | import roslib; roslib.load_manifest('differential_drive') 24 | import rospy 25 | from geometry_msgs.msg import Twist 26 | import inspect, os 27 | 28 | 29 | from PySide import QtGui, QtCore 30 | 31 | ########################################################################## 32 | ########################################################################## 33 | class MainWindow(QtGui.QMainWindow): 34 | ########################################################################## 35 | ########################################################################## 36 | 37 | ##################################################################### 38 | def __init__(self): 39 | ##################################################################### 40 | super(MainWindow, self).__init__() 41 | self.timer_rate = rospy.get_param('~publish_rate', 50) 42 | self.pub_twist = rospy.Publisher('mobile_base_controller/cmd_vel', Twist, queue_size=10) 43 | 44 | self.initUI() 45 | 46 | ##################################################################### 47 | def initUI(self): 48 | ##################################################################### 49 | 50 | img_path = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe()))) + "/../images/crosshair.jpg" 51 | rospy.loginfo("initUI img_path: %s" % img_path) 52 | 53 | self.statusBar() 54 | 55 | self.setStyleSheet("QMainWindow { border-image: url(%s); }" % img_path) 56 | 57 | 58 | self.setGeometry(0, 600, 200, 200) 59 | self.setWindowTitle('Virtual Joystick') 60 | self.show() 61 | self.timer = QtCore.QBasicTimer() 62 | 63 | self.statusBar().showMessage('started') 64 | 65 | ##################################################################### 66 | def mousePressEvent(self, event): 67 | ##################################################################### 68 | self.statusBar().showMessage('mouse clicked') 69 | self.timer.start(self.timer_rate, self) 70 | self.get_position(event) 71 | 72 | ##################################################################### 73 | def mouseReleaseEvent(self, event): 74 | ##################################################################### 75 | self.statusBar().showMessage('mouse released') 76 | self.timer.stop() 77 | 78 | ##################################################################### 79 | def mouseMoveEvent(self, event): 80 | ##################################################################### 81 | self.get_position(event) 82 | 83 | ##################################################################### 84 | def get_position(self, event): 85 | ##################################################################### 86 | s = self.size() 87 | s_w = s.width() 88 | s_h = s.height() 89 | pos = event.pos() 90 | self.x = 1.0 * pos.x() / s_w 91 | self.y = 1.0 * pos.y() / s_h 92 | 93 | self.statusBar().showMessage('point (%0.2f, %0.2f)' % (self.x,self.y)) 94 | 95 | ##################################################################### 96 | def timerEvent(self, event): 97 | ##################################################################### 98 | # self.statusBar().showMessage("timer tick") 99 | self.pubTwist() 100 | 101 | ####################################################### 102 | def pubTwist(self): 103 | ####################################################### 104 | # rospy.loginfo("publishing twist from (%0.3f,%0.3f)" %(self.x,self.y)) 105 | self.twist = Twist() 106 | self.twist.linear.x = (1-self.y) * (x_max - x_min) + x_min 107 | self.twist.linear.y = 0 108 | self.twist.linear.z = 0 109 | self.twist.angular.x = 0 110 | self.twist.angular.y = 0 111 | self.twist.angular.z = (1-self.x) * (r_max - r_min) + r_min 112 | 113 | if self.twist.linear.x > x_max: 114 | self.twist.linear.x = x_max 115 | if self.twist.linear.x < x_min: 116 | self.twist.linear.x = x_min 117 | if self.twist.angular.z > r_max: 118 | self.twist.angular.z = r_max 119 | if self.twist.angular.z < r_min: 120 | self.twist.angular.z = r_min 121 | 122 | self.pub_twist.publish( self.twist ) 123 | 124 | ########################################################################## 125 | ########################################################################## 126 | def main(): 127 | ########################################################################## 128 | ########################################################################## 129 | rospy.init_node('virtual_joystick') 130 | rospy.loginfo('virtual_joystick started') 131 | global x_min 132 | global x_max 133 | global r_min 134 | global r_max 135 | 136 | x_min = rospy.get_param("~x_min", -0.03) 137 | x_max = rospy.get_param("~x_max", 0.03) 138 | r_min = rospy.get_param("~r_min", -0.4) 139 | r_max = rospy.get_param("~r_max", 0.4) 140 | 141 | app = QtGui.QApplication(sys.argv) 142 | ex = MainWindow() 143 | sys.exit(app.exec_()) 144 | 145 | if __name__ == '__main__': 146 | try: 147 | main() 148 | except rospy.ROSInterruptException: pass 149 | -------------------------------------------------------------------------------- /differential_drive/scripts/wheel_loopback.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | """ 3 | wheel_loopback - simulates a wheel - just for testing 4 | 5 | """ 6 | 7 | #!/usr/bin/env python 8 | # Copyright 2012 Jon Stephan 9 | # jfstepha@gmail.com 10 | # 11 | # This program is free software: you can redistribute it and/or modify 12 | # it under the terms of the GNU General Public License as published by 13 | # the Free Software Foundation, either version 3 of the License, or 14 | # (at your option) any later version. 15 | # 16 | # This program is distributed in the hope that it will be useful, 17 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 18 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 19 | # GNU General Public License for more details. 20 | # 21 | # You should have received a copy of the GNU General Public License 22 | # along with this program. If not, see . 23 | 24 | import rospy 25 | import roslib 26 | from std_msgs.msg import Float32 27 | from std_msgs.msg import Int16 28 | 29 | ################################################ 30 | ################################################ 31 | class WheelLoopback(): 32 | ################################################ 33 | ################################################ 34 | 35 | ############################################### 36 | def __init__(self): 37 | ############################################### 38 | rospy.init_node("wheel_loopback"); 39 | self.nodename = rospy.get_name() 40 | rospy.loginfo("%s started" % self.nodename) 41 | 42 | self.rate = rospy.get_param("~rate", 200) 43 | self.timeout_secs = rospy.get_param("~timeout_secs", 0.5) 44 | self.ticks_meter = float(rospy.get_param('~ticks_meter', 50)) 45 | self.velocity_scale = float(rospy.get_param('~velocity_scale', 255)) 46 | self.latest_motor = 0 47 | 48 | self.wheel = 0 49 | 50 | rospy.Subscriber('motor', Float32, self.motor_callback) 51 | 52 | self.pub_wheel = rospy.Publisher('wheel', Int16, queue_size=10) 53 | 54 | ############################################### 55 | def spin(self): 56 | ############################################### 57 | r = rospy.Rate 58 | self.secs_since_target = self.timeout_secs 59 | self.then = rospy.Time.now() 60 | self.latest_msg_time = rospy.Time.now() 61 | rospy.loginfo("-D- spinning") 62 | 63 | ###### main loop ######### 64 | while not rospy.is_shutdown(): 65 | while not rospy.is_shutdown() and self.secs_since_target < self.timeout_secs: 66 | self.spinOnce() 67 | r.sleep 68 | self.secs_since_target = rospy.Time.now() - self.latest_msg_time 69 | self.secs_since_target = self.secs_since_target.to_sec() 70 | #rospy.loginfo(" inside: secs_since_target: %0.3f" % self.secs_since_target) 71 | 72 | # it's been more than timeout_ticks since we recieved a message 73 | self.secs_since_target = rospy.Time.now() - self.latest_msg_time 74 | self.secs_since_target = self.secs_since_target.to_sec() 75 | # rospy.loginfo(" outside: secs_since_target: %0.3f" % self.secs_since_target) 76 | self.velocity = 0 77 | r.sleep 78 | 79 | ############################################### 80 | def spinOnce(self): 81 | ############################################### 82 | self.velocity = self.latest_motor / self.velocity_scale 83 | if abs(self.velocity) > 0: 84 | self.seconds_per_tick = abs( 1 / (self.velocity * self.ticks_meter)) 85 | elapsed = rospy.Time.now() - self.then 86 | elapsed = elapsed.to_sec() 87 | rospy.loginfo("spinOnce: vel=%0.3f sec/tick=%0.3f elapsed:%0.3f" % (self.velocity, self.seconds_per_tick, elapsed)) 88 | 89 | if (elapsed > self.seconds_per_tick): 90 | rospy.loginfo("incrementing wheel") 91 | if self.velocity > 0: 92 | self.wheel += 1 93 | else: 94 | self.wheel -= 1 95 | self.pub_wheel.publish(self.wheel) 96 | self.then = rospy.Time.now() 97 | 98 | 99 | 100 | ############################################### 101 | def motor_callback(self, msg): 102 | ############################################### 103 | # rospy.loginfo("%s recieved %d" % (self.nodename, msg.data)) 104 | self.latest_motor = msg.data 105 | self.latest_msg_time = rospy.Time.now() 106 | 107 | 108 | 109 | ################################################ 110 | ################################################ 111 | if __name__ == '__main__': 112 | """ main """ 113 | wheelLoopback = WheelLoopback() 114 | wheelLoopback.spin() 115 | -------------------------------------------------------------------------------- /differential_drive/scripts/wheel_scaler.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """ 4 | wheel_scaler 5 | scales the wheel readings (and inverts the sign) 6 | 7 | Copyright (C) 2012 Jon Stephan. 8 | 9 | This program is free software: you can redistribute it and/or modify 10 | it under the terms of the GNU General Public License as published by 11 | the Free Software Foundation, either version 3 of the License, or 12 | (at your option) any later version. 13 | 14 | This program is distributed in the hope that it will be useful, 15 | but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | GNU General Public License for more details. 18 | 19 | You should have received a copy of the GNU General Public License 20 | along with this program. If not, see . 21 | 22 | """ 23 | 24 | import rospy 25 | import roslib 26 | 27 | from std_msgs.msg import Int16 28 | 29 | def lwheelCallback(msg): 30 | lscaled_pub.publish( msg.data * -1 * scale) 31 | 32 | def rwheelCallback(msg): 33 | rscaled_pub.publish( msg.data * -1 * scale) 34 | 35 | 36 | if __name__ == '__main__': 37 | """main""" 38 | rospy.init_node("wheel_scaler") 39 | rospy.loginfo("wheel_scaler started") 40 | 41 | scale = rospy.get_param('distance_scale', 1) 42 | rospy.loginfo("wheel_scaler scale: %0.2f", scale) 43 | 44 | rospy.Subscriber("lwheel", Int16, lwheelCallback) 45 | rospy.Subscriber("rwheel", Int16, rwheelCallback) 46 | 47 | lscaled_pub = rospy.Publisher("lwheel_scaled", Int16, queue_size=10) 48 | rscaled_pub = rospy.Publisher("rwheel_scaled", Int16, queue_size=10) 49 | 50 | ### testing sleep CPU usage 51 | r = rospy.Rate(50) 52 | while not rospy.is_shutdown: 53 | r.sleep() 54 | 55 | rospy.spin() 56 | -------------------------------------------------------------------------------- /firmware/ARC_2.0/ARC_2.0.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include "MapFloat.h" 7 | 8 | RMCS2303 rmcs; // creation of motor driver object 9 | // slave ids to be set on the motor driver refer to the manual in the reference section 10 | byte slave_id1 = 1; 11 | byte slave_id2 = 2; 12 | byte slave_id3 = 3; 13 | byte slave_id4 = 4; 14 | 15 | ros::NodeHandle nh; // Node handle Object 16 | geometry_msgs::Twist msg; // msg variable of data type twist 17 | 18 | std_msgs::Int32 lwheel; // for storing left encoder value 19 | std_msgs::Int32 rwheel; // for storing right encoder value 20 | 21 | // Publisher object with topic names left_ticks and right_ticks for publishing Enc Values 22 | ros::Publisher left_ticks("left_ticks", &lwheel); 23 | ros::Publisher right_ticks("right_ticks", &rwheel); 24 | // Make sure to specify the correct values here 25 | //******************************************* 26 | double wheel_rad = 0.0625, wheel_sep = 0.300; // wheel radius and wheel sepration in meters. 27 | //****************************************** 28 | double w_r = 0, w_l = 0; 29 | double speed_ang; 30 | double speed_lin; 31 | double leftPWM; 32 | double rightPWM; 33 | 34 | void messageCb(const geometry_msgs::Twist& msg) // cmd_vel callback function definition 35 | { 36 | speed_lin = max(min(msg.linear.x, 1.0f), -1.0f); // limits the linear x value from -1 to 1 37 | speed_ang = max(min(msg.angular.z, 1.0f), -1.0f); // limits the angular z value from -1 to 1 38 | 39 | // Kinematic equation for finding the left and right velocities 40 | w_r = (speed_lin / wheel_rad) + ((speed_ang * wheel_sep) / (2.0 * wheel_rad)); 41 | w_l = (speed_lin / wheel_rad) - ((speed_ang * wheel_sep) / (2.0 * wheel_rad)); 42 | 43 | if (w_r == 0) 44 | { 45 | rightPWM = 0; 46 | rmcs.Disable_Digital_Mode(slave_id1,0); 47 | rmcs.Disable_Digital_Mode(slave_id2,0); // if right motor velocity is zero set right pwm to zero and disabling motors 48 | rmcs.Disable_Digital_Mode(slave_id3,0); 49 | rmcs.Disable_Digital_Mode(slave_id4,0); 50 | } 51 | else 52 | rightPWM = mapFloat(fabs(w_r), 0.0, 18.0, 1500,17200); // mapping the right wheel velocity with respect to Motor PWM values 53 | 54 | if (w_l == 0) 55 | { 56 | leftPWM = 0; 57 | rmcs.Disable_Digital_Mode(slave_id1,0); 58 | rmcs.Disable_Digital_Mode(slave_id2,0); // if left motor velocity is zero set left pwm to zero and disabling motors 59 | rmcs.Disable_Digital_Mode(slave_id3,0); 60 | rmcs.Disable_Digital_Mode(slave_id4,0); 61 | } 62 | 63 | else 64 | leftPWM = mapFloat(fabs(w_l), 0.0, 18.0, 1500, 65 | 17200); // mapping the right wheel velocity with respect to Motor PWM values 66 | 67 | rmcs.Speed(slave_id1,rightPWM); 68 | rmcs.Speed(slave_id2,rightPWM); 69 | rmcs.Speed(slave_id3,leftPWM); 70 | rmcs.Speed(slave_id4,leftPWM); 71 | 72 | if (w_r > 0 && w_l > 0) 73 | { 74 | rmcs.Enable_Digital_Mode(slave_id1,1); 75 | rmcs.Enable_Digital_Mode(slave_id2,1); // forward condition 76 | rmcs.Enable_Digital_Mode(slave_id3,0); 77 | rmcs.Enable_Digital_Mode(slave_id4,0); 78 | } 79 | 80 | else if (w_r < 0 && w_l < 0) 81 | { 82 | rmcs.Enable_Digital_Mode(slave_id1,0); 83 | rmcs.Enable_Digital_Mode(slave_id2,0); // backward condition 84 | rmcs.Enable_Digital_Mode(slave_id3,1); 85 | rmcs.Enable_Digital_Mode(slave_id4,1); 86 | } 87 | else if (w_r > 0 && w_l < 0) 88 | { 89 | rmcs.Enable_Digital_Mode(slave_id1,1); 90 | rmcs.Enable_Digital_Mode(slave_id2,1); // Leftward condition 91 | rmcs.Enable_Digital_Mode(slave_id3,1); 92 | rmcs.Enable_Digital_Mode(slave_id4,1); 93 | } 94 | 95 | else if (w_r < 0 && w_l > 0) 96 | { 97 | rmcs.Enable_Digital_Mode(slave_id1,0); 98 | rmcs.Enable_Digital_Mode(slave_id2,0); // rightward condition 99 | rmcs.Enable_Digital_Mode(slave_id3,0); 100 | rmcs.Enable_Digital_Mode(slave_id4,0); 101 | } 102 | 103 | else 104 | { 105 | rmcs.Brake_Motor(slave_id1,0); 106 | rmcs.Brake_Motor(slave_id2,0); 107 | rmcs.Brake_Motor(slave_id3,0); 108 | rmcs.Brake_Motor(slave_id4,0); // if none of the above break the motors both in clockwise n anti-clockwise direction 109 | rmcs.Brake_Motor(slave_id1,1); 110 | rmcs.Brake_Motor(slave_id2,1); 111 | rmcs.Brake_Motor(slave_id3,1); 112 | rmcs.Brake_Motor(slave_id4,1); 113 | } 114 | } 115 | ros::Subscriber sub("cmd_vel",&messageCb); // creation of subscriber object sub for recieving the cmd_vel 116 | 117 | void setup() 118 | { 119 | rmcs.Serial_selection(0); // 0 -> for Harware serial tx1 rx1 of arduino mega 120 | rmcs.Serial0(9600); 121 | rmcs.begin(&Serial3, 9600); 122 | 123 | nh.initNode(); // initialzing the node handle object 124 | nh.subscribe(sub); // subscribing to cmd vel with sub object 125 | 126 | nh.advertise(left_ticks); // advertise the left_ticks topic 127 | nh.advertise(right_ticks); // advertise the left_ticks topic 128 | } 129 | 130 | void loop() 131 | { 132 | lwheel.data = 133 | rmcs.Position_Feedback(slave_id4); // the function reads the encoder value from the motor with slave id 4 134 | rwheel.data = 135 | -rmcs.Position_Feedback(slave_id2); // the function reads the encoder value from the motor with slave id 4 136 | 137 | left_ticks.publish(&lwheel); // publish left enc values 138 | right_ticks.publish(&rwheel); // publish right enc values 139 | nh.spinOnce(); 140 | } 141 | -------------------------------------------------------------------------------- /navbot_description/LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2018 Toshinori Kitamura 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 | -------------------------------------------------------------------------------- /navbot_description/launch/controller.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /navbot_description/launch/controller.yaml: -------------------------------------------------------------------------------- 1 | navbot_controller: 2 | # Publish all joint states ----------------------------------- 3 | joint_state_controller: 4 | type: joint_state_controller/JointStateController 5 | publish_rate: 50 6 | 7 | # Position Controllers -------------------------------------- 8 | Rev1_position_controller: 9 | type: effort_controllers/JointPositionController 10 | joint: Rev1 11 | pid: {p: 100.0, i: 0.01, d: 10.0} 12 | Rev3_position_controller: 13 | type: effort_controllers/JointPositionController 14 | joint: Rev3 15 | pid: {p: 100.0, i: 0.01, d: 10.0} 16 | Rev4_position_controller: 17 | type: effort_controllers/JointPositionController 18 | joint: Rev4 19 | pid: {p: 100.0, i: 0.01, d: 10.0} 20 | Rev5_position_controller: 21 | type: effort_controllers/JointPositionController 22 | joint: Rev5 23 | pid: {p: 100.0, i: 0.01, d: 10.0} 24 | -------------------------------------------------------------------------------- /navbot_description/launch/display.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /navbot_description/launch/gazebo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /navbot_description/launch/world.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /navbot_description/maps/mymap.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jerinpeter/4wdNavbot/0962ac854c9dd4a1dd54da58aef3486182f03057/navbot_description/maps/mymap.pgm -------------------------------------------------------------------------------- /navbot_description/maps/mymap.yaml: -------------------------------------------------------------------------------- 1 | image: mymap.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 | -------------------------------------------------------------------------------- /navbot_description/meshes/back_left_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jerinpeter/4wdNavbot/0962ac854c9dd4a1dd54da58aef3486182f03057/navbot_description/meshes/back_left_1.stl -------------------------------------------------------------------------------- /navbot_description/meshes/back_right_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jerinpeter/4wdNavbot/0962ac854c9dd4a1dd54da58aef3486182f03057/navbot_description/meshes/back_right_1.stl -------------------------------------------------------------------------------- /navbot_description/meshes/base_link.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jerinpeter/4wdNavbot/0962ac854c9dd4a1dd54da58aef3486182f03057/navbot_description/meshes/base_link.stl -------------------------------------------------------------------------------- /navbot_description/meshes/camera_link_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jerinpeter/4wdNavbot/0962ac854c9dd4a1dd54da58aef3486182f03057/navbot_description/meshes/camera_link_1.stl -------------------------------------------------------------------------------- /navbot_description/meshes/camera_link_v1_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jerinpeter/4wdNavbot/0962ac854c9dd4a1dd54da58aef3486182f03057/navbot_description/meshes/camera_link_v1_1.stl -------------------------------------------------------------------------------- /navbot_description/meshes/front_left_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jerinpeter/4wdNavbot/0962ac854c9dd4a1dd54da58aef3486182f03057/navbot_description/meshes/front_left_1.stl -------------------------------------------------------------------------------- /navbot_description/meshes/front_right_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jerinpeter/4wdNavbot/0962ac854c9dd4a1dd54da58aef3486182f03057/navbot_description/meshes/front_right_1.stl -------------------------------------------------------------------------------- /navbot_description/meshes/laser_link_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jerinpeter/4wdNavbot/0962ac854c9dd4a1dd54da58aef3486182f03057/navbot_description/meshes/laser_link_1.stl -------------------------------------------------------------------------------- /navbot_description/meshes/laser_link_v1_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jerinpeter/4wdNavbot/0962ac854c9dd4a1dd54da58aef3486182f03057/navbot_description/meshes/laser_link_v1_1.stl -------------------------------------------------------------------------------- /navbot_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | navbot_description 4 | 0.0.0 5 | The navbot_description package 6 | 7 | 8 | 9 | 10 | syuntoku14 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | rospy 53 | rospy 54 | rospy 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | -------------------------------------------------------------------------------- /navbot_description/urdf/materials.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /navbot_description/urdf/navbot.gazebo: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | ${body_color} 11 | 0.2 12 | 0.2 13 | true 14 | true 15 | 16 | 17 | 18 | ${body_color} 19 | 0.2 20 | 0.2 21 | true 22 | 23 | 24 | 25 | ${body_color} 26 | 0.2 27 | 0.2 28 | true 29 | 30 | 31 | 32 | ${body_color} 33 | 0.2 34 | 0.2 35 | true 36 | 37 | 38 | 39 | ${body_color} 40 | 0.2 41 | 0.2 42 | true 43 | 44 | 45 | 46 | ${body_color} 47 | 0.2 48 | 0.2 49 | true 50 | 51 | 52 | 53 | ${body_color} 54 | 0.2 55 | 0.2 56 | true 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /navbot_description/urdf/navbot.trans: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | transmission_interface/SimpleTransmission 6 | 7 | hardware_interface/EffortJointInterface 8 | 9 | 10 | hardware_interface/EffortJointInterface 11 | 1 12 | 13 | 14 | 15 | 16 | transmission_interface/SimpleTransmission 17 | 18 | hardware_interface/EffortJointInterface 19 | 20 | 21 | hardware_interface/EffortJointInterface 22 | 1 23 | 24 | 25 | 26 | 27 | transmission_interface/SimpleTransmission 28 | 29 | hardware_interface/EffortJointInterface 30 | 31 | 32 | hardware_interface/EffortJointInterface 33 | 1 34 | 35 | 36 | 37 | 38 | transmission_interface/SimpleTransmission 39 | 40 | hardware_interface/EffortJointInterface 41 | 42 | 43 | hardware_interface/EffortJointInterface 44 | 1 45 | 46 | 47 | 48 | 49 | -------------------------------------------------------------------------------- /navbot_description/urdf/navbot_gazebo_plugins.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 100.0 7 | / 8 | Rev5 9 | Rev3 10 | Rev4 11 | Rev1 12 | 0.4 13 | 0.125 14 | base_footprint 15 | 20 16 | cmd_vel 17 | true 18 | 19 | 20 | 21 | !-- hokuyo --> 22 | 23 | 24 | 0 0 0 0 0 0 25 | true 26 | 40 27 | 28 | 29 | 30 | 720 31 | 1 32 | -1.570796 33 | 1.570796 34 | 35 | 36 | 37 | 0.10 38 | 30.0 39 | 0.01 40 | 41 | 42 | gaussian 43 | 47 | 0.0 48 | 0.01 49 | 50 | 51 | 52 | scan 53 | laser_link_v1_1 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | -------------------------------------------------------------------------------- /navbot_navigation/launch/amcl.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 | -------------------------------------------------------------------------------- /navbot_navigation/launch/move_base.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 | -------------------------------------------------------------------------------- /navbot_navigation/launch/t265_odom.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /navbot_navigation/launch/tf.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /navbot_navigation/maps/hall.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jerinpeter/4wdNavbot/0962ac854c9dd4a1dd54da58aef3486182f03057/navbot_navigation/maps/hall.pgm -------------------------------------------------------------------------------- /navbot_navigation/maps/hall.yaml: -------------------------------------------------------------------------------- 1 | image: /home/jetson/catkin_ws/src/dbot_navigation/map/hall.pgm 2 | resolution: 0.100000 3 | origin: [-102.449997, -102.449997, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /navbot_navigation/maps/outdooe.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jerinpeter/4wdNavbot/0962ac854c9dd4a1dd54da58aef3486182f03057/navbot_navigation/maps/outdooe.pgm -------------------------------------------------------------------------------- /navbot_navigation/maps/outdooe.yaml: -------------------------------------------------------------------------------- 1 | image: outdooe.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 | -------------------------------------------------------------------------------- /navbot_navigation/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | navbot_navigation 4 | 0.0.0 5 | The navbot_navigation package 6 | 7 | 8 | 9 | 10 | jerinpeter 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | move_base 53 | roscpp 54 | rospy 55 | move_base 56 | roscpp 57 | rospy 58 | move_base 59 | roscpp 60 | rospy 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | -------------------------------------------------------------------------------- /navbot_navigation/param/base_local_planner_params.yaml: -------------------------------------------------------------------------------- 1 | TebLocalPlannerROS: 2 | odom_topic: odom 3 | map_frame: /odom 4 | 5 | # Trajectory 6 | 7 | teb_autosize: True 8 | dt_ref: 0.3 9 | dt_hysteresis: 0.1 10 | global_plan_overwrite_orientation: True 11 | max_global_plan_lookahead_dist: 3.0 12 | feasibility_check_no_poses: 5 13 | 14 | # Robot 15 | 16 | max_vel_x: 0.2 17 | max_vel_x_backwards: 0.1 18 | max_vel_theta: 1.5 19 | acc_lim_x: 0.45 20 | acc_lim_theta: 0.5 21 | min_turning_radius: 0.0 22 | footprint_model: # types: "point", "circular", "two_circles", "line", "polygon" 23 | type: "polygon" 24 | radius: 0.2 # for type "circular" 25 | line_start: [-0.3, 0.0] # for type "line" 26 | line_end: [0.3, 0.0] # for type "line" 27 | front_offset: 0.2 # for type "two_circles" 28 | front_radius: 0.2 # for type "two_circles" 29 | rear_offset: 0.2 # for type "two_circles" 30 | rear_radius: 0.2 # for type "two_circles" 31 | vertices: [[-0.15, -0.125], [-0.15, 0.125], [0.15, 0.125], [0.15, -0.125]] # for type "polygon" 32 | 33 | # GoalTolerance 34 | 35 | xy_goal_tolerance: 0.2 36 | yaw_goal_tolerance: 0.1 37 | free_goal_vel: False 38 | 39 | # Obstacles 40 | 41 | min_obstacle_dist: 0.45 42 | include_costmap_obstacles: True 43 | costmap_obstacles_behind_robot_dist: 1.0 44 | obstacle_poses_affected: 30 45 | costmap_converter_plugin: "" 46 | costmap_converter_spin_thread: True 47 | costmap_converter_rate: 5 48 | 49 | # Optimization 50 | 51 | no_inner_iterations: 5 52 | no_outer_iterations: 4 53 | optimization_activate: True 54 | optimization_verbose: False 55 | penalty_epsilon: 0.1 56 | weight_max_vel_x: 2 57 | weight_max_vel_theta: 1 58 | weight_acc_lim_x: 1 59 | weight_acc_lim_theta: 1 60 | weight_kinematics_nh: 1000 61 | weight_kinematics_forward_drive: 1 62 | weight_kinematics_turning_radius: 0 63 | weight_optimaltime: 1 64 | weight_obstacle: 50 65 | weight_dynamic_obstacle: 10 # not in use yet 66 | alternative_time_cost: False # not in use yet 67 | 68 | # Homotopy Class Planner 69 | 70 | enable_homotopy_class_planning: True 71 | enable_multithreading: True 72 | simple_exploration: False 73 | max_number_classes: 4 74 | roadmap_graph_no_samples: 15 75 | roadmap_graph_area_width: 5 76 | h_signature_prescaler: 0.5 77 | h_signature_threshold: 0.1 78 | obstacle_keypoint_offset: 0.1 79 | obstacle_heading_threshold: 0.45 80 | visualize_hc_graph: False -------------------------------------------------------------------------------- /navbot_navigation/param/base_local_planner_params.yaml.save: -------------------------------------------------------------------------------- 1 | WAPlannerROS: 2 | max_trans_vel: 0.50 3 | min_trans_vel: 0.01 4 | max_vel_x: 0.50 5 | min_vel_x: 0.025 6 | max_vel_y: 0.0 7 | min_vel_y: 0.0 8 | max_rot_vel: 0.30 9 | min_rot_vel: -0.30 10 | acc_lim_x: 1.25 11 | acc_lim_y: 0.0 12 | acc_lim_theta: 5 13 | acc_lim_trans: 1.25 14 | 15 | prune_plan: false 16 | 17 | xy_goal_tolerance: 0.25 18 | yaw_goal_tolerance: 0.1 19 | trans_stopped_vel: 0.1 20 | rot_stopped_vel: 0.1 21 | sim_time: 3.0 22 | sim_granularity: 0.1 23 | angular_sim_granularity: 0.1 24 | path_distance_bias: 34.0 25 | goal_distance_bias: 24.0 26 | occdist_scale: 0.05 27 | twirling_scale: 0.0 28 | stop_time_buffer: 0.5 29 | oscillation_reset_dist: 0.05 30 | oscillation_reset_angle: 0.2 31 | forward_point_distance: 0.3 32 | scaling_speed: 0.25 33 | max_scaling_factor: 0.2 34 | vx_samples: 20 35 | vy_samples: 0 36 | vth_samples: 40 37 | 38 | use_dwa: true 39 | restore_defaults: false 40 | -------------------------------------------------------------------------------- /navbot_navigation/param/base_local_planner_params.yaml.save.1: -------------------------------------------------------------------------------- 1 | TrajectoryPlannerROS: 2 | max_vel_x: 0. #tested 0.0285 3 | min_vel_x: 0.2 4 | max_vel_theta: 1.0 5 | min_in_place_vel_theta: 0.4 6 | 7 | acc_lim_theta: 10.0 #was 3.2 8 | acc_lim_x: 2.5 9 | acc_lim_y: 2.5 10 | 11 | #holonomic_robot: true 12 | 13 | # newly added 14 | 15 | escape_vel: -0.15 16 | holonomic_robot: false 17 | 18 | # Goal Tolerance Parameters 19 | yaw_goal_tolerance: 0.57 # 0.3 20 | xy_goal_tolerance: 0.35 # 0.25 21 | 22 | # Forward Simulation Parameters 23 | sim_time: 3.0 # 3.0 24 | vx_samples: 15 25 | vtheta_samples: 24 26 | controller_frequency: 10 # overwritten by move_base_params 27 | 28 | # Trajectory Scoring Parameters 29 | meter_scoring: true 30 | pdist_scale: 0.6 31 | gdist_scale: 0.8 32 | occdist_scale: 0.01 33 | heading_lookahead: 0.0 # same as fwd_dist in DWA 34 | heading_scoring: false 35 | heading_scoring_timestep: 1.0 36 | dwa: true 37 | publish_cost_grid_pc: false 38 | global_frame_id: "map" # "odom" # should be the same as global frame in local costmap! 39 | 40 | # Oscillation Prevention Parameters 41 | oscillation_reset_dist: 0.05 # overwritten by move_base_params 42 | 43 | # Global Plan Parameters 44 | prune_plan: true 45 | 46 | 47 | # --------------------------------------------------------------- 48 | # DWA-specific params that are not included in base_local_planner 49 | DWAPlannerROS: 50 | 51 | use_dwa: true # instead of Rollback 52 | forward_point_distance: 0.00 # recommended 0.0 for diff_drive # used to align the robot to the path 53 | sim_time: 3.0 # helps if robot is stuck 54 | vx_samples: 25 # 15 55 | vy_samples: 1 # -1 # for differential-drive robot vy samples doesn't make sense in terms of controller (linear.x and angular.z allowed only) 56 | vth_samples: 25 # 24 57 | 58 | scaling_speed: 1.0 # The absolute value of the velocity at which to start scaling the robot's footprint, in m/s 59 | 60 | min_rot_vel: 1.35 61 | max_rot_vel: 1.5 62 | 63 | min_vel_x: 0.2 64 | max_vel_x: 0.5 65 | 66 | min_trans_vel: 0.00 # makes robot can't rotate in place? 67 | max_trans_vel: 0.15 68 | 69 | rot_stopped_vel: 0.10 # 0.2 70 | trans_stopped_vel: 0.00 # 0,00 can be difficult when trying to start but allows robot to rotate in place? 71 | 72 | acc_lim_theta: 3.0 73 | acc_lim_x: 3.0 74 | acc_lim_y: 0.0 75 | acc_limit_trans: 3.0 76 | 77 | yaw_goal_tolerance: 0.45 78 | xy_goal_tolerance: 0.30 79 | 80 | max_scaling_factor: 0.2 # The maximum factor to scale the robot's footprint by 81 | stop_time_buffer: 0.3 # The amount of time that the robot must stop before a collision in order for a trajectory to be considered valid in seconds 82 | prune_plan: true # clears part of a path that robot already travelled 83 | ples: 1 84 | -------------------------------------------------------------------------------- /navbot_navigation/param/costmap_common_params.yaml: -------------------------------------------------------------------------------- 1 | obstacle_range: 2.5 2 | raytrace_range: 3.0 3 | footprint: [[-0.15, -0.125], [-0.15, 0.125], [0.15, 0.125], [0.15, -0.125]] 4 | inflation_radius: 0.40 5 | transform_tolerance: 0.3 6 | 7 | observation_sources: scan 8 | scan: 9 | data_type: LaserScan 10 | topic: scan 11 | marking: true 12 | clearing: true 13 | 14 | map_type: costmap 15 | -------------------------------------------------------------------------------- /navbot_navigation/param/global_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | global_frame: map 3 | robot_base_frame: base_footprint 4 | update_frequency: 5.0 #before: 5.0 5 | publish_frequency: 0.5 #before 0.5 6 | static_map: true 7 | transform_tolerance: 0.5 8 | cost_scaling_factor: 10.0 9 | inflation_radius: 0.4 10 | -------------------------------------------------------------------------------- /navbot_navigation/param/local_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | global_frame: odom 3 | robot_base_frame: base_footprint 4 | update_frequency: 3.5 #before 5.0 5 | publish_frequency: 2.0 #before 2.0 6 | static_map: false 7 | rolling_window: true 8 | width: 1.5 9 | height: 1.5 10 | resolution: 0.01 #increase to for higher res 0.025 11 | transform_tolerance: 0.5 12 | cost_scaling_factor: 5 13 | inflation_radius: 0.4 14 | -------------------------------------------------------------------------------- /navbot_navigation/param/move_base_params.yaml: -------------------------------------------------------------------------------- 1 | base_global_planner: global_planner/GlobalPlanner 2 | base_local_planner: teb_local_planner/TebLocalPlannerROS 3 | 4 | shutdown_costmaps: false 5 | 6 | controller_frequency: 5.0 #before 5.0 7 | controller_patience: 3.0 8 | 9 | planner_frequency: 0.5 10 | planner_patience: 5.0 11 | 12 | oscillation_timeout: 10.0 13 | oscillation_distance: 0.2 14 | 15 | conservative_reset_dist: 0.1 #distance from an obstacle at which it will unstuck itself 16 | 17 | cost_factor: 1.0 18 | neutral_cost: 55 19 | lethal_cost: 253 -------------------------------------------------------------------------------- /reconf_params/final_reconf_params_real.yaml: -------------------------------------------------------------------------------- 1 | !!python/object/new:dynamic_reconfigure.encoding.Config 2 | dictitems: 3 | acc_lim_theta: 2.5 4 | acc_lim_trans: 0.1 5 | acc_lim_x: 0.2 6 | acc_lim_y: 0.0 7 | angular_sim_granularity: 0.1 8 | forward_point_distance: 0.0 9 | goal_distance_bias: 0.8 10 | groups: !!python/object/new:dynamic_reconfigure.encoding.Config 11 | dictitems: 12 | acc_lim_theta: 2.5 13 | acc_lim_trans: 0.1 14 | acc_lim_x: 0.2 15 | acc_lim_y: 0.0 16 | angular_sim_granularity: 0.1 17 | forward_point_distance: 0.0 18 | goal_distance_bias: 0.8 19 | groups: !!python/object/new:dynamic_reconfigure.encoding.Config 20 | state: [] 21 | id: 0 22 | max_scaling_factor: 0.2 23 | max_vel_theta: 1.0 24 | max_vel_trans: 0.2 25 | max_vel_x: 0.3 26 | max_vel_y: 0.1 27 | min_vel_theta: 0.4 28 | min_vel_trans: 0.07 29 | min_vel_x: 0.1 30 | min_vel_y: -0.1 31 | name: Default 32 | occdist_scale: 0.01 33 | oscillation_reset_angle: 0.2 34 | oscillation_reset_dist: 0.05 35 | parameters: !!python/object/new:dynamic_reconfigure.encoding.Config 36 | state: [] 37 | parent: 0 38 | path_distance_bias: 0.6 39 | prune_plan: true 40 | restore_defaults: false 41 | scaling_speed: 1.0 42 | sim_granularity: 0.025 43 | sim_time: 3.0 44 | state: true 45 | stop_time_buffer: 0.3 46 | theta_stopped_vel: 0.1 47 | trans_stopped_vel: 0.0 48 | twirling_scale: 0.0 49 | type: '' 50 | use_dwa: true 51 | vth_samples: 25 52 | vx_samples: 25 53 | vy_samples: 1 54 | xy_goal_tolerance: 0.3 55 | yaw_goal_tolerance: 0.45 56 | state: [] 57 | max_scaling_factor: 0.2 58 | max_vel_theta: 1.0 59 | max_vel_trans: 0.2 60 | max_vel_x: 0.3 61 | max_vel_y: 0.1 62 | min_vel_theta: 0.4 63 | min_vel_trans: 0.07 64 | min_vel_x: 0.1 65 | min_vel_y: -0.1 66 | occdist_scale: 0.01 67 | oscillation_reset_angle: 0.2 68 | oscillation_reset_dist: 0.05 69 | path_distance_bias: 0.6 70 | prune_plan: true 71 | restore_defaults: false 72 | scaling_speed: 1.0 73 | sim_granularity: 0.025 74 | sim_time: 3.0 75 | stop_time_buffer: 0.3 76 | theta_stopped_vel: 0.1 77 | trans_stopped_vel: 0.0 78 | twirling_scale: 0.0 79 | use_dwa: true 80 | vth_samples: 25 81 | vx_samples: 25 82 | vy_samples: 1 83 | xy_goal_tolerance: 0.3 84 | yaw_goal_tolerance: 0.45 85 | state: [] 86 | -------------------------------------------------------------------------------- /reconf_params/final_reconf_params_sim.yaml: -------------------------------------------------------------------------------- 1 | !!python/object/new:dynamic_reconfigure.encoding.Config 2 | dictitems: 3 | acc_lim_theta: 2.5 4 | acc_lim_trans: 0.1 5 | acc_lim_x: 0.2 6 | acc_lim_y: 0.0 7 | angular_sim_granularity: 0.1 8 | forward_point_distance: 0.0 9 | goal_distance_bias: 0.8 10 | groups: !!python/object/new:dynamic_reconfigure.encoding.Config 11 | dictitems: 12 | acc_lim_theta: 2.5 13 | acc_lim_trans: 0.1 14 | acc_lim_x: 0.2 15 | acc_lim_y: 0.0 16 | angular_sim_granularity: 0.1 17 | forward_point_distance: 0.0 18 | goal_distance_bias: 0.8 19 | groups: !!python/object/new:dynamic_reconfigure.encoding.Config 20 | state: [] 21 | id: 0 22 | max_scaling_factor: 0.2 23 | max_vel_theta: 1.0 24 | max_vel_trans: 0.5 25 | max_vel_x: 0.5 26 | max_vel_y: 0.1 27 | min_vel_theta: 0.4 28 | min_vel_trans: 0.07 29 | min_vel_x: 0.1 30 | min_vel_y: -0.1 31 | name: Default 32 | occdist_scale: 0.01 33 | oscillation_reset_angle: 0.2 34 | oscillation_reset_dist: 0.05 35 | parameters: !!python/object/new:dynamic_reconfigure.encoding.Config 36 | state: [] 37 | parent: 0 38 | path_distance_bias: 0.6 39 | prune_plan: true 40 | restore_defaults: false 41 | scaling_speed: 1.0 42 | sim_granularity: 0.025 43 | sim_time: 3.0 44 | state: true 45 | stop_time_buffer: 0.3 46 | theta_stopped_vel: 1.0 47 | trans_stopped_vel: 0.5 48 | twirling_scale: 0.0 49 | type: '' 50 | use_dwa: true 51 | vth_samples: 25 52 | vx_samples: 25 53 | vy_samples: 1 54 | xy_goal_tolerance: 0.3 55 | yaw_goal_tolerance: 0.45 56 | state: [] 57 | max_scaling_factor: 0.2 58 | max_vel_theta: 1.0 59 | max_vel_trans: 0.5 60 | max_vel_x: 0.5 61 | max_vel_y: 0.1 62 | min_vel_theta: 0.4 63 | min_vel_trans: 0.07 64 | min_vel_x: 0.1 65 | min_vel_y: -0.1 66 | occdist_scale: 0.01 67 | oscillation_reset_angle: 0.2 68 | oscillation_reset_dist: 0.05 69 | path_distance_bias: 0.6 70 | prune_plan: true 71 | restore_defaults: false 72 | scaling_speed: 1.0 73 | sim_granularity: 0.025 74 | sim_time: 3.0 75 | stop_time_buffer: 0.3 76 | theta_stopped_vel: 1.0 77 | trans_stopped_vel: 0.5 78 | twirling_scale: 0.0 79 | use_dwa: true 80 | vth_samples: 25 81 | vx_samples: 25 82 | vy_samples: 1 83 | xy_goal_tolerance: 0.3 84 | yaw_goal_tolerance: 0.45 85 | state: [] 86 | -------------------------------------------------------------------------------- /slam_gmapping/README.md: -------------------------------------------------------------------------------- 1 | slam_gmapping [![Build Status](https://travis-ci.com/ros-perception/slam_gmapping.svg?branch=melodic-devel)](https://travis-ci.org/ros-perception/slam_gmapping) 2 | ================================================================================================================================================================ 3 | -------------------------------------------------------------------------------- /slam_gmapping/gmapping/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | project(gmapping) 3 | 4 | find_package(catkin REQUIRED nav_msgs nodelet openslam_gmapping roscpp tf rosbag_storage) 5 | 6 | find_package(Boost REQUIRED) 7 | 8 | include_directories(${Boost_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}) 9 | 10 | include_directories(src) 11 | 12 | catkin_package() 13 | 14 | add_executable(slam_gmapping src/slam_gmapping.cpp src/main.cpp) 15 | target_link_libraries(slam_gmapping ${Boost_LIBRARIES} ${catkin_LIBRARIES}) 16 | if(catkin_EXPORTED_TARGETS) 17 | add_dependencies(slam_gmapping ${catkin_EXPORTED_TARGETS}) 18 | endif() 19 | 20 | add_library(slam_gmapping_nodelet src/slam_gmapping.cpp src/nodelet.cpp) 21 | target_link_libraries(slam_gmapping_nodelet ${catkin_LIBRARIES}) 22 | 23 | add_executable(slam_gmapping_replay src/slam_gmapping.cpp src/replay.cpp) 24 | target_link_libraries(slam_gmapping_replay ${Boost_LIBRARIES} ${catkin_LIBRARIES}) 25 | if(catkin_EXPORTED_TARGETS) 26 | add_dependencies(slam_gmapping_replay ${catkin_EXPORTED_TARGETS}) 27 | endif() 28 | 29 | install(TARGETS slam_gmapping slam_gmapping_nodelet slam_gmapping_replay 30 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 31 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 32 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 33 | ) 34 | 35 | install(FILES nodelet_plugins.xml 36 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 37 | ) 38 | 39 | if(CATKIN_ENABLE_TESTING) 40 | find_package(rostest REQUIRED) 41 | if(TARGET tests) 42 | add_executable(gmapping-rtest EXCLUDE_FROM_ALL test/rtest.cpp) 43 | target_link_libraries(gmapping-rtest ${catkin_LIBRARIES} ${GTEST_LIBRARIES}) 44 | add_dependencies(tests gmapping-rtest) 45 | endif() 46 | 47 | # Need to make the tests more robust; currently the output map can differ 48 | # substantially between runs. 49 | catkin_download_test_data( 50 | ${PROJECT_NAME}_basic_localization_stage_indexed.bag 51 | http://download.ros.org/data/gmapping/basic_localization_stage_indexed.bag 52 | DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test 53 | MD5 322a0014f47bcfbb0ad16a317738b0dc) 54 | catkin_download_test_data( 55 | ${PROJECT_NAME}_hallway_slow_2011-03-04-21-41-33.bag 56 | http://download.ros.org/data/gmapping/hallway_slow_2011-03-04-21-41-33.bag 57 | DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test 58 | MD5 e772b89713693adc610f4c5b96f5dc03) 59 | catkin_download_test_data( 60 | ${PROJECT_NAME}_basic_localization_stage_groundtruth.pgm 61 | http://download.ros.org/data/gmapping/basic_localization_stage_groundtruth.pgm 62 | DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test 63 | MD5 abf208f721053915145215b18c98f9b3) 64 | catkin_download_test_data( 65 | ${PROJECT_NAME}_test_replay_crash.bag 66 | https://github.com/ros-perception/slam_gmapping_test_data/raw/master/test_replay_crash.bag 67 | DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test 68 | MD5 bb0e086207eb4fccf0b13d3406f610a1) 69 | catkin_download_test_data( 70 | ${PROJECT_NAME}_test_turtlebot.bag 71 | https://github.com/ros-perception/slam_gmapping_test_data/raw/master/test_turtlebot.bag 72 | DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test 73 | MD5 402e1e5f7c00445d2a446e58e3151830) 74 | catkin_download_test_data( 75 | ${PROJECT_NAME}_test_upside_down.bag 76 | https://github.com/ros-perception/slam_gmapping_test_data/raw/master/test_upside_down.bag 77 | DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test 78 | MD5 3b026a2144ec14f3fdf218b5c7077d54) 79 | set(LOCAL_DEPENDENCIES gmapping-rtest ${PROJECT_NAME}_basic_localization_stage_indexed.bag 80 | ${PROJECT_NAME}_test_replay_crash.bag 81 | ${PROJECT_NAME}_test_turtlebot.bag 82 | ${PROJECT_NAME}_test_upside_down.bag 83 | ${PROJECT_NAME}_hallway_slow_2011-03-04-21-41-33.bag 84 | ${PROJECT_NAME}_basic_localization_stage_groundtruth.pgm 85 | slam_gmapping 86 | slam_gmapping_replay 87 | ) 88 | add_rostest(test/basic_localization_stage.launch DEPENDENCIES ${LOCAL_DEPENDENCIES}) 89 | add_rostest(test/basic_localization_stage_replay.launch DEPENDENCIES ${LOCAL_DEPENDENCIES}) 90 | add_rostest(test/basic_localization_stage_replay2.launch DEPENDENCIES ${LOCAL_DEPENDENCIES}) 91 | add_rostest(test/basic_localization_symmetry.launch DEPENDENCIES ${LOCAL_DEPENDENCIES}) 92 | add_rostest(test/basic_localization_upside_down.launch DEPENDENCIES ${LOCAL_DEPENDENCIES}) 93 | add_rostest(test/basic_localization_laser_different_beamcount.test DEPENDENCIES ${LOCAL_DEPENDENCIES}) 94 | endif() 95 | -------------------------------------------------------------------------------- /slam_gmapping/gmapping/launch/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 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /slam_gmapping/gmapping/launch/slam_gmapping_pr2.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /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.2 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/src/main.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * slam_gmapping 3 | * Copyright (c) 2008, Willow Garage, Inc. 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 | * * Redistributions of source code must retain the above copyright notice, 8 | * this list of conditions and the following disclaimer. 9 | * * Redistributions in binary form must reproduce the above copyright 10 | * notice, this list of conditions and the following disclaimer in the 11 | * documentation and/or other materials provided with the distribution. 12 | * * Neither the names of Stanford University or Willow Garage, Inc. nor the names of its 13 | * contributors may be used to endorse or promote products derived from 14 | * this software without specific prior written permission. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 19 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 20 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 21 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 22 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 23 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 24 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 25 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 26 | * POSSIBILITY OF SUCH DAMAGE. 27 | * 28 | */ 29 | 30 | /* Author: Brian Gerkey */ 31 | 32 | #include 33 | 34 | #include "slam_gmapping.h" 35 | 36 | int 37 | main(int argc, char** argv) 38 | { 39 | ros::init(argc, argv, "slam_gmapping"); 40 | 41 | SlamGMapping gn; 42 | gn.startLiveSlam(); 43 | ros::spin(); 44 | 45 | return(0); 46 | } 47 | 48 | -------------------------------------------------------------------------------- /slam_gmapping/gmapping/src/nodelet.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * slam_gmapping 3 | * Copyright (c) 2008, Willow Garage, Inc. 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 | * * Redistributions of source code must retain the above copyright notice, 8 | * this list of conditions and the following disclaimer. 9 | * * Redistributions in binary form must reproduce the above copyright 10 | * notice, this list of conditions and the following disclaimer in the 11 | * documentation and/or other materials provided with the distribution. 12 | * * Neither the names of Stanford University or Willow Garage, Inc. nor the names of its 13 | * contributors may be used to endorse or promote products derived from 14 | * this software without specific prior written permission. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 19 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 20 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 21 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 22 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 23 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 24 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 25 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 26 | * POSSIBILITY OF SUCH DAMAGE. 27 | * 28 | */ 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | #include "slam_gmapping.h" 35 | 36 | class SlamGMappingNodelet : public nodelet::Nodelet 37 | { 38 | public: 39 | SlamGMappingNodelet() {} 40 | 41 | ~SlamGMappingNodelet() {} 42 | 43 | virtual void onInit() 44 | { 45 | NODELET_INFO_STREAM("Initialising Slam GMapping nodelet..."); 46 | sg_.reset(new SlamGMapping(getNodeHandle(), getPrivateNodeHandle())); 47 | NODELET_INFO_STREAM("Starting live SLAM..."); 48 | sg_->startLiveSlam(); 49 | } 50 | 51 | private: 52 | boost::shared_ptr sg_; 53 | }; 54 | 55 | PLUGINLIB_EXPORT_CLASS(SlamGMappingNodelet, nodelet::Nodelet) 56 | -------------------------------------------------------------------------------- /slam_gmapping/gmapping/src/replay.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2015 Aldebaran 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | #include "slam_gmapping.h" 18 | 19 | #include 20 | #include 21 | 22 | #include 23 | #include 24 | 25 | int 26 | main(int argc, char** argv) 27 | { 28 | /** Define and parse the program options 29 | */ 30 | namespace po = boost::program_options; 31 | po::options_description desc("Options"); 32 | desc.add_options() 33 | ("help", "Print help messages") 34 | ("scan_topic", po::value()->default_value("/scan") ,"topic that contains the laserScan in the rosbag") 35 | ("bag_filename", po::value()->required(), "ros bag filename") 36 | ("seed", po::value()->default_value(0), "seed") 37 | ("max_duration_buffer", po::value()->default_value(99999), "max tf buffer duration") 38 | ("on_done", po::value(), "command to execute when done") ; 39 | 40 | po::variables_map vm; 41 | try 42 | { 43 | po::store(po::parse_command_line(argc, argv, desc), 44 | vm); // can throw 45 | 46 | /** --help option 47 | */ 48 | if ( vm.count("help") ) 49 | { 50 | std::cout << "Basic Command Line Parameter App" << std::endl 51 | << desc << std::endl; 52 | return 0; 53 | } 54 | 55 | po::notify(vm); // throws on error, so do after help in case 56 | // there are any problems 57 | } 58 | catch(po::error& e) 59 | { 60 | std::cerr << "ERROR: " << e.what() << std::endl << std::endl; 61 | std::cerr << desc << std::endl; 62 | return -1; 63 | } 64 | 65 | std::string bag_fname = vm["bag_filename"].as(); 66 | std::string scan_topic = vm["scan_topic"].as(); 67 | unsigned long int seed = vm["seed"].as(); 68 | unsigned long int max_duration_buffer = vm["max_duration_buffer"].as(); 69 | 70 | ros::init(argc, argv, "slam_gmapping"); 71 | SlamGMapping gn(seed, max_duration_buffer) ; 72 | gn.startReplay(bag_fname, scan_topic); 73 | ROS_INFO("replay stopped."); 74 | 75 | if ( vm.count("on_done") ) 76 | { 77 | // Run the "on_done" command and then exit 78 | system(vm["on_done"].as().c_str()); 79 | } 80 | else 81 | { 82 | ros::spin(); // wait so user can save the map 83 | } 84 | return(0); 85 | 86 | 87 | } 88 | 89 | -------------------------------------------------------------------------------- /slam_gmapping/gmapping/src/slam_gmapping.h: -------------------------------------------------------------------------------- 1 | /* 2 | * slam_gmapping 3 | * Copyright (c) 2008, Willow Garage, Inc. 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 | * * Redistributions of source code must retain the above copyright notice, 8 | * this list of conditions and the following disclaimer. 9 | * * Redistributions in binary form must reproduce the above copyright 10 | * notice, this list of conditions and the following disclaimer in the 11 | * documentation and/or other materials provided with the distribution. 12 | * * Neither the names of Stanford University or Willow Garage, Inc. nor the names of its 13 | * contributors may be used to endorse or promote products derived from 14 | * this software without specific prior written permission. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 19 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 20 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 21 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 22 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 23 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 24 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 25 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 26 | * POSSIBILITY OF SUCH DAMAGE. 27 | * 28 | */ 29 | 30 | /* Author: Brian Gerkey */ 31 | 32 | #include "ros/ros.h" 33 | #include "sensor_msgs/LaserScan.h" 34 | #include "std_msgs/Float64.h" 35 | #include "nav_msgs/GetMap.h" 36 | #include "tf/transform_listener.h" 37 | #include "tf/transform_broadcaster.h" 38 | #include "message_filters/subscriber.h" 39 | #include "tf/message_filter.h" 40 | 41 | #include "gmapping/gridfastslam/gridslamprocessor.h" 42 | #include "gmapping/sensor/sensor_base/sensor.h" 43 | 44 | #include 45 | 46 | class SlamGMapping 47 | { 48 | public: 49 | SlamGMapping(); 50 | SlamGMapping(ros::NodeHandle& nh, ros::NodeHandle& pnh); 51 | SlamGMapping(unsigned long int seed, unsigned long int max_duration_buffer); 52 | ~SlamGMapping(); 53 | 54 | void init(); 55 | void startLiveSlam(); 56 | void startReplay(const std::string & bag_fname, std::string scan_topic); 57 | void publishTransform(); 58 | 59 | void laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan); 60 | bool mapCallback(nav_msgs::GetMap::Request &req, 61 | nav_msgs::GetMap::Response &res); 62 | void publishLoop(double transform_publish_period); 63 | 64 | private: 65 | ros::NodeHandle node_; 66 | ros::Publisher entropy_publisher_; 67 | ros::Publisher sst_; 68 | ros::Publisher sstm_; 69 | ros::ServiceServer ss_; 70 | tf::TransformListener tf_; 71 | message_filters::Subscriber* scan_filter_sub_; 72 | tf::MessageFilter* scan_filter_; 73 | tf::TransformBroadcaster* tfB_; 74 | 75 | GMapping::GridSlamProcessor* gsp_; 76 | GMapping::RangeSensor* gsp_laser_; 77 | // The angles in the laser, going from -x to x (adjustment is made to get the laser between 78 | // symmetrical bounds as that's what gmapping expects) 79 | std::vector laser_angles_; 80 | // The pose, in the original laser frame, of the corresponding centered laser with z facing up 81 | tf::Stamped centered_laser_pose_; 82 | // Depending on the order of the elements in the scan and the orientation of the scan frame, 83 | // We might need to change the order of the scan 84 | bool do_reverse_range_; 85 | unsigned int gsp_laser_beam_count_; 86 | GMapping::OdometrySensor* gsp_odom_; 87 | 88 | bool got_first_scan_; 89 | 90 | bool got_map_; 91 | nav_msgs::GetMap::Response map_; 92 | 93 | ros::Duration map_update_interval_; 94 | tf::Transform map_to_odom_; 95 | boost::mutex map_to_odom_mutex_; 96 | boost::mutex map_mutex_; 97 | 98 | int laser_count_; 99 | int throttle_scans_; 100 | 101 | boost::thread* transform_thread_; 102 | 103 | std::string base_frame_; 104 | std::string laser_frame_; 105 | std::string map_frame_; 106 | std::string odom_frame_; 107 | 108 | void updateMap(const sensor_msgs::LaserScan& scan); 109 | bool getOdomPose(GMapping::OrientedPoint& gmap_pose, const ros::Time& t); 110 | bool initMapper(const sensor_msgs::LaserScan& scan); 111 | bool addScan(const sensor_msgs::LaserScan& scan, GMapping::OrientedPoint& gmap_pose); 112 | double computePoseEntropy(); 113 | 114 | // Parameters used by GMapping 115 | double maxRange_; 116 | double maxUrange_; 117 | double maxrange_; 118 | double minimum_score_; 119 | double sigma_; 120 | int kernelSize_; 121 | double lstep_; 122 | double astep_; 123 | int iterations_; 124 | double lsigma_; 125 | double ogain_; 126 | int lskip_; 127 | double srr_; 128 | double srt_; 129 | double str_; 130 | double stt_; 131 | double linearUpdate_; 132 | double angularUpdate_; 133 | double temporalUpdate_; 134 | double resampleThreshold_; 135 | int particles_; 136 | double xmin_; 137 | double ymin_; 138 | double xmax_; 139 | double ymax_; 140 | double delta_; 141 | double occ_thresh_; 142 | double llsamplerange_; 143 | double llsamplestep_; 144 | double lasamplerange_; 145 | double lasamplestep_; 146 | 147 | ros::NodeHandle private_nh_; 148 | 149 | unsigned long int seed_; 150 | 151 | double transform_publish_period_; 152 | double tf_delay_; 153 | }; 154 | -------------------------------------------------------------------------------- /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/gmapping/test/rtest.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 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 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 the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | /* Author: Brian Gerkey */ 31 | 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | 40 | ros::NodeHandle* g_n=NULL; 41 | double g_res, g_width, g_height, g_min_free_ratio, g_max_free_ratio; 42 | 43 | class MapClientTest : public testing::Test 44 | { 45 | public: 46 | MapClientTest() 47 | { 48 | got_map_ = false; 49 | got_map_metadata_ = false; 50 | } 51 | 52 | ~MapClientTest() 53 | { 54 | } 55 | 56 | bool got_map_; 57 | boost::shared_ptr map_; 58 | void mapCallback(const boost::shared_ptr& map) 59 | { 60 | map_ = map; 61 | got_map_ = true; 62 | } 63 | 64 | bool got_map_metadata_; 65 | boost::shared_ptr map_metadata_; 66 | void mapMetaDataCallback(const boost::shared_ptr& map_metadata) 67 | { 68 | map_metadata_ = map_metadata; 69 | got_map_metadata_ = true; 70 | } 71 | 72 | void checkMapMetaData(const nav_msgs::MapMetaData& map_metadata) 73 | { 74 | EXPECT_FLOAT_EQ(map_metadata.resolution, g_res); 75 | EXPECT_FLOAT_EQ(map_metadata.width, g_width); 76 | EXPECT_FLOAT_EQ(map_metadata.height, g_height); 77 | } 78 | 79 | void checkMapData(const nav_msgs::OccupancyGrid& map) 80 | { 81 | unsigned int i; 82 | unsigned int free_cnt = 0; 83 | for(i=0; i < map.info.width * map.info.height; i++) 84 | { 85 | if(map.data[i] == 0) 86 | free_cnt++; 87 | } 88 | double free_ratio = free_cnt / (double)(i); 89 | ROS_INFO("Min / ratio / Max free ratio: %f / %f / %f", g_min_free_ratio, free_ratio, g_max_free_ratio); 90 | EXPECT_GE(free_ratio, g_min_free_ratio); 91 | EXPECT_LE(free_ratio, g_max_free_ratio); 92 | } 93 | }; 94 | 95 | /* Try to retrieve the map via service, and compare to ground truth */ 96 | TEST_F(MapClientTest, call_service) 97 | { 98 | nav_msgs::GetMap::Request req; 99 | nav_msgs::GetMap::Response resp; 100 | ASSERT_TRUE(ros::service::waitForService("dynamic_map", 5000)); 101 | ASSERT_TRUE(ros::service::call("dynamic_map", req, resp)); 102 | checkMapMetaData(resp.map.info); 103 | checkMapData(resp.map); 104 | } 105 | 106 | /* Try to retrieve the map via topic, and compare to ground truth */ 107 | TEST_F(MapClientTest, subscribe_topic) 108 | { 109 | ros::Subscriber sub = g_n->subscribe("map", 1, boost::bind(&MapClientTest::mapCallback, this, _1)); 110 | 111 | // Try a few times, because the server may not be up yet. 112 | int i=20; 113 | while(!got_map_ && i > 0) 114 | { 115 | ros::spinOnce(); 116 | ros::WallDuration d(0.25); 117 | d.sleep(); 118 | i--; 119 | } 120 | ASSERT_TRUE(got_map_); 121 | checkMapMetaData(map_->info); 122 | checkMapData(*(map_.get())); 123 | } 124 | 125 | /* Try to retrieve the metadata via topic, and compare to ground truth */ 126 | TEST_F(MapClientTest, subscribe_topic_metadata) 127 | { 128 | ros::Subscriber sub = g_n->subscribe("map_metadata", 1, boost::bind(&MapClientTest::mapMetaDataCallback, this, _1)); 129 | 130 | // Try a few times, because the server may not be up yet. 131 | int i=20; 132 | while(!got_map_metadata_ && i > 0) 133 | { 134 | ros::spinOnce(); 135 | ros::WallDuration d(0.25); 136 | d.sleep(); 137 | i--; 138 | } 139 | ASSERT_TRUE(got_map_metadata_); 140 | checkMapMetaData(*(map_metadata_.get())); 141 | } 142 | 143 | int main(int argc, char **argv) 144 | { 145 | testing::InitGoogleTest(&argc, argv); 146 | 147 | ros::init(argc, argv, "map_client_test"); 148 | g_n = new ros::NodeHandle(); 149 | 150 | // Required args are, in order: 151 | // 152 | ROS_ASSERT(argc == 7); 153 | ros::Duration delay = ros::Duration(atof(argv[1])); 154 | g_res = atof(argv[2]); 155 | g_width = atof(argv[3]); 156 | g_height = atof(argv[4]); 157 | g_min_free_ratio = atof(argv[5]); 158 | g_max_free_ratio = atof(argv[6]); 159 | 160 | while(ros::Time::now().toSec() == 0.0) 161 | { 162 | ROS_INFO("Waiting for initial time publication"); 163 | ros::Duration(0.25).sleep(); 164 | ros::spinOnce(); 165 | } 166 | ros::Time start_time = ros::Time::now(); 167 | while((ros::Time::now() - start_time) < delay) 168 | { 169 | ROS_INFO("Waiting for delay expiration (%.3f - %.3f) < %.3f", 170 | ros::Time::now().toSec(), 171 | start_time.toSec(), 172 | delay.toSec()); 173 | ros::Duration(0.25).sleep(); 174 | ros::spinOnce(); 175 | } 176 | 177 | int result = RUN_ALL_TESTS(); 178 | delete g_n; 179 | return result; 180 | } 181 | -------------------------------------------------------------------------------- /slam_gmapping/gmapping/test/test_map.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2008, Willow Garage, Inc. 5 | # All rights reserved. 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions 9 | # are met: 10 | # 11 | # * Redistributions of source code must retain the above copyright 12 | # notice, this list of conditions and the following disclaimer. 13 | # * Redistributions in binary form must reproduce the above 14 | # copyright notice, this list of conditions and the following 15 | # disclaimer in the documentation and/or other materials provided 16 | # with the distribution. 17 | # * Neither the name of the Willow Garage nor the names of its 18 | # contributors may be used to endorse or promote products derived 19 | # from this software without specific prior written permission. 20 | # 21 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | # POSSIBILITY OF SUCH DAMAGE. 33 | # 34 | 35 | 36 | import PIL.Image 37 | import unittest 38 | import subprocess 39 | import sys 40 | 41 | import roslib 42 | import os 43 | roslib.load_manifest('gmapping') 44 | import rostest 45 | 46 | class TestGmapping(unittest.TestCase): 47 | 48 | # Test that 2 map files are approximately the same 49 | def cmp_maps(self, f0, f1): 50 | im0 = PIL.Image.open(f0+'.pgm') 51 | im1 = PIL.Image.open(f1+'.pgm') 52 | 53 | size = 100,100 54 | im0.thumbnail(size,PIL.Image.ANTIALIAS) 55 | im1.thumbnail(size,PIL.Image.ANTIALIAS) 56 | 57 | # get raw data for comparison 58 | im0d = im0.getdata() 59 | im1d = im1.getdata() 60 | 61 | # assert len(i0)==len(i1) 62 | self.assertTrue(len(im0d) == len(im1d)) 63 | 64 | #compare pixel by pixel for thumbnails 65 | error_count = 0 66 | error_total = 0 67 | pixel_tol = 0 68 | total_error_tol = 0.1 69 | for i in range(len(im0d)): 70 | (p0) = im0d[i] 71 | (p1) = im1d[i] 72 | if abs(p0-p1) > pixel_tol: 73 | error_count = error_count + 1 74 | error_total = error_total + abs(p0-p1) 75 | error_avg = float(error_total)/float(len(im0d)) 76 | print '%d / %d = %.6f (%.6f)'%(error_total,len(im0d),error_avg,total_error_tol) 77 | self.assertTrue(error_avg <= total_error_tol) 78 | 79 | def test_basic_localization_stage(self): 80 | if sys.argv > 1: 81 | target_time = float(sys.argv[1]) 82 | 83 | import time 84 | import rospy 85 | rospy.init_node('test', anonymous=True) 86 | while(rospy.rostime.get_time() == 0.0): 87 | print 'Waiting for initial time publication' 88 | time.sleep(0.1) 89 | start_time = rospy.rostime.get_time() 90 | 91 | while (rospy.rostime.get_time() - start_time) < target_time: 92 | print 'Waiting for end time %.6f (current: %.6f)'%(target_time,(rospy.rostime.get_time() - start_time)) 93 | time.sleep(0.1) 94 | 95 | f0 = os.path.join(roslib.packages.get_pkg_dir('gmapping'),'test','basic_localization_stage_groundtruth') 96 | f1 = os.path.join(roslib.packages.get_pkg_dir('gmapping'),'test','basic_localization_stage_generated') 97 | 98 | cmd = ['rosrun', 'map_server', 'map_saver', 'map:=dynamic_map', '-f', f1] 99 | self.assertTrue(subprocess.call(cmd) == 0) 100 | 101 | self.cmp_maps(f0,f1) 102 | 103 | if __name__ == '__main__': 104 | rostest.run('gmapping', 'gmapping_slam', TestGmapping, sys.argv) 105 | -------------------------------------------------------------------------------- /slam_gmapping/slam_gmapping/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package slam_gmapping 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.4.2 (2020-10-02) 6 | ------------------ 7 | 8 | 1.4.1 (2020-03-16) 9 | ------------------ 10 | 11 | 1.4.0 (2019-07-12) 12 | ------------------ 13 | * update license to BSD and maintainer to ros-orphaned-packages@googlegroups.com 14 | 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 15 | * Contributors: Kei Okada 16 | 17 | 1.3.10 (2018-01-23) 18 | ------------------- 19 | 20 | 1.3.9 (2017-10-22) 21 | ------------------ 22 | 23 | 1.3.8 (2015-07-31) 24 | ------------------ 25 | 26 | 1.3.7 (2015-07-04) 27 | ------------------ 28 | 29 | 1.3.6 (2015-06-26) 30 | ------------------ 31 | 32 | 1.3.5 (2014-08-28) 33 | ------------------ 34 | 35 | 1.3.4 (2014-08-07) 36 | ------------------ 37 | 38 | 1.3.3 (2014-06-23) 39 | ------------------ 40 | 41 | 1.3.2 (2014-01-14) 42 | ------------------ 43 | 44 | 1.3.1 (2014-01-13) 45 | ------------------ 46 | 47 | 1.3.0 (2013-06-28) 48 | ------------------ 49 | * Renamed to gmapping, adding metapackage for slam_gmapping 50 | * Contributors: Mike Ferguson 51 | -------------------------------------------------------------------------------- /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.2 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 | -------------------------------------------------------------------------------- /ydlidar_ros/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(ydlidar_ros) 3 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 4 | #add_definitions(-std=c++11) # Use C++11 5 | 6 | find_package(catkin REQUIRED COMPONENTS 7 | rosconsole 8 | roscpp 9 | sensor_msgs 10 | ) 11 | 12 | #add_subdirectory(sdk) 13 | 14 | set(SDK_PATH "./sdk/") 15 | 16 | FILE(GLOB SDK_SRC 17 | "${SDK_PATH}/src/impl/unix/*.cpp" 18 | "${SDK_PATH}/src/*.cpp" 19 | "${SDK_PATH}/src/*.c" 20 | ) 21 | 22 | catkin_package() 23 | 24 | include_directories( 25 | ${catkin_INCLUDE_DIRS} 26 | ${PROJECT_SOURCE_DIR}/src 27 | ${PROJECT_SOURCE_DIR}/sdk/include 28 | ${PROJECT_SOURCE_DIR}/sdk/src 29 | ) 30 | 31 | add_executable(ydlidar_node src/ydlidar_node.cpp ${SDK_SRC}) 32 | add_executable(ydlidar_client src/ydlidar_client.cpp) 33 | 34 | target_link_libraries(ydlidar_node 35 | ${catkin_LIBRARIES} 36 | ) 37 | target_link_libraries(ydlidar_client 38 | ${catkin_LIBRARIES} 39 | ) 40 | 41 | install(TARGETS ydlidar_node ydlidar_client 42 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 43 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 44 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 45 | ) 46 | 47 | 48 | install(DIRECTORY launch startup sdk 49 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 50 | USE_SOURCE_PERMISSIONS 51 | ) 52 | 53 | 54 | -------------------------------------------------------------------------------- /ydlidar_ros/docs/paramters.md: -------------------------------------------------------------------------------- 1 | # ROS Paramters Table 2 | 3 | ## Paramters Description 4 | | Parameter | Type | Description | 5 | |-----------------------|------------------------|-----------------------------------------------------| 6 | | `port` | String | port of lidar (ex. /dev/ttyUSB0) | 7 | | `baudrate` | int | baudrate of lidar (ex. 230400) | 8 | | `frame_id` | String | TF frame of sensor, default: `laser_frame` | 9 | | `isSingleChannel` | bool | Whether LiDAR is a single-channel, default: false | 10 | | `resolution_fixed` | bool | Fixed angluar resolution, default: true | 11 | | `auto_reconnect` | bool | Automatically reconnect the LiDAR, default: true | 12 | | `reversion` | bool | Reversion LiDAR, default: true | 13 | | `isTOFLidar` | bool | Whether LiDAR is TOF Type, default: false | 14 | | `angle_min` | float | Minimum Valid Angle, defalut: -180.0 | 15 | | `angle_max` | float | Maximum Valid Angle, defalut: 180.0 | 16 | | `range_min` | float | Minimum Valid range, defalut: 0.01m | 17 | | `range_max` | float | Maximum Valid range, defalut: 64.0m | 18 | | `ignore_array` | String | LiDAR filtering angle area, default: "" | 19 | | `samp_rate` | int | sampling rate of lidar, default: 9 | 20 | | `frequency` | float | scan frequency of lidar,default: 10.0 | 21 | 22 | ## Baudrate Table 23 | 24 | | LiDAR | baudrate | 25 | |-----------------------------------------------|-----------------------| 26 | |F4/S2/X2/X2L/S4/TX8/TX20/G4C | 115200 | 27 | |X4 | 128000 | 28 | |S4B | 153600 | 29 | |G1/G2/R2/G4/G4PRO/F4PRO | 230400 | 30 | |G6/TG15/TG30/TG50 | 512000 | 31 | 32 | ## SingleChannel Table 33 | 34 | | LiDAR | isSingleChannel | 35 | |-----------------------------------------------------------|-----------------------| 36 | |G1/G2/G4/G6/F4/F4PRO/S4/S4B/X4/R2/G4C | false | 37 | |S2/X2/X2L | true | 38 | |TG15/TG30/TG50 | false | 39 | |TX8/TX20 | true | 40 | 41 | ##　isToFLidar Table 42 | 43 | | LiDAR | isTOFLidar | 44 | |-----------------------------------------------------------------------|-----------------------| 45 | |G1/G2/G4/G6/F4/F4PRO/S4/S4B/X4/R2/G4C/S2/X2/X2L | false | 46 | |TG15/TG30/TG50/TX8/TX20 | true | 47 | 48 | ## Sampling Rate Table 49 | 50 | | LiDAR | samp_rate | 51 | |-----------------------------|------------------------| 52 | |G4/F4 | 4,8,9 | 53 | |F4PRO | 4,6 | 54 | |G6 | 8,16,18 | 55 | |G1/G2/R2/X4 | 5 | 56 | |S4/S4B/G4C/TX8/TX20 | 4 | 57 | |S2 | 3 | 58 | |TG15/TG30/TG50 | 10,18,20 | 59 | 60 | ## Frequency Table 61 | 62 | 63 | | LiDAR | frequency | 64 | |-----------------------------------------------|------------------------| 65 | |G1/G2/R2/G6/G4/G4PRO/F4/F4PRO | 5-12Hz | 66 | |S4/S4B/S2/TX8/TX20/X4 | Not Support | 67 | |TG15/TG30/TG50 | 3-16Hz | 68 | 69 | Note: For unsupported LiDARs, adjusting the scanning frequency requires external access to PWM speed control. 70 | -------------------------------------------------------------------------------- /ydlidar_ros/docs/ydlidar.md: -------------------------------------------------------------------------------- 1 | # YDLIDAR ROS Package Download and Build 2 | ## Step1: create a ROS workspace, if there is no workspace, othereise Skip to Step2 3 | #### Linux/OS X 4 | $mkdir -p ~/ydlidar_ros_ws/src 5 | $cd ~/ydlidar_ros_ws/src 6 | #### Windows 7 | $md \dev\ydlidar_ros_ws\src 8 | $cd \dev\ydlidar_ros_ws\src 9 | 10 | ## Step2: clone ydlidar ros package 11 | $git clone https://github.com/YDLIDAR/ydlidar_ros 12 | 13 | ## Step3: Build [ydlidar_ros](https://github.com/YDLIDAR/ydlidar_ros) package 14 | $cd .. 15 | $catkin_make 16 | 17 | Note: Set ROS Workspace Environment Variables 18 | 19 | $echo "source ~/ydlidar_ros_ws/devel/setup.bash" >> ~/.bashrc 20 | $source ~/.bashrc 21 | 22 | ## Step4:Configure LiDAR [paramters](../launch/lidar.launch) 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 41 | 42 | 43 | Note: How to configure paramters, see [here](paramters.md) 44 | 45 | ## Step5:Create serial port Alias[/dev/ydlidar] 46 | $chmod 0777 src/ydlidar_ros/startup/* 47 | $sudo sh src/ydlidar_ros/startup/initenv.sh 48 | Note: After completing the previous operation, replug the LiDAR again. 49 | 50 | ## Step6:Run ydlidar_ros node 51 | 52 | $roslaunch ydlidar_ros lidar.launch 53 | 54 | $rosrun ydlidar_ros ydlidar_client 55 | 56 | or 57 | 58 | $roslaunch ydlidar_ros lidar_view.launch 59 | 60 | -------------------------------------------------------------------------------- /ydlidar_ros/launch/G1.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 19 | 20 | -------------------------------------------------------------------------------- /ydlidar_ros/launch/G2.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 19 | 20 | -------------------------------------------------------------------------------- /ydlidar_ros/launch/G2C.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 19 | 20 | -------------------------------------------------------------------------------- /ydlidar_ros/launch/G4.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 19 | 20 | -------------------------------------------------------------------------------- /ydlidar_ros/launch/G6.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 19 | 20 | -------------------------------------------------------------------------------- /ydlidar_ros/launch/R2.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 19 | 20 | -------------------------------------------------------------------------------- /ydlidar_ros/launch/S2.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 20 | 21 | -------------------------------------------------------------------------------- /ydlidar_ros/launch/S4.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 19 | 20 | -------------------------------------------------------------------------------- /ydlidar_ros/launch/TG.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 20 | 21 | -------------------------------------------------------------------------------- /ydlidar_ros/launch/TX20.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 21 | 22 | -------------------------------------------------------------------------------- /ydlidar_ros/launch/TX8.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 21 | 22 | -------------------------------------------------------------------------------- /ydlidar_ros/launch/X2L.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 20 | 21 | -------------------------------------------------------------------------------- /ydlidar_ros/launch/X4.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 19 | 20 | -------------------------------------------------------------------------------- /ydlidar_ros/launch/display.launch: -------------------------------------------------------------------------------- 1 | 2 | 4 | 7 | 10 | 13 | 17 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 42 | 43 | -------------------------------------------------------------------------------- /ydlidar_ros/launch/gazebo.launch: -------------------------------------------------------------------------------- 1 | 2 | 4 | 9 | 15 | 20 | 21 | -------------------------------------------------------------------------------- /ydlidar_ros/launch/laser.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /ydlidar_ros/launch/lidar.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 19 | 20 | -------------------------------------------------------------------------------- /ydlidar_ros/launch/lidar.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /LidarLaserScan1 10 | Splitter Ratio: 0.5 11 | Tree Height: 413 12 | - Class: rviz/Selection 13 | Name: Selection 14 | - Class: rviz/Tool Properties 15 | Expanded: 16 | - /2D Pose Estimate1 17 | - /2D Nav Goal1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.588679 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz/Time 26 | Experimental: false 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: "" 30 | Visualization Manager: 31 | Class: "" 32 | Displays: 33 | - Alpha: 0.5 34 | Cell Size: 1 35 | Class: rviz/Grid 36 | Color: 160; 160; 164 37 | Enabled: true 38 | Line Style: 39 | Line Width: 0.03 40 | Value: Lines 41 | Name: Grid 42 | Normal Cell Count: 0 43 | Offset: 44 | X: 0 45 | Y: 0 46 | Z: 0 47 | Plane: XY 48 | Plane Cell Count: 10 49 | Reference Frame: 50 | Value: true 51 | - Alpha: 1 52 | Autocompute Intensity Bounds: true 53 | Autocompute Value Bounds: 54 | Max Value: 0 55 | Min Value: 0 56 | Value: true 57 | Axis: Z 58 | Channel Name: intensity 59 | Class: rviz/LaserScan 60 | Color: 255; 255; 255 61 | Color Transformer: AxisColor 62 | Decay Time: 0 63 | Enabled: true 64 | Invert Rainbow: false 65 | Max Color: 255; 255; 255 66 | Max Intensity: 4096 67 | Min Color: 0; 0; 0 68 | Min Intensity: 0 69 | Name: LidarLaserScan 70 | Position Transformer: XYZ 71 | Queue Size: 1000 72 | Selectable: true 73 | Size (Pixels): 5 74 | Size (m): 0.03 75 | Style: Squares 76 | Topic: /scan 77 | Use Fixed Frame: true 78 | Use rainbow: true 79 | Value: true 80 | Enabled: true 81 | Global Options: 82 | Background Color: 48; 48; 48 83 | Fixed Frame: laser_frame 84 | Frame Rate: 30 85 | Name: root 86 | Tools: 87 | - Class: rviz/MoveCamera 88 | - Class: rviz/Interact 89 | Hide Inactive Objects: true 90 | - Class: rviz/Select 91 | - Class: rviz/SetInitialPose 92 | Topic: /initialpose 93 | - Class: rviz/SetGoal 94 | Topic: /move_base_simple/goal 95 | Value: true 96 | Views: 97 | Current: 98 | Class: rviz/Orbit 99 | Distance: 11.1184 100 | Focal Point: 101 | X: -0.0344972 102 | Y: 0.065886 103 | Z: 0.148092 104 | Name: Current View 105 | Near Clip Distance: 0.01 106 | Pitch: 0.615399 107 | Target Frame: 108 | Value: Orbit (rviz) 109 | Yaw: 5.82358 110 | Saved: ~ 111 | Window Geometry: 112 | Displays: 113 | collapsed: false 114 | Height: 626 115 | Hide Left Dock: false 116 | Hide Right Dock: false 117 | QMainWindow State: 000000ff00000000fd0000000400000000000001950000022cfc0200000006fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000280000022c000000dd00fffffffb0000000a0049006d006100670065000000028d000000c60000000000000000000000010000010b00000294fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003300000294000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a0000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000003240000022c00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 118 | Selection: 119 | collapsed: false 120 | Time: 121 | collapsed: false 122 | Tool Properties: 123 | collapsed: false 124 | Views: 125 | collapsed: false 126 | Width: 1215 127 | X: 65 128 | Y: 24 129 | -------------------------------------------------------------------------------- /ydlidar_ros/launch/lidar_view.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /ydlidar_ros/launch/yd_lidar.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /ydlidar_ros/meshes/ydlidar.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jerinpeter/4wdNavbot/0962ac854c9dd4a1dd54da58aef3486182f03057/ydlidar_ros/meshes/ydlidar.png -------------------------------------------------------------------------------- /ydlidar_ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ydlidar_ros 4 | 1.4.6 5 | The ydlidar package 6 | 7 | 8 | 9 | 10 | Tony 11 | 12 | 13 | 14 | 15 | 16 | BSD 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | rosconsole 44 | roscpp 45 | sensor_msgs 46 | rosconsole 47 | roscpp 48 | sensor_msgs 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /ydlidar_ros/sdk/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | #add_definitions(-std=c++11) # Use C++11 3 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 4 | include_directories(include) 5 | include_directories(src) 6 | 7 | IF (WIN32) 8 | FILE(GLOB SDK_SRC 9 | "src/*.cpp" 10 | "src/*.c" 11 | "src/*.h" 12 | "src/impl/windows/*.cpp" 13 | "src/impl/windows/*.h" 14 | ) 15 | 16 | ELSE() 17 | FILE(GLOB SDK_SRC 18 | "src/*.cpp" 19 | "src/*.c" 20 | "src/*.h" 21 | "src/impl/unix/*.cpp" 22 | "src/impl/unix/*.h" 23 | ) 24 | 25 | 26 | ENDIF() 27 | 28 | add_subdirectory(samples) 29 | 30 | add_library(ydlidar_driver STATIC ${SDK_SRC}) 31 | IF (WIN32) 32 | target_link_libraries(ydlidar_driver setupapi Winmm) 33 | ELSE() 34 | target_link_libraries(ydlidar_driver rt pthread) 35 | ENDIF() 36 | -------------------------------------------------------------------------------- /ydlidar_ros/sdk/doc/API.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jerinpeter/4wdNavbot/0962ac854c9dd4a1dd54da58aef3486182f03057/ydlidar_ros/sdk/doc/API.pdf -------------------------------------------------------------------------------- /ydlidar_ros/sdk/doc/README.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jerinpeter/4wdNavbot/0962ac854c9dd4a1dd54da58aef3486182f03057/ydlidar_ros/sdk/doc/README.pdf -------------------------------------------------------------------------------- /ydlidar_ros/sdk/image/EAI.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jerinpeter/4wdNavbot/0962ac854c9dd4a1dd54da58aef3486182f03057/ydlidar_ros/sdk/image/EAI.png -------------------------------------------------------------------------------- /ydlidar_ros/sdk/image/FlowChart.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jerinpeter/4wdNavbot/0962ac854c9dd4a1dd54da58aef3486182f03057/ydlidar_ros/sdk/image/FlowChart.png -------------------------------------------------------------------------------- /ydlidar_ros/sdk/image/G1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jerinpeter/4wdNavbot/0962ac854c9dd4a1dd54da58aef3486182f03057/ydlidar_ros/sdk/image/G1.png -------------------------------------------------------------------------------- /ydlidar_ros/sdk/image/G2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jerinpeter/4wdNavbot/0962ac854c9dd4a1dd54da58aef3486182f03057/ydlidar_ros/sdk/image/G2.png -------------------------------------------------------------------------------- /ydlidar_ros/sdk/image/G4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jerinpeter/4wdNavbot/0962ac854c9dd4a1dd54da58aef3486182f03057/ydlidar_ros/sdk/image/G4.png -------------------------------------------------------------------------------- /ydlidar_ros/sdk/image/G6.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jerinpeter/4wdNavbot/0962ac854c9dd4a1dd54da58aef3486182f03057/ydlidar_ros/sdk/image/G6.png -------------------------------------------------------------------------------- /ydlidar_ros/sdk/image/T15.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jerinpeter/4wdNavbot/0962ac854c9dd4a1dd54da58aef3486182f03057/ydlidar_ros/sdk/image/T15.jpg -------------------------------------------------------------------------------- /ydlidar_ros/sdk/image/TG30.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jerinpeter/4wdNavbot/0962ac854c9dd4a1dd54da58aef3486182f03057/ydlidar_ros/sdk/image/TG30.png -------------------------------------------------------------------------------- /ydlidar_ros/sdk/image/TOF.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jerinpeter/4wdNavbot/0962ac854c9dd4a1dd54da58aef3486182f03057/ydlidar_ros/sdk/image/TOF.png -------------------------------------------------------------------------------- /ydlidar_ros/sdk/image/TS4.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jerinpeter/4wdNavbot/0962ac854c9dd4a1dd54da58aef3486182f03057/ydlidar_ros/sdk/image/TS4.jpg -------------------------------------------------------------------------------- /ydlidar_ros/sdk/image/TX20.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jerinpeter/4wdNavbot/0962ac854c9dd4a1dd54da58aef3486182f03057/ydlidar_ros/sdk/image/TX20.png -------------------------------------------------------------------------------- /ydlidar_ros/sdk/image/YDLidar.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jerinpeter/4wdNavbot/0962ac854c9dd4a1dd54da58aef3486182f03057/ydlidar_ros/sdk/image/YDLidar.jpg -------------------------------------------------------------------------------- /ydlidar_ros/sdk/image/image.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jerinpeter/4wdNavbot/0962ac854c9dd4a1dd54da58aef3486182f03057/ydlidar_ros/sdk/image/image.png -------------------------------------------------------------------------------- /ydlidar_ros/sdk/image/index-TX8.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jerinpeter/4wdNavbot/0962ac854c9dd4a1dd54da58aef3486182f03057/ydlidar_ros/sdk/image/index-TX8.jpg -------------------------------------------------------------------------------- /ydlidar_ros/sdk/image/index-X2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jerinpeter/4wdNavbot/0962ac854c9dd4a1dd54da58aef3486182f03057/ydlidar_ros/sdk/image/index-X2.jpg -------------------------------------------------------------------------------- /ydlidar_ros/sdk/image/index-X4.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jerinpeter/4wdNavbot/0962ac854c9dd4a1dd54da58aef3486182f03057/ydlidar_ros/sdk/image/index-X4.jpg -------------------------------------------------------------------------------- /ydlidar_ros/sdk/image/sdk_architecture.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jerinpeter/4wdNavbot/0962ac854c9dd4a1dd54da58aef3486182f03057/ydlidar_ros/sdk/image/sdk_architecture.png -------------------------------------------------------------------------------- /ydlidar_ros/sdk/image/sdk_init.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jerinpeter/4wdNavbot/0962ac854c9dd4a1dd54da58aef3486182f03057/ydlidar_ros/sdk/image/sdk_init.png -------------------------------------------------------------------------------- /ydlidar_ros/sdk/image/sdk_scanning.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jerinpeter/4wdNavbot/0962ac854c9dd4a1dd54da58aef3486182f03057/ydlidar_ros/sdk/image/sdk_scanning.png -------------------------------------------------------------------------------- /ydlidar_ros/sdk/image/step1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jerinpeter/4wdNavbot/0962ac854c9dd4a1dd54da58aef3486182f03057/ydlidar_ros/sdk/image/step1.png -------------------------------------------------------------------------------- /ydlidar_ros/sdk/image/step2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jerinpeter/4wdNavbot/0962ac854c9dd4a1dd54da58aef3486182f03057/ydlidar_ros/sdk/image/step2.png -------------------------------------------------------------------------------- /ydlidar_ros/sdk/image/step3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jerinpeter/4wdNavbot/0962ac854c9dd4a1dd54da58aef3486182f03057/ydlidar_ros/sdk/image/step3.png -------------------------------------------------------------------------------- /ydlidar_ros/sdk/image/step4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jerinpeter/4wdNavbot/0962ac854c9dd4a1dd54da58aef3486182f03057/ydlidar_ros/sdk/image/step4.png -------------------------------------------------------------------------------- /ydlidar_ros/sdk/image/step5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jerinpeter/4wdNavbot/0962ac854c9dd4a1dd54da58aef3486182f03057/ydlidar_ros/sdk/image/step5.png -------------------------------------------------------------------------------- /ydlidar_ros/sdk/image/step6.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jerinpeter/4wdNavbot/0962ac854c9dd4a1dd54da58aef3486182f03057/ydlidar_ros/sdk/image/step6.png -------------------------------------------------------------------------------- /ydlidar_ros/sdk/include/thread.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "v8stdint.h" 3 | 4 | #ifdef _WIN32 5 | #include 6 | #include 7 | #include 8 | #include 9 | #else 10 | #include 11 | #include 12 | #endif 13 | 14 | #define UNUSED(x) (void)x 15 | 16 | #if defined(__ANDROID__) 17 | #define pthread_cancel(x) 0 18 | #endif 19 | 20 | #define CLASS_THREAD(c , x ) Thread::ThreadCreateObjectFunctor(this) 21 | 22 | class Thread { 23 | public: 24 | 25 | template static Thread 26 | ThreadCreateObjectFunctor(CLASS *pthis) { 27 | return createThread(createThreadAux, pthis); 28 | } 29 | 30 | template static _size_t THREAD_PROC 31 | createThreadAux(void *param) { 32 | return (static_cast(param)->*PROC)(); 33 | } 34 | 35 | static Thread createThread(thread_proc_t proc, void *param = NULL) { 36 | Thread thread_(proc, param); 37 | #if defined(_WIN32) 38 | thread_._handle = (_size_t)(_beginthreadex(NULL, 0, 39 | (unsigned int (__stdcall *)(void *))proc, param, 0, NULL)); 40 | #else 41 | assert(sizeof(thread_._handle) >= sizeof(pthread_t)); 42 | 43 | pthread_create((pthread_t *)&thread_._handle, NULL, (void *(*)(void *))proc, 44 | param); 45 | #endif 46 | return thread_; 47 | } 48 | 49 | public: 50 | explicit Thread(): _param(NULL), _func(NULL), _handle(0) {} 51 | virtual ~Thread() {} 52 | _size_t getHandle() { 53 | return _handle; 54 | } 55 | int terminate() { 56 | #if defined(_WIN32) 57 | 58 | if (!this->_handle) { 59 | return 0; 60 | } 61 | 62 | if (TerminateThread(reinterpret_cast(this->_handle), -1)) { 63 | CloseHandle(reinterpret_cast(this->_handle)); 64 | this->_handle = NULL; 65 | return 0; 66 | } else { 67 | return -2; 68 | } 69 | 70 | #else 71 | 72 | if (!this->_handle) { 73 | return 0; 74 | } 75 | 76 | return pthread_cancel((pthread_t)this->_handle); 77 | #endif 78 | } 79 | void *getParam() { 80 | return _param; 81 | } 82 | int join(unsigned long timeout = -1) { 83 | if (!this->_handle) { 84 | return 0; 85 | } 86 | 87 | #if defined(_WIN32) 88 | 89 | switch (WaitForSingleObject(reinterpret_cast(this->_handle), timeout)) { 90 | case WAIT_OBJECT_0: 91 | CloseHandle(reinterpret_cast(this->_handle)); 92 | this->_handle = NULL; 93 | return 0; 94 | 95 | case WAIT_ABANDONED: 96 | return -2; 97 | 98 | case WAIT_TIMEOUT: 99 | return -1; 100 | } 101 | 102 | #else 103 | UNUSED(timeout); 104 | void *res; 105 | int s = -1; 106 | s = pthread_cancel((pthread_t)(this->_handle)); 107 | 108 | if (s != 0) { 109 | } 110 | 111 | s = pthread_join((pthread_t)(this->_handle), &res); 112 | 113 | if (s != 0) { 114 | } 115 | 116 | if (res == PTHREAD_CANCELED) { 117 | printf("%lu thread has been canceled\n", this->_handle); 118 | this->_handle = 0; 119 | } 120 | 121 | #endif 122 | return 0; 123 | } 124 | 125 | bool operator== (const Thread &right) { 126 | return this->_handle == right._handle; 127 | } 128 | protected: 129 | explicit Thread(thread_proc_t proc, void *param): _param(param), _func(proc), 130 | _handle(0) {} 131 | void *_param; 132 | thread_proc_t _func; 133 | _size_t _handle; 134 | }; 135 | 136 | -------------------------------------------------------------------------------- /ydlidar_ros/sdk/include/timer.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "v8stdint.h" 3 | #include 4 | #include 5 | #include 6 | 7 | 8 | 9 | #define BEGIN_STATIC_CODE( _blockname_ ) \ 10 | static class _static_code_##_blockname_ { \ 11 | public: \ 12 | _static_code_##_blockname_ () 13 | 14 | 15 | #define END_STATIC_CODE( _blockname_ ) \ 16 | } _instance_##_blockname_; 17 | 18 | 19 | #if defined(_WIN32) 20 | #include 21 | #define delay(x) ::Sleep(x) 22 | #else 23 | #include 24 | #include 25 | 26 | static inline void delay(uint32_t ms) { 27 | while (ms >= 1000) { 28 | usleep(1000 * 1000); 29 | ms -= 1000; 30 | }; 31 | 32 | if (ms != 0) { 33 | usleep(ms * 1000); 34 | } 35 | } 36 | #endif 37 | 38 | 39 | namespace impl { 40 | 41 | #if defined(_WIN32) 42 | void HPtimer_reset(); 43 | #endif 44 | uint32_t getHDTimer(); 45 | uint64_t getCurrentTime(); 46 | } // namespace impl 47 | 48 | 49 | #define getms() impl::getHDTimer() 50 | #define getTime() impl::getCurrentTime() 51 | -------------------------------------------------------------------------------- /ydlidar_ros/sdk/include/utils.h: -------------------------------------------------------------------------------- 1 | 2 | #pragma once 3 | 4 | #ifdef WIN32 5 | #ifdef ydlidar_IMPORTS 6 | #define YDLIDAR_API __declspec(dllimport) 7 | #else 8 | #ifdef ydlidarStatic_IMPORTS 9 | #define YDLIDAR_API 10 | #else 11 | 12 | #define YDLIDAR_API __declspec(dllexport) 13 | #endif // YDLIDAR_STATIC_EXPORTS 14 | #endif 15 | 16 | #else 17 | #define YDLIDAR_API 18 | #endif // ifdef WIN32 19 | -------------------------------------------------------------------------------- /ydlidar_ros/sdk/include/v8stdint.h: -------------------------------------------------------------------------------- 1 | #ifndef V8STDINT_H_ 2 | #define V8STDINT_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #if defined(_MSC_VER) 15 | #include 16 | #endif 17 | 18 | #if !defined(_MSC_VER) 19 | #include 20 | #endif 21 | 22 | #define UNUSED(x) (void)x 23 | 24 | #if !defined(_MSC_VER) 25 | # define _access access 26 | #endif 27 | 28 | #if defined(_WIN32) && !defined(__MINGW32__) 29 | typedef signed char int8_t; 30 | typedef unsigned char uint8_t; 31 | typedef short int16_t; 32 | typedef unsigned short uint16_t; 33 | typedef int int32_t; 34 | typedef unsigned int uint32_t; 35 | typedef __int64 int64_t; 36 | typedef unsigned __int64 uint64_t; 37 | #else 38 | 39 | #include 40 | 41 | #endif 42 | 43 | #define __small_endian 44 | 45 | #ifndef __GNUC__ 46 | #define __attribute__(x) 47 | #endif 48 | 49 | 50 | #ifdef _AVR_ 51 | typedef uint8_t _size_t; 52 | #define THREAD_PROC 53 | #elif defined (WIN64) 54 | typedef uint64_t _size_t; 55 | #define THREAD_PROC __stdcall 56 | #elif defined (WIN32) 57 | typedef uint32_t _size_t; 58 | #define THREAD_PROC __stdcall 59 | #elif defined (_M_X64) 60 | typedef uint64_t _size_t; 61 | #define THREAD_PROC __stdcall 62 | #elif defined (__GNUC__) 63 | typedef unsigned long _size_t; 64 | #define THREAD_PROC 65 | #elif defined (__ICCARM__) 66 | typedef uint32_t _size_t; 67 | #define THREAD_PROC 68 | #endif 69 | 70 | typedef _size_t (THREAD_PROC *thread_proc_t)(void *); 71 | 72 | typedef int32_t result_t; 73 | 74 | #define RESULT_OK 0 75 | #define RESULT_TIMEOUT -1 76 | #define RESULT_FAIL -2 77 | 78 | #define INVALID_TIMESTAMP (0) 79 | 80 | enum { 81 | DEVICE_DRIVER_TYPE_SERIALPORT = 0x0, 82 | DEVICE_DRIVER_TYPE_TCP = 0x1, 83 | }; 84 | 85 | 86 | #define IS_OK(x) ( (x) == RESULT_OK ) 87 | #define IS_TIMEOUT(x) ( (x) == RESULT_TIMEOUT ) 88 | #define IS_FAIL(x) ( (x) == RESULT_FAIL ) 89 | 90 | 91 | // Determine if sigaction is available 92 | #if __APPLE__ || _POSIX_C_SOURCE >= 1 || _XOPEN_SOURCE || _POSIX_SOURCE 93 | #define HAS_SIGACTION 94 | #endif 95 | 96 | 97 | static volatile sig_atomic_t g_signal_status = 0; 98 | 99 | #ifdef HAS_SIGACTION 100 | static struct sigaction old_action; 101 | #else 102 | typedef void (* signal_handler_t)(int); 103 | static signal_handler_t old_signal_handler = 0; 104 | #endif 105 | 106 | #ifdef HAS_SIGACTION 107 | inline struct sigaction 108 | set_sigaction(int signal_value, const struct sigaction &action) 109 | #else 110 | inline signal_handler_t 111 | set_signal_handler(int signal_value, signal_handler_t signal_handler) 112 | #endif 113 | { 114 | #ifdef HAS_SIGACTION 115 | struct sigaction old_action; 116 | ssize_t ret = sigaction(signal_value, &action, &old_action); 117 | 118 | if (ret == -1) 119 | #else 120 | signal_handler_t old_signal_handler = std::signal(signal_value, signal_handler); 121 | 122 | // NOLINTNEXTLINE(readability/braces) 123 | if (old_signal_handler == SIG_ERR) 124 | #endif 125 | { 126 | const size_t error_length = 1024; 127 | // NOLINTNEXTLINE(runtime/arrays) 128 | char error_string[error_length]; 129 | #ifndef _WIN32 130 | #if (defined(_GNU_SOURCE) && !defined(ANDROID) &&(_POSIX_C_SOURCE >= 200112L)) 131 | char *msg = strerror_r(errno, error_string, error_length); 132 | 133 | if (msg != error_string) { 134 | strncpy(error_string, msg, error_length); 135 | msg[error_length - 1] = '\0'; 136 | } 137 | 138 | #else 139 | int error_status = strerror_r(errno, error_string, error_length); 140 | 141 | if (error_status != 0) { 142 | throw std::runtime_error("Failed to get error string for errno: " + 143 | std::to_string(errno)); 144 | } 145 | 146 | #endif 147 | #else 148 | strerror_s(error_string, error_length, errno); 149 | #endif 150 | // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here) 151 | throw std::runtime_error( 152 | std::string("Failed to set SIGINT signal handler: (" + std::to_string(errno) + ")") + 153 | error_string); 154 | // *INDENT-ON* 155 | } 156 | 157 | #ifdef HAS_SIGACTION 158 | return old_action; 159 | #else 160 | return old_signal_handler; 161 | #endif 162 | } 163 | 164 | inline void trigger_interrupt_guard_condition(int signal_value) { 165 | g_signal_status = signal_value; 166 | signal(signal_value, SIG_DFL); 167 | } 168 | 169 | inline void 170 | #ifdef HAS_SIGACTION 171 | signal_handler(int signal_value, siginfo_t *siginfo, void *context) 172 | #else 173 | signal_handler(int signal_value) 174 | #endif 175 | { 176 | // TODO(wjwwood): remove? move to console logging at some point? 177 | printf("signal_handler(%d)\n", signal_value); 178 | 179 | #ifdef HAS_SIGACTION 180 | 181 | if (old_action.sa_flags & SA_SIGINFO) { 182 | if (old_action.sa_sigaction != NULL) { 183 | old_action.sa_sigaction(signal_value, siginfo, context); 184 | } 185 | } else { 186 | if ( 187 | old_action.sa_handler != NULL && // Is set 188 | old_action.sa_handler != SIG_DFL && // Is not default 189 | old_action.sa_handler != SIG_IGN) { // Is not ignored 190 | old_action.sa_handler(signal_value); 191 | } 192 | } 193 | 194 | #else 195 | 196 | if (old_signal_handler) { 197 | old_signal_handler(signal_value); 198 | } 199 | 200 | #endif 201 | 202 | trigger_interrupt_guard_condition(signal_value); 203 | } 204 | 205 | namespace ydlidar { 206 | 207 | inline void init(int argc, char *argv[]) { 208 | UNUSED(argc); 209 | UNUSED(argv); 210 | #ifdef HAS_SIGACTION 211 | struct sigaction action; 212 | memset(&action, 0, sizeof(action)); 213 | sigemptyset(&action.sa_mask); 214 | action.sa_sigaction = ::signal_handler; 215 | action.sa_flags = SA_SIGINFO; 216 | ::old_action = set_sigaction(SIGINT, action); 217 | set_sigaction(SIGTERM, action); 218 | 219 | #else 220 | ::old_signal_handler = set_signal_handler(SIGINT, ::signal_handler); 221 | // Register an on_shutdown hook to restore the old signal handler. 222 | #endif 223 | } 224 | inline bool ok() { 225 | return g_signal_status == 0; 226 | } 227 | inline void shutdownNow() { 228 | trigger_interrupt_guard_condition(SIGINT); 229 | } 230 | 231 | //inline bool fileExists(const std::string filename) { 232 | // return 0 == _access(filename.c_str(), 0x00 ); // 0x00 = Check for existence only! 233 | //} 234 | 235 | inline bool fileExists(const std::string filename) { 236 | #ifdef _WIN32 237 | struct _stat info = {0}; 238 | int ret = _stat(filename.c_str(), &info); 239 | #else 240 | struct stat info = {0}; 241 | int ret = stat(filename.c_str(), &info); 242 | #endif 243 | return (ret == 0); 244 | /*return 0 == _access(filename.c_str(), 0x00 ); // 0x00 = Check for existence only!*/ 245 | } 246 | 247 | 248 | }// namespace ydlidar 249 | 250 | 251 | #endif // V8STDINT_H_ 252 | -------------------------------------------------------------------------------- /ydlidar_ros/sdk/license: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2015-2019, YDLIDAR Team. (http://www.ydlidar.com). 5 | * Copyright (c) 2015-2018, EAIBOT Co., Ltd. (http://www.eaibot.com) 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the Willow Garage nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | *********************************************************************/ 35 | -------------------------------------------------------------------------------- /ydlidar_ros/sdk/samples/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | 2 | cmake_minimum_required(VERSION 2.8) 3 | PROJECT(ydlidar_test) 4 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 5 | add_definitions(-std=c++11) # Use C++11 6 | 7 | #Include directories 8 | INCLUDE_DIRECTORIES( 9 | ${CMAKE_SOURCE_DIR} 10 | ${CMAKE_SOURCE_DIR}/../ 11 | ${CMAKE_CURRENT_BINARY_DIR} 12 | ) 13 | 14 | SET(EXECUTABLE_OUTPUT_PATH ${CMAKE_BINARY_DIR}) 15 | ADD_EXECUTABLE(${PROJECT_NAME} 16 | main.cpp) 17 | 18 | # Add the required libraries for linking: 19 | TARGET_LINK_LIBRARIES(${PROJECT_NAME} ydlidar_driver) 20 | -------------------------------------------------------------------------------- /ydlidar_ros/sdk/src/common.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2018, EAIBOT, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | #pragma once 35 | 36 | #if defined(_WIN32) 37 | 38 | #include "impl\windows\win.h" 39 | #include "impl\windows\win_serial.h" 40 | #elif defined(__GNUC__) 41 | #include "impl/unix/unix.h" 42 | #include "impl/unix/unix_serial.h" 43 | #else 44 | #error "unsupported target" 45 | #endif 46 | #include "locker.h" 47 | #include "thread.h" 48 | #include "timer.h" 49 | 50 | #define SDKVerision "1.4.7" 51 | -------------------------------------------------------------------------------- /ydlidar_ros/sdk/src/impl/list_ports/list_ports_win.cpp: -------------------------------------------------------------------------------- 1 | #if defined(_WIN32) 2 | 3 | /* 4 | * Copyright (c) 2014 Craig Lilley 5 | * This software is made available under the terms of the MIT licence. 6 | * A copy of the licence can be obtained from: 7 | * http://opensource.org/licenses/MIT 8 | */ 9 | 10 | #include "serial.h" 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | using serial::PortInfo; 19 | using std::vector; 20 | using std::string; 21 | 22 | static const DWORD port_name_max_length = 256; 23 | static const DWORD friendly_name_max_length = 256; 24 | static const DWORD hardware_id_max_length = 256; 25 | 26 | // Convert a wide Unicode string to an UTF8 string 27 | std::string utf8_encode(const std::wstring &wstr) { 28 | int size_needed = WideCharToMultiByte(CP_UTF8, 0, &wstr[0], (int)wstr.size(), NULL, 0, NULL, NULL); 29 | std::string strTo(size_needed, 0); 30 | WideCharToMultiByte(CP_UTF8, 0, &wstr[0], (int)wstr.size(), &strTo[0], size_needed, NULL, NULL); 31 | return strTo; 32 | } 33 | 34 | vector 35 | serial::list_ports() { 36 | vector devices_found; 37 | 38 | HDEVINFO device_info_set = SetupDiGetClassDevs( 39 | (const GUID *) &GUID_DEVCLASS_PORTS, 40 | NULL, 41 | NULL, 42 | DIGCF_PRESENT); 43 | 44 | unsigned int device_info_set_index = 0; 45 | SP_DEVINFO_DATA device_info_data; 46 | 47 | device_info_data.cbSize = sizeof(SP_DEVINFO_DATA); 48 | 49 | while (SetupDiEnumDeviceInfo(device_info_set, device_info_set_index, &device_info_data)) { 50 | device_info_set_index++; 51 | 52 | // Get port name 53 | 54 | HKEY hkey = SetupDiOpenDevRegKey( 55 | device_info_set, 56 | &device_info_data, 57 | DICS_FLAG_GLOBAL, 58 | 0, 59 | DIREG_DEV, 60 | KEY_READ); 61 | 62 | TCHAR port_name[port_name_max_length]; 63 | DWORD port_name_length = port_name_max_length; 64 | 65 | LONG return_code = RegQueryValueEx( 66 | hkey, 67 | _T("PortName"), 68 | NULL, 69 | NULL, 70 | (LPBYTE)port_name, 71 | &port_name_length); 72 | 73 | RegCloseKey(hkey); 74 | 75 | if (return_code != EXIT_SUCCESS) { 76 | continue; 77 | } 78 | 79 | if (port_name_length > 0 && port_name_length <= port_name_max_length) { 80 | port_name[port_name_length - 1] = '\0'; 81 | } else { 82 | port_name[0] = '\0'; 83 | } 84 | 85 | // Ignore parallel ports 86 | 87 | if (_tcsstr(port_name, _T("LPT")) != NULL) { 88 | continue; 89 | } 90 | 91 | // Get port friendly name 92 | 93 | TCHAR friendly_name[friendly_name_max_length]; 94 | DWORD friendly_name_actual_length = 0; 95 | 96 | BOOL got_friendly_name = SetupDiGetDeviceRegistryProperty( 97 | device_info_set, 98 | &device_info_data, 99 | SPDRP_FRIENDLYNAME, 100 | NULL, 101 | (PBYTE)friendly_name, 102 | friendly_name_max_length, 103 | &friendly_name_actual_length); 104 | 105 | if (got_friendly_name == TRUE && friendly_name_actual_length > 0) { 106 | friendly_name[friendly_name_actual_length - 1] = '\0'; 107 | } else { 108 | friendly_name[0] = '\0'; 109 | } 110 | 111 | // Get hardware ID 112 | 113 | TCHAR hardware_id[hardware_id_max_length]; 114 | DWORD hardware_id_actual_length = 0; 115 | 116 | BOOL got_hardware_id = SetupDiGetDeviceRegistryProperty( 117 | device_info_set, 118 | &device_info_data, 119 | SPDRP_HARDWAREID, 120 | NULL, 121 | (PBYTE)hardware_id, 122 | hardware_id_max_length, 123 | &hardware_id_actual_length); 124 | 125 | if (got_hardware_id == TRUE && hardware_id_actual_length > 0) { 126 | hardware_id[hardware_id_actual_length - 1] = '\0'; 127 | } else { 128 | hardware_id[0] = '\0'; 129 | } 130 | 131 | #ifdef UNICODE 132 | std::string portName = utf8_encode(port_name); 133 | std::string friendlyName = utf8_encode(friendly_name); 134 | std::string hardwareId = utf8_encode(hardware_id); 135 | #else 136 | std::string portName = port_name; 137 | std::string friendlyName = friendly_name; 138 | std::string hardwareId = hardware_id; 139 | #endif 140 | 141 | PortInfo port_entry; 142 | port_entry.port = portName; 143 | port_entry.description = friendlyName; 144 | port_entry.hardware_id = hardwareId; 145 | 146 | devices_found.push_back(port_entry); 147 | } 148 | 149 | SetupDiDestroyDeviceInfoList(device_info_set); 150 | 151 | return devices_found; 152 | } 153 | 154 | #endif // #if defined(_WIN32) 155 | -------------------------------------------------------------------------------- /ydlidar_ros/sdk/src/impl/unix/unix.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | // libc dep 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | // libc++ dep 13 | #include 14 | #include 15 | 16 | // linux specific 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | -------------------------------------------------------------------------------- /ydlidar_ros/sdk/src/impl/unix/unix_serial.h: -------------------------------------------------------------------------------- 1 | #if !defined(_WIN32) 2 | 3 | #ifndef SERIAL_IMPL_UNIX_H 4 | #define SERIAL_IMPL_UNIX_H 5 | 6 | #include 7 | #include 8 | #include 9 | #include "serial.h" 10 | 11 | namespace serial { 12 | 13 | using std::size_t; 14 | using std::string; 15 | 16 | 17 | class MillisecondTimer { 18 | public: 19 | explicit MillisecondTimer(const uint32_t millis); 20 | int64_t remaining(); 21 | 22 | private: 23 | static timespec timespec_now(); 24 | timespec expiry; 25 | }; 26 | 27 | class Serial::SerialImpl { 28 | public: 29 | explicit SerialImpl(const string &port, 30 | unsigned long baudrate, 31 | bytesize_t bytesize, 32 | parity_t parity, 33 | stopbits_t stopbits, 34 | flowcontrol_t flowcontrol); 35 | 36 | virtual ~SerialImpl(); 37 | 38 | bool open(); 39 | 40 | void close(); 41 | 42 | bool isOpen() const; 43 | 44 | size_t available(); 45 | 46 | bool waitReadable(uint32_t timeout); 47 | 48 | void waitByteTimes(size_t count); 49 | 50 | int waitfordata(size_t data_count, uint32_t timeout, size_t *returned_size); 51 | 52 | size_t read(uint8_t *buf, size_t size = 1); 53 | 54 | size_t write(const uint8_t *data, size_t length); 55 | 56 | 57 | void flush(); 58 | 59 | void flushInput(); 60 | 61 | void flushOutput(); 62 | 63 | void sendBreak(int duration); 64 | 65 | bool setBreak(bool level); 66 | 67 | bool setRTS(bool level); 68 | 69 | bool setDTR(bool level); 70 | 71 | bool waitForChange(); 72 | 73 | bool getCTS(); 74 | 75 | bool getDSR(); 76 | 77 | bool getRI(); 78 | 79 | bool getCD(); 80 | 81 | uint32_t getByteTime(); 82 | 83 | void setPort(const string &port); 84 | 85 | string getPort() const; 86 | 87 | void setTimeout(Timeout &timeout); 88 | 89 | Timeout getTimeout() const; 90 | 91 | bool setBaudrate(unsigned long baudrate); 92 | 93 | bool setStandardBaudRate(speed_t baudrate); 94 | 95 | bool setCustomBaudRate(unsigned long baudrate); 96 | 97 | unsigned long getBaudrate() const; 98 | 99 | bool setBytesize(bytesize_t bytesize); 100 | 101 | bytesize_t getBytesize() const; 102 | 103 | bool setParity(parity_t parity); 104 | 105 | parity_t getParity() const; 106 | 107 | bool setStopbits(stopbits_t stopbits); 108 | 109 | stopbits_t getStopbits() const; 110 | 111 | bool setFlowcontrol(flowcontrol_t flowcontrol); 112 | 113 | flowcontrol_t getFlowcontrol() const; 114 | 115 | bool setTermios(const termios *tio); 116 | 117 | bool getTermios(termios *tio); 118 | 119 | int readLock(); 120 | 121 | int readUnlock(); 122 | 123 | int writeLock(); 124 | 125 | int writeUnlock(); 126 | 127 | 128 | private: 129 | string port_; // Path to the file descriptor 130 | int fd_; // The current file descriptor 131 | pid_t pid; 132 | 133 | bool is_open_; 134 | bool xonxoff_; 135 | bool rtscts_; 136 | 137 | Timeout timeout_; // Timeout for read operations 138 | unsigned long baudrate_; // Baudrate 139 | uint32_t byte_time_ns_; // Nanoseconds to transmit/receive a single byte 140 | 141 | parity_t parity_; // Parity 142 | bytesize_t bytesize_; // Size of the bytes 143 | stopbits_t stopbits_; // Stop Bits 144 | flowcontrol_t flowcontrol_; // Flow Control 145 | 146 | // Mutex used to lock the read functions 147 | pthread_mutex_t read_mutex; 148 | // Mutex used to lock the write functions 149 | pthread_mutex_t write_mutex; 150 | }; 151 | 152 | } 153 | 154 | #endif // SERIAL_IMPL_UNIX_H 155 | 156 | #endif // !defined(_WIN32) 157 | -------------------------------------------------------------------------------- /ydlidar_ros/sdk/src/impl/unix/unix_timer.cpp: -------------------------------------------------------------------------------- 1 | #if !defined(_WIN32) 2 | #include "timer.h" 3 | 4 | namespace impl { 5 | uint32_t getHDTimer() { 6 | struct timespec t; 7 | t.tv_sec = t.tv_nsec = 0; 8 | clock_gettime(CLOCK_MONOTONIC, &t); 9 | return t.tv_sec * 1000L + t.tv_nsec / 1000000L; 10 | } 11 | uint64_t getCurrentTime() { 12 | #if HAS_CLOCK_GETTIME 13 | struct timespec tim; 14 | clock_gettime(CLOCK_REALTIME, &tim); 15 | return static_cast(tim.tv_sec) * 1000000000LL + tim.tv_nsec; 16 | #else 17 | struct timeval timeofday; 18 | gettimeofday(&timeofday, NULL); 19 | return static_cast(timeofday.tv_sec) * 1000000000LL + 20 | static_cast(timeofday.tv_usec) * 1000LL; 21 | #endif 22 | } 23 | } 24 | #endif 25 | -------------------------------------------------------------------------------- /ydlidar_ros/sdk/src/impl/windows/list_ports_win.cpp: -------------------------------------------------------------------------------- 1 | #if defined(_WIN32) 2 | 3 | /* 4 | * Copyright (c) 2014 Craig Lilley 5 | * This software is made available under the terms of the MIT licence. 6 | * A copy of the licence can be obtained from: 7 | * http://opensource.org/licenses/MIT 8 | */ 9 | #pragma comment(lib, "setupapi.lib") 10 | #undef UNICODE 11 | #include "serial.h" 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | using serial::PortInfo; 20 | using std::vector; 21 | using std::string; 22 | 23 | static const DWORD port_name_max_length = 256; 24 | static const DWORD friendly_name_max_length = 256; 25 | static const DWORD hardware_id_max_length = 256; 26 | static const DWORD device_id_max_length = 256; 27 | 28 | 29 | // Convert a wide Unicode string to an UTF8 string 30 | std::string utf8_encode(const std::wstring &wstr) { 31 | int size_needed = WideCharToMultiByte(CP_UTF8, 0, &wstr[0], (int)wstr.size(), NULL, 0, NULL, NULL); 32 | std::string strTo(size_needed, 0); 33 | WideCharToMultiByte(CP_UTF8, 0, &wstr[0], (int)wstr.size(), &strTo[0], size_needed, NULL, NULL); 34 | return strTo; 35 | } 36 | 37 | vector 38 | serial::list_ports() { 39 | vector devices_found; 40 | 41 | HDEVINFO device_info_set = SetupDiGetClassDevs( 42 | (const GUID *) &GUID_DEVCLASS_PORTS, 43 | NULL, 44 | NULL, 45 | DIGCF_PRESENT); 46 | 47 | unsigned int device_info_set_index = 0; 48 | SP_DEVINFO_DATA device_info_data; 49 | 50 | device_info_data.cbSize = sizeof(SP_DEVINFO_DATA); 51 | 52 | while (SetupDiEnumDeviceInfo(device_info_set, device_info_set_index, &device_info_data)) { 53 | device_info_set_index++; 54 | 55 | // Get port name 56 | 57 | HKEY hkey = SetupDiOpenDevRegKey( 58 | device_info_set, 59 | &device_info_data, 60 | DICS_FLAG_GLOBAL, 61 | 0, 62 | DIREG_DEV, 63 | KEY_READ); 64 | 65 | TCHAR port_name[port_name_max_length]; 66 | DWORD port_name_length = port_name_max_length; 67 | 68 | LONG return_code = RegQueryValueEx( 69 | hkey, 70 | _T("PortName"), 71 | NULL, 72 | NULL, 73 | (LPBYTE)port_name, 74 | &port_name_length); 75 | 76 | RegCloseKey(hkey); 77 | 78 | if (return_code != EXIT_SUCCESS) { 79 | continue; 80 | } 81 | 82 | if (port_name_length > 0 && port_name_length <= port_name_max_length) { 83 | port_name[port_name_length - 1] = '\0'; 84 | } else { 85 | port_name[0] = '\0'; 86 | } 87 | 88 | // Ignore parallel ports 89 | 90 | if (_tcsstr(port_name, _T("LPT")) != NULL) { 91 | continue; 92 | } 93 | 94 | // Get port friendly name 95 | 96 | TCHAR friendly_name[friendly_name_max_length]; 97 | DWORD friendly_name_actual_length = 0; 98 | 99 | BOOL got_friendly_name = SetupDiGetDeviceRegistryProperty( 100 | device_info_set, 101 | &device_info_data, 102 | SPDRP_FRIENDLYNAME, 103 | NULL, 104 | (PBYTE)friendly_name, 105 | friendly_name_max_length, 106 | &friendly_name_actual_length); 107 | 108 | if (got_friendly_name == TRUE && friendly_name_actual_length > 0) { 109 | friendly_name[friendly_name_actual_length - 1] = '\0'; 110 | } else { 111 | friendly_name[0] = '\0'; 112 | } 113 | 114 | // Get hardware ID 115 | 116 | TCHAR hardware_id[hardware_id_max_length]; 117 | DWORD hardware_id_actual_length = 0; 118 | 119 | BOOL got_hardware_id = SetupDiGetDeviceRegistryProperty( 120 | device_info_set, 121 | &device_info_data, 122 | SPDRP_HARDWAREID, 123 | NULL, 124 | (PBYTE)hardware_id, 125 | hardware_id_max_length, 126 | &hardware_id_actual_length); 127 | 128 | if (got_hardware_id == TRUE && hardware_id_actual_length > 0) { 129 | hardware_id[hardware_id_actual_length - 1] = '\0'; 130 | } else { 131 | hardware_id[0] = '\0'; 132 | } 133 | 134 | TCHAR device_id[device_id_max_length]; 135 | DWORD device_id_actual_length = 0; 136 | 137 | BOOL got_device_id = SetupDiGetDeviceRegistryProperty( 138 | device_info_set, 139 | &device_info_data, 140 | SPDRP_LOCATION_INFORMATION, 141 | NULL, 142 | (PBYTE)device_id, 143 | device_id_max_length, 144 | &device_id_actual_length); 145 | 146 | if (got_device_id == TRUE && device_id_actual_length > 0) { 147 | device_id[device_id_actual_length - 1] = '\0'; 148 | } else { 149 | device_id[0] = '\0'; 150 | } 151 | 152 | 153 | 154 | #ifdef UNICODE 155 | std::string portName = utf8_encode(port_name); 156 | std::string friendlyName = utf8_encode(friendly_name); 157 | std::string hardwareId = utf8_encode(hardware_id); 158 | std::string deviceId = utf8_encode(device_id); 159 | 160 | #else 161 | std::string portName = port_name; 162 | std::string friendlyName = friendly_name; 163 | std::string hardwareId = hardware_id; 164 | std::string deviceId = device_id; 165 | #endif 166 | size_t pos = deviceId.find("#"); 167 | 168 | if (pos != std::string::npos) { 169 | deviceId = deviceId.substr(pos + 1, 4); 170 | deviceId = std::to_string(atoi(deviceId.c_str())); 171 | } 172 | 173 | if (hardwareId.find("VID_10C4&PID_EA60") != std::string::npos || 174 | hardwareId.find("VID_0483&PID_5740") != std::string::npos) { 175 | PortInfo port_entry; 176 | port_entry.port = portName; 177 | port_entry.description = friendlyName; 178 | port_entry.hardware_id = hardwareId; 179 | port_entry.device_id = deviceId; 180 | devices_found.push_back(port_entry); 181 | 182 | } 183 | 184 | 185 | } 186 | 187 | SetupDiDestroyDeviceInfoList(device_info_set); 188 | 189 | return devices_found; 190 | } 191 | 192 | #endif // #if defined(_WIN32) 193 | -------------------------------------------------------------------------------- /ydlidar_ros/sdk/src/impl/windows/win.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | 11 | -------------------------------------------------------------------------------- /ydlidar_ros/sdk/src/impl/windows/win_serial.h: -------------------------------------------------------------------------------- 1 | #if defined(_WIN32) 2 | 3 | #ifndef SERIAL_IMPL_WINDOWS_H 4 | #define SERIAL_IMPL_WINDOWS_H 5 | 6 | #include "serial.h" 7 | #include "time.h" 8 | #include 9 | 10 | #ifndef UNICODE 11 | #define UNICODE 12 | #define UNICODE_WAS_UNDEFINED 13 | #endif 14 | #include "windows.h" 15 | 16 | #ifndef UNICODE_WAS_UNDEFINED 17 | #undef UNICODE 18 | #endif 19 | 20 | namespace serial { 21 | 22 | using std::string; 23 | using std::wstring; 24 | using std::invalid_argument; 25 | 26 | 27 | class Serial::SerialImpl { 28 | public: 29 | explicit SerialImpl(const string &port, 30 | unsigned long baudrate, 31 | bytesize_t bytesize, 32 | parity_t parity, 33 | stopbits_t stopbits, 34 | flowcontrol_t flowcontrol); 35 | 36 | virtual ~SerialImpl(); 37 | 38 | bool open(); 39 | 40 | void close(); 41 | 42 | bool isOpen() const; 43 | 44 | size_t available(); 45 | 46 | bool waitReadable(uint32_t timeout); 47 | 48 | void waitByteTimes(size_t count); 49 | 50 | int waitfordata(size_t data_count, uint32_t timeout, size_t *returned_size); 51 | 52 | size_t read(uint8_t *buf, size_t size = 1); 53 | 54 | size_t write(const uint8_t *data, size_t length); 55 | 56 | void flush(); 57 | 58 | void flushInput(); 59 | 60 | void flushOutput(); 61 | 62 | void sendBreak(int duration); 63 | 64 | bool setBreak(bool level); 65 | 66 | bool setRTS(bool level); 67 | 68 | bool setDTR(bool level); 69 | 70 | bool waitForChange(); 71 | 72 | bool getCTS(); 73 | 74 | bool getDSR(); 75 | 76 | bool getRI(); 77 | 78 | bool getCD(); 79 | 80 | uint32_t getByteTime(); 81 | 82 | void setPort(const string &port); 83 | 84 | string getPort() const; 85 | 86 | void setTimeout(Timeout &timeout); 87 | 88 | Timeout getTimeout() const; 89 | 90 | bool setBaudrate(unsigned long baudrate); 91 | 92 | unsigned long getBaudrate() const; 93 | 94 | bool setBytesize(bytesize_t bytesize); 95 | 96 | bytesize_t getBytesize() const; 97 | 98 | bool setParity(parity_t parity); 99 | 100 | parity_t getParity() const; 101 | 102 | bool setStopbits(stopbits_t stopbits); 103 | 104 | stopbits_t getStopbits() const; 105 | 106 | bool setFlowcontrol(flowcontrol_t flowcontrol); 107 | 108 | flowcontrol_t getFlowcontrol() const; 109 | 110 | 111 | bool setDcb(DCB *dcb); 112 | 113 | 114 | bool getDcb(DCB *dcb); 115 | 116 | int readLock(); 117 | 118 | int readUnlock(); 119 | 120 | int writeLock(); 121 | 122 | int writeUnlock(); 123 | 124 | protected: 125 | bool reconfigurePort(); 126 | 127 | public: 128 | enum { 129 | DEFAULT_RX_BUFFER_SIZE = 2048, 130 | DEFAULT_TX_BUFFER_SIZE = 128, 131 | }; 132 | 133 | 134 | private: 135 | wstring port_; // Path to the file descriptor 136 | HANDLE fd_; 137 | OVERLAPPED _wait_o; 138 | 139 | OVERLAPPED communicationOverlapped; 140 | OVERLAPPED readCompletionOverlapped; 141 | OVERLAPPED writeCompletionOverlapped; 142 | DWORD originalEventMask; 143 | DWORD triggeredEventMask; 144 | 145 | COMMTIMEOUTS currentCommTimeouts; 146 | COMMTIMEOUTS restoredCommTimeouts; 147 | 148 | bool is_open_; 149 | 150 | Timeout timeout_; // Timeout for read operations 151 | unsigned long baudrate_; // Baudrate 152 | uint32_t byte_time_ns_; // Nanoseconds to transmit/receive a single byte 153 | 154 | parity_t parity_; // Parity 155 | bytesize_t bytesize_; // Size of the bytes 156 | stopbits_t stopbits_; // Stop Bits 157 | flowcontrol_t flowcontrol_; // Flow Control 158 | 159 | // Mutex used to lock the read functions 160 | HANDLE read_mutex; 161 | // Mutex used to lock the write functions 162 | HANDLE write_mutex; 163 | }; 164 | 165 | } 166 | 167 | #endif // SERIAL_IMPL_WINDOWS_H 168 | 169 | #endif // if defined(_WIN32) 170 | -------------------------------------------------------------------------------- /ydlidar_ros/sdk/src/impl/windows/win_timer.cpp: -------------------------------------------------------------------------------- 1 | #if defined(_WIN32) 2 | #include "timer.h" 3 | #include 4 | #pragma comment(lib, "Winmm.lib") 5 | 6 | namespace impl { 7 | 8 | static LARGE_INTEGER _current_freq; 9 | 10 | void HPtimer_reset() { 11 | BOOL ans = QueryPerformanceFrequency(&_current_freq); 12 | _current_freq.QuadPart /= 1000; 13 | } 14 | 15 | uint32_t getHDTimer() { 16 | LARGE_INTEGER current; 17 | QueryPerformanceCounter(¤t); 18 | 19 | return (uint32_t)(current.QuadPart / (_current_freq.QuadPart)); 20 | } 21 | 22 | uint64_t getCurrentTime() { 23 | FILETIME t; 24 | GetSystemTimeAsFileTime(&t); 25 | return ((((uint64_t)t.dwHighDateTime) << 32) | ((uint64_t)t.dwLowDateTime)) * 100; 26 | } 27 | 28 | 29 | BEGIN_STATIC_CODE(timer_cailb) { 30 | HPtimer_reset(); 31 | } END_STATIC_CODE(timer_cailb) 32 | 33 | } 34 | #endif 35 | -------------------------------------------------------------------------------- /ydlidar_ros/src/.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "python.autoComplete.extraPaths": [ 3 | "/apollo/cyber/py_wrapper", 4 | "/apollo/cyber/python", 5 | "/opt/ros/kinetic/lib/python2.7/dist-packages" 6 | ] 7 | } -------------------------------------------------------------------------------- /ydlidar_ros/src/ydlidar_client.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * YDLIDAR SYSTEM 3 | * YDLIDAR ROS Node Client 4 | * 5 | * Copyright 2015 - 2018 EAI TEAM 6 | * http://www.ydlidar.com 7 | * 8 | */ 9 | 10 | #include "ros/ros.h" 11 | #include "sensor_msgs/LaserScan.h" 12 | 13 | #define RAD2DEG(x) ((x)*180./M_PI) 14 | 15 | void scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan) 16 | { 17 | int count = scan->scan_time / scan->time_increment; 18 | printf("[YDLIDAR INFO]: I heard a laser scan %s[%d]:\n", scan->header.frame_id.c_str(), count); 19 | printf("[YDLIDAR INFO]: angle_range : [%f, %f]\n", RAD2DEG(scan->angle_min), RAD2DEG(scan->angle_max)); 20 | 21 | for(int i = 0; i < count; i++) { 22 | float degree = RAD2DEG(scan->angle_min + scan->angle_increment * i); 23 | if(degree > -5 && degree< 5) 24 | printf("[YDLIDAR INFO]: angle-distance : [%f, %f, %i]\n", degree, scan->ranges[i], i); 25 | } 26 | } 27 | 28 | int main(int argc, char **argv) 29 | { 30 | ros::init(argc, argv, "ydlidar_client"); 31 | ros::NodeHandle n; 32 | ros::Subscriber sub = n.subscribe("/scan", 1000, scanCallback); 33 | ros::spin(); 34 | 35 | return 0; 36 | } 37 | -------------------------------------------------------------------------------- /ydlidar_ros/src/ydlidar_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * YDLIDAR SYSTEM 3 | * YDLIDAR ROS Node Client 4 | * 5 | * Copyright 2015 - 2018 EAI TEAM 6 | * http://www.ydlidar.com 7 | * 8 | */ 9 | 10 | #include "ros/ros.h" 11 | #include "sensor_msgs/LaserScan.h" 12 | #include "CYdLidar.h" 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | using namespace ydlidar; 19 | 20 | #define ROSVerision "1.4.6" 21 | 22 | 23 | std::vector split(const std::string &s, char delim) { 24 | std::vector elems; 25 | std::stringstream ss(s); 26 | std::string number; 27 | while(std::getline(ss, number, delim)) { 28 | elems.push_back(atof(number.c_str())); 29 | } 30 | return elems; 31 | } 32 | 33 | 34 | int main(int argc, char * argv[]) { 35 | ros::init(argc, argv, "ydlidar_node"); 36 | printf("__ ______ _ ___ ____ _ ____ \n"); 37 | printf("\\ \\ / / _ \\| | |_ _| _ \\ / \\ | _ \\ \n"); 38 | printf(" \\ V /| | | | | | || | | |/ _ \\ | |_) | \n"); 39 | printf(" | | | |_| | |___ | || |_| / ___ \\| _ < \n"); 40 | printf(" |_| |____/|_____|___|____/_/ \\_\\_| \\_\\ \n"); 41 | printf("\n"); 42 | fflush(stdout); 43 | 44 | std::string port; 45 | int baudrate=230400; 46 | std::string frame_id; 47 | bool reversion, resolution_fixed; 48 | bool auto_reconnect; 49 | double angle_max,angle_min; 50 | result_t op_result; 51 | std::string list; 52 | std::vector ignore_array; 53 | double max_range, min_range; 54 | double frequency; 55 | int samp_rate = 5; 56 | bool inverted = true; 57 | bool isSingleChannel = false; 58 | bool isTOFLidar = false; 59 | 60 | ros::NodeHandle nh; 61 | ros::Publisher scan_pub = nh.advertise("scan", 1000); 62 | ros::NodeHandle nh_private("~"); 63 | nh_private.param("port", port, "/dev/ydlidar"); 64 | nh_private.param("baudrate", baudrate, 230400); 65 | nh_private.param("frame_id", frame_id, "laser_frame"); 66 | nh_private.param("resolution_fixed", resolution_fixed, "true"); 67 | nh_private.param("auto_reconnect", auto_reconnect, "true"); 68 | nh_private.param("reversion", reversion, "true"); 69 | nh_private.param("angle_max", angle_max , 180); 70 | nh_private.param("angle_min", angle_min , -180); 71 | nh_private.param("range_max", max_range , 64.0); 72 | nh_private.param("range_min", min_range , 0.01); 73 | nh_private.param("frequency", frequency , 10.0); 74 | nh_private.param("ignore_array",list,""); 75 | nh_private.param("samp_rate", samp_rate, samp_rate); 76 | nh_private.param("isSingleChannel", isSingleChannel, isSingleChannel); 77 | nh_private.param("isTOFLidar", isTOFLidar, isTOFLidar); 78 | 79 | 80 | ignore_array = split(list ,','); 81 | if(ignore_array.size()%2){ 82 | ROS_ERROR_STREAM("ignore array is odd need be even"); 83 | } 84 | 85 | for(uint16_t i =0 ; i < ignore_array.size();i++){ 86 | if(ignore_array[i] < -180 && ignore_array[i] > 180){ 87 | ROS_ERROR_STREAM("ignore array should be between 0 and 360"); 88 | } 89 | } 90 | 91 | CYdLidar laser; 92 | if(frequency<3){ 93 | frequency = 7.0; 94 | } 95 | if(frequency>15.7){ 96 | frequency = 15.7; 97 | } 98 | if(angle_max < angle_min){ 99 | double temp = angle_max; 100 | angle_max = angle_min; 101 | angle_min = temp; 102 | } 103 | 104 | ROS_INFO("[YDLIDAR INFO] Now YDLIDAR ROS SDK VERSION:%s .......", ROSVerision); 105 | laser.setSerialPort(port); 106 | laser.setSerialBaudrate(baudrate); 107 | laser.setMaxRange(max_range); 108 | laser.setMinRange(min_range); 109 | laser.setMaxAngle(angle_max); 110 | laser.setMinAngle(angle_min); 111 | laser.setReversion(reversion); 112 | laser.setFixedResolution(resolution_fixed); 113 | laser.setAutoReconnect(auto_reconnect); 114 | laser.setScanFrequency(frequency); 115 | laser.setIgnoreArray(ignore_array); 116 | laser.setSampleRate(samp_rate); 117 | laser.setInverted(inverted); 118 | laser.setSingleChannel(isSingleChannel); 119 | laser.setLidarType(isTOFLidar ? TYPE_TOF : TYPE_TRIANGLE); 120 | bool ret = laser.initialize(); 121 | if (ret) { 122 | ret = laser.turnOn(); 123 | if (!ret) { 124 | ROS_ERROR("Failed to start scan mode!!!"); 125 | } 126 | } else { 127 | ROS_ERROR("Error initializing YDLIDAR Comms and Status!!!"); 128 | } 129 | ros::Rate rate(20); 130 | 131 | while (ret&&ros::ok()) { 132 | bool hardError; 133 | LaserScan scan; 134 | if(laser.doProcessSimple(scan, hardError )){ 135 | sensor_msgs::LaserScan scan_msg; 136 | ros::Time start_scan_time; 137 | start_scan_time.sec = scan.stamp/1000000000ul; 138 | start_scan_time.nsec = scan.stamp%1000000000ul; 139 | scan_msg.header.stamp = start_scan_time; 140 | scan_msg.header.frame_id = frame_id; 141 | scan_msg.angle_min =(scan.config.min_angle); 142 | scan_msg.angle_max = (scan.config.max_angle); 143 | scan_msg.angle_increment = (scan.config.angle_increment); 144 | scan_msg.scan_time = scan.config.scan_time; 145 | scan_msg.time_increment = scan.config.time_increment; 146 | scan_msg.range_min = (scan.config.min_range); 147 | scan_msg.range_max = (scan.config.max_range); 148 | int size = (scan.config.max_angle - scan.config.min_angle)/ scan.config.angle_increment + 1; 149 | scan_msg.ranges.resize(size); 150 | scan_msg.intensities.resize(size); 151 | for(int i=0; i < scan.points.size(); i++) { 152 | int index = std::ceil((scan.points[i].angle - scan.config.min_angle)/scan.config.angle_increment); 153 | if(index >=0 && index < size) { 154 | scan_msg.ranges[index] = scan.points[i].range; 155 | scan_msg.intensities[index] = scan.points[i].intensity; 156 | } 157 | } 158 | scan_pub.publish(scan_msg); 159 | } 160 | rate.sleep(); 161 | ros::spinOnce(); 162 | } 163 | 164 | laser.turnOff(); 165 | ROS_INFO("[YDLIDAR INFO] Now YDLIDAR is stopping ......."); 166 | laser.disconnecting(); 167 | return 0; 168 | } 169 | -------------------------------------------------------------------------------- /ydlidar_ros/startup/initenv.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | echo 'KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", MODE:="0666", GROUP:="dialout", SYMLINK+="ydlidar"' >/etc/udev/rules.d/ydlidar.rules 3 | 4 | echo 'KERNEL=="ttyACM*", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="5740", MODE:="0666", GROUP:="dialout", SYMLINK+="ydlidar"' >/etc/udev/rules.d/ydlidar-V2.rules 5 | 6 | echo 'KERNEL=="ttyUSB*", ATTRS{idVendor}=="067b", ATTRS{idProduct}=="2303", MODE:="0666", GROUP:="dialout", SYMLINK+="ydlidar"' >/etc/udev/rules.d/ydlidar-2303.rules 7 | 8 | service udev reload 9 | sleep 2 10 | service udev restart 11 | 12 | -------------------------------------------------------------------------------- /ydlidar_ros/urdf/ydlidar.urdf: -------------------------------------------------------------------------------- 1 | 3 | 5 | 6 | 9 | 11 | 18 | 19 | 20 | 23 | 24 | 26 | 27 | 29 | 31 | 32 | 33 | 34 | 37 | 38 | 40 | 41 | 42 | 43 | 45 | 46 | 49 | 51 | 58 | 59 | 73 | 82 | 83 | 86 | 89 | 91 | 93 | 95 | 96 | 97 | -------------------------------------------------------------------------------- /ydlidar_ros/ydlidar.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /RobotModel1 10 | - /RobotModel1/Links1 11 | Splitter Ratio: 0.5 12 | Tree Height: 781 13 | - Class: rviz/Selection 14 | Name: Selection 15 | - Class: rviz/Tool Properties 16 | Expanded: 17 | - /2D Pose Estimate1 18 | - /2D Nav Goal1 19 | - /Publish Point1 20 | Name: Tool Properties 21 | Splitter Ratio: 0.5886790156364441 22 | - Class: rviz/Views 23 | Expanded: 24 | - /Current View1 25 | Name: Views 26 | Splitter Ratio: 0.5 27 | - Class: rviz/Time 28 | Experimental: false 29 | Name: Time 30 | SyncMode: 0 31 | SyncSource: LaserScan 32 | Visualization Manager: 33 | Class: "" 34 | Displays: 35 | - Alpha: 0.5 36 | Cell Size: 1 37 | Class: rviz/Grid 38 | Color: 160; 160; 164 39 | Enabled: true 40 | Line Style: 41 | Line Width: 0.029999999329447746 42 | Value: Lines 43 | Name: Grid 44 | Normal Cell Count: 0 45 | Offset: 46 | X: 0 47 | Y: 0 48 | Z: 0 49 | Plane: XY 50 | Plane Cell Count: 10 51 | Reference Frame: 52 | Value: true 53 | - Alpha: 1 54 | Class: rviz/RobotModel 55 | Collision Enabled: false 56 | Enabled: true 57 | Links: 58 | All Links Enabled: true 59 | Expand Joint Details: false 60 | Expand Link Details: false 61 | Expand Tree: false 62 | Link Tree Style: Links in Alphabetic Order 63 | base_laser: 64 | Alpha: 1 65 | Show Axes: false 66 | Show Trail: false 67 | Value: true 68 | laser_frame: 69 | Alpha: 1 70 | Show Axes: true 71 | Show Trail: false 72 | Name: RobotModel 73 | Robot Description: robot_description 74 | TF Prefix: "" 75 | Update Interval: 0 76 | Value: true 77 | Visual Enabled: true 78 | - Alpha: 1 79 | Autocompute Intensity Bounds: true 80 | Autocompute Value Bounds: 81 | Max Value: 10 82 | Min Value: -10 83 | Value: true 84 | Axis: Z 85 | Channel Name: intensity 86 | Class: rviz/LaserScan 87 | Color: 239; 41; 41 88 | Color Transformer: FlatColor 89 | Decay Time: 0 90 | Enabled: true 91 | Invert Rainbow: false 92 | Max Color: 255; 255; 255 93 | Max Intensity: 10 94 | Min Color: 0; 0; 0 95 | Min Intensity: 10 96 | Name: LaserScan 97 | Position Transformer: XYZ 98 | Queue Size: 10 99 | Selectable: true 100 | Size (Pixels): 3 101 | Size (m): 0.03999999910593033 102 | Style: Flat Squares 103 | Topic: /scan 104 | Unreliable: false 105 | Use Fixed Frame: true 106 | Use rainbow: true 107 | Value: true 108 | Enabled: true 109 | Global Options: 110 | Background Color: 48; 48; 48 111 | Default Light: true 112 | Fixed Frame: base_laser 113 | Frame Rate: 30 114 | Name: root 115 | Tools: 116 | - Class: rviz/Interact 117 | Hide Inactive Objects: true 118 | - Class: rviz/MoveCamera 119 | - Class: rviz/Select 120 | - Class: rviz/FocusCamera 121 | - Class: rviz/Measure 122 | - Class: rviz/SetInitialPose 123 | Topic: /initialpose 124 | - Class: rviz/SetGoal 125 | Topic: /move_base_simple/goal 126 | - Class: rviz/PublishPoint 127 | Single click: true 128 | Topic: /clicked_point 129 | Value: true 130 | Views: 131 | Current: 132 | Class: rviz/Orbit 133 | Distance: 0.2649034559726715 134 | Enable Stereo Rendering: 135 | Stereo Eye Separation: 0.05999999865889549 136 | Stereo Focal Distance: 1 137 | Swap Stereo Eyes: false 138 | Value: false 139 | Focal Point: 140 | X: 0 141 | Y: 0 142 | Z: 0 143 | Focal Shape Fixed Size: true 144 | Focal Shape Size: 0.05000000074505806 145 | Invert Z Axis: false 146 | Name: Current View 147 | Near Clip Distance: 0.009999999776482582 148 | Pitch: 0.35479697585105896 149 | Target Frame: 150 | Value: Orbit (rviz) 151 | Yaw: 1.2972157001495361 152 | Saved: ~ 153 | Window Geometry: 154 | Displays: 155 | collapsed: false 156 | Height: 1056 157 | Hide Left Dock: false 158 | Hide Right Dock: false 159 | QMainWindow State: 000000ff00000000fd00000004000000000000016a00000398fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002700000398000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000398fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002700000398000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004ba0000039800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 160 | Selection: 161 | collapsed: false 162 | Time: 163 | collapsed: false 164 | Tool Properties: 165 | collapsed: false 166 | Views: 167 | collapsed: false 168 | Width: 1855 169 | X: 65 170 | Y: 24 171 | --------------------------------------------------------------------------------