├── CMakeLists.txt ├── README.md ├── Rviz ├── rosbag.rviz └── sim.rviz ├── images ├── 1.PNG ├── 1200px-Heriot-Watt_University_logo.jpg ├── 2.PNG ├── 3.PNG ├── 9e9dd76fd4f547150d948ba49b7f92b3_74108.jpeg ├── BlueROV2-4-lumen-1-300x300.png ├── Screenshot from 2020-03-02 15-56-21.png ├── hh.jpg └── pi.pi ├── launch.launch ├── launch ├── bag.launch ├── octomap.launch └── sim.launch ├── package.xml ├── py_dvl.py ├── rosbag ├── dynamic_sonar.bag └── static_sonar.bag ├── save_data.py ├── sonar_simulation.py └── src ├── 3D_map_Point_cloud.py ├── converting.py ├── dvl.py ├── filtering.py ├── frame.py ├── laserscan_to_poincloud.py ├── octomam.py ├── octomap_launch.py ├── pointcloudtolaserscan.py ├── sonar_simulation.py └── sonar_simulation_dynamic.py /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(sonar_mapping) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | rospy 12 | ) 13 | 14 | ## System dependencies are found with CMake's conventions 15 | # find_package(Boost REQUIRED COMPONENTS system) 16 | 17 | 18 | ## Uncomment this if the package has a setup.py. This macro ensures 19 | ## modules and global scripts declared therein get installed 20 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 21 | # catkin_python_setup() 22 | 23 | ################################################ 24 | ## Declare ROS messages, services and actions ## 25 | ################################################ 26 | 27 | ## To declare and build messages, services or actions from within this 28 | ## package, follow these steps: 29 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 30 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 31 | ## * In the file package.xml: 32 | ## * add a build_depend tag for "message_generation" 33 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 34 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 35 | ## but can be declared for certainty nonetheless: 36 | ## * add a exec_depend tag for "message_runtime" 37 | ## * In this file (CMakeLists.txt): 38 | ## * add "message_generation" and every package in MSG_DEP_SET to 39 | ## find_package(catkin REQUIRED COMPONENTS ...) 40 | ## * add "message_runtime" and every package in MSG_DEP_SET to 41 | ## catkin_package(CATKIN_DEPENDS ...) 42 | ## * uncomment the add_*_files sections below as needed 43 | ## and list every .msg/.srv/.action file to be processed 44 | ## * uncomment the generate_messages entry below 45 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 46 | 47 | ## Generate messages in the 'msg' folder 48 | # add_message_files( 49 | # FILES 50 | # Message1.msg 51 | # Message2.msg 52 | # ) 53 | 54 | ## Generate services in the 'srv' folder 55 | # add_service_files( 56 | # FILES 57 | # Service1.srv 58 | # Service2.srv 59 | # ) 60 | 61 | ## Generate actions in the 'action' folder 62 | # add_action_files( 63 | # FILES 64 | # Action1.action 65 | # Action2.action 66 | # ) 67 | 68 | ## Generate added messages and services with any dependencies listed here 69 | # generate_messages( 70 | # DEPENDENCIES 71 | # std_msgs # Or other packages containing msgs 72 | # ) 73 | 74 | ################################################ 75 | ## Declare ROS dynamic reconfigure parameters ## 76 | ################################################ 77 | 78 | ## To declare and build dynamic reconfigure parameters within this 79 | ## package, follow these steps: 80 | ## * In the file package.xml: 81 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 82 | ## * In this file (CMakeLists.txt): 83 | ## * add "dynamic_reconfigure" to 84 | ## find_package(catkin REQUIRED COMPONENTS ...) 85 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 86 | ## and list every .cfg file to be processed 87 | 88 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 89 | # generate_dynamic_reconfigure_options( 90 | # cfg/DynReconf1.cfg 91 | # cfg/DynReconf2.cfg 92 | # ) 93 | 94 | ################################### 95 | ## catkin specific configuration ## 96 | ################################### 97 | ## The catkin_package macro generates cmake config files for your package 98 | ## Declare things to be passed to dependent projects 99 | ## INCLUDE_DIRS: uncomment this if your package contains header files 100 | ## LIBRARIES: libraries you create in this project that dependent projects also need 101 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 102 | ## DEPENDS: system dependencies of this project that dependent projects also need 103 | catkin_package( 104 | # INCLUDE_DIRS include 105 | # LIBRARIES sonar_mapping 106 | # CATKIN_DEPENDS rospy 107 | # DEPENDS system_lib 108 | ) 109 | 110 | ########### 111 | ## Build ## 112 | ########### 113 | 114 | ## Specify additional locations of header files 115 | ## Your package locations should be listed before other locations 116 | include_directories( 117 | # include 118 | ${catkin_INCLUDE_DIRS} 119 | ) 120 | 121 | ## Declare a C++ library 122 | # add_library(${PROJECT_NAME} 123 | # src/${PROJECT_NAME}/sonar_mapping.cpp 124 | # ) 125 | 126 | ## Add cmake target dependencies of the library 127 | ## as an example, code may need to be generated before libraries 128 | ## either from message generation or dynamic reconfigure 129 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 130 | 131 | ## Declare a C++ executable 132 | ## With catkin_make all packages are built within a single CMake context 133 | ## The recommended prefix ensures that target names across packages don't collide 134 | # add_executable(${PROJECT_NAME}_node src/sonar_mapping_node.cpp) 135 | 136 | ## Rename C++ executable without prefix 137 | ## The above recommended prefix causes long target names, the following renames the 138 | ## target back to the shorter version for ease of user use 139 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 140 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 141 | 142 | ## Add cmake target dependencies of the executable 143 | ## same as for the library above 144 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 145 | 146 | ## Specify libraries to link a library or executable target against 147 | # target_link_libraries(${PROJECT_NAME}_node 148 | # ${catkin_LIBRARIES} 149 | # ) 150 | 151 | ############# 152 | ## Install ## 153 | ############# 154 | 155 | # all install targets should use catkin DESTINATION variables 156 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 157 | 158 | ## Mark executable scripts (Python etc.) for installation 159 | ## in contrast to setup.py, you can choose the destination 160 | # install(PROGRAMS 161 | # scripts/my_python_script 162 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 163 | # ) 164 | 165 | ## Mark executables for installation 166 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 167 | # install(TARGETS ${PROJECT_NAME}_node 168 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 169 | # ) 170 | 171 | ## Mark libraries for installation 172 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 173 | # install(TARGETS ${PROJECT_NAME} 174 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 175 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 176 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 177 | # ) 178 | 179 | ## Mark cpp header files for installation 180 | # install(DIRECTORY include/${PROJECT_NAME}/ 181 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 182 | # FILES_MATCHING PATTERN "*.h" 183 | # PATTERN ".svn" EXCLUDE 184 | # ) 185 | 186 | ## Mark other files for installation (e.g. launch and bag files, etc.) 187 | # install(FILES 188 | # # myfile1 189 | # # myfile2 190 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 191 | # ) 192 | 193 | ############# 194 | ## Testing ## 195 | ############# 196 | 197 | ## Add gtest based cpp test target and link libraries 198 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_sonar_mapping.cpp) 199 | # if(TARGET ${PROJECT_NAME}-test) 200 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 201 | # endif() 202 | 203 | ## Add folders to be run by python nosetests 204 | # catkin_add_nosetests(test) 205 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # BlueRov2 SLAM 2 | 3 |

4 | 5 |

6 | 7 | 8 | The aim of this project is to provide a start of a 3D SLAM for underwater ROV. This project uses UUV simulation to fufill this objectif. The model is simulated using Deskitek Saga but you can use your own model as well. We modified the sonar to be se same as the [Micron Sonar](https://www.tritech.co.uk/product/small-rov-mechanical-sector-scanning-sonar-tritech-micron) from Tritech. 9 | 10 |

11 | 12 |

13 | 14 | 15 | * Mapping using UUV simulation - static: 16 | 17 |

18 | 20 |

21 | 22 | * Mapping using UUV simulation - dynamic: 23 | 24 |

25 | 27 |

28 | 29 | * Mapping using Micron Sonar in a water tank: 30 |

31 | 33 |

34 | 35 | 36 | 37 | 38 | ## Related Packages 39 | 40 | * UUV-simulator: 41 | 42 | https://github.com/uuvsimulator/uuv_simulator 43 | 44 | * Octomap_server : 45 | 46 | https://github.com/OctoMap/octomap_mapping.git 47 | 48 | * Desistek SAGA ROV vehicle: 49 | 50 | https://github.com/uuvsimulator/desistek_saga.git 51 | 52 | * Point cloud Converter: 53 | 54 | https://github.com/pal-robotics-forks/point_cloud_converter.git 55 | 56 | * AMCL3: 57 | 58 | https://github.com/fada-catec/amcl3d.git 59 | 60 | 61 | 62 | 63 | To install every packages needed run the commands: 64 | 65 | ``` 66 | $ cd ~/catkin_ws/src 67 | $ git clone https://github.com/uuvsimulator/uuv_simulator 68 | $ git clone https://github.com/OctoMap/octomap_mapping.git 69 | $ git clone https://github.com/uuvsimulator/desistek_saga.git 70 | $ git clone https://github.com/pal-robotics-forks/point_cloud_converter.git 71 | $ git clone https://github.com/fada-catec/amcl3d.git 72 | $ cd ~/catkin_ws 73 | $ catkin_make 74 | ``` 75 | 76 | ## Installation 77 | 78 | ``` 79 | $ cd ~/catkin_ws/src 80 | $ git clone https://github.com/Bluerov2/MASTER.git 81 | $ cd ~/catkin_ws 82 | $ catkin_make # or , if you are using catkin_tools 83 | ``` 84 | 85 | ## Add the DVL 86 | 87 | The original Robot doesn't have any DLV installed. Thus, we need to provide one : 88 | 89 | Run the following command: 90 | ``` 91 | $ roscd desistek_saga_description/urdf/ 92 | $ sudo gedit desistek_saga_sensors.xacro 93 | ``` 94 | 95 | then add the following command: 96 | ```xml 97 | 98 | 102 | 103 | 104 | ``` 105 | 106 | ## Mechanical Sonar 107 | 108 | you will need to change some URDF code from the ROV to allow a 360° vision. 109 | for this run the following commands: 110 | 111 | ``` 112 | $ roscd uuv_simulator 113 | $ cd .. 114 | $ cd uuv_sensor_plugins/uuv_sensor_ros_plugins/urdf 115 | $ sudo gedit sonar_snippets.xacro 116 | ``` 117 | and change the forward_multibeam_p900 with those new values. 118 | ```xml 119 | 120 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | ``` 143 | ![sonar2](https://github.com/Bluerov2/MASTER/blob/sonar_mapping/images/9e9dd76fd4f547150d948ba49b7f92b3_74108.jpeg) 144 | 145 | ## Octomap launching 146 | 147 | before launching anything you will have to do some modification: 148 | We call ocotmap server 50 seconds after the beginning of the simulation. we are doing this because for some reason 149 | the first 360 value of the sonar are drifted. Therefore, we launch a python file that will wait for 50 seconds before launching octomap.launch. but to achived this we will have to change the path of the file: 150 | 151 | ```unix 152 | $ roscd sonar_mapping 153 | $ cd src 154 | $ sudo gedit octomap_launch.py 155 | ``` 156 | change the path: 157 | 158 | >"/home/tim/bluerov_ws/src/sonar_mapping/launch/octomap.launch" 159 | 160 | ## Launch 161 | 162 | To launch the Simulation using UUV simulation, playground and robotmodel: 163 | 164 | ``` 165 | $ roslaunch sonar_mapping sim.launch 166 | ``` 167 | 168 | To launch a rosbag of the real sonar in a square tank : 169 | 170 | ``` 171 | $ roslaunch sonar_mapping bag.launch 172 | ``` 173 | 174 | ## List of Task 175 | 176 | - [x] UUV simulation senario 177 | - [x] IMU & DLV fused (/odom) 178 | - [x] 3D Mapping (octomap) 179 | - [ ] 2D SLAM using Particle Filter 180 | - [ ] 3D SLAM using Particle Filter 181 | -------------------------------------------------------------------------------- /Rviz/rosbag.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 | - /Grid1 10 | - /PointCloud21 11 | - /original sonar1 12 | - /3D sonar1 13 | - /filltered sonar1 14 | - /Map1 15 | Splitter Ratio: 0.5 16 | Tree Height: 775 17 | - Class: rviz/Selection 18 | Name: Selection 19 | - Class: rviz/Tool Properties 20 | Expanded: 21 | - /2D Pose Estimate1 22 | - /2D Nav Goal1 23 | - /Publish Point1 24 | Name: Tool Properties 25 | Splitter Ratio: 0.588679016 26 | - Class: rviz/Views 27 | Expanded: 28 | - /Current View1 29 | Name: Views 30 | Splitter Ratio: 0.5 31 | - Class: rviz/Time 32 | Experimental: false 33 | Name: Time 34 | SyncMode: 0 35 | SyncSource: original sonar 36 | Toolbars: 37 | toolButtonStyle: 2 38 | Visualization Manager: 39 | Class: "" 40 | Displays: 41 | - Alpha: 0.5 42 | Cell Size: 1 43 | Class: rviz/Grid 44 | Color: 160; 160; 164 45 | Enabled: true 46 | Line Style: 47 | Line Width: 0.0299999993 48 | Value: Lines 49 | Name: Grid 50 | Normal Cell Count: 0 51 | Offset: 52 | X: 0 53 | Y: 0 54 | Z: 0 55 | Plane: XY 56 | Plane Cell Count: 10 57 | Reference Frame: 58 | Value: true 59 | - Alpha: 1 60 | Autocompute Intensity Bounds: true 61 | Autocompute Value Bounds: 62 | Max Value: 10 63 | Min Value: -10 64 | Value: true 65 | Axis: Z 66 | Channel Name: intensity 67 | Class: rviz/PointCloud2 68 | Color: 255; 255; 255 69 | Color Transformer: "" 70 | Decay Time: 0 71 | Enabled: false 72 | Invert Rainbow: false 73 | Max Color: 255; 255; 255 74 | Max Intensity: 4096 75 | Min Color: 0; 0; 0 76 | Min Intensity: 0 77 | Name: PointCloud2 78 | Position Transformer: "" 79 | Queue Size: 10 80 | Selectable: true 81 | Size (Pixels): 3 82 | Size (m): 0.00999999978 83 | Style: Flat Squares 84 | Topic: /velodyne_points 85 | Unreliable: false 86 | Use Fixed Frame: true 87 | Use rainbow: true 88 | Value: false 89 | - Alpha: 1 90 | Autocompute Intensity Bounds: true 91 | Autocompute Value Bounds: 92 | Max Value: 10 93 | Min Value: -10 94 | Value: true 95 | Axis: Z 96 | Channel Name: intensity 97 | Class: rviz/PointCloud 98 | Color: 255; 255; 255 99 | Color Transformer: Intensity 100 | Decay Time: 10 101 | Enabled: false 102 | Invert Rainbow: false 103 | Max Color: 255; 255; 255 104 | Max Intensity: 138 105 | Min Color: 0; 0; 0 106 | Min Intensity: 0 107 | Name: original sonar 108 | Position Transformer: XYZ 109 | Queue Size: 10 110 | Selectable: true 111 | Size (Pixels): 3 112 | Size (m): 0.0500000007 113 | Style: Spheres 114 | Topic: /tritech_micron/scan 115 | Unreliable: false 116 | Use Fixed Frame: true 117 | Use rainbow: true 118 | Value: false 119 | - Alpha: 1 120 | Autocompute Intensity Bounds: true 121 | Autocompute Value Bounds: 122 | Max Value: 10 123 | Min Value: -10 124 | Value: true 125 | Axis: Z 126 | Channel Name: intensity 127 | Class: rviz/PointCloud 128 | Color: 255; 255; 255 129 | Color Transformer: Intensity 130 | Decay Time: 10 131 | Enabled: true 132 | Invert Rainbow: false 133 | Max Color: 255; 255; 255 134 | Max Intensity: 146 135 | Min Color: 0; 0; 0 136 | Min Intensity: 0 137 | Name: 3D sonar 138 | Position Transformer: XYZ 139 | Queue Size: 10 140 | Selectable: true 141 | Size (Pixels): 3 142 | Size (m): 0.0500000007 143 | Style: Spheres 144 | Topic: /tritech_micron/scan_3D 145 | Unreliable: false 146 | Use Fixed Frame: true 147 | Use rainbow: true 148 | Value: true 149 | - Alpha: 1 150 | Autocompute Intensity Bounds: true 151 | Autocompute Value Bounds: 152 | Max Value: 10 153 | Min Value: -10 154 | Value: true 155 | Axis: Z 156 | Channel Name: intensity 157 | Class: rviz/PointCloud 158 | Color: 255; 255; 255 159 | Color Transformer: Intensity 160 | Decay Time: 10 161 | Enabled: false 162 | Invert Rainbow: false 163 | Max Color: 255; 255; 255 164 | Max Intensity: 145 165 | Min Color: 0; 0; 0 166 | Min Intensity: 0 167 | Name: filltered sonar 168 | Position Transformer: XYZ 169 | Queue Size: 10 170 | Selectable: true 171 | Size (Pixels): 3 172 | Size (m): 0.0500000007 173 | Style: Spheres 174 | Topic: /tritech_micron/filttered_scan 175 | Unreliable: false 176 | Use Fixed Frame: true 177 | Use rainbow: true 178 | Value: false 179 | - Alpha: 0.699999988 180 | Class: rviz/Map 181 | Color Scheme: map 182 | Draw Behind: false 183 | Enabled: false 184 | Name: Map 185 | Topic: /projected_map 186 | Unreliable: false 187 | Use Timestamp: false 188 | Value: false 189 | Enabled: true 190 | Global Options: 191 | Background Color: 48; 48; 48 192 | Default Light: true 193 | Fixed Frame: sonar 194 | Frame Rate: 30 195 | Name: root 196 | Tools: 197 | - Class: rviz/Interact 198 | Hide Inactive Objects: true 199 | - Class: rviz/MoveCamera 200 | - Class: rviz/Select 201 | - Class: rviz/FocusCamera 202 | - Class: rviz/Measure 203 | - Class: rviz/SetInitialPose 204 | Topic: /initialpose 205 | - Class: rviz/SetGoal 206 | Topic: /move_base_simple/goal 207 | - Class: rviz/PublishPoint 208 | Single click: true 209 | Topic: /clicked_point 210 | Value: true 211 | Views: 212 | Current: 213 | Class: rviz/Orbit 214 | Distance: 21.0511322 215 | Enable Stereo Rendering: 216 | Stereo Eye Separation: 0.0599999987 217 | Stereo Focal Distance: 1 218 | Swap Stereo Eyes: false 219 | Value: false 220 | Focal Point: 221 | X: 0 222 | Y: 0 223 | Z: 0 224 | Focal Shape Fixed Size: true 225 | Focal Shape Size: 0.0500000007 226 | Invert Z Axis: false 227 | Name: Current View 228 | Near Clip Distance: 0.00999999978 229 | Pitch: 0.595397234 230 | Target Frame: 231 | Value: Orbit (rviz) 232 | Yaw: 0.940396309 233 | Saved: ~ 234 | Window Geometry: 235 | Displays: 236 | collapsed: false 237 | Height: 1056 238 | Hide Left Dock: false 239 | Hide Right Dock: false 240 | QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000004ba0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 241 | Selection: 242 | collapsed: false 243 | Time: 244 | collapsed: false 245 | Tool Properties: 246 | collapsed: false 247 | Views: 248 | collapsed: false 249 | Width: 1855 250 | X: 65 251 | Y: 24 252 | -------------------------------------------------------------------------------- /Rviz/sim.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 | - /TF1 11 | - /Odometry1 12 | - /Waypoint path1 13 | - /Sonar_Dynamic1 14 | - /3Dmap1 15 | Splitter Ratio: 0.5 16 | Tree Height: 443 17 | - Class: rviz/Selection 18 | Name: Selection 19 | - Class: rviz/Tool Properties 20 | Expanded: 21 | - /2D Pose Estimate1 22 | - /2D Nav Goal1 23 | - /Publish Point1 24 | Name: Tool Properties 25 | Splitter Ratio: 0.588679016 26 | - Class: rviz/Views 27 | Expanded: 28 | - /Current View1 29 | Name: Views 30 | Splitter Ratio: 0.5 31 | - Class: rviz/Time 32 | Experimental: false 33 | Name: Time 34 | SyncMode: 0 35 | SyncSource: Image 36 | Toolbars: 37 | toolButtonStyle: 2 38 | Visualization Manager: 39 | Class: "" 40 | Displays: 41 | - Alpha: 0.5 42 | Cell Size: 1 43 | Class: rviz/Grid 44 | Color: 160; 160; 164 45 | Enabled: true 46 | Line Style: 47 | Line Width: 0.0299999993 48 | Value: Lines 49 | Name: Grid 50 | Normal Cell Count: 0 51 | Offset: 52 | X: 0 53 | Y: 0 54 | Z: 0 55 | Plane: XY 56 | Plane Cell Count: 10 57 | Reference Frame: 58 | Value: true 59 | - Alpha: 1 60 | Class: rviz/RobotModel 61 | Collision Enabled: false 62 | Enabled: true 63 | Links: 64 | All Links Enabled: true 65 | Expand Joint Details: false 66 | Expand Link Details: false 67 | Expand Tree: false 68 | Link Tree Style: Links in Alphabetic Order 69 | desistek_saga/base_link: 70 | Alpha: 1 71 | Show Axes: false 72 | Show Trail: false 73 | Value: true 74 | desistek_saga/camera_link: 75 | Alpha: 1 76 | Show Axes: false 77 | Show Trail: false 78 | Value: true 79 | desistek_saga/camera_link_optical: 80 | Alpha: 1 81 | Show Axes: false 82 | Show Trail: false 83 | Value: true 84 | desistek_saga/cc_sensor_link: 85 | Alpha: 1 86 | Show Axes: false 87 | Show Trail: false 88 | desistek_saga/dvl_link: 89 | Alpha: 1 90 | Show Axes: false 91 | Show Trail: false 92 | Value: true 93 | desistek_saga/dvl_sonar0_link: 94 | Alpha: 1 95 | Show Axes: false 96 | Show Trail: false 97 | Value: true 98 | desistek_saga/dvl_sonar1_link: 99 | Alpha: 1 100 | Show Axes: false 101 | Show Trail: false 102 | Value: true 103 | desistek_saga/dvl_sonar2_link: 104 | Alpha: 1 105 | Show Axes: false 106 | Show Trail: false 107 | Value: true 108 | desistek_saga/dvl_sonar3_link: 109 | Alpha: 1 110 | Show Axes: false 111 | Show Trail: false 112 | Value: true 113 | desistek_saga/gps_link: 114 | Alpha: 1 115 | Show Axes: false 116 | Show Trail: false 117 | desistek_saga/imu_link: 118 | Alpha: 1 119 | Show Axes: false 120 | Show Trail: false 121 | desistek_saga/pose_sensor_link_default: 122 | Alpha: 1 123 | Show Axes: false 124 | Show Trail: false 125 | desistek_saga/sonar_link: 126 | Alpha: 1 127 | Show Axes: false 128 | Show Trail: false 129 | Value: true 130 | desistek_saga/thruster_0: 131 | Alpha: 1 132 | Show Axes: false 133 | Show Trail: false 134 | Value: true 135 | desistek_saga/thruster_1: 136 | Alpha: 1 137 | Show Axes: false 138 | Show Trail: false 139 | Value: true 140 | desistek_saga/thruster_2: 141 | Alpha: 1 142 | Show Axes: false 143 | Show Trail: false 144 | Value: true 145 | Name: RobotModel 146 | Robot Description: desistek_saga/robot_description 147 | TF Prefix: "" 148 | Update Interval: 0 149 | Value: true 150 | Visual Enabled: true 151 | - Class: rviz/TF 152 | Enabled: false 153 | Frame Timeout: 15 154 | Frames: 155 | All Enabled: true 156 | Marker Scale: 1 157 | Name: TF 158 | Show Arrows: true 159 | Show Axes: true 160 | Show Names: true 161 | Tree: 162 | {} 163 | Update Interval: 0 164 | Value: false 165 | - Class: rviz/Marker 166 | Enabled: true 167 | Marker Topic: /desistek_saga/current_velocity_marker 168 | Name: Current velocity marker 169 | Namespaces: 170 | {} 171 | Queue Size: 100 172 | Value: true 173 | - Class: rviz/MarkerArray 174 | Enabled: true 175 | Marker Topic: /world_models 176 | Name: World models 177 | Namespaces: 178 | "": true 179 | Queue Size: 100 180 | Value: true 181 | - Angle Tolerance: 0.100000001 182 | Class: rviz/Odometry 183 | Covariance: 184 | Orientation: 185 | Alpha: 0.5 186 | Color: 255; 255; 127 187 | Color Style: Unique 188 | Frame: Local 189 | Offset: 1 190 | Scale: 1 191 | Value: true 192 | Position: 193 | Alpha: 0.300000012 194 | Color: 204; 51; 204 195 | Scale: 1 196 | Value: true 197 | Value: true 198 | Enabled: true 199 | Keep: 100 200 | Name: Odometry 201 | Position Tolerance: 0.100000001 202 | Shape: 203 | Alpha: 1 204 | Axes Length: 1 205 | Axes Radius: 0.100000001 206 | Color: 255; 25; 0 207 | Head Length: 0.300000012 208 | Head Radius: 0.100000001 209 | Shaft Length: 1 210 | Shaft Radius: 0.0500000007 211 | Value: Arrow 212 | Topic: /odom 213 | Unreliable: false 214 | Value: true 215 | - Alpha: 1 216 | Buffer Length: 1 217 | Class: rviz/Path 218 | Color: 255; 85; 255 219 | Enabled: true 220 | Head Diameter: 0.300000012 221 | Head Length: 0.200000003 222 | Length: 0.300000012 223 | Line Style: Lines 224 | Line Width: 0.0299999993 225 | Name: Trajectory 226 | Offset: 227 | X: 0 228 | Y: 0 229 | Z: 0 230 | Pose Color: 255; 85; 255 231 | Pose Style: None 232 | Radius: 0.0299999993 233 | Shaft Diameter: 0.100000001 234 | Shaft Length: 0.100000001 235 | Topic: /desistek_saga/trajectory_marker 236 | Unreliable: false 237 | Value: true 238 | - Class: rviz/MarkerArray 239 | Enabled: true 240 | Marker Topic: /desistek_saga/waypoint_markers 241 | Name: Waypoints 242 | Namespaces: 243 | {} 244 | Queue Size: 100 245 | Value: true 246 | - Alpha: 1 247 | Buffer Length: 1 248 | Class: rviz/Path 249 | Color: 255; 85; 255 250 | Enabled: true 251 | Head Diameter: 0.300000012 252 | Head Length: 0.200000003 253 | Length: 0.300000012 254 | Line Style: Lines 255 | Line Width: 0.0299999993 256 | Name: Waypoint path 257 | Offset: 258 | X: 0 259 | Y: 0 260 | Z: 0 261 | Pose Color: 255; 85; 255 262 | Pose Style: None 263 | Radius: 0.0299999993 264 | Shaft Diameter: 0.100000001 265 | Shaft Length: 0.100000001 266 | Topic: /desistek_saga/waypoint_path_marker 267 | Unreliable: false 268 | Value: true 269 | - Class: rviz/Image 270 | Enabled: true 271 | Image Topic: /desistek_saga/desistek_saga/camera/camera_image 272 | Max Value: 1 273 | Median window: 5 274 | Min Value: 0 275 | Name: Image 276 | Normalize Range: true 277 | Queue Size: 2 278 | Transport Hint: raw 279 | Unreliable: false 280 | Value: true 281 | - Alpha: 1 282 | Autocompute Intensity Bounds: true 283 | Autocompute Value Bounds: 284 | Max Value: 10 285 | Min Value: -10 286 | Value: true 287 | Axis: Z 288 | Channel Name: intensity 289 | Class: rviz/PointCloud2 290 | Color: 255; 255; 255 291 | Color Transformer: Intensity 292 | Decay Time: 0 293 | Enabled: true 294 | Invert Rainbow: false 295 | Max Color: 255; 255; 255 296 | Max Intensity: 4096 297 | Min Color: 0; 0; 0 298 | Min Intensity: 0 299 | Name: Sonar_Dynamic 300 | Position Transformer: XYZ 301 | Queue Size: 10 302 | Selectable: true 303 | Size (Pixels): 3 304 | Size (m): 0.100000001 305 | Style: Flat Squares 306 | Topic: /own/simulated/dynamic/sonar_PC2 307 | Unreliable: false 308 | Use Fixed Frame: true 309 | Use rainbow: true 310 | Value: true 311 | - Class: rviz/MarkerArray 312 | Enabled: true 313 | Marker Topic: /occupied_cells_vis_array 314 | Name: 3Dmap 315 | Namespaces: 316 | {} 317 | Queue Size: 100 318 | Value: true 319 | Enabled: true 320 | Global Options: 321 | Background Color: 48; 48; 48 322 | Default Light: true 323 | Fixed Frame: world 324 | Frame Rate: 30 325 | Name: root 326 | Tools: 327 | - Class: rviz/Interact 328 | Hide Inactive Objects: true 329 | - Class: rviz/MoveCamera 330 | - Class: rviz/Select 331 | - Class: rviz/FocusCamera 332 | - Class: rviz/Measure 333 | - Class: rviz/SetInitialPose 334 | Topic: /initialpose 335 | - Class: rviz/SetGoal 336 | Topic: /move_base_simple/goal 337 | - Class: rviz/PublishPoint 338 | Single click: true 339 | Topic: /clicked_point 340 | Value: true 341 | Views: 342 | Current: 343 | Class: rviz/Orbit 344 | Distance: 110.318367 345 | Enable Stereo Rendering: 346 | Stereo Eye Separation: 0.0599999987 347 | Stereo Focal Distance: 1 348 | Swap Stereo Eyes: false 349 | Value: false 350 | Focal Point: 351 | X: 0.0241317265 352 | Y: -0.313088357 353 | Z: -1.08766317 354 | Focal Shape Fixed Size: false 355 | Focal Shape Size: 0.0500000007 356 | Invert Z Axis: false 357 | Name: Current View 358 | Near Clip Distance: 0.00999999978 359 | Pitch: 0.305398911 360 | Target Frame: desistek_saga/base_link 361 | Value: Orbit (rviz) 362 | Yaw: 4.69356823 363 | Saved: ~ 364 | Window Geometry: 365 | Displays: 366 | collapsed: false 367 | Height: 1056 368 | Hide Left Dock: false 369 | Hide Right Dock: true 370 | Image: 371 | collapsed: false 372 | QMainWindow State: 000000ff00000000fd0000000400000000000001e500000396fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000280000024a000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650100000278000001460000001600ffffff000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000396000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000005540000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 373 | Selection: 374 | collapsed: false 375 | Time: 376 | collapsed: false 377 | Tool Properties: 378 | collapsed: false 379 | Views: 380 | collapsed: true 381 | Width: 1855 382 | X: 65 383 | Y: 24 384 | -------------------------------------------------------------------------------- /images/1.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Bluerov2/MASTER/d20b13c776ab764a24dfb42a1c2cb80a4c6de648/images/1.PNG -------------------------------------------------------------------------------- /images/1200px-Heriot-Watt_University_logo.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Bluerov2/MASTER/d20b13c776ab764a24dfb42a1c2cb80a4c6de648/images/1200px-Heriot-Watt_University_logo.jpg -------------------------------------------------------------------------------- /images/2.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Bluerov2/MASTER/d20b13c776ab764a24dfb42a1c2cb80a4c6de648/images/2.PNG -------------------------------------------------------------------------------- /images/3.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Bluerov2/MASTER/d20b13c776ab764a24dfb42a1c2cb80a4c6de648/images/3.PNG -------------------------------------------------------------------------------- /images/9e9dd76fd4f547150d948ba49b7f92b3_74108.jpeg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Bluerov2/MASTER/d20b13c776ab764a24dfb42a1c2cb80a4c6de648/images/9e9dd76fd4f547150d948ba49b7f92b3_74108.jpeg -------------------------------------------------------------------------------- /images/BlueROV2-4-lumen-1-300x300.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Bluerov2/MASTER/d20b13c776ab764a24dfb42a1c2cb80a4c6de648/images/BlueROV2-4-lumen-1-300x300.png -------------------------------------------------------------------------------- /images/Screenshot from 2020-03-02 15-56-21.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Bluerov2/MASTER/d20b13c776ab764a24dfb42a1c2cb80a4c6de648/images/Screenshot from 2020-03-02 15-56-21.png -------------------------------------------------------------------------------- /images/hh.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Bluerov2/MASTER/d20b13c776ab764a24dfb42a1c2cb80a4c6de648/images/hh.jpg -------------------------------------------------------------------------------- /images/pi.pi: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /launch.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /launch/bag.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 | -------------------------------------------------------------------------------- /launch/octomap.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /launch/sim.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 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sonar_mapping 4 | 0.0.0 5 | The sonar_mapping package 6 | 7 | 8 | 9 | 10 | tim 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 | -------------------------------------------------------------------------------- /py_dvl.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | 4 | ## 5 | ## This program reads the values from csv files generated by "save_data.py" 6 | ## Convert the DVL datas into a trajectory estimation 7 | ## Plot the estimated and real results 8 | ## 9 | 10 | 11 | import matplotlib.pyplot as plt 12 | import numpy as np 13 | import os 14 | import csv 15 | import math 16 | 17 | 18 | 19 | ############## 20 | ############## 21 | # PARAMETERS # 22 | ############## 23 | ############## 24 | 25 | # Position of the DVL on the robot, where (0,0,0) is the center of gravity of the robot 26 | OFFSET_X = 0.0 27 | OFFSET_Y = 0.75 28 | OFFSET_Z = 0.0 29 | 30 | 31 | 32 | ######################################################################################################### 33 | ## 34 | ## Lists where the datas from csv file will be saved 35 | ## 36 | 37 | # ----- DVL ----- 38 | dvl_timestamp = [] 39 | dvl_x = [] 40 | dvl_y = [] 41 | dvl_z = [] 42 | # ----- Robot ----- 43 | robot_timestamp = [] 44 | robot_x = [] 45 | robot_y = [] 46 | robot_z = [] 47 | robot_yaw = [] 48 | 49 | ## 50 | ## Read csv files and save them into the lists previously created, then remove the first value (which is '0') 51 | ## 52 | 53 | # ----- DVL ----- 54 | my_path = os.path.abspath(os.path.dirname(__file__)) 55 | path = os.path.join(my_path, "data/dvl.csv") 56 | with open(path) as csv_file: 57 | csv_reader = csv.DictReader(csv_file, delimiter=',') 58 | for row in csv_reader: 59 | dvl_timestamp.append(float(row['time'])) 60 | dvl_x.append(float(row['X'])) 61 | dvl_y.append(float(row['Y'])) 62 | dvl_z.append(float(row['Z'])) 63 | del dvl_timestamp[0] 64 | del dvl_x[0] 65 | del dvl_y[0] 66 | del dvl_z[0] 67 | # ----- Robot ----- 68 | my_path = os.path.abspath(os.path.dirname(__file__)) 69 | path = os.path.join(my_path, "data/robot.csv") 70 | with open(path) as csv_file: 71 | csv_reader = csv.DictReader(csv_file, delimiter=',') 72 | for row in csv_reader: 73 | robot_timestamp.append(float(row['time'])) 74 | robot_x.append(float(row['X'])) 75 | robot_y.append(float(row['Y'])) 76 | robot_z.append(float(row['Z'])) 77 | robot_yaw.append(float(row['Yaw'])) 78 | del robot_timestamp[0] 79 | del robot_x[0] 80 | del robot_y[0] 81 | del robot_z[0] 82 | del robot_yaw[0] 83 | ######################################################################################################### 84 | 85 | ## 86 | ## Convert the datas from dvl to an estimated trajectory 87 | ## 88 | 89 | previous_time = 0 90 | time = 0 91 | dt = 0 92 | estimated_traj_x = [] 93 | estimated_traj_y = [] 94 | estimated_traj_z = [] 95 | real_traj_x = [] 96 | real_traj_y = [] 97 | real_traj_z = [] 98 | 99 | # We do the assumption that the robot turns only around the z axis. 100 | prevYaw = 0; 101 | 102 | for i in range(len(robot_timestamp)-1): 103 | for j in range(len(dvl_timestamp)): 104 | # Find a match between the two times 105 | if robot_timestamp[i] <= dvl_timestamp[j] and robot_timestamp[i+1] > dvl_timestamp[j]: 106 | # Take the 1st values 107 | if len(estimated_traj_x) == 0: 108 | previous_time = dvl_timestamp[j] 109 | estimated_traj_x.append(robot_x[i]) 110 | estimated_traj_y.append(robot_y[i]) 111 | estimated_traj_z.append(robot_z[i]) 112 | prevYaw = robot_yaw[i] 113 | else: 114 | time = dvl_timestamp[j] 115 | dt = float(time - previous_time) 116 | previous_time = time 117 | X = dvl_x[j] - OFFSET_X*(robot_yaw[i]-prevYaw)/dt 118 | Y = dvl_y[j] - OFFSET_Y*(robot_yaw[i]-prevYaw)/dt 119 | prevYaw = robot_yaw[i] 120 | estimated_traj_x.append(estimated_traj_x[-1] - X * dt * math.cos(robot_yaw[i]) + Y * dt * math.sin(robot_yaw[i]) ) 121 | estimated_traj_y.append(estimated_traj_y[-1] - X * dt * math.sin(robot_yaw[i]) - Y * dt * math.cos(robot_yaw[i]) ) 122 | estimated_traj_z.append(estimated_traj_z[-1] - dvl_z[j] * dt ) 123 | 124 | # Reduce the number of values for the real trajectory 125 | real_traj_x.append(robot_x[i]) 126 | real_traj_y.append(robot_y[i]) 127 | real_traj_z.append(robot_z[i]) 128 | ######################################################################################################### 129 | 130 | ## 131 | ## Plot the results 132 | ## 133 | 134 | """ 135 | path = os.path.join(os.path.abspath(os.path.dirname(__file__)), "data/trajectories.csv") 136 | with open(path, mode='w') as csv_file: 137 | writer = csv.writer(csv_file, delimiter=',', quotechar='"', quoting=csv.QUOTE_MINIMAL) 138 | writer.writerow(['estimation_X','estimation_Y','estimation_Z','real_X','real_Y','real_Z']) 139 | for i in range(len(estimated_traj_x)): 140 | writer.writerow([estimated_traj_x[i], estimated_traj_y[i], estimated_traj_z[i], real_traj_x[i], real_traj_y[i], real_traj_z[i]]) 141 | """ 142 | 143 | plt.title("trajectories") 144 | plt.plot(estimated_traj_x,estimated_traj_y,'r') 145 | plt.plot(real_traj_x,real_traj_y,'b') 146 | plt.show() 147 | 148 | # Error: 149 | error = [] 150 | for i in range(len(estimated_traj_x)): 151 | error.append(math.sqrt((estimated_traj_x[i]-real_traj_x[i])**2+(estimated_traj_y[i]-real_traj_y[i])**2)) 152 | 153 | plt.title("error") 154 | plt.plot(error) 155 | plt.show() 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | -------------------------------------------------------------------------------- /rosbag/dynamic_sonar.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Bluerov2/MASTER/d20b13c776ab764a24dfb42a1c2cb80a4c6de648/rosbag/dynamic_sonar.bag -------------------------------------------------------------------------------- /rosbag/static_sonar.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Bluerov2/MASTER/d20b13c776ab764a24dfb42a1c2cb80a4c6de648/rosbag/static_sonar.bag -------------------------------------------------------------------------------- /save_data.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | ## 4 | ## This program reads the /g500/dvl and /g500/pose topics generated by the dynamic function of UWsim on robot Girona 500 5 | ## Save the datas from DVL into a csv file "/data/dvl.csv" 6 | ## Save the real robot position into a csv file "/data/robot.csv" 7 | ## 8 | ## Press 'p' when you want the datas to be written on these files. 9 | ## 10 | 11 | 12 | import rospy 13 | from underwater_sensor_msgs.msg import DVL 14 | from geometry_msgs.msg import Pose 15 | import matplotlib.pyplot as plt 16 | import numpy as np 17 | import math 18 | import termios, fcntl, sys, os 19 | import csv 20 | 21 | class dvl: 22 | def __init__(self): 23 | #Console input variables to teleop it from the console 24 | fd = sys.stdin.fileno() 25 | oldterm = termios.tcgetattr(fd) 26 | newattr = termios.tcgetattr(fd) 27 | newattr[3] = newattr[3] & ~termios.ICANON & ~termios.ECHO 28 | termios.tcsetattr(fd, termios.TCSANOW, newattr) 29 | oldflags = fcntl.fcntl(fd, fcntl.F_GETFL) 30 | fcntl.fcntl(fd, fcntl.F_SETFL, oldflags | os.O_NONBLOCK) 31 | 32 | # Subscribe to the topics 33 | sub_dvl = rospy.Subscriber('/g500/dvl', DVL, self.dvl_sub) 34 | sub_pose = rospy.Subscriber('/g500/pose',Pose, self.pose_sub) 35 | 36 | self.timeDVL = [0] 37 | self.timeRobot = [0] 38 | 39 | self.dvlX = [0] 40 | self.dvlY = [0] 41 | self.dvlZ = [0] 42 | 43 | self.robotX = [0] 44 | self.robotY = [0] 45 | self.robotZ = [0] 46 | self.robotYaw = [0] 47 | 48 | # Callback of dvl topic -> save the datas in some lists 49 | def dvl_sub(self,msg): 50 | self.timeDVL.append(rospy.get_time()) 51 | self.dvlX.append(msg.bi_x_axis) 52 | self.dvlY.append(msg.bi_y_axis) 53 | self.dvlZ.append(msg.bi_z_axis) 54 | # When 'p' is pressed, the datas are saved 55 | try: 56 | c = sys.stdin.read(1) 57 | print c 58 | if c == 'p' or c == 'P': 59 | self.displayDatas() 60 | except IOError: pass 61 | 62 | # Callback of pose topic -> save the datas in some lists 63 | def pose_sub(self,msg): 64 | self.timeRobot.append(rospy.get_time()) 65 | self.robotX.append(msg.position.x) 66 | self.robotY.append(msg.position.y) 67 | self.robotZ.append(msg.position.z) 68 | self.robotYaw.append(self.quaternion_to_euler(msg.orientation.x,msg.orientation.y,msg.orientation.z,msg.orientation.w)[2]) 69 | 70 | # Convert quaternion to euler angles (radians) 71 | def quaternion_to_euler(self,x,y,z,w): 72 | t0 = +2.0 * (w * x + y * z) 73 | t1 = +1.0 - 2.0 * (x * x + y * y) 74 | X = math.degrees(math.atan2(t0, t1)) 75 | 76 | t2 = +2.0 * (w * y - z * x) 77 | t2 = +1.0 if t2 > +1.0 else t2 78 | t2 = -1.0 if t2 < -1.0 else t2 79 | Y = math.degrees(math.asin(t2)) 80 | 81 | t3 = +2.0 * (w * z + x * y) 82 | t4 = +1.0 - 2.0 * (y * y + z * z) 83 | Z = math.atan2(t3, t4) 84 | 85 | return X, Y, Z 86 | 87 | # Save the datas at this moment, then write them in he csv files 88 | def displayDatas(self): 89 | stimeDVL = self.timeDVL 90 | stimeRobot = self.timeRobot 91 | sdvlX = self.dvlX 92 | sdvlY = self.dvlY 93 | sdvlZ = self.dvlZ 94 | srobotX = self.robotX 95 | srobotY = self.robotY 96 | srobotZ = self.robotZ 97 | srobotYaw = self.robotYaw 98 | 99 | path = os.path.join(os.path.abspath(os.path.dirname(__file__)), "data/dvl.csv") 100 | with open(path, mode='w') as csv_file: 101 | writer = csv.writer(csv_file, delimiter=',', quotechar='"', quoting=csv.QUOTE_MINIMAL) 102 | writer.writerow(['time','X','Y','Z']) 103 | 104 | for i in range(len(stimeDVL)): 105 | writer.writerow([stimeDVL[i], sdvlX[i], sdvlY[i], sdvlZ[i]]) 106 | 107 | path = os.path.join(os.path.abspath(os.path.dirname(__file__)), "data/robot.csv") 108 | with open(path, mode='w') as csv_file: 109 | writer = csv.writer(csv_file, delimiter=',', quotechar='"', quoting=csv.QUOTE_MINIMAL) 110 | writer.writerow(['time','X','Y','Z','Yaw']) 111 | 112 | for i in range(len(stimeRobot)): 113 | writer.writerow([stimeRobot[i], srobotX[i], srobotY[i], srobotZ[i], srobotYaw[i]]) 114 | 115 | 116 | def main(args): 117 | rospy.init_node('read_dvl', anonymous=True) 118 | 119 | start = dvl() 120 | 121 | try: 122 | rospy.spin() 123 | except KeyboardInterrupt: 124 | print("Shutting down") 125 | cv2.destroyAllWindows() 126 | 127 | if __name__ == '__main__': 128 | main(sys.argv) -------------------------------------------------------------------------------- /sonar_simulation.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # PointCloud2 color cube 3 | # https://answers.ros.org/question/289576/understanding-the-bytes-in-a-pcl2-message/ 4 | import rospy 5 | import struct 6 | 7 | 8 | from sensor_msgs.msg import LaserScan 9 | from std_msgs.msg import Header 10 | 11 | 12 | def callback(arg): 13 | 14 | laser = LaserScan() 15 | 16 | laser.header = arg.header # timestamp in the header is the acquisition time on 17 | laser.angle_min = arg.angle_min # start angle of the scan [rad] 18 | laser.angle_max = arg.angle_max # end angle of the scan [rad] 19 | laser.angle_increment = arg.angle_increment # angular distance between measurements [rad] 20 | laser.time_increment = arg.time_increment # time between measurements [seconds] - if your scanne 21 | laser.scan_time = arg.scan_time # time between scans [seconds] 22 | laser.range_min = arg.range_min # minimum range value [m] 23 | laser.range_max = arg.range_max # maximum range value [m] 24 | 25 | 26 | for i in range(len(arg.ranges)): 27 | laser.ranges.append(arg.ranges[i]) 28 | rospy.sleep(1) 29 | pub.publish(laser) 30 | 31 | if __name__ == "__main__": 32 | while not rospy.is_shutdown(): 33 | rospy.init_node("create_cloud_xyzrgb") 34 | pub = rospy.Publisher("/sonar", LaserScan, queue_size=1) 35 | sub = rospy.Subscriber("/desistek_saga/sonar", LaserScan, callback) 36 | -------------------------------------------------------------------------------- /src/3D_map_Point_cloud.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | 4 | import rospy 5 | import roslib 6 | import sys 7 | import matplotlib.pyplot as plt 8 | import numpy as np 9 | from sensor_msgs.msg import PointCloud 10 | from mpl_toolkits import mplot3d 11 | from mpl_toolkits.mplot3d import Axes3D 12 | 13 | 14 | 15 | class points(): 16 | 17 | def __init__(self): 18 | 19 | self.pub = rospy.Publisher("/PointCloud",PointCloud,queue_size=1) #Create a topic with the new PointCloud 20 | self.sub = rospy.Subscriber("/tritech_micron/scan", PointCloud, self.get_data) # Subscribs to the topcis to get the data 21 | 22 | def get_data(self,var): 23 | 24 | 25 | PC = PointCloud() # create the new object PointCloud to be published later 26 | PC.header = var.header # copy the content of topic to another PointCloud in order to manipulate it 27 | PC.channels = var.channels 28 | PC.points = var.points 29 | 30 | for i in range((len(PC.points))): 31 | 32 | PC.points[i].z = PC.channels[0].values[i]/100 # write in the z axis of the points the values of the "intensity" divided by 100 to make it readable on rviz 33 | 34 | self.pub.publish(PC) # publish it into the new topic 35 | 36 | 37 | 38 | if __name__ == "__main__": 39 | # craeting the node 40 | rospy.init_node("get_data") 41 | classe = points() 42 | rospy.spin() 43 | -------------------------------------------------------------------------------- /src/converting.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # PointCloud2 color cube 3 | # https://answers.ros.org/question/289576/understanding-the-bytes-in-a-pcl2-message/ 4 | import rospy 5 | import struct 6 | 7 | from sensor_msgs import point_cloud2 8 | from sensor_msgs.msg import PointCloud ,PointCloud2, PointField 9 | from std_msgs.msg import Header 10 | 11 | 12 | def callback(arg): 13 | 14 | points = [] 15 | for k in range(len(arg.points)): 16 | 17 | x = float(arg.points[k].x) 18 | y = float(arg.points[k].y) 19 | z = float(0) 20 | 21 | r = int(255.0) 22 | g = int(255.0) 23 | b = int(255.0) 24 | a = 255 25 | 26 | rgb = struct.unpack('I', struct.pack('BBBB', b, g, r, a))[0] 27 | pt = [x, y, z, rgb] 28 | points.append(pt) 29 | 30 | fields = [PointField('x', 0, PointField.FLOAT32, 1), 31 | PointField('y', 0, PointField.FLOAT32, 1), 32 | PointField('z', 0, PointField.FLOAT32, 1), 33 | # PointField('rgb', 12, PointField.UINT32, 1), 34 | PointField('rgba', 12, PointField.UINT32, 1), 35 | ] 36 | 37 | 38 | header = Header() 39 | header.frame_id = "sonar" 40 | pc2 = point_cloud2.create_cloud(header, fields, points) 41 | pc2.header.stamp = rospy.Time.now() 42 | pub.publish(pc2) 43 | 44 | if __name__ == "__main__": 45 | while not rospy.is_shutdown(): 46 | rospy.init_node("create_cloud_xyzrgb") 47 | pub = rospy.Publisher("point_cloud2", PointCloud2, queue_size=1) 48 | sub = rospy.Subscriber("/tritech_micron/filttered_scan", PointCloud, callback) 49 | -------------------------------------------------------------------------------- /src/dvl.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """ 4 | This program converts datas from DVL and IMU into the position of the robot. 5 | 6 | Input: /desisek_saga/dvl 7 | /desistek_saga/imu 8 | 9 | Output: /odom 10 | 11 | In __init__, set OFFSET_X, OFFSET_Y and OFFSET_Z equal to the distance in xyz between the DVL and the inertial center of the robot. 12 | , set STARTING_X, Y, Z equal to the xyz starting position of the robot, and STARTING_radianX, Y, Z equal to its orientation in radians 13 | """ 14 | 15 | import rospy 16 | from uuv_sensor_ros_plugins_msgs.msg import DVL 17 | from sensor_msgs.msg import Imu 18 | from nav_msgs.msg import Odometry 19 | import matplotlib.pyplot as plt 20 | import numpy as np 21 | import math 22 | import sys 23 | 24 | class dvl: 25 | def __init__(self): 26 | sub_dvl = rospy.Subscriber('/desistek_saga/dvl', DVL, self.dvl_sub) 27 | sub_imu = rospy.Subscriber('/desistek_saga/imu', Imu, self.imu_sub) 28 | 29 | self.pubOdom = rospy.Publisher("/odom", Odometry, queue_size=1) 30 | 31 | self.timeDVL = rospy.get_time() 32 | self.previous_time = 0 33 | self.dvlseq = 0 34 | self.dvlsecs = 0 35 | self.dvlnsecs = 0 36 | self.dvlX = 0 37 | self.dvlY = 0 38 | self.dvlZ = 0 39 | 40 | ####################### To be se correctly depending on the robot configuration ################### 41 | self.OFFSET_X = 0 42 | self.OFFSET_Y = 0 43 | self.OFFSET_Z = 0 44 | 45 | self.STARTING_X = -250 46 | self.STARTING_Y = 300 47 | self.STARTING_Z = -5 48 | self.STARTING_radianX = 3.1415/4 49 | self.STARTING_radianY = 0 50 | self.STARTING_radianZ = 0 51 | ################################################################################################### 52 | 53 | self.timeIMU = rospy.get_time() 54 | self.quaternionX = 0 55 | self.quaternionY = 0 56 | self.quaternionZ = 0 57 | self.quaternionW = 0 58 | self.imuX = 0 59 | self.imuY = 0 60 | self.imuZ = 0 61 | self.angvelX = 0 62 | self.angvelY = 0 63 | self.angvelZ = 0 64 | self.lastImuX = 0 65 | self.lastImuY = 0 66 | self.lastImuZ = 0 67 | 68 | self.dvlReceived = False 69 | 70 | self.estimated_traj_x = self.STARTING_X 71 | self.estimated_traj_y = self.STARTING_Y 72 | self.estimated_traj_z = self.STARTING_Z 73 | 74 | # The frequency of the DVL is lower than the IMU 75 | def dvl_sub(self,msg): 76 | self.timeDVL = rospy.get_time() 77 | 78 | self.dvlseq = msg.header.seq 79 | self.dvlsecs = msg.header.stamp.secs 80 | self.dvlnsecs = msg.header.stamp.nsecs 81 | self.dvlX = msg.velocity.z ############ /!\ ########### 82 | self.dvlY = msg.velocity.y 83 | self.dvlZ = msg.velocity.x ############ /!\ ########### 84 | 85 | self.dvlReceived = True 86 | 87 | # The frequency of the IMU is bigger than the DVL's 88 | def imu_sub(self,msg): 89 | if self.dvlReceived == True: 90 | self.dvlReceived = False 91 | self.timeIMU = rospy.get_time() 92 | self.quaternionX = msg.orientation.x 93 | self.quaternionY = msg.orientation.y 94 | self.quaternionZ = msg.orientation.z 95 | self.quaternionW = msg.orientation.w 96 | X,Y,Z = self.quaternion_to_euler(self.quaternionX,self.quaternionY,self.quaternionZ,self.quaternionW) 97 | self.imuX = X 98 | self.imuY = Y 99 | self.imuZ = Z 100 | 101 | self.angvelX = msg.angular_velocity.x 102 | self.angvelY = msg.angular_velocity.y 103 | self.angvelZ = msg.angular_velocity.z 104 | 105 | self.estimateTraj() 106 | 107 | 108 | def estimateTraj(self): 109 | dt = float(self.timeDVL - self.previous_time) 110 | X = self.dvlX - self.OFFSET_X*(self.imuZ-self.lastImuZ)/dt 111 | Y = self.dvlY - self.OFFSET_Y*(self.imuZ-self.lastImuZ)/dt 112 | 113 | self.estimated_traj_x = self.estimated_traj_x + (X * dt * math.cos(self.imuZ) - Y * dt * math.sin(self.imuZ)) 114 | self.estimated_traj_y = self.estimated_traj_y + (X * dt * math.sin(self.imuZ) + Y * dt * math.cos(self.imuZ)) 115 | self.estimated_traj_z = self.estimated_traj_z - self.dvlZ * dt 116 | self.previous_time = self.timeDVL 117 | self.lastImuX = self.imuX + self.STARTING_radianX 118 | self.lastImuY = self.imuY + self.STARTING_radianY 119 | self.lastImuZ = self.imuZ + self.STARTING_radianZ 120 | 121 | self.convert_to_odom() 122 | 123 | def convert_to_odom(self): 124 | odm = Odometry() 125 | odm.header.seq = self.dvlseq 126 | rostime = rospy.get_time() 127 | odm.header.stamp.secs = int(rostime) 128 | odm.header.stamp.nsecs = 1000000000*(rostime-int(rostime)) 129 | 130 | odm.header.frame_id = "world" 131 | #odm.child_frame_id = "desistek_saga/base_link" 132 | 133 | odm.pose.pose.position.x = self.estimated_traj_x 134 | odm.pose.pose.position.y = self.estimated_traj_y 135 | odm.pose.pose.position.z = self.estimated_traj_z 136 | 137 | odm.pose.pose.orientation.x = self.quaternionX 138 | odm.pose.pose.orientation.y = self.quaternionY 139 | odm.pose.pose.orientation.z = self.quaternionZ 140 | odm.pose.pose.orientation.w = self.quaternionW 141 | self.pubOdom.publish(odm) 142 | 143 | def quaternion_to_euler(self,x,y,z,w): 144 | t0 = +2.0 * (w * x + y * z) 145 | t1 = +1.0 - 2.0 * (x * x + y * y) 146 | X = math.degrees(math.atan2(t0, t1)) 147 | 148 | t2 = +2.0 * (w * y - z * x) 149 | t2 = +1.0 if t2 > +1.0 else t2 150 | t2 = -1.0 if t2 < -1.0 else t2 151 | Y = math.degrees(math.asin(t2)) 152 | 153 | t3 = +2.0 * (w * z + x * y) 154 | t4 = +1.0 - 2.0 * (y * y + z * z) 155 | Z = math.atan2(t3, t4) 156 | 157 | return X, Y, Z 158 | 159 | def main(args): 160 | rospy.init_node('read_dvl', anonymous=True) 161 | 162 | a = dvl() 163 | 164 | try: 165 | rospy.spin() 166 | except KeyboardInterrupt: 167 | print("Shutting down") 168 | cv2.destroyAllWindows() 169 | 170 | if __name__ == '__main__': 171 | main(sys.argv) 172 | -------------------------------------------------------------------------------- /src/filtering.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | 4 | import rospy 5 | import roslib 6 | import sys 7 | import numpy as np 8 | from sensor_msgs.msg import PointCloud, PointCloud2 9 | 10 | 11 | 12 | class points(): 13 | 14 | def __init__(self): 15 | self.pub1 = rospy.Publisher("/tritech_micron/filttered_scan",PointCloud,queue_size=1) #Create a topic with the new PointCloud 16 | self.pub2 = rospy.Publisher("/tritech_micron/scan_3D",PointCloud,queue_size=1) #Create a topic with the new PointCloud 17 | self.sub = rospy.Subscriber("/tritech_micron/scan", PointCloud, self.get_data) # Subscribs to the topcis to get the data 18 | 19 | def get_data(self,var): 20 | 21 | 22 | PC = PointCloud() # create the new object PointCloud to be published later 23 | 24 | PC.header = var.header # copy the content of topic to another PointCloud in order to manipulate it 25 | PC.channels = var.channels 26 | PC.points = var.points 27 | 28 | 29 | """ 30 | Function to get the "3D" map 31 | """ 32 | 33 | for i in range((len(PC.points))): 34 | 35 | PC.points[i].z = PC.channels[0].values[i]/100 # write in the z axis of the points the values of the "intensity" divided by 100 to make it readable on rviz 36 | 37 | self.pub2.publish(PC) # publish it into the new topic 38 | 39 | """ 40 | Function to filtter the map and get only one point by degree 41 | """ 42 | 43 | index_max = PC.channels[0].values.index(max(PC.channels[0].values)) 44 | 45 | for i in range(len(PC.points)): #396 46 | 47 | if i != index_max: 48 | PC.points[i].x = PC.points[i].y = PC.points[i].z = 0 49 | else: 50 | PC.points[i].z = 0 51 | 52 | self.pub1.publish(PC) # publish it into the new topic 53 | 54 | 55 | ###################################################################################### 56 | 57 | 58 | 59 | 60 | 61 | if __name__ == "__main__": 62 | # craeting the node 63 | rospy.init_node("get_data") 64 | classe = points() 65 | rospy.spin() 66 | -------------------------------------------------------------------------------- /src/frame.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import roslib 4 | import rospy 5 | import tf 6 | 7 | if __name__ == '__main__': 8 | rospy.init_node("sonar_fixed_frame") 9 | br = tf.TransformBroadcaster() 10 | rate = rospy.Rate(10.0) 11 | while not rospy.is_shutdown(): 12 | br.sendTransform((0.0,0.0,0.0), 13 | (0.0,0.0,0.0,1.0), 14 | rospy.Time.now(), 15 | "fakescan", 16 | "sonar") 17 | rate.sleep() -------------------------------------------------------------------------------- /src/laserscan_to_poincloud.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | 4 | import rospy 5 | from sensor_msgs.msg import PointCloud2 6 | from sensor_msgs.msg import LaserScan 7 | from laser_geometry import LaserProjection 8 | import math 9 | 10 | class Laser2PC(): 11 | 12 | def __init__(self): 13 | self.laserProj = LaserProjection() 14 | 15 | self.pub = rospy.Publisher("/own/true/sonar_PC2",PointCloud2, queue_size=1) 16 | self.pub1 = rospy.Publisher("/own/simulated/sonar_PC2",PointCloud2, queue_size=1) 17 | self.pub2 = rospy.Publisher("/own/simulated//dynamic/sonar_PC2",PointCloud2, queue_size=1) 18 | 19 | self.sub = rospy.Subscriber("/desistek_saga/sonar",LaserScan, self.callback) 20 | self.sub1 = rospy.Subscriber("/own/simulated/sonar_LS",LaserScan, self.callback1) 21 | self.sub2 = rospy.Subscriber("/own/simulated/dynamic/sonar_LS",LaserScan, self.callback2) 22 | 23 | def callback(self,data): 24 | 25 | laser = LaserScan() 26 | 27 | laser.header = data.header 28 | laser.angle_min = data.angle_min 29 | laser.angle_max = data.angle_max 30 | laser.angle_increment = data.angle_increment 31 | laser.time_increment = data.time_increment 32 | laser.scan_time = data.scan_time 33 | laser.range_min = data.range_min 34 | laser.range_max = data.range_max 35 | laser.intensities = data.intensities 36 | laser.ranges = data.ranges 37 | 38 | cloud = self.laserProj.projectLaser(laser) 39 | 40 | self.pub.publish(cloud) 41 | 42 | 43 | def callback1(self,arg): 44 | 45 | laser = LaserScan() 46 | 47 | laser.header = arg.header 48 | laser.angle_min = arg.angle_min 49 | laser.angle_max = arg.angle_max 50 | laser.angle_increment = arg.angle_increment 51 | laser.time_increment = arg.time_increment 52 | laser.scan_time = arg.scan_time 53 | laser.range_min = arg.range_min 54 | laser.range_max = arg.range_max 55 | laser.intensities = arg.intensities 56 | laser.ranges = arg.ranges 57 | 58 | cloud = self.laserProj.projectLaser(laser) 59 | 60 | self.pub1.publish(cloud) 61 | 62 | def callback2(self,arg): 63 | 64 | laser = LaserScan() 65 | 66 | laser.header = arg.header 67 | laser.angle_min = arg.angle_min 68 | laser.angle_max = arg.angle_max 69 | laser.angle_increment = arg.angle_increment 70 | laser.time_increment = arg.time_increment 71 | laser.scan_time = arg.scan_time 72 | laser.range_min = arg.range_min 73 | laser.range_max = arg.range_max 74 | laser.intensities = arg.intensities 75 | laser.ranges = arg.ranges 76 | 77 | cloud = self.laserProj.projectLaser(laser) 78 | 79 | self.pub2.publish(cloud) 80 | 81 | 82 | 83 | if __name__ == "__main__": 84 | 85 | rospy.init_node("laser_to_cloud") 86 | l2px = Laser2PC() 87 | rospy.spin() 88 | -------------------------------------------------------------------------------- /src/octomam.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | 4 | import rospy 5 | import struct 6 | import numpy as np 7 | 8 | from octomap_msgs.msg import Octomap 9 | from sensor_msgs.msg import LaserScan 10 | from std_msgs.msg import Header 11 | 12 | def callback(arg): 13 | 14 | print(arg.resolution) 15 | 16 | 17 | 18 | 19 | if __name__ == "__main__": 20 | 21 | rospy.init_node("3Dmapreader") 22 | while not rospy.is_shutdown(): 23 | sub = rospy.Subscriber("/octomap_full",Octomap,callback) 24 | -------------------------------------------------------------------------------- /src/octomap_launch.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | 4 | import roslaunch 5 | import rospy 6 | 7 | 8 | rospy.sleep(80) 9 | rospy.init_node('en_Mapping', anonymous=True) 10 | uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) 11 | roslaunch.configure_logging(uuid) 12 | launch = roslaunch.parent.ROSLaunchParent(uuid, ["/home/tim/bluerov_ws/src/sonar_mapping/launch/octomap.launch"]) 13 | launch.start() 14 | rospy.loginfo("started") 15 | 16 | while not rospy.is_shutdown(): 17 | pass 18 | -------------------------------------------------------------------------------- /src/pointcloudtolaserscan.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | import roslib 5 | import sys 6 | import math 7 | from sensor_msgs.msg import PointCloud 8 | from sensor_msgs.msg import LaserScan 9 | import tf 10 | 11 | class ConvertPC2LS(): 12 | def __init__(self): 13 | self.sub = rospy.Subscriber("/tritech_micron/filtered_scan", PointCloud, self.convert) 14 | self.publs = rospy.Publisher("/tritech_micron/sonar_laserscan", LaserScan, queue_size=1) 15 | 16 | self.i = 0 17 | self.scan = [0] * 200 # number of values from sonar 18 | 19 | self.seq = 0 20 | self.secs = 0 21 | self.nsecs = 0 22 | 23 | def convert(self,msg): 24 | self.seq = msg.header.seq 25 | self.secs = msg.header.stamp.secs 26 | self.nsecs = msg.header.stamp.nsecs 27 | 28 | w = 0 29 | while w < 392: 30 | if msg.points[w].x != 0 or msg.points[w].y != 0: 31 | self.scan[self.i] = math.sqrt(msg.points[w].x**2+msg.points[w].y**2) 32 | w = 1000 33 | else: 34 | w+=1 35 | if w < 1000: 36 | self.scan[self.i] = 0 37 | self.i +=1 38 | if self.i >= 200: 39 | self.i = 0 40 | self.publish() 41 | 42 | def publish(self): 43 | ls = LaserScan() 44 | 45 | ls.header.seq = self.seq 46 | ls.header.stamp.secs = self.secs 47 | ls.header.stamp.nsecs = self.nsecs 48 | ls.header.frame_id = "sonar" 49 | 50 | ls.angle_min = -math.pi 51 | ls.angle_max = math.pi 52 | ls.angle_increment = 1.8*math.pi/180 53 | ls.scan_time = 9.4 54 | ls.range_min:0.0 55 | ls.range_max = 70.0 56 | ls.ranges = self.scan 57 | self.publs.publish(ls) 58 | 59 | if __name__ == "__main__": 60 | # creating the node 61 | rospy.init_node("convert_data") 62 | classe = ConvertPC2LS() 63 | rospy.spin() 64 | -------------------------------------------------------------------------------- /src/sonar_simulation.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | 4 | import rospy 5 | import struct 6 | import numpy as np 7 | 8 | 9 | from sensor_msgs.msg import LaserScan 10 | from std_msgs.msg import Header 11 | 12 | 13 | # initialization of the variables 14 | laser = LaserScan() 15 | final_laser = LaserScan() 16 | laser_list = [] 17 | 18 | 19 | 20 | def callback(arg): 21 | """ 22 | This callback is made to retrive data from the laserscan and make it global 23 | """ 24 | global laser 25 | laser = arg #laser is the current state of the laserscan 26 | 27 | 28 | 29 | if __name__ == "__main__": 30 | 31 | rospy.init_node("sonar") # initialization of the node 32 | pub = rospy.Publisher("/own/simulated/sonar_LS", LaserScan, queue_size=1) # initialization of the final publisher 33 | 34 | while not rospy.is_shutdown(): 35 | 36 | for i in range(1,200): #for every value in the Laserscan 37 | 38 | sub = rospy.Subscriber("/desistek_saga/sonar", LaserScan, callback) # update the value of the laserscan 39 | 40 | #if the laser is empty does nothing 41 | if len(laser.ranges) == 0: 42 | pass 43 | 44 | else: 45 | 46 | #otherwise if lenght of the value is less than the max lenght of the laserscan 47 | if len(laser_list) < 200: 48 | 49 | laser_list.append(laser.ranges[i])# add another element to a list 50 | 51 | else: 52 | 53 | laser_list[i] = laser.ranges[i] # if the list is already filled just change de value 54 | 55 | 56 | laser_tuple = tuple(laser_list)# change list into a tuple 57 | 58 | final_laser.header = laser.header 59 | final_laser.angle_min = laser.angle_min 60 | final_laser.angle_max = laser.angle_max 61 | final_laser.angle_increment = laser.angle_increment 62 | final_laser.time_increment = laser.time_increment 63 | final_laser.scan_time = laser.scan_time 64 | final_laser.range_min = laser.range_min 65 | final_laser.range_max = laser.range_max 66 | 67 | final_laser.ranges = laser_tuple 68 | # make the final_laser with the same time as the real conversion 69 | # but the ranges will be incremented one by one such as a sonar 70 | rospy.sleep(0.25) 71 | """ 72 | print (" ") 73 | print(laser_list) 74 | print(" ") 75 | rospy.sleep(0.25) 76 | print(len(laser_list)) 77 | #laser_list = [] 78 | """ 79 | pub.publish(final_laser) # publish the final laser 80 | -------------------------------------------------------------------------------- /src/sonar_simulation_dynamic.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | 4 | import rospy 5 | import struct 6 | import numpy as np 7 | 8 | 9 | from sensor_msgs.msg import LaserScan 10 | from std_msgs.msg import Header 11 | 12 | 13 | # initialization of the variables 14 | laser = LaserScan() 15 | final_laser = LaserScan() 16 | laser_list = [] 17 | 18 | 19 | 20 | def callback(arg): 21 | """ 22 | This callback is made to retrive data from the laserscan and make it global 23 | """ 24 | global laser 25 | laser = arg #laser is the current state of the laserscan 26 | 27 | 28 | 29 | if __name__ == "__main__": 30 | 31 | rospy.init_node("sonar_dynamic") # initialization of the node 32 | pub = rospy.Publisher("/own/simulated/dynamic/sonar_LS", LaserScan, queue_size=1) # initialization of the final publisher 33 | 34 | while not rospy.is_shutdown(): 35 | 36 | for i in range(1,200): #for every value in the Laserscan 37 | 38 | sub = rospy.Subscriber("/desistek_saga/sonar", LaserScan, callback) # update the value of the laserscan 39 | 40 | #if the laser is empty does nothing 41 | if len(laser.ranges) == 0: 42 | pass 43 | 44 | else: 45 | 46 | #otherwise if lenght of the value is less than the max lenght of the laserscan 47 | if len(laser_list) < 200: 48 | 49 | laser_list.append(laser.ranges[i])# add another element to a list 50 | 51 | if len(laser_list) > 0 : 52 | 53 | for a in range(len(laser_list)-1): 54 | 55 | laser_list[a] = 0 56 | 57 | else: 58 | 59 | laser_list[i] = laser.ranges[i] # if the list is already filled just change de value 60 | 61 | 62 | for a in range(i-1): 63 | 64 | laser_list[a] = 0 65 | 66 | 67 | 68 | 69 | 70 | laser_tuple = tuple(laser_list)# change list into a tuple 71 | 72 | final_laser.header = laser.header 73 | final_laser.angle_min = laser.angle_min 74 | final_laser.angle_max = laser.angle_max 75 | final_laser.angle_increment = laser.angle_increment 76 | final_laser.time_increment = laser.time_increment 77 | final_laser.scan_time = laser.scan_time 78 | final_laser.range_min = laser.range_min 79 | final_laser.range_max = laser.range_max 80 | 81 | final_laser.ranges = laser_tuple 82 | # make the final_laser with the same time as the real conversion 83 | # but the ranges will be incremented one by one such as a sonar 84 | rospy.sleep(0.25) 85 | """ 86 | print (" ") 87 | print(laser_list) 88 | print(" ") 89 | rospy.sleep(0.25) 90 | print(len(laser_list)) 91 | #laser_list = [] 92 | """ 93 | pub.publish(final_laser) # publish the final laser 94 | --------------------------------------------------------------------------------