├── .gitignore ├── LICENSE ├── README.md ├── smarc_auvs ├── CMakeLists.txt ├── models │ └── example_auv │ │ ├── launch │ │ └── upload_example_auv.launch │ │ ├── mesh │ │ ├── Fin.dae │ │ ├── Propeller.dae │ │ └── Romulus.dae │ │ ├── robots │ │ └── example_auv_default.urdf.xacro │ │ ├── rviz │ │ └── example_auv.rviz │ │ └── urdf │ │ └── example_auv_base.urdf.xacro ├── package.xml └── rviz │ └── example_auv.rviz ├── smarc_gazebo_plugins ├── smarc_gazebo_plugins │ ├── CMakeLists.txt │ ├── include │ │ └── smarc_gazebo_plugins │ │ │ ├── PopulationIntensities.hh │ │ │ ├── PopulationIntensitiesPrv.hh │ │ │ ├── SemanticMultiRayShape.hh │ │ │ ├── UnderwaterSonarPlugin.hh │ │ │ ├── UnderwaterSonarSensor.hh │ │ │ └── UnderwaterSonarSensorPrivate.hh │ ├── package.xml │ └── src │ │ ├── PopulationIntensities.cc │ │ ├── RegisterUnderwaterSonarSensor.cc │ │ ├── SemanticMultiRayShape.cc │ │ ├── UnderwaterSonarPlugin.cc │ │ └── UnderwaterSonarSensor.cc └── smarc_gazebo_ros_plugins │ ├── CMakeLists.txt │ ├── include │ └── smarc_gazebo_ros_plugins │ │ ├── gazebo_ros_image_sonar.h │ │ ├── gazebo_ros_underwater_sonar.h │ │ └── gazebo_ros_underwater_sonar_ray.h │ ├── msg │ └── SonarEntities.msg │ ├── package.xml │ └── src │ ├── gazebo_ros_image_sonar.cpp │ ├── gazebo_ros_underwater_sonar.cpp │ └── gazebo_ros_underwater_sonar_ray.cpp ├── smarc_sensor_plugins └── smarc_sensor_plugins_ros │ ├── CMakeLists.txt │ ├── launch │ └── waterfall.launch │ ├── package.xml │ ├── src │ ├── combine_waterfalls_node.cpp │ └── sidescan_waterfall_image_node.cpp │ └── urdf │ └── sonar_snippets.xacro ├── smarc_simulated_comms ├── CMakeLists.txt ├── README.md ├── launch │ └── smarc_simulated_comms.launch ├── package.xml └── scripts │ ├── comms_master.py │ └── comms_tester.py └── smarc_worlds ├── CMakeLists.txt ├── Media ├── materials │ ├── programs │ │ ├── SimpleWaves.frag │ │ └── SimpleWaves.vert │ ├── scripts │ │ ├── fiducials.material │ │ ├── sand.material │ │ ├── water.material │ │ ├── waves.material │ │ └── waves.program │ └── textures │ │ ├── License and source for textures.txt │ │ ├── clouds.jpg │ │ ├── clouds_bk.jpg │ │ ├── clouds_dn.jpg │ │ ├── clouds_fr.jpg │ │ ├── clouds_lf.jpg │ │ ├── clouds_rt.jpg │ │ ├── clouds_up.jpg │ │ ├── fiducials │ │ ├── License and source for textures.txt │ │ └── tag36_11_00000.jpg │ │ ├── soil_sand_0045_01.jpg │ │ ├── water_water_0046_01.jpg │ │ ├── water_water_0051_01.jpg │ │ ├── water_water_0076_03_s.jpg │ │ ├── water_water_0093_01.jpg │ │ └── wave_normals.dds └── models │ └── sea_surface_1000m_x_1000m.dae ├── launch ├── ekf_localization_tests.launch ├── pipe_following.launch ├── pool_world.launch ├── random_rocks.launch ├── skytteren.launch └── sonar_intensities.launch ├── package.xml ├── pool └── model.sdf ├── scripts └── spawn_model.py ├── world_models ├── duck │ ├── images │ │ └── duckCM.png │ ├── meshes │ │ └── duck.dae │ └── model.sdf ├── duckCM.png ├── dummy_laser │ ├── model.config │ └── model.sdf ├── large_rock │ ├── meshes │ │ ├── large_rock.dae │ │ └── large_rock_collision.stl │ ├── model.config │ └── model.sdf ├── pipeline │ ├── meshes │ │ ├── pipeline.dae │ │ └── pipeline_collision.stl │ ├── model.config │ └── model.sdf ├── sand_heightmap_pipe │ ├── meshes │ │ ├── License and source for textures.txt │ │ ├── heightmap.dae │ │ ├── heightmap_pipe.dae │ │ ├── rust.jpg │ │ └── soil_sand_0045_01.jpg │ ├── model.config │ └── model.sdf ├── skytteren │ ├── meshes │ │ ├── skytteren.dae │ │ └── skytteren_repaired.dae │ ├── model.config │ └── model.sdf └── small_ocean_box │ ├── model.config │ └── model.sdf └── worlds ├── pipe_following.world ├── pool.world ├── random_rocks.world ├── rocks_random.world ├── skytteren.world └── sonar_intensities.world /.gitignore: -------------------------------------------------------------------------------- 1 | *~ 2 | *.pyc 3 | 4 | # Qt files 5 | *.config 6 | *.creator 7 | *.creator.user 8 | *.files 9 | *.includes 10 | 11 | # ROS bags 12 | *.bag 13 | 14 | # auv models 15 | /smarc_auvs/models/* -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | The 3-Clause BSD License 2 | 3 | Copyright 2018 Nils Bore (nbore@kth.se), Ignacio Torroba (ignacio@kth.se) 4 | 5 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 6 | 7 | 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 8 | 9 | 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 10 | 11 | 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 12 | 13 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 14 | 15 | Significant portions have been modified from the original sources at: 16 | 17 | gazebo 18 | ====== 19 | 20 | Website: https://bitbucket.org/osrf/gazebo 21 | 22 | Copyright (C) 2014 Open Source Robotics Foundation 23 | 24 | Licensed under the Apache License, Version 2.0 (the "License"); 25 | you may not use this file except in compliance with the License. 26 | You may obtain a copy of the License at 27 | 28 | http://www.apache.org/licenses/LICENSE-2.0 29 | 30 | Unless required by applicable law or agreed to in writing, software 31 | distributed under the License is distributed on an "AS IS" BASIS, 32 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 33 | See the License for the specific language governing permissions and 34 | limitations under the License. 35 | 36 | uuv_simulator 37 | ============= 38 | 39 | Website: https://github.com/uuvsimulator/uuv_simulator 40 | 41 | Copyright (c) 2016 The UUV Simulator Authors. 42 | All rights reserved. 43 | 44 | Licensed under the Apache License, Version 2.0 (the "License"); 45 | you may not use this file except in compliance with the License. 46 | You may obtain a copy of the License at 47 | 48 | http://www.apache.org/licenses/LICENSE-2.0 49 | 50 | Unless required by applicable law or agreed to in writing, software 51 | distributed under the License is distributed on an "AS IS" BASIS, 52 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 53 | See the License for the specific language governing permissions and 54 | limitations under the License. 55 | 56 | 57 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # smarc_simulations 2 | Simulation environments for smarc scenarios 3 | 4 | # Setting up gazebo and uuv_simulator 5 | 6 | ## Install 7 | 8 | Follow the instructions on http://gazebosim.org/tutorials?tut=install_ubuntu and 9 | in https://github.com/uuvsimulator/uuv_simulator/wiki#using-uuv-simulator-with-ros-kinetic-and-gazebo-7 . 10 | 11 | Then clone https://github.com/smarc-project/uuv_simulator.git in your catkin workspace. 12 | Compile it with `catkin_make`, followed by `catkin_make install`. 13 | 14 | Then install the following packages: 15 | ``` 16 | sudo apt-get install ros-kinetic-ros-control ros-kinetic-gazebo-ros-control ros-kinetic-joint-state-controller 17 | ``` 18 | 19 | ## Environment variables 20 | 21 | Add the following lines to your `.bashrc`, replace `GAZEBO_PREFIX` to be the path to your catkin workspace. 22 | ``` 23 | source /usr/share/gazebo-7/setup.sh 24 | export GAZEBO_MODEL_DATABASE_URI=http://gazebosim.org/models 25 | export GAZEBO_PREFIX=$HOME/path/to/your/catkin_ws/install 26 | export GAZEBO_RESOURCE_PATH=${GAZEBO_PREFIX}/share/uuv_descriptions:${GAZEBO_RESOURCE_PATH} 27 | export GAZEBO_MODEL_PATH=${GAZEBO_PREFIX}/share/uuv_descriptions/worlds:${GAZEBO_MODEL_PATH} 28 | export GAZEBO_PLUGIN_PATH=${GAZEBO_PREFIX}/lib:${GAZEBO_PREFIX}/lib/x86_64-linux-gnu:${GAZEBO_PLUGIN_PATH} 29 | ``` 30 | 31 | # Running a basic simulation 32 | 33 | ## Launching 34 | 35 | Open a new tab, don't forget to source your catkin workspace every time you do this. 36 | 37 | Then launch gazebo with an pipeline world using: 38 | ``` 39 | roslaunch smarc_worlds pipe_following.launch 40 | ``` 41 | Then we open a new tab and launch a simulation of the small smarc auv: 42 | ``` 43 | roslaunch smarc_auvs upload_example_auv.launch 44 | ``` 45 | 46 | ## Playing around 47 | 48 | You can fire the thruster of the AUV by running the following command, feed it a value between 0-100: 49 | ``` 50 | rostopic pub /example_auv/thrusters/0/input uuv_gazebo_r_plugins_msgs/FloatStamped "header: 51 | seq: 0 52 | stamp: 53 | secs: 0 54 | nsecs: 0 55 | frame_id: '' 56 | data: 100.0" 57 | ``` 58 | Check `rostopic list` and try to control e.g. the fins. 59 | 60 | ## Rviz 61 | 62 | In rviz, you can get the camera image on `/example_auv/example_auv/camera/camera_image` 63 | and the left and right side scans on `example_auv/sss_left` and `example_auv/sss_right`, respectively. 64 | 65 | Rviz configs that display a lot of information for the different auvs are saved in the `smarc_auvs/rviz` folder. 66 | To use them, click `File > Open Config` in rviz and navigate to the `.rviz` file that you want to use. 67 | -------------------------------------------------------------------------------- /smarc_auvs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(smarc_auvs) 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) 11 | 12 | ## System dependencies are found with CMake's conventions 13 | # find_package(Boost REQUIRED COMPONENTS system) 14 | set(PACKAGE_NAME smarc_auvs) 15 | 16 | 17 | ## Uncomment this if the package has a setup.py. This macro ensures 18 | ## modules and global scripts declared therein get installed 19 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 20 | # catkin_python_setup() 21 | 22 | ################################################ 23 | ## Declare ROS messages, services and actions ## 24 | ################################################ 25 | 26 | ## To declare and build messages, services or actions from within this 27 | ## package, follow these steps: 28 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 29 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 30 | ## * In the file package.xml: 31 | ## * add a build_depend tag for "message_generation" 32 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 33 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 34 | ## but can be declared for certainty nonetheless: 35 | ## * add a run_depend tag for "message_runtime" 36 | ## * In this file (CMakeLists.txt): 37 | ## * add "message_generation" and every package in MSG_DEP_SET to 38 | ## find_package(catkin REQUIRED COMPONENTS ...) 39 | ## * add "message_runtime" and every package in MSG_DEP_SET to 40 | ## catkin_package(CATKIN_DEPENDS ...) 41 | ## * uncomment the add_*_files sections below as needed 42 | ## and list every .msg/.srv/.action file to be processed 43 | ## * uncomment the generate_messages entry below 44 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 45 | 46 | ## Generate messages in the 'msg' folder 47 | # add_message_files( 48 | # FILES 49 | # Message1.msg 50 | # Message2.msg 51 | # ) 52 | 53 | ## Generate services in the 'srv' folder 54 | # add_service_files( 55 | # FILES 56 | # Service1.srv 57 | # Service2.srv 58 | # ) 59 | 60 | ## Generate actions in the 'action' folder 61 | # add_action_files( 62 | # FILES 63 | # Action1.action 64 | # Action2.action 65 | # ) 66 | 67 | ## Generate added messages and services with any dependencies listed here 68 | # generate_messages( 69 | # DEPENDENCIES 70 | # std_msgs # Or other packages containing msgs 71 | # ) 72 | 73 | ################################################ 74 | ## Declare ROS dynamic reconfigure parameters ## 75 | ################################################ 76 | 77 | ## To declare and build dynamic reconfigure parameters within this 78 | ## package, follow these steps: 79 | ## * In the file package.xml: 80 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 81 | ## * In this file (CMakeLists.txt): 82 | ## * add "dynamic_reconfigure" to 83 | ## find_package(catkin REQUIRED COMPONENTS ...) 84 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 85 | ## and list every .cfg file to be processed 86 | 87 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 88 | # generate_dynamic_reconfigure_options( 89 | # cfg/DynReconf1.cfg 90 | # cfg/DynReconf2.cfg 91 | # ) 92 | 93 | ################################### 94 | ## catkin specific configuration ## 95 | ################################### 96 | ## The catkin_package macro generates cmake config files for your package 97 | ## Declare things to be passed to dependent projects 98 | ## INCLUDE_DIRS: uncomment this if you package contains header files 99 | ## LIBRARIES: libraries you create in this project that dependent projects also need 100 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 101 | ## DEPENDS: system dependencies of this project that dependent projects also need 102 | catkin_package( 103 | # INCLUDE_DIRS include 104 | # LIBRARIES small_smarc_auv 105 | # CATKIN_DEPENDS other_catkin_pkg 106 | # DEPENDS system_lib 107 | ) 108 | 109 | ########### 110 | ## Build ## 111 | ########### 112 | 113 | ## Specify additional locations of header files 114 | ## Your package locations should be listed before other locations 115 | include_directories( 116 | # include 117 | # ${catkin_INCLUDE_DIRS} 118 | ) 119 | 120 | ## Declare a C++ library 121 | # add_library(${PROJECT_NAME} 122 | # src/${PROJECT_NAME}/small_smarc_auv.cpp 123 | # ) 124 | 125 | ## Add cmake target dependencies of the library 126 | ## as an example, code may need to be generated before libraries 127 | ## either from message generation or dynamic reconfigure 128 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 129 | 130 | ## Declare a C++ executable 131 | ## With catkin_make all packages are built within a single CMake context 132 | ## The recommended prefix ensures that target names across packages don't collide 133 | # add_executable(${PROJECT_NAME}_node src/small_smarc_auv_node.cpp) 134 | 135 | ## Rename C++ executable without prefix 136 | ## The above recommended prefix causes long target names, the following renames the 137 | ## target back to the shorter version for ease of user use 138 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 139 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 140 | 141 | ## Add cmake target dependencies of the executable 142 | ## same as for the library above 143 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 144 | 145 | ## Specify libraries to link a library or executable target against 146 | # target_link_libraries(${PROJECT_NAME}_node 147 | # ${catkin_LIBRARIES} 148 | # ) 149 | 150 | ############# 151 | ## Install ## 152 | ############# 153 | 154 | # all install targets should use catkin DESTINATION variables 155 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 156 | 157 | ## Mark executable scripts (Python etc.) for installation 158 | ## in contrast to setup.py, you can choose the destination 159 | # install(PROGRAMS 160 | # scripts/my_python_script 161 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 162 | # ) 163 | 164 | ## Mark executables and/or libraries for installation 165 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 166 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 167 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 168 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 169 | # ) 170 | 171 | ## Mark cpp header files for installation 172 | # install(DIRECTORY include/${PROJECT_NAME}/ 173 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 174 | # FILES_MATCHING PATTERN "*.h" 175 | # PATTERN ".svn" EXCLUDE 176 | # ) 177 | 178 | ## Mark other files for installation (e.g. launch and bag files, etc.) 179 | install(DIRECTORY models 180 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${PACKAGE_NAME} 181 | PATTERN "*~" EXCLUDE) 182 | 183 | ############# 184 | ## Testing ## 185 | ############# 186 | 187 | ## Add gtest based cpp test target and link libraries 188 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_small_smarc_auv.cpp) 189 | # if(TARGET ${PROJECT_NAME}-test) 190 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 191 | # endif() 192 | 193 | ## Add folders to be run by python nosetests 194 | # catkin_add_nosetests(test) 195 | -------------------------------------------------------------------------------- /smarc_auvs/models/example_auv/launch/upload_example_auv.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 18 | 19 | 20 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /smarc_auvs/models/example_auv/robots/example_auv_default.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | $(arg namespace) 29 | 50 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /smarc_auvs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | smarc_auvs 4 | 0.0.0 5 | The small_smarc_auv package 6 | 7 | 8 | 9 | 10 | Nils Bore 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 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /smarc_gazebo_plugins/smarc_gazebo_plugins/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(smarc_gazebo_plugins) 3 | 4 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 5 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}") 6 | 7 | find_package(gazebo REQUIRED) 8 | find_package(Boost REQUIRED COMPONENTS system) 9 | find_package(Eigen3 REQUIRED) 10 | find_package(Protobuf REQUIRED) 11 | 12 | #find_package(OGRE REQUIRED) 13 | 14 | ## Compile as C++11, supported in ROS Kinetic and newer 15 | # add_compile_options(-std=c++11) 16 | 17 | ## Find catkin macros and libraries 18 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 19 | ## is used, also find other catkin packages 20 | find_package(catkin REQUIRED) 21 | 22 | ## System dependencies are found with CMake's conventions 23 | # find_package(Boost REQUIRED COMPONENTS system) 24 | 25 | 26 | ## Uncomment this if the package has a setup.py. This macro ensures 27 | ## modules and global scripts declared therein get installed 28 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 29 | # catkin_python_setup() 30 | 31 | ################################################ 32 | ## Declare ROS messages, services and actions ## 33 | ################################################ 34 | 35 | ## To declare and build messages, services or actions from within this 36 | ## package, follow these steps: 37 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 38 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 39 | ## * In the file package.xml: 40 | ## * add a build_depend tag for "message_generation" 41 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 42 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 43 | ## but can be declared for certainty nonetheless: 44 | ## * add a run_depend tag for "message_runtime" 45 | ## * In this file (CMakeLists.txt): 46 | ## * add "message_generation" and every package in MSG_DEP_SET to 47 | ## find_package(catkin REQUIRED COMPONENTS ...) 48 | ## * add "message_runtime" and every package in MSG_DEP_SET to 49 | ## catkin_package(CATKIN_DEPENDS ...) 50 | ## * uncomment the add_*_files sections below as needed 51 | ## and list every .msg/.srv/.action file to be processed 52 | ## * uncomment the generate_messages entry below 53 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 54 | 55 | ## Generate messages in the 'msg' folder 56 | # add_message_files( 57 | # FILES 58 | # Message1.msg 59 | # Message2.msg 60 | # ) 61 | 62 | ## Generate services in the 'srv' folder 63 | # add_service_files( 64 | # FILES 65 | # Service1.srv 66 | # Service2.srv 67 | # ) 68 | 69 | ## Generate actions in the 'action' folder 70 | # add_action_files( 71 | # FILES 72 | # Action1.action 73 | # Action2.action 74 | # ) 75 | 76 | ## Generate added messages and services with any dependencies listed here 77 | # generate_messages( 78 | # DEPENDENCIES 79 | # std_msgs # Or other packages containing msgs 80 | # ) 81 | 82 | ################################################ 83 | ## Declare ROS dynamic reconfigure parameters ## 84 | ################################################ 85 | 86 | ## To declare and build dynamic reconfigure parameters within this 87 | ## package, follow these steps: 88 | ## * In the file package.xml: 89 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 90 | ## * In this file (CMakeLists.txt): 91 | ## * add "dynamic_reconfigure" to 92 | ## find_package(catkin REQUIRED COMPONENTS ...) 93 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 94 | ## and list every .cfg file to be processed 95 | 96 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 97 | # generate_dynamic_reconfigure_options( 98 | # cfg/DynReconf1.cfg 99 | # cfg/DynReconf2.cfg 100 | # ) 101 | 102 | ################################### 103 | ## catkin specific configuration ## 104 | ################################### 105 | ## The catkin_package macro generates cmake config files for your package 106 | ## Declare things to be passed to dependent projects 107 | ## INCLUDE_DIRS: uncomment this if your package contains header files 108 | ## LIBRARIES: libraries you create in this project that dependent projects also need 109 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 110 | ## DEPENDS: system dependencies of this project that dependent projects also need 111 | catkin_package( 112 | INCLUDE_DIRS include 113 | ${CMAKE_CURRENT_BINARY_DIR} # for generated messages 114 | #${PROJECT_BINARY_DIR}/msgs 115 | ${Boost_INCLUDE_DIR} 116 | ${catkin_INCLUDE_DIRS} 117 | ${GAZEBO_INCLUDE_DIRS} 118 | ${GAZEBO_MSG_INCLUDE_DIRS} 119 | ${EIGEN3_INCLUDE_DIRS} 120 | LIBRARIES underwater_sonar_sensor underwater_sonar_plugin population_intensities 121 | DEPENDS gazebo 122 | ) 123 | 124 | include_directories(${PROJECT_SOURCE_DIR}/include 125 | ${CMAKE_CURRENT_BINARY_DIR} # for generated messages 126 | ${Boost_INCLUDE_DIR} 127 | ${catkin_INCLUDE_DIRS} 128 | ${GAZEBO_INCLUDE_DIRS} 129 | ${GAZEBO_MSG_INCLUDE_DIRS} 130 | ${EIGEN3_INCLUDE_DIRS}) 131 | 132 | link_directories(${EIGEN3_LIBRARY_DIRS} ${GAZEBO_LIBRARY_DIRS}) 133 | 134 | ## Declare a C++ library 135 | add_library(semantic_multi_ray_shape 136 | src/SemanticMultiRayShape.cc 137 | ) 138 | 139 | add_library(underwater_sonar_sensor 140 | src/UnderwaterSonarSensor.cc 141 | ) 142 | 143 | add_library(underwater_sonar_plugin 144 | src/UnderwaterSonarPlugin.cc 145 | ) 146 | 147 | add_library(register_underwater_sonar_sensor 148 | src/RegisterUnderwaterSonarSensor.cc 149 | ) 150 | 151 | add_library(population_intensities 152 | src/PopulationIntensities.cc 153 | ) 154 | ## Add cmake target dependencies of the library 155 | ## as an example, code may need to be generated before libraries 156 | ## either from message generation or dynamic reconfigure 157 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 158 | 159 | ## Declare a C++ executable 160 | ## With catkin_make all packages are built within a single CMake context 161 | ## The recommended prefix ensures that target names across packages don't collide 162 | # add_executable(${PROJECT_NAME}_node src/smarc_gazebo_plugins_node.cpp) 163 | 164 | ## Rename C++ executable without prefix 165 | ## The above recommended prefix causes long target names, the following renames the 166 | ## target back to the shorter version for ease of user use 167 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 168 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 169 | 170 | ## Add cmake target dependencies of the executable 171 | ## same as for the library above 172 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 173 | 174 | ## Specify libraries to link a library or executable target against 175 | target_link_libraries(semantic_multi_ray_shape 176 | ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} 177 | ) 178 | 179 | target_link_libraries(underwater_sonar_sensor 180 | semantic_multi_ray_shape 181 | ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} 182 | ) 183 | 184 | target_link_libraries(underwater_sonar_plugin 185 | underwater_sonar_sensor 186 | ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} 187 | ) 188 | 189 | target_link_libraries(register_underwater_sonar_sensor 190 | underwater_sonar_sensor 191 | ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} 192 | ) 193 | 194 | target_link_libraries(population_intensities 195 | ${GAZEBO_LIBRARIES} 196 | ) 197 | 198 | ############# 199 | ## Install ## 200 | ############# 201 | 202 | # all install targets should use catkin DESTINATION variables 203 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 204 | 205 | ## Mark executable scripts (Python etc.) for installation 206 | ## in contrast to setup.py, you can choose the destination 207 | # install(PROGRAMS 208 | # scripts/my_python_script 209 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 210 | # ) 211 | 212 | ## Mark executables and/or libraries for installation 213 | install(TARGETS 214 | semantic_multi_ray_shape 215 | underwater_sonar_sensor 216 | underwater_sonar_plugin 217 | register_underwater_sonar_sensor 218 | population_intensities 219 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 220 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 221 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 222 | ) 223 | 224 | install(DIRECTORY include/${PROJECT_NAME}/ 225 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 226 | FILES_MATCHING PATTERN "*.hh" 227 | PATTERN "*~" EXCLUDE 228 | ) 229 | 230 | install(DIRECTORY include/ 231 | DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} 232 | FILES_MATCHING PATTERN ".hh" 233 | ) 234 | 235 | ## Mark other files for installation (e.g. launch and bag files, etc.) 236 | # install(FILES 237 | # # myfile1 238 | # # myfile2 239 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 240 | # ) 241 | 242 | ############# 243 | ## Testing ## 244 | ############# 245 | 246 | ## Add gtest based cpp test target and link libraries 247 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_smarc_gazebo_plugins.cpp) 248 | # if(TARGET ${PROJECT_NAME}-test) 249 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 250 | # endif() 251 | 252 | ## Add folders to be run by python nosetests 253 | # catkin_add_nosetests(test) 254 | -------------------------------------------------------------------------------- /smarc_gazebo_plugins/smarc_gazebo_plugins/include/smarc_gazebo_plugins/PopulationIntensitiesPrv.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * This file was modified from the original version within Gazebo: 3 | * 4 | * Copyright (C) 2014 Open Source Robotics Foundation 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | * 18 | * Modifications: 19 | * 20 | * Copyright 2018 Nils Bore (nbore@kth.se) 21 | * 22 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 23 | * 24 | * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 25 | * 26 | * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 27 | * 28 | * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 29 | * 30 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | */ 33 | 34 | #ifndef _GAZEBO_POPULATIONINTENSITIESPRV_HH_ 35 | #define _GAZEBO_POPULATIONINTENSITIESPRV_HH_ 36 | 37 | #include 38 | #include 39 | #include "gazebo/physics/World.hh" 40 | 41 | namespace gazebo 42 | { 43 | namespace physics 44 | { 45 | /// \brief Private data for the Population class 46 | class PopulationPrivateIntensities 47 | { 48 | /// \brief The Population's SDF values. 49 | public: sdf::ElementPtr populationElem; 50 | 51 | /// \brief Pointer to the world. 52 | public: boost::shared_ptr world; 53 | }; 54 | } 55 | } 56 | #endif 57 | -------------------------------------------------------------------------------- /smarc_gazebo_plugins/smarc_gazebo_plugins/include/smarc_gazebo_plugins/SemanticMultiRayShape.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * This file was modified from the original version within Gazebo: 3 | * 4 | * Copyright (C) 2014 Open Source Robotics Foundation 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | * 18 | * Modifications: 19 | * 20 | * Copyright 2018 Nils Bore (nbore@kth.se) 21 | * 22 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 23 | * 24 | * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 25 | * 26 | * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 27 | * 28 | * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 29 | * 30 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | */ 33 | 34 | #ifndef SEMANTIC_MULTIRAY_SHAPE_HH 35 | #define SEMANTIC_MULTIRAY_SHAPE_HH 36 | 37 | #include 38 | 39 | namespace gazebo 40 | { 41 | 42 | //class MultiRayShape; 43 | 44 | namespace physics { 45 | 46 | /// \class MultiRayShape MultiRayShape.hh physics/physics.hh 47 | /// \brief Laser collision contains a set of ray-collisions, 48 | /// structured to simulate a laser range scanner. 49 | class SemanticMultiRayShape : public MultiRayShape 50 | { 51 | /// \brief Constructor. 52 | /// \param[in] _parent Parent collision shape. 53 | public: explicit SemanticMultiRayShape(CollisionPtr _parent); 54 | 55 | /// \brief Destructor. 56 | public: virtual ~SemanticMultiRayShape(); 57 | 58 | public: RayShapePtr GetRay(unsigned int _index); 59 | 60 | public: static RayShapePtr StaticGetRay(MultiRayShapePtr _parent, unsigned int _index); 61 | 62 | }; 63 | 64 | typedef boost::shared_ptr SemanticMultiRayShapePtr; 65 | 66 | } 67 | 68 | } 69 | 70 | #endif // SEMANTIC_MULTIRAY_SHAPE_HH 71 | -------------------------------------------------------------------------------- /smarc_gazebo_plugins/smarc_gazebo_plugins/include/smarc_gazebo_plugins/UnderwaterSonarPlugin.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * This file was modified from the original version within Gazebo: 3 | * 4 | * Copyright (C) 2014 Open Source Robotics Foundation 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | * 18 | * Modifications: 19 | * 20 | * Copyright 2018 Nils Bore (nbore@kth.se) 21 | * 22 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 23 | * 24 | * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 25 | * 26 | * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 27 | * 28 | * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 29 | * 30 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | */ 33 | 34 | #ifndef _GAZEBO_UNDERWATER_SONAR_PLUGIN_HH_ 35 | #define _GAZEBO_UNDERWATER_SONAR_PLUGIN_HH_ 36 | 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | 43 | namespace gazebo 44 | { 45 | /// \brief A Ray Sensor Plugin 46 | class GAZEBO_VISIBLE UnderwaterSonarPlugin : public SensorPlugin 47 | { 48 | /// \brief Constructor 49 | public: UnderwaterSonarPlugin(); 50 | 51 | /// \brief Destructor 52 | public: virtual ~UnderwaterSonarPlugin(); 53 | 54 | /// \brief Update callback 55 | public: virtual void OnNewLaserScans(); 56 | 57 | /// \brief Load the plugin 58 | /// \param take in SDF root element 59 | public: void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf); 60 | 61 | /// \brief Pointer to parent 62 | protected: physics::WorldPtr world; 63 | 64 | /// \brief The parent sensor 65 | private: std::shared_ptr parentSensor; 66 | 67 | /// \brief The connection tied to UnderwaterSonarPlugin::OnNewLaserScans() 68 | private: event::ConnectionPtr newLaserScansConnection; 69 | }; 70 | } 71 | #endif // _GAZEBO_UNDERWATER_SONAR_PLUGIN_HH_ 72 | -------------------------------------------------------------------------------- /smarc_gazebo_plugins/smarc_gazebo_plugins/include/smarc_gazebo_plugins/UnderwaterSonarSensorPrivate.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * This file was modified from the original version within Gazebo: 3 | * 4 | * Copyright (C) 2014 Open Source Robotics Foundation 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | * 18 | * Modifications: 19 | * 20 | * Copyright 2018 Nils Bore (nbore@kth.se) 21 | * 22 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 23 | * 24 | * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 25 | * 26 | * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 27 | * 28 | * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 29 | * 30 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | */ 33 | 34 | #ifndef _GAZEBO_SENSORS_RAYSENSOR_PRIVATE_HH_ 35 | #define _GAZEBO_SENSORS_RAYSENSOR_PRIVATE_HH_ 36 | 37 | #include 38 | 39 | #include "gazebo/msgs/msgs.hh" 40 | #include "gazebo/physics/PhysicsTypes.hh" 41 | #include "gazebo/transport/TransportTypes.hh" 42 | 43 | namespace gazebo 44 | { 45 | namespace sensors 46 | { 47 | /// \internal 48 | /// \brief Ray sensor private data. 49 | class UnderwaterSonarSensorPrivate 50 | { 51 | /// \brief Laser collision pointer. 52 | public: physics::CollisionPtr laserCollision; 53 | 54 | /// \brief Multi ray shapre pointer. 55 | public: physics::MultiRayShapePtr laserShape; 56 | 57 | /// \brief Parent entity pointer 58 | public: physics::EntityPtr parentEntity; 59 | 60 | /// \brief Publisher for the scans 61 | public: transport::PublisherPtr scanPub; 62 | 63 | /// \brief Publisher for the entities 64 | public: transport::PublisherPtr entityPub; 65 | // 66 | /// \brief Mutex to protect laserMsg 67 | public: std::mutex mutex; 68 | 69 | /// \brief Laser message. 70 | public: msgs::LaserScanStamped laserMsg; 71 | 72 | /// \brief Entity message 73 | public: msgs::GzString_V entityMsg; 74 | }; 75 | } 76 | } 77 | #endif 78 | -------------------------------------------------------------------------------- /smarc_gazebo_plugins/smarc_gazebo_plugins/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | smarc_gazebo_plugins 4 | 0.0.0 5 | The smarc_gazebo_plugins package 6 | 7 | 8 | 9 | 10 | Nils Bore 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 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /smarc_gazebo_plugins/smarc_gazebo_plugins/src/RegisterUnderwaterSonarSensor.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * This file was modified from the original version within Gazebo: 3 | * 4 | * Copyright (C) 2014 Open Source Robotics Foundation 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | * 18 | * Modifications: 19 | * 20 | * Copyright 2018 Nils Bore (nbore@kth.se) 21 | * 22 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 23 | * 24 | * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 25 | * 26 | * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 27 | * 28 | * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 29 | * 30 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | */ 33 | 34 | #include 35 | #include 36 | 37 | namespace gazebo 38 | { 39 | class RegisterUnderwaterSonarSensorPlugin : public SystemPlugin 40 | { 41 | ///////////////////////////////////////////// 42 | /// \brief Destructor 43 | public: virtual ~RegisterUnderwaterSonarSensorPlugin() 44 | { 45 | } 46 | 47 | ///////////////////////////////////////////// 48 | /// \brief Called after the plugin has been constructed. 49 | public: void Load(int /*_argc*/, char ** /*_argv*/) 50 | { 51 | RegisterUnderwaterSonarSensor(); 52 | printf("Loaded the underwater sonar sensor!\n"); 53 | } 54 | 55 | ///////////////////////////////////////////// 56 | // \brief Called once after Load 57 | private: void Init() 58 | { 59 | } 60 | 61 | }; 62 | 63 | // Register this plugin with the simulator 64 | GZ_REGISTER_SYSTEM_PLUGIN(RegisterUnderwaterSonarSensorPlugin) 65 | } 66 | -------------------------------------------------------------------------------- /smarc_gazebo_plugins/smarc_gazebo_plugins/src/SemanticMultiRayShape.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * This file was modified from the original version within Gazebo: 3 | * 4 | * Copyright (C) 2014 Open Source Robotics Foundation 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | * 18 | * Modifications: 19 | * 20 | * Copyright 2018 Nils Bore (nbore@kth.se) 21 | * 22 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 23 | * 24 | * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 25 | * 26 | * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 27 | * 28 | * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 29 | * 30 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | */ 33 | 34 | #include 35 | 36 | using namespace gazebo; 37 | using namespace physics; 38 | 39 | RayShapePtr SemanticMultiRayShape::StaticGetRay(MultiRayShapePtr _parent, unsigned int _index) 40 | { 41 | return static_cast(_parent.get())->GetRay(_index); 42 | } 43 | 44 | RayShapePtr SemanticMultiRayShape::GetRay(unsigned int _index) 45 | { 46 | return rays[_index]; 47 | } 48 | 49 | SemanticMultiRayShape::SemanticMultiRayShape(CollisionPtr _parent) : MultiRayShape(_parent) 50 | { 51 | 52 | } 53 | 54 | SemanticMultiRayShape::~SemanticMultiRayShape() 55 | { 56 | 57 | } 58 | -------------------------------------------------------------------------------- /smarc_gazebo_plugins/smarc_gazebo_plugins/src/UnderwaterSonarPlugin.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * This file was modified from the original version within Gazebo: 3 | * 4 | * Copyright (C) 2014 Open Source Robotics Foundation 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | * 18 | * Modifications: 19 | * 20 | * Copyright 2018 Nils Bore (nbore@kth.se) 21 | * 22 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 23 | * 24 | * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 25 | * 26 | * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 27 | * 28 | * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 29 | * 30 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | */ 33 | 34 | #include 35 | 36 | #include 37 | #include 38 | 39 | using namespace gazebo; 40 | 41 | // Register this plugin with the simulator 42 | GZ_REGISTER_SENSOR_PLUGIN(UnderwaterSonarPlugin) 43 | 44 | ///////////////////////////////////////////////// 45 | UnderwaterSonarPlugin::UnderwaterSonarPlugin() 46 | { 47 | } 48 | 49 | ///////////////////////////////////////////////// 50 | UnderwaterSonarPlugin::~UnderwaterSonarPlugin() 51 | { 52 | this->parentSensor->LaserShape()->DisconnectNewLaserScans( 53 | this->newLaserScansConnection); 54 | this->newLaserScansConnection.reset(); 55 | 56 | this->parentSensor.reset(); 57 | this->world.reset(); 58 | } 59 | 60 | ///////////////////////////////////////////////// 61 | void UnderwaterSonarPlugin::Load(sensors::SensorPtr _parent, sdf::ElementPtr /*_sdf*/) 62 | { 63 | // Get then name of the parent sensor 64 | this->parentSensor = 65 | std::dynamic_pointer_cast(_parent); 66 | 67 | if (!this->parentSensor) 68 | gzthrow("UnderwaterSonarPlugin requires a Ray Sensor as its parent"); 69 | 70 | this->world = physics::get_world(this->parentSensor->WorldName()); 71 | 72 | this->newLaserScansConnection = 73 | this->parentSensor->LaserShape()->ConnectNewLaserScans( 74 | std::bind(&UnderwaterSonarPlugin::OnNewLaserScans, this)); 75 | } 76 | 77 | ///////////////////////////////////////////////// 78 | void UnderwaterSonarPlugin::OnNewLaserScans() 79 | { 80 | /* overload with useful callback here */ 81 | } 82 | -------------------------------------------------------------------------------- /smarc_gazebo_plugins/smarc_gazebo_ros_plugins/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(smarc_gazebo_ros_plugins) 3 | 4 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 5 | set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} /usr/share/OGRE/cmake/modules) 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 | message_generation 12 | smarc_gazebo_plugins 13 | sensor_msgs 14 | geometry_msgs 15 | std_msgs 16 | roscpp 17 | visualization_msgs 18 | cv_bridge 19 | gazebo_plugins 20 | ) 21 | 22 | ## System dependencies are found with CMake's conventions 23 | # find_package(Boost REQUIRED COMPONENTS system) 24 | 25 | find_package(OGRE REQUIRED) 26 | find_package(gazebo REQUIRED) 27 | 28 | ## Uncomment this if the package has a setup.py. This macro ensures 29 | ## modules and global scripts declared therein get installed 30 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 31 | # catkin_python_setup() 32 | 33 | ################################################ 34 | ## Declare ROS messages, services and actions ## 35 | ################################################ 36 | 37 | ## To declare and build messages, services or actions from within this 38 | ## package, follow these steps: 39 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 40 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 41 | ## * In the file package.xml: 42 | ## * add a build_depend tag for "message_generation" 43 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 44 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 45 | ## but can be declared for certainty nonetheless: 46 | ## * add a run_depend tag for "message_runtime" 47 | ## * In this file (CMakeLists.txt): 48 | ## * add "message_generation" and every package in MSG_DEP_SET to 49 | ## find_package(catkin REQUIRED COMPONENTS ...) 50 | ## * add "message_runtime" and every package in MSG_DEP_SET to 51 | ## catkin_package(CATKIN_DEPENDS ...) 52 | ## * uncomment the add_*_files sections below as needed 53 | ## and list every .msg/.srv/.action file to be processed 54 | ## * uncomment the generate_messages entry below 55 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 56 | 57 | ## Generate messages in the 'msg' folder 58 | add_message_files( 59 | FILES 60 | SonarEntities.msg 61 | ) 62 | 63 | ## Generate services in the 'srv' folder 64 | # add_service_files( 65 | # FILES 66 | # Service1.srv 67 | # Service2.srv 68 | # ) 69 | 70 | ## Generate actions in the 'action' folder 71 | # add_action_files( 72 | # FILES 73 | # Action1.action 74 | # Action2.action 75 | # ) 76 | 77 | ## Generate added messages and services with any dependencies listed here 78 | generate_messages( 79 | DEPENDENCIES 80 | std_msgs # Or other packages containing msgs 81 | ) 82 | 83 | ################################################ 84 | ## Declare ROS dynamic reconfigure parameters ## 85 | ################################################ 86 | 87 | ## To declare and build dynamic reconfigure parameters within this 88 | ## package, follow these steps: 89 | ## * In the file package.xml: 90 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 91 | ## * In this file (CMakeLists.txt): 92 | ## * add "dynamic_reconfigure" to 93 | ## find_package(catkin REQUIRED COMPONENTS ...) 94 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 95 | ## and list every .cfg file to be processed 96 | 97 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 98 | # generate_dynamic_reconfigure_options( 99 | # cfg/DynReconf1.cfg 100 | # cfg/DynReconf2.cfg 101 | # ) 102 | 103 | ################################### 104 | ## catkin specific configuration ## 105 | ################################### 106 | ## The catkin_package macro generates cmake config files for your package 107 | ## Declare things to be passed to dependent projects 108 | ## INCLUDE_DIRS: uncomment this if your package contains header files 109 | ## LIBRARIES: libraries you create in this project that dependent projects also need 110 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 111 | ## DEPENDS: system dependencies of this project that dependent projects also need 112 | catkin_package( 113 | INCLUDE_DIRS include 114 | ${catkin_INCLUDE_DIRS} 115 | ${GAZEBO_INCLUDE_DIRS} 116 | ${GAZEBO_MSG_INCLUDE_DIRS} 117 | LIBRARIES underwater_sonar_ros_plugin underwater_sonar_ray_ros_plugin image_sonar_ros_plugin 118 | CATKIN_DEPENDS smarc_gazebo_plugins 119 | sensor_msgs 120 | geometry_msgs 121 | std_msgs 122 | # DEPENDS system_lib 123 | ) 124 | 125 | ########### 126 | ## Build ## 127 | ########### 128 | 129 | ## Specify additional locations of header files 130 | ## Your package locations should be listed before other locations 131 | include_directories( 132 | include 133 | ${catkin_INCLUDE_DIRS} 134 | ${GAZEBO_INCLUDE_DIRS} 135 | ${GAZEBO_MSG_INCLUDE_DIRS} 136 | ${OGRE_INCLUDE_DIRS} 137 | ) 138 | 139 | link_directories( 140 | ${GAZEBO_LIBRARY_DIRS} 141 | ) 142 | 143 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}") 144 | 145 | ## Declare a C++ library 146 | add_library(underwater_sonar_ros_plugin src/gazebo_ros_underwater_sonar.cpp) 147 | add_library(underwater_sonar_ray_ros_plugin src/gazebo_ros_underwater_sonar_ray.cpp) 148 | add_library(image_sonar_ros_plugin src/gazebo_ros_image_sonar.cpp) 149 | 150 | ## Add cmake target dependencies of the library 151 | ## as an example, code may need to be generated before libraries 152 | ## either from message generation or dynamic reconfigure 153 | add_dependencies(underwater_sonar_ros_plugin ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 154 | add_dependencies(underwater_sonar_ray_ros_plugin ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 155 | add_dependencies(image_sonar_ros_plugin ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 156 | 157 | ## Declare a C++ executable 158 | ## With catkin_make all packages are built within a single CMake context 159 | ## The recommended prefix ensures that target names across packages don't collide 160 | # add_executable(${PROJECT_NAME}_node src/smarc_gazebo_ros_plugins_node.cpp) 161 | 162 | ## Rename C++ executable without prefix 163 | ## The above recommended prefix causes long target names, the following renames the 164 | ## target back to the shorter version for ease of user use 165 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 166 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 167 | 168 | ## Add cmake target dependencies of the executable 169 | ## same as for the library above 170 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 171 | 172 | ## Specify libraries to link a library or executable target against 173 | target_link_libraries(underwater_sonar_ros_plugin 174 | ${catkin_LIBRARIES} 175 | ) 176 | 177 | target_link_libraries(underwater_sonar_ray_ros_plugin 178 | ${catkin_LIBRARIES} 179 | ) 180 | 181 | target_link_libraries(image_sonar_ros_plugin 182 | ${catkin_LIBRARIES} 183 | ${GAZEBO_LIBRARIES} 184 | ${OGRE_LIBRARIES} 185 | ) 186 | 187 | ############# 188 | ## Install ## 189 | ############# 190 | 191 | # all install targets should use catkin DESTINATION variables 192 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 193 | 194 | ## Mark executable scripts (Python etc.) for installation 195 | ## in contrast to setup.py, you can choose the destination 196 | # install(PROGRAMS 197 | # scripts/my_python_script 198 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 199 | # ) 200 | 201 | ## Mark executables and/or libraries for installation 202 | install(TARGETS underwater_sonar_ros_plugin underwater_sonar_ray_ros_plugin image_sonar_ros_plugin 203 | DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 204 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 205 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 206 | ) 207 | 208 | ## Mark cpp header files for installation 209 | # install(DIRECTORY include/${PROJECT_NAME}/ 210 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 211 | # FILES_MATCHING PATTERN "*.h" 212 | # PATTERN ".svn" EXCLUDE 213 | # ) 214 | 215 | ## Mark other files for installation (e.g. launch and bag files, etc.) 216 | # install(FILES 217 | # # myfile1 218 | # # myfile2 219 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 220 | # ) 221 | 222 | ############# 223 | ## Testing ## 224 | ############# 225 | 226 | ## Add gtest based cpp test target and link libraries 227 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_smarc_gazebo_ros_plugins.cpp) 228 | # if(TARGET ${PROJECT_NAME}-test) 229 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 230 | # endif() 231 | 232 | ## Add folders to be run by python nosetests 233 | # catkin_add_nosetests(test) 234 | -------------------------------------------------------------------------------- /smarc_gazebo_plugins/smarc_gazebo_ros_plugins/include/smarc_gazebo_ros_plugins/gazebo_ros_image_sonar.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file was modified from the original version within Gazebo: 3 | * 4 | * Copyright (C) 2014 Open Source Robotics Foundation 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | * 18 | * Modifications: 19 | * 20 | * Copyright 2018 Nils Bore (nbore@kth.se) 21 | * 22 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 23 | * 24 | * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 25 | * 26 | * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 27 | * 28 | * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 29 | * 30 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | */ 33 | 34 | #ifndef GAZEBO_ROS_DEPTH_CAMERA_HH 35 | #define GAZEBO_ROS_DEPTH_CAMERA_HH 36 | 37 | // ros stuff 38 | #include 39 | #include 40 | #include 41 | 42 | // ros messages stuff 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | 50 | // gazebo stuff 51 | #include 52 | #include 53 | #include 54 | #include 55 | #include 56 | #include 57 | #include 58 | 59 | // dynamic reconfigure stuff 60 | #include 61 | #include 62 | 63 | // boost stuff 64 | #include 65 | 66 | // camera stuff 67 | #include 68 | 69 | #include 70 | 71 | namespace gazebo 72 | { 73 | class GazeboRosImageSonar : public SensorPlugin, GazeboRosCameraUtils 74 | { 75 | /// \brief Constructor 76 | /// \param parent The parent entity, must be a Model or a Sensor 77 | public: GazeboRosImageSonar(); 78 | 79 | /// \brief Destructor 80 | public: ~GazeboRosImageSonar(); 81 | 82 | /// \brief Load the plugin 83 | /// \param take in SDF root element 84 | public: virtual void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf); 85 | 86 | /// \brief Advertise point cloud and depth image 87 | public: virtual void Advertise(); 88 | 89 | /// \brief Update the controller 90 | protected: virtual void OnNewDepthFrame(const float *_image, 91 | unsigned int _width, unsigned int _height, 92 | unsigned int _depth, const std::string &_format); 93 | 94 | /// \brief Update the controller 95 | protected: virtual void OnNewRGBPointCloud(const float *_pcd, 96 | unsigned int _width, unsigned int _height, 97 | unsigned int _depth, const std::string &_format); 98 | 99 | /// \brief Update the controller 100 | protected: virtual void OnNewImageFrame(const unsigned char *_image, 101 | unsigned int _width, unsigned int _height, 102 | unsigned int _depth, const std::string &_format); 103 | 104 | /// \brief Put camera data to the ROS topic 105 | private: void FillPointdCloud(const float *_src); 106 | 107 | /// \brief push depth image data into ros topic 108 | private: void FillDepthImage(const float *_src); 109 | 110 | /// \brief push depth image data into ros topic 111 | private: void ComputeSonarImage(const float *_src); 112 | private: cv::Mat ComputeNormalImage(cv::Mat& depth); 113 | private: cv::Mat ConstructSonarImage(cv::Mat& depth, cv::Mat& normals); 114 | private: cv::Mat ConstructScanImage(cv::Mat& depth, cv::Mat& SNR); 115 | private: void ApplySpeckleNoise(cv::Mat& scan, float fov); 116 | private: void ApplySmoothing(cv::Mat& scan, float fov); 117 | private: void ApplyMedianFilter(cv::Mat& scan); 118 | private: cv::Mat ConstructVisualScanImage(cv::Mat& raw_scan); 119 | 120 | /// \brief Keep track of number of connctions for point clouds 121 | private: int point_cloud_connect_count_; 122 | private: void PointCloudConnect(); 123 | private: void PointCloudDisconnect(); 124 | 125 | /// \brief Keep track of number of connctions for point clouds 126 | private: int depth_image_connect_count_; 127 | private: void DepthImageConnect(); 128 | private: void DepthImageDisconnect(); 129 | private: void NormalImageConnect(); 130 | private: void NormalImageDisconnect(); 131 | private: void MultibeamImageConnect(); 132 | private: void MultibeamImageDisconnect(); 133 | private: void SonarImageConnect(); 134 | private: void SonarImageDisconnect(); 135 | private: void RawSonarImageConnect(); 136 | private: void RawSonarImageDisconnect(); 137 | private: common::Time last_depth_image_camera_info_update_time_; 138 | 139 | private: bool FillPointCloudHelper(sensor_msgs::PointCloud2 &point_cloud_msg, 140 | uint32_t rows_arg, uint32_t cols_arg, 141 | uint32_t step_arg, void* data_arg); 142 | 143 | private: bool FillDepthImageHelper( sensor_msgs::Image& image_msg, 144 | uint32_t rows_arg, uint32_t cols_arg, 145 | uint32_t step_arg, void* data_arg); 146 | 147 | /// \brief A pointer to the ROS node. A node will be instantiated if it does not exist. 148 | private: ros::Publisher point_cloud_pub_; 149 | private: ros::Publisher depth_image_pub_; 150 | private: ros::Publisher normal_image_pub_; 151 | private: ros::Publisher multibeam_image_pub_; 152 | private: ros::Publisher sonar_image_pub_; 153 | private: ros::Publisher raw_sonar_image_pub_; 154 | 155 | /// \brief PointCloud2 point cloud message 156 | private: sensor_msgs::PointCloud2 point_cloud_msg_; 157 | private: sensor_msgs::Image depth_image_msg_; 158 | private: sensor_msgs::Image normal_image_msg_; 159 | private: sensor_msgs::Image multibeam_image_msg_; 160 | private: sensor_msgs::Image sonar_image_msg_; 161 | private: sensor_msgs::Image raw_sonar_image_msg_; 162 | 163 | private: double point_cloud_cutoff_; 164 | 165 | /// \brief ROS image topic name 166 | private: std::string point_cloud_topic_name_; 167 | std::default_random_engine generator; 168 | 169 | private: void InfoConnect(); 170 | private: void InfoDisconnect(); 171 | 172 | using GazeboRosCameraUtils::PublishCameraInfo; 173 | protected: virtual void PublishCameraInfo(); 174 | 175 | /// \brief image where each pixel contains the depth information 176 | private: std::string depth_image_topic_name_; 177 | private: std::string depth_image_camera_info_topic_name_; 178 | private: int depth_info_connect_count_; 179 | private: void DepthInfoConnect(); 180 | private: void DepthInfoDisconnect(); 181 | 182 | // overload with our own 183 | private: common::Time depth_sensor_update_time_; 184 | protected: ros::Publisher depth_image_camera_info_pub_; 185 | 186 | private: event::ConnectionPtr load_connection_; 187 | 188 | // from DepthCameraPlugin 189 | protected: unsigned int width, height, depth; 190 | protected: std::string format; 191 | 192 | // precomputed things for the forward-looking sonar 193 | protected: cv::Mat dist_matrix_; 194 | std::vector > angle_range_indices_; 195 | std::vector angle_nbr_indices_; 196 | 197 | protected: sensors::DepthCameraSensorPtr parentSensor; 198 | protected: rendering::DepthCameraPtr depthCamera; 199 | 200 | private: event::ConnectionPtr newDepthFrameConnection; 201 | private: event::ConnectionPtr newRGBPointCloudConnection; 202 | private: event::ConnectionPtr newImageFrameConnection; 203 | }; 204 | 205 | } 206 | #endif 207 | 208 | -------------------------------------------------------------------------------- /smarc_gazebo_plugins/smarc_gazebo_ros_plugins/include/smarc_gazebo_ros_plugins/gazebo_ros_underwater_sonar.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2012 Open Source Robotics Foundation 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 | 18 | #ifndef GAZEBO_UNDERWATER_SONAR_HH 19 | #define GAZEBO_UNDERWATER_SONAR_HH 20 | 21 | #include 22 | 23 | #include 24 | #include 25 | 26 | #include 27 | #include 28 | #include 29 | 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | #include 43 | #include 44 | 45 | namespace gazebo 46 | { 47 | class GazeboUnderwaterSonar : public UnderwaterSonarPlugin 48 | { 49 | /// \brief Constructor 50 | public: GazeboUnderwaterSonar(); 51 | 52 | /// \brief Destructor 53 | public: ~GazeboUnderwaterSonar(); 54 | 55 | /// \brief Load the plugin 56 | /// \param take in SDF root element 57 | public: void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf); 58 | 59 | /// \brief Keep track of number of connctions 60 | private: int laser_connect_count_; 61 | private: int entities_connect_count_; 62 | private: void LaserConnect(); 63 | private: void LaserDisconnect(); 64 | private: void EntitiesConnect(); 65 | private: void EntitiesDisconnect(); 66 | 67 | // Pointer to the model 68 | GazeboRosPtr gazebo_ros_; 69 | private: std::string world_name_; 70 | private: physics::WorldPtr world_; 71 | /// \brief The parent sensor 72 | private: std::shared_ptr parent_ray_sensor_; 73 | 74 | /// \brief pointer to ros node 75 | private: ros::NodeHandle* rosnode_; 76 | private: ros::Publisher pub_; 77 | private: ros::Publisher entities_pub_; 78 | private: PubQueue::Ptr pub_queue_; 79 | private: PubQueue::Ptr entities_pub_queue_; 80 | 81 | /// \brief topic name 82 | private: std::string topic_name_; 83 | 84 | /// \brief frame transform name, should match link name 85 | private: std::string frame_name_; 86 | 87 | /// \brief tf prefix 88 | private: std::string tf_prefix_; 89 | 90 | /// \brief for setting ROS name space 91 | private: std::string robot_namespace_; 92 | 93 | // deferred load in case ros is blocking 94 | private: sdf::ElementPtr sdf; 95 | private: void LoadThread(); 96 | private: boost::thread deferred_load_thread_; 97 | private: unsigned int seed; 98 | 99 | private: gazebo::transport::NodePtr gazebo_node_; 100 | private: gazebo::transport::SubscriberPtr laser_scan_sub_; 101 | private: gazebo::transport::SubscriberPtr entities_sub_; 102 | private: void OnScan(ConstLaserScanStampedPtr &_msg); 103 | private: void OnEntities(ConstGzString_VPtr &_msg); 104 | 105 | /// \brief prevents blocking 106 | private: PubMultiQueue pmq; 107 | }; 108 | } 109 | #endif // GAZEBO_UNDERWATER_SONAR_HH 110 | -------------------------------------------------------------------------------- /smarc_gazebo_plugins/smarc_gazebo_ros_plugins/include/smarc_gazebo_ros_plugins/gazebo_ros_underwater_sonar_ray.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file was modified from the original version within Gazebo: 3 | * 4 | * Copyright (C) 2014 Open Source Robotics Foundation 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | * 18 | * Modifications: 19 | * 20 | * Copyright 2018 Nils Bore (nbore@kth.se) 21 | * 22 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 23 | * 24 | * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 25 | * 26 | * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 27 | * 28 | * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 29 | * 30 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | */ 33 | 34 | #ifndef GAZEBO_UNDERWATER_SONAR_RAY_HH 35 | #define GAZEBO_UNDERWATER_SONAR_RAY_HH 36 | 37 | #include 38 | 39 | #include 40 | #include 41 | 42 | #include 43 | #include 44 | #include 45 | 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | #include 53 | #include 54 | #include 55 | #include 56 | #include 57 | 58 | #include 59 | 60 | namespace gazebo 61 | { 62 | class GazeboUnderwaterSonarRay : public RayPlugin 63 | { 64 | /// \brief Constructor 65 | public: GazeboUnderwaterSonarRay(); 66 | 67 | /// \brief Destructor 68 | public: ~GazeboUnderwaterSonarRay(); 69 | 70 | /// \brief Load the plugin 71 | /// \param take in SDF root element 72 | public: void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf); 73 | 74 | /// \brief Keep track of number of connctions 75 | private: int laser_connect_count_; 76 | private: void LaserConnect(); 77 | private: void LaserDisconnect(); 78 | 79 | // Pointer to the model 80 | GazeboRosPtr gazebo_ros_; 81 | private: std::string world_name_; 82 | private: physics::WorldPtr world_; 83 | /// \brief The parent sensor 84 | private: std::shared_ptr parent_ray_sensor_; 85 | 86 | /// \brief pointer to ros node 87 | private: ros::NodeHandle* rosnode_; 88 | private: ros::Publisher pub_; 89 | private: PubQueue::Ptr pub_queue_; 90 | 91 | /// \brief topic name 92 | private: std::string topic_name_; 93 | 94 | /// \brief frame transform name, should match link name 95 | private: std::string frame_name_; 96 | 97 | /// \brief tf prefix 98 | private: std::string tf_prefix_; 99 | 100 | /// \brief for setting ROS name space 101 | private: std::string robot_namespace_; 102 | 103 | // deferred load in case ros is blocking 104 | private: sdf::ElementPtr sdf; 105 | private: void LoadThread(); 106 | private: boost::thread deferred_load_thread_; 107 | private: unsigned int seed; 108 | 109 | private: gazebo::transport::NodePtr gazebo_node_; 110 | private: gazebo::transport::SubscriberPtr laser_scan_sub_; 111 | private: void OnScan(ConstLaserScanStampedPtr &_msg); 112 | 113 | /// \brief prevents blocking 114 | private: PubMultiQueue pmq; 115 | }; 116 | } 117 | #endif // GAZEBO_UNDERWATER_SONAR_RAY_HH 118 | -------------------------------------------------------------------------------- /smarc_gazebo_plugins/smarc_gazebo_ros_plugins/msg/SonarEntities.msg: -------------------------------------------------------------------------------- 1 | string[] entities 2 | -------------------------------------------------------------------------------- /smarc_gazebo_plugins/smarc_gazebo_ros_plugins/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | smarc_gazebo_ros_plugins 4 | 0.0.0 5 | The smarc_gazebo_ros_plugins package 6 | 7 | 8 | 9 | 10 | Nils Bore 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 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | smarc_gazebo_plugins 53 | sensor_msgs 54 | geometry_msgs 55 | std_msgs 56 | roscpp 57 | visualization_msgs 58 | smarc_gazebo_plugins 59 | sensor_msgs 60 | geometry_msgs 61 | std_msgs 62 | roscpp 63 | visualization_msgs 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | -------------------------------------------------------------------------------- /smarc_sensor_plugins/smarc_sensor_plugins_ros/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(smarc_sensor_plugins_ros) 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 sensor_msgs smarc_gazebo_ros_plugins cv_bridge message_filters tf) 11 | 12 | ## System dependencies are found with CMake's conventions 13 | # find_package(Boost REQUIRED COMPONENTS system) 14 | 15 | 16 | ## Uncomment this if the package has a setup.py. This macro ensures 17 | ## modules and global scripts declared therein get installed 18 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 19 | # catkin_python_setup() 20 | 21 | ################################################ 22 | ## Declare ROS messages, services and actions ## 23 | ################################################ 24 | 25 | ## To declare and build messages, services or actions from within this 26 | ## package, follow these steps: 27 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 28 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 29 | ## * In the file package.xml: 30 | ## * add a build_depend tag for "message_generation" 31 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 32 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 33 | ## but can be declared for certainty nonetheless: 34 | ## * add a run_depend tag for "message_runtime" 35 | ## * In this file (CMakeLists.txt): 36 | ## * add "message_generation" and every package in MSG_DEP_SET to 37 | ## find_package(catkin REQUIRED COMPONENTS ...) 38 | ## * add "message_runtime" and every package in MSG_DEP_SET to 39 | ## catkin_package(CATKIN_DEPENDS ...) 40 | ## * uncomment the add_*_files sections below as needed 41 | ## and list every .msg/.srv/.action file to be processed 42 | ## * uncomment the generate_messages entry below 43 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 44 | 45 | ## Generate messages in the 'msg' folder 46 | # add_message_files( 47 | # FILES 48 | # Message1.msg 49 | # Message2.msg 50 | # ) 51 | 52 | ## Generate services in the 'srv' folder 53 | # add_service_files( 54 | # FILES 55 | # Service1.srv 56 | # Service2.srv 57 | # ) 58 | 59 | ## Generate actions in the 'action' folder 60 | # add_action_files( 61 | # FILES 62 | # Action1.action 63 | # Action2.action 64 | # ) 65 | 66 | ## Generate added messages and services with any dependencies listed here 67 | # generate_messages( 68 | # DEPENDENCIES 69 | # std_msgs # Or other packages containing msgs 70 | # ) 71 | 72 | ################################################ 73 | ## Declare ROS dynamic reconfigure parameters ## 74 | ################################################ 75 | 76 | ## To declare and build dynamic reconfigure parameters within this 77 | ## package, follow these steps: 78 | ## * In the file package.xml: 79 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 80 | ## * In this file (CMakeLists.txt): 81 | ## * add "dynamic_reconfigure" to 82 | ## find_package(catkin REQUIRED COMPONENTS ...) 83 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 84 | ## and list every .cfg file to be processed 85 | 86 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 87 | # generate_dynamic_reconfigure_options( 88 | # cfg/DynReconf1.cfg 89 | # cfg/DynReconf2.cfg 90 | # ) 91 | 92 | ################################### 93 | ## catkin specific configuration ## 94 | ################################### 95 | ## The catkin_package macro generates cmake config files for your package 96 | ## Declare things to be passed to dependent projects 97 | ## INCLUDE_DIRS: uncomment this if you package contains header files 98 | ## LIBRARIES: libraries you create in this project that dependent projects also need 99 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 100 | ## DEPENDS: system dependencies of this project that dependent projects also need 101 | catkin_package( 102 | # INCLUDE_DIRS include 103 | # LIBRARIES smarc_sensor_plugins_ros 104 | # CATKIN_DEPENDS other_catkin_pkg 105 | # DEPENDS system_lib 106 | ) 107 | 108 | ########### 109 | ## Build ## 110 | ########### 111 | 112 | ## Specify additional locations of header files 113 | ## Your package locations should be listed before other locations 114 | include_directories( 115 | # include 116 | ${catkin_INCLUDE_DIRS} 117 | ) 118 | 119 | ## Declare a C++ library 120 | # add_library(${PROJECT_NAME} 121 | # src/${PROJECT_NAME}/smarc_sensor_plugins_ros.cpp 122 | # ) 123 | 124 | ## Add cmake target dependencies of the library 125 | ## as an example, code may need to be generated before libraries 126 | ## either from message generation or dynamic reconfigure 127 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 128 | 129 | ## Declare a C++ executable 130 | ## With catkin_make all packages are built within a single CMake context 131 | ## The recommended prefix ensures that target names across packages don't collide 132 | add_executable(sidescan_waterfall_image_node src/sidescan_waterfall_image_node.cpp) 133 | add_executable(combine_waterfalls_node src/combine_waterfalls_node.cpp) 134 | 135 | ## Rename C++ executable without prefix 136 | ## The above recommended prefix causes long target names, the following renames the 137 | ## target back to the shorter version for ease of user use 138 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 139 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 140 | 141 | ## Add cmake target dependencies of the executable 142 | ## same as for the library above 143 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 144 | 145 | ## Specify libraries to link a library or executable target against 146 | target_link_libraries(sidescan_waterfall_image_node 147 | ${catkin_LIBRARIES} 148 | ) 149 | 150 | target_link_libraries(combine_waterfalls_node 151 | ${catkin_LIBRARIES} 152 | ) 153 | 154 | ############# 155 | ## Install ## 156 | ############# 157 | 158 | # all install targets should use catkin DESTINATION variables 159 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 160 | 161 | ## Mark executable scripts (Python etc.) for installation 162 | ## in contrast to setup.py, you can choose the destination 163 | # install(PROGRAMS 164 | # scripts/my_python_script 165 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 166 | # ) 167 | 168 | ## Mark executables and/or libraries for installation 169 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 170 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 171 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 172 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 173 | # ) 174 | 175 | ## Mark cpp header files for installation 176 | # install(DIRECTORY include/${PROJECT_NAME}/ 177 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 178 | # FILES_MATCHING PATTERN "*.h" 179 | # PATTERN ".svn" EXCLUDE 180 | # ) 181 | 182 | ## Mark other files for installation (e.g. launch and bag files, etc.) 183 | # install(FILES 184 | # # myfile1 185 | # # myfile2 186 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 187 | # ) 188 | 189 | ############# 190 | ## Testing ## 191 | ############# 192 | 193 | ## Add gtest based cpp test target and link libraries 194 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_smarc_sensor_plugins_ros.cpp) 195 | # if(TARGET ${PROJECT_NAME}-test) 196 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 197 | # endif() 198 | 199 | ## Add folders to be run by python nosetests 200 | # catkin_add_nosetests(test) 201 | -------------------------------------------------------------------------------- /smarc_sensor_plugins/smarc_sensor_plugins_ros/launch/waterfall.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /smarc_sensor_plugins/smarc_sensor_plugins_ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | smarc_sensor_plugins_ros 4 | 0.0.0 5 | The smarc_sensor_plugins_ros package 6 | 7 | 8 | 9 | 10 | nbore 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | smarc_gazebo_ros_plugins 44 | cv_bridge 45 | smarc_gazebo_ros_plugins 46 | cv_bridge 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | -------------------------------------------------------------------------------- /smarc_sensor_plugins/smarc_sensor_plugins_ros/src/combine_waterfalls_node.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright 2018 Nils Bore (nbore@kth.se) 2 | * 3 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 4 | * 5 | * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 6 | * 7 | * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 8 | * 9 | * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 10 | * 11 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 12 | */ 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | #include 26 | #include 27 | #include 28 | 29 | using namespace std; 30 | 31 | class CombineWaterfallImageNode { 32 | 33 | public: 34 | 35 | typedef message_filters::sync_policies::ApproximateTime MySyncPolicy; 36 | 37 | ros::NodeHandle n; 38 | ros::Publisher waterfall_pub; 39 | message_filters::Subscriber* waterfall_left_sub; 40 | message_filters::Subscriber* waterfall_right_sub; 41 | message_filters::Synchronizer* sync; 42 | 43 | string auv_namespace; 44 | 45 | CombineWaterfallImageNode() 46 | { 47 | ros::NodeHandle pn("~"); 48 | string auv_name; 49 | pn.param("auv_name", auv_name, "small_smarc_auv"); 50 | auv_namespace = string("/") + auv_name; 51 | 52 | waterfall_left_sub = new message_filters::Subscriber(n, auv_namespace+"/sss_left_waterfall", 1); 53 | waterfall_right_sub = new message_filters::Subscriber(n, auv_namespace+"/sss_right_waterfall", 1); 54 | 55 | // ApproximateTime takes a queue size as its constructor argument, hence MySyncPolicy(10) 56 | sync = new message_filters::Synchronizer(MySyncPolicy(10), *waterfall_left_sub, *waterfall_right_sub); 57 | sync->registerCallback(boost::bind(&CombineWaterfallImageNode::callback, this, _1, _2)); 58 | 59 | waterfall_pub = n.advertise(auv_namespace+"/waterfall", 1); 60 | } 61 | 62 | void callback(const sensor_msgs::Image::ConstPtr& image_left, const sensor_msgs::Image::ConstPtr& image_right) 63 | { 64 | cv_bridge::CvImagePtr cv_left_ptr; 65 | try { 66 | cv_left_ptr = cv_bridge::toCvCopy(image_left, sensor_msgs::image_encodings::MONO8); 67 | } 68 | catch (cv_bridge::Exception& e) { 69 | ROS_ERROR("cv_bridge exception: %s", e.what()); 70 | return; 71 | } 72 | 73 | cv_bridge::CvImagePtr cv_right_ptr; 74 | try { 75 | cv_right_ptr = cv_bridge::toCvCopy(image_right, sensor_msgs::image_encodings::MONO8); 76 | } 77 | catch (cv_bridge::Exception& e) { 78 | ROS_ERROR("cv_bridge exception: %s", e.what()); 79 | return; 80 | } 81 | 82 | cv::Mat image = cv_left_ptr->image + cv_right_ptr->image; 83 | 84 | sensor_msgs::Image img_msg; // >> message to be sent 85 | std_msgs::Header header; // empty header 86 | //header.seq = counter; // user defined counter 87 | header.stamp = ros::Time::now(); // time 88 | cv_bridge::CvImage img_bridge; 89 | img_bridge = cv_bridge::CvImage(header, sensor_msgs::image_encodings::MONO8, image); 90 | img_bridge.toImageMsg(img_msg); // from cv_bridge to sensor_msgs::Image 91 | 92 | waterfall_pub.publish(img_msg); // ros::Publisher pub_img = node.advertise("topic", queuesize); 93 | } 94 | 95 | }; 96 | 97 | int main(int argc, char** argv) 98 | { 99 | ros::init(argc, argv, "combine_waterfalls_node"); 100 | 101 | CombineWaterfallImageNode node; 102 | 103 | ros::spin(); 104 | 105 | return 0; 106 | } 107 | -------------------------------------------------------------------------------- /smarc_sensor_plugins/smarc_sensor_plugins_ros/src/sidescan_waterfall_image_node.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright 2018 Nils Bore (nbore@kth.se) 2 | * 3 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 4 | * 5 | * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 6 | * 7 | * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 8 | * 9 | * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 10 | * 11 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 12 | */ 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | using namespace std; 28 | 29 | class WaterfallImageNode { 30 | 31 | public: 32 | 33 | ros::NodeHandle n; 34 | ros::Subscriber sonar_sub; 35 | ros::Subscriber entity_sub; 36 | ros::Publisher waterfall_pub; 37 | tf::TransformListener listener; 38 | 39 | string auv_namespace; 40 | string auv_frame; 41 | cv::Mat waterfall_image; 42 | int timesteps; 43 | double start_angle; 44 | double fov; 45 | int rays; 46 | int samples; 47 | int offset; 48 | int width; 49 | bool angle_unknown; 50 | bool is_left; 51 | 52 | std::default_random_engine generator; 53 | 54 | //Eigen::VectorXi sample_indices; 55 | //Eigen::Vector3d last_pos; 56 | cv::Point3f last_pos; 57 | 58 | //EIGEN_MAKE_ALIGNED_OPERATOR_NEW 59 | 60 | WaterfallImageNode() 61 | { 62 | ros::NodeHandle pn("~"); 63 | string auv_name; 64 | pn.param("auv_name", auv_name, "small_smarc_auv"); 65 | auv_namespace = string("/") + auv_name; 66 | auv_frame = auv_name+"/base_link"; 67 | pn.param("timesteps", timesteps, 200); 68 | pn.param("start_angle", start_angle, 0.0); 69 | pn.param("fov", fov, 60.0); 70 | fov *= M_PI/180.0; 71 | pn.param("rays", rays, 120); 72 | pn.param("samples", samples, 60); 73 | rays -= 1; 74 | pn.param("angle_unknown", angle_unknown, true); 75 | pn.param("is_left", is_left, true); 76 | string side_name; 77 | if (is_left) { 78 | side_name = "left"; 79 | } 80 | else { 81 | side_name = "right"; 82 | } 83 | 84 | offset = 5; 85 | width = 2*samples + 2*width; 86 | waterfall_image = cv::Mat::zeros(timesteps, width, CV_8UC1); 87 | 88 | ROS_INFO("Waiting for transform..."); 89 | try { 90 | listener.waitForTransform("/world", auv_frame, 91 | ros::Time(0), ros::Duration(60.0)); 92 | } 93 | catch (tf::TransformException ex) { 94 | ROS_ERROR("%s",ex.what()); 95 | ros::Duration(1.0).sleep(); 96 | } 97 | ROS_INFO("Got transform..."); 98 | 99 | waterfall_pub = n.advertise(auv_namespace+"/sss_"+side_name+"_waterfall", 1); 100 | sonar_sub = n.subscribe(auv_namespace+"/sss_"+side_name, 10, &WaterfallImageNode::sonar_callback, this); 101 | entity_sub = n.subscribe(auv_namespace+"/sss_"+side_name+"_entities", 10, &WaterfallImageNode::entities_callback, this); 102 | } 103 | 104 | void sonar_callback(const sensor_msgs::LaserScan::ConstPtr& scan) 105 | { 106 | tf::StampedTransform transform; 107 | try { 108 | listener.lookupTransform("/world", auv_frame, 109 | ros::Time(0), transform); 110 | } 111 | catch (tf::TransformException ex) { 112 | ROS_ERROR("%s",ex.what()); 113 | ros::Duration(1.0).sleep(); 114 | } 115 | 116 | auto min_range_iter = std::min_element(scan->ranges.begin(), scan->ranges.end()); 117 | double depth = double(*min_range_iter); 118 | cv::Point3f current_pos(transform.getOrigin().x(), transform.getOrigin().y(), transform.getOrigin().z()); 119 | double pixel_distance = tan(fov)*depth/double(samples); 120 | 121 | if (cv::norm(current_pos - last_pos) < pixel_distance) { 122 | return; 123 | } 124 | last_pos = current_pos; 125 | 126 | auto max_intensity_iter = std::max_element(scan->intensities.begin(), scan->intensities.end()); 127 | auto max_range_iter = std::max_element(scan->ranges.begin(), scan->ranges.end()); 128 | double min_range = sin(0.0)*scan->ranges[0]; 129 | double max_range = sin(fov)*(*max_range_iter) - min_range; 130 | 131 | 132 | size_t max_index = std::distance(scan->intensities.begin(), max_intensity_iter); 133 | 134 | double scale = 1.0; //255.0/max_index; 135 | std::normal_distribution noise_distribution(0.0, 10.0); 136 | 137 | // now we should move everything now one notch 138 | // let's just do the left for now, maybe synch later 139 | cv::Mat shifted = cv::Mat::zeros(timesteps, width, waterfall_image.type()); 140 | waterfall_image(cv::Rect(0, 0, waterfall_image.cols, waterfall_image.rows-1)).copyTo(shifted(cv::Rect(0, 1, shifted.cols, shifted.rows-1))); 141 | shifted.copyTo(waterfall_image); 142 | 143 | std::normal_distribution shift_distribution(0.0, 1.0); 144 | int noise_shift = int(shift_distribution(generator)); 145 | 146 | //if (angle_unknown) { 147 | int ray = 0; 148 | for (int i = 0; i < samples && ray < rays; ) { 149 | int index = is_left? width/2-offset-1-i : width/2+offset+i; 150 | index += noise_shift; 151 | if ((sin(fov*ray/rays)*scan->ranges[ray] - min_range) > max_range*i/samples) { 152 | ++i; 153 | } 154 | else { 155 | double value = scale*scan->intensities[ray] + noise_distribution(generator); 156 | if (index >= 0 && index < waterfall_image.cols) { 157 | waterfall_image.at(0, index) = uchar(std::max(0.0, std::min(255.0, value))); 158 | } 159 | ++ray; 160 | } 161 | } 162 | 163 | sensor_msgs::Image img_msg; // >> message to be sent 164 | 165 | std_msgs::Header header; // empty header 166 | //header.seq = counter; // user defined counter 167 | header.stamp = ros::Time::now(); // time 168 | cv_bridge::CvImage img_bridge; 169 | img_bridge = cv_bridge::CvImage(header, sensor_msgs::image_encodings::MONO8, waterfall_image); 170 | img_bridge.toImageMsg(img_msg); // from cv_bridge to sensor_msgs::Image 171 | 172 | waterfall_pub.publish(img_msg); // ros::Publisher pub_img = node.advertise("topic", queuesize); 173 | } 174 | 175 | void entities_callback(const smarc_gazebo_ros_plugins::SonarEntities::ConstPtr& msg) 176 | { 177 | 178 | } 179 | 180 | }; 181 | 182 | int main(int argc, char** argv) 183 | { 184 | ros::init(argc, argv, "sidescan_waterfall_image_node"); 185 | 186 | WaterfallImageNode node; 187 | 188 | ros::spin(); 189 | 190 | return 0; 191 | } 192 | -------------------------------------------------------------------------------- /smarc_sensor_plugins/smarc_sensor_plugins_ros/urdf/sonar_snippets.xacro: -------------------------------------------------------------------------------- 1 | 2 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 0 0 0 0 0 0 75 | false 76 | true 77 | ${update_rate} 78 | 79 | 80 | 81 | ${samples} 82 | 1 83 | -${0.5*fov} 84 | ${0.5*fov} 85 | 86 | 87 | 0.5 88 | 2 89 | -${0.5*fov/samples} 90 | ${0.5*fov/samples} 91 | 92 | 93 | 94 | ${range_min} 95 | ${range_max} 96 | 0.01 97 | 98 | 99 | gaussian 100 | 0.0 101 | ${range_stddev} 102 | 103 | 104 | 105 | ${topic} 106 | mbes_sonar${suffix}_link 107 | 108 | 109 | 114 | 115 | 116 | 117 | 118 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | ${fov} 151 | 152 | ${width} 153 | ${height} 154 | R8G8B8 155 | 156 | 157 | 0.1 158 | 17 159 | 160 | 161 | /tmp/camera 162 | 163 | 164 | 165 | ${topic} 166 | forward_sonar${suffix}_optical_frame 167 | 168 | true 169 | ${update_rate} 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | -------------------------------------------------------------------------------- /smarc_simulated_comms/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(smarc_simulated_comms) 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) 11 | 12 | ## System dependencies are found with CMake's conventions 13 | # find_package(Boost REQUIRED COMPONENTS system) 14 | 15 | 16 | ## Uncomment this if the package has a setup.py. This macro ensures 17 | ## modules and global scripts declared therein get installed 18 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 19 | # catkin_python_setup() 20 | 21 | ################################################ 22 | ## Declare ROS messages, services and actions ## 23 | ################################################ 24 | 25 | ## To declare and build messages, services or actions from within this 26 | ## package, follow these steps: 27 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 28 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 29 | ## * In the file package.xml: 30 | ## * add a build_depend tag for "message_generation" 31 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 32 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 33 | ## but can be declared for certainty nonetheless: 34 | ## * add a run_depend tag for "message_runtime" 35 | ## * In this file (CMakeLists.txt): 36 | ## * add "message_generation" and every package in MSG_DEP_SET to 37 | ## find_package(catkin REQUIRED COMPONENTS ...) 38 | ## * add "message_runtime" and every package in MSG_DEP_SET to 39 | ## catkin_package(CATKIN_DEPENDS ...) 40 | ## * uncomment the add_*_files sections below as needed 41 | ## and list every .msg/.srv/.action file to be processed 42 | ## * uncomment the generate_messages entry below 43 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 44 | 45 | ## Generate messages in the 'msg' folder 46 | # add_message_files( 47 | # FILES 48 | # Message1.msg 49 | # Message2.msg 50 | # ) 51 | 52 | ## Generate services in the 'srv' folder 53 | # add_service_files( 54 | # FILES 55 | # Service1.srv 56 | # Service2.srv 57 | # ) 58 | 59 | ## Generate actions in the 'action' folder 60 | # add_action_files( 61 | # FILES 62 | # Action1.action 63 | # Action2.action 64 | # ) 65 | 66 | ## Generate added messages and services with any dependencies listed here 67 | # generate_messages( 68 | # DEPENDENCIES 69 | # std_msgs # Or other packages containing msgs 70 | # ) 71 | 72 | ################################################ 73 | ## Declare ROS dynamic reconfigure parameters ## 74 | ################################################ 75 | 76 | ## To declare and build dynamic reconfigure parameters within this 77 | ## package, follow these steps: 78 | ## * In the file package.xml: 79 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 80 | ## * In this file (CMakeLists.txt): 81 | ## * add "dynamic_reconfigure" to 82 | ## find_package(catkin REQUIRED COMPONENTS ...) 83 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 84 | ## and list every .cfg file to be processed 85 | 86 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 87 | # generate_dynamic_reconfigure_options( 88 | # cfg/DynReconf1.cfg 89 | # cfg/DynReconf2.cfg 90 | # ) 91 | 92 | ################################### 93 | ## catkin specific configuration ## 94 | ################################### 95 | ## The catkin_package macro generates cmake config files for your package 96 | ## Declare things to be passed to dependent projects 97 | ## INCLUDE_DIRS: uncomment this if your package contains header files 98 | ## LIBRARIES: libraries you create in this project that dependent projects also need 99 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 100 | ## DEPENDS: system dependencies of this project that dependent projects also need 101 | catkin_package( 102 | # INCLUDE_DIRS include 103 | # LIBRARIES smarc_comms 104 | # CATKIN_DEPENDS other_catkin_pkg 105 | # DEPENDS system_lib 106 | ) 107 | 108 | ########### 109 | ## Build ## 110 | ########### 111 | 112 | ## Specify additional locations of header files 113 | ## Your package locations should be listed before other locations 114 | include_directories( 115 | # include 116 | # ${catkin_INCLUDE_DIRS} 117 | ) 118 | 119 | ## Declare a C++ library 120 | # add_library(${PROJECT_NAME} 121 | # src/${PROJECT_NAME}/smarc_comms.cpp 122 | # ) 123 | 124 | ## Add cmake target dependencies of the library 125 | ## as an example, code may need to be generated before libraries 126 | ## either from message generation or dynamic reconfigure 127 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 128 | 129 | ## Declare a C++ executable 130 | ## With catkin_make all packages are built within a single CMake context 131 | ## The recommended prefix ensures that target names across packages don't collide 132 | # add_executable(${PROJECT_NAME}_node src/smarc_comms_node.cpp) 133 | 134 | ## Rename C++ executable without prefix 135 | ## The above recommended prefix causes long target names, the following renames the 136 | ## target back to the shorter version for ease of user use 137 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 138 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 139 | 140 | ## Add cmake target dependencies of the executable 141 | ## same as for the library above 142 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 143 | 144 | ## Specify libraries to link a library or executable target against 145 | # target_link_libraries(${PROJECT_NAME}_node 146 | # ${catkin_LIBRARIES} 147 | # ) 148 | 149 | ############# 150 | ## Install ## 151 | ############# 152 | 153 | # all install targets should use catkin DESTINATION variables 154 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 155 | 156 | ## Mark executable scripts (Python etc.) for installation 157 | ## in contrast to setup.py, you can choose the destination 158 | # install(PROGRAMS 159 | # scripts/my_python_script 160 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 161 | # ) 162 | 163 | ## Mark executables and/or libraries for installation 164 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 165 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 166 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 167 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 168 | # ) 169 | 170 | ## Mark cpp header files for installation 171 | # install(DIRECTORY include/${PROJECT_NAME}/ 172 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 173 | # FILES_MATCHING PATTERN "*.h" 174 | # PATTERN ".svn" EXCLUDE 175 | # ) 176 | 177 | ## Mark other files for installation (e.g. launch and bag files, etc.) 178 | # install(FILES 179 | # # myfile1 180 | # # myfile2 181 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 182 | # ) 183 | 184 | ############# 185 | ## Testing ## 186 | ############# 187 | 188 | ## Add gtest based cpp test target and link libraries 189 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_smarc_comms.cpp) 190 | # if(TARGET ${PROJECT_NAME}-test) 191 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 192 | # endif() 193 | 194 | ## Add folders to be run by python nosetests 195 | # catkin_add_nosetests(test) 196 | -------------------------------------------------------------------------------- /smarc_simulated_comms/README.md: -------------------------------------------------------------------------------- 1 | # Smarc_comms 2 | 3 | Communication between agents and other entities. 4 | 5 | ``` 6 | from smarc_msgs.msg import CommsMessage 7 | ``` 8 | 9 | CommsMessage: 10 | ``` 11 | Header header 12 | string source_ns 13 | string target_ns 14 | string data 15 | ``` 16 | 17 | source_ns should be the agent's root namespace. Ex: 'lolo_auv_1/some/other/part' source_ns='lolo_auv_1' 18 | same for target_ns. Alternatively, target_ns can be 'broadcast'. This will make it so that everyone in range receives the message. 19 | 20 | data can be anything. If using python, piclikng is the easiest way to get a string representation of your data. 21 | 22 | Publish CommsMessage to '/comms/inbound'. 23 | 24 | Any messages to a an agent will be published to 'target_ns/comms_inbound'. Subscribe to that topic to receive the messages. 25 | It is up to the agent to decide what to do with these messages. 26 | 27 | As of 18/04, only distance between agents is checked as a message receiving condition. 28 | 29 | All communicators must have 'lolo' or 'sam' as the first part of their source_ns. Ex: 'lolo_asd_123', 'sam_the_greatest' are okay but 'my_name_is_sam' is not. 30 | Acceptable names are collected from the list of all topics, so if there is no topic being published for an agent, it will not receive messages. 31 | 32 | 33 | ## TODO: 34 | Make distance check a rosparam. 35 | Make rescan_period a rosparam. 36 | Make it possible for arbitrarly named agents to connect. 37 | Add other checks? Line of sight? 38 | 39 | -------------------------------------------------------------------------------- /smarc_simulated_comms/launch/smarc_simulated_comms.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | ["lolo","sam","comm_node"] 9 | 10 | 11 | -------------------------------------------------------------------------------- /smarc_simulated_comms/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | smarc_simulated_comms 4 | 0.0.0 5 | The smarc_comms package 6 | 7 | 8 | 9 | 10 | Ozer Ozkahraman 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 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /smarc_simulated_comms/scripts/comms_master.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | # vim:fenc=utf-8 4 | # 5 | # Author: Ozer Ozkahraman (ozkahramanozer@gmail.com) 6 | # Date: 2018-04-11 7 | 8 | import time 9 | import rospy 10 | import random 11 | import math 12 | 13 | from gazebo_msgs.msg import ModelStates 14 | from smarc_msgs.msg import CommsMessage 15 | 16 | 17 | class CommsMaster: 18 | def __init__(self): 19 | # know all the positions of the models 20 | rospy.Subscriber('/gazebo/model_states', ModelStates, self._CB_model_states) 21 | self.model_states = None 22 | 23 | # listen to incoming packets 24 | rospy.Subscriber('/comms/inbound', CommsMessage, self._CB_packet_received) 25 | 26 | # names to scan for 27 | self._names_to_scan = rospy.get_param('comms_master/scanned_names', default=['lolo','sam','comm_node']) 28 | 29 | # a list of known communicators. 30 | # there should be 'namespace:publisher' pairs in here 31 | self.known_agents = {} 32 | self._scan_topics() 33 | 34 | self.MAX_DIST = rospy.get_param('comms_master/max_comm_distance', default=100) 35 | self.RESCAN_PERIOD = rospy.get_param('comms_master/rescan_period', default=10) 36 | self._last_update = rospy.Time.now() 37 | 38 | 39 | 40 | def update(self): 41 | dt = rospy.Time.now() - self._last_update 42 | if dt.secs > self.RESCAN_PERIOD: 43 | self._scan_topics() 44 | self._last_update = rospy.Time.now() 45 | 46 | def _scan_topics(self): 47 | """ 48 | this looks at all topics and tries to find lolos and sams 49 | """ 50 | target_names = self._names_to_scan 51 | all_topics = rospy.get_published_topics() 52 | for name in target_names: 53 | for topic, topic_type in all_topics: 54 | # get_published_topics returns [ ['/topic/of/something', 'type of that topic], ... ] 55 | # get the structure, ex: 'lolo_auv_0' 56 | found_ns = topic.split('/')[1] 57 | # get the name of the namespace without the numbers and stuff 58 | # ex 'lolo' 59 | parts = found_ns.split('_') 60 | if parts[0] == name: 61 | # create a publisher for that agent 62 | # this is slightly inefficient as it will do this multiple times per auv 63 | if self.known_agents.get(name) is None: 64 | # no need to spam publishers 65 | self.known_agents[found_ns] = rospy.Publisher(found_ns+'/comms_inbound', CommsMessage, queue_size=1) 66 | 67 | 68 | 69 | 70 | def _CB_model_states(self, data): 71 | self.model_states = {'names':data.name, 'positions':data.pose} 72 | 73 | 74 | def _get_dist(self, source_ns, target_ns): 75 | source_idx = None 76 | target_idx = None 77 | for i,name in enumerate(self.model_states['names']): 78 | if name == source_ns: 79 | source_idx = i 80 | if name == target_ns: 81 | target_idx = i 82 | 83 | if source_idx is None or target_idx is None: 84 | rospy.logwarn('[COMMS] Attempted to _get_dist but an idx is still None!') 85 | return math.inf 86 | 87 | source_pose = self.model_states['positions'][source_idx] 88 | target_pose = self.model_states['positions'][target_idx] 89 | dx = source_pose.position.x - target_pose.position.x 90 | dy = source_pose.position.y - target_pose.position.y 91 | dz = source_pose.position.z - target_pose.position.z 92 | 93 | dist = math.sqrt(dx**2 + dy**2 + dz**2) 94 | 95 | return dist 96 | 97 | 98 | def _relay(self, comms_msg, target_ns=None): 99 | """ 100 | simply get the created publisher from known agents and publish to its inbound topic 101 | only relays if distance is close enough 102 | """ 103 | if target_ns is None: 104 | target_ns = comms_msg.target_ns 105 | 106 | if self._get_dist(comms_msg.source_ns, target_ns) <= self.MAX_DIST: 107 | try: 108 | publisher = self.known_agents[target_ns] 109 | except: 110 | rospy.logwarn('[COMMS] Could not find target_ns in known_agents, message not delivered!') 111 | publisher.publish(comms_msg) 112 | return True 113 | return False 114 | 115 | 116 | def _CB_packet_received(self, comms_msg): 117 | if comms_msg.target_ns == 'broadcast': 118 | # we need to check distances to all known agents and relay to the ones in range 119 | for a_target_ns in self.known_agents.keys(): 120 | self._relay(comms_msg, target_ns=a_target_ns) 121 | else: 122 | self._relay(comms_msg) 123 | 124 | 125 | if __name__=='__main__': 126 | rospy.init_node('comms_master', anonymous=True) 127 | rate = rospy.Rate(60) 128 | comms = CommsMaster() 129 | while not rospy.is_shutdown(): 130 | comms.update() 131 | rate.sleep() 132 | -------------------------------------------------------------------------------- /smarc_simulated_comms/scripts/comms_tester.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | # vim:fenc=utf-8 4 | # 5 | # Copyright 2018 Ozer Ozkahraman (ozkahramanozer@gmail.com) 6 | # 7 | # Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 8 | # 9 | # 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 10 | # 11 | # 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 12 | # 13 | # 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 14 | # 15 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 16 | 17 | import rospy 18 | import time 19 | from smarc_msgs.msg import CommsMessage 20 | 21 | class DummyPublisher: 22 | def __init__(self): 23 | self.pub = rospy.Publisher('/comms/inbound', CommsMessage, queue_size=1) 24 | 25 | def send(self, source, target, data): 26 | msg = CommsMessage() 27 | msg.source_ns = source 28 | msg.target_ns = target 29 | msg.data = str(data) 30 | self.pub.publish(msg) 31 | 32 | if __name__ == '__main__': 33 | rospy.init_node('comms_tester',anonymous=True) 34 | rate = rospy.Rate(3) 35 | 36 | pubs = DummyPublisher() 37 | 38 | time.sleep(1) 39 | pubs.send('lolo_auv_0','lolo_auv_1','0to1') 40 | time.sleep(1) 41 | pubs.send('lolo_auv_0','broadcast','0toAll') 42 | time.sleep(1) 43 | 44 | while True: 45 | pubs.send('lolo_auv_0','lolo_auv_1','time is now'+str(rospy.Time.now())) 46 | rate.sleep() 47 | -------------------------------------------------------------------------------- /smarc_worlds/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(smarc_worlds) 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) 11 | find_package(gazebo REQUIRED) 12 | 13 | catkin_package( 14 | # INCLUDE_DIRS include 15 | # LIBRARIES uuv_descriptions 16 | # CATKIN_DEPENDS other_catkin_pkg 17 | # DEPENDS system_lib 18 | ) 19 | 20 | # Install python scripts 21 | catkin_install_python(PROGRAMS scripts/spawn_model.py 22 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 23 | 24 | 25 | install(DIRECTORY launch Media worlds world_models 26 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 27 | PATTERN "*~" EXCLUDE) 28 | -------------------------------------------------------------------------------- /smarc_worlds/Media/materials/programs/SimpleWaves.frag: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2016 The UUV Simulator Authors. 2 | // All rights reserved. 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 | // Input parameters 17 | uniform sampler2D bumpMap; 18 | uniform samplerCube cubeMap; 19 | uniform vec4 deepColor; 20 | uniform vec4 shallowColor; 21 | uniform float fresnelPower; 22 | uniform float hdrMultiplier; 23 | 24 | // Input computed in vertex shader 25 | varying mat3 rotMatrix; 26 | varying vec3 eyeVec; 27 | varying vec2 bumpCoord; 28 | 29 | void main(void) 30 | { 31 | // Apply bump mapping to normal vector to make waves look more detailed: 32 | vec4 bump = texture2D(bumpMap, bumpCoord)*2.0 - 1.0; 33 | vec3 N = normalize(rotMatrix * bump.xyz); 34 | 35 | // Reflected ray: 36 | vec3 E = normalize(eyeVec); 37 | vec3 R = reflect(E, N); 38 | // Gazebo requires rotated cube map lookup. 39 | R = vec3(R.x, R.z, R.y); 40 | 41 | // Get environment color of reflected ray: 42 | vec4 envColor = textureCube(cubeMap, R, 0.0); 43 | 44 | // Cheap hdr effect: 45 | envColor.rgb *= (envColor.r+envColor.g+envColor.b)*hdrMultiplier; 46 | 47 | // Compute refraction ratio (Fresnel): 48 | float facing = 1.0 - dot(-E, N); 49 | float refractionRatio = clamp(pow(facing, fresnelPower), 0.0, 1.0); 50 | 51 | // Refracted ray only considers deep and shallow water colors: 52 | vec4 waterColor = mix(shallowColor, deepColor, facing); 53 | 54 | // Perform linear interpolation between reflection and refraction. 55 | vec4 color = mix(waterColor, envColor, refractionRatio); 56 | gl_FragColor = vec4(color.xyz, 0.9); 57 | } 58 | -------------------------------------------------------------------------------- /smarc_worlds/Media/materials/programs/SimpleWaves.vert: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2016 The UUV Simulator Authors. 2 | // All rights reserved. 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.s 15 | 16 | // Input parameters 17 | uniform vec3 eyePos; 18 | uniform float rescale; 19 | uniform vec2 bumpScale; 20 | uniform vec2 bumpSpeed; 21 | uniform float time; 22 | uniform float frequency; 23 | uniform float amplitude; 24 | uniform float steepness; 25 | 26 | // Output variables 27 | varying mat3 rotMatrix; 28 | varying vec3 eyeVec; 29 | varying vec2 bumpCoord; 30 | 31 | // Compute linear combination of Gerstner waves as described in 32 | // GPU Gems, chapter 01: "Effective Water Simulation from Physical Models" 33 | // http://http.developer.nvidia.com/GPUGems/gpugems_ch01.html 34 | 35 | // Information regarding a single wave 36 | struct WaveParameters { 37 | float w; // frequency 38 | float a; // amplitude 39 | float phi; // phase constant of speed 40 | vec2 d; // horizontal direction of wave 41 | float q; // steepness for Gerstner wave (q=0: rolling sine waves) 42 | }; 43 | 44 | void main(void) 45 | { 46 | // Use combination of three waves. Values here are chosen rather arbitrarily. 47 | // Other parameters might lead to better-looking waves. 48 | 49 | #define N_WAVES 3 50 | WaveParameters waves[N_WAVES]; 51 | waves[0] = WaveParameters(frequency, 0.6*amplitude, 0.5, vec2(-1, 0), steepness); 52 | waves[1] = WaveParameters(3.2*frequency, 0.4*amplitude, 1.7, vec2(-0.7, 0.7), 1.5*steepness); 53 | waves[2] = WaveParameters(1.8*frequency, 0.3*amplitude, 1.0, vec2(0.7, 0.7), 0.8*steepness); 54 | 55 | vec4 P = gl_Vertex; 56 | 57 | // Iteratively compute binormal, tangent, and normal vectors: 58 | vec3 B = vec3(1.0, 0.0, 0.0); 59 | vec3 T = vec3(0.0, 1.0, 0.0); 60 | vec3 N = vec3(0.0, 0.0, 1.0); 61 | 62 | // Wave synthesis using linear combination of Gerstner waves 63 | for(int i = 0; i < N_WAVES; ++i) 64 | { 65 | // Evaluate wave equation: 66 | float angle = dot(waves[i].d, P.xy)*waves[i].w + time*waves[i].phi; 67 | float c = cos(angle); 68 | float s = sin(angle); 69 | float q = waves[i].q; 70 | 71 | // Displacement of point due to wave (Eq. 9) 72 | P.x += q*waves[i].a*c*waves[i].d.x; 73 | P.y += q*waves[i].a*c*waves[i].d.y; 74 | P.z += waves[i].a*s; 75 | 76 | // Modify normals due to wave displacement (Eq. 10-12) 77 | float wa = waves[i].a*waves[i].w; 78 | float qwas = q*wa*s; 79 | float wac = wa*c; 80 | float dx = waves[i].d.x; 81 | float dy = waves[i].d.y; 82 | float dxy = dx*dy; 83 | 84 | B += vec3(-qwas*dx*dx, -qwas*dxy, wac*dx); 85 | T += vec3(-qwas*dxy, -qwas*dy*dy, wac*dy); 86 | N += vec3(-dx*wac, -dy*wac, -qwas); 87 | } 88 | 89 | // Compute (Surf2World * Rescale) matrix 90 | B = normalize(B)*rescale; 91 | T = normalize(T)*rescale; 92 | N = normalize(N); 93 | rotMatrix = mat3(B, T, N); 94 | 95 | gl_Position = gl_ModelViewProjectionMatrix*P; 96 | 97 | // Compute texture coordinates for bump map 98 | bumpCoord = gl_MultiTexCoord0.xy*bumpScale + time*bumpSpeed; 99 | 100 | eyeVec = P.xyz - eyePos; // eye position in vertex space 101 | } 102 | -------------------------------------------------------------------------------- /smarc_worlds/Media/materials/scripts/fiducials.material: -------------------------------------------------------------------------------- 1 | material AprilTag/Tag36_11_00000 2 | { 3 | technique 4 | { 5 | pass 6 | { 7 | ambient 1 1 1 1 8 | diffuse 1 1 1 1 9 | specular 0 0 0 0 10 | 11 | texture_unit 12 | { 13 | texture fiducials/tag36_11_00000.jpg 14 | filtering anistropic 15 | max_anisotropy 16 16 | } 17 | } 18 | } 19 | } -------------------------------------------------------------------------------- /smarc_worlds/Media/materials/scripts/sand.material: -------------------------------------------------------------------------------- 1 | material UUVSimulator/SandAndStones 2 | { 3 | receive_shadows on 4 | technique 5 | { 6 | pass 7 | { 8 | ambient 0.5 0.5 0.5 1.0 9 | diffuse 0.5 0.5 0.5 1.0 10 | specular 0.2 0.2 0.2 1.0 12.5 11 | 12 | texture_unit 13 | { 14 | texture soil_sand_0045_01.jpg 15 | filtering anistropic 16 | max_anisotropy 16 17 | scale 0.5 0.5 18 | } 19 | } 20 | } 21 | } -------------------------------------------------------------------------------- /smarc_worlds/Media/materials/scripts/water.material: -------------------------------------------------------------------------------- 1 | material UUVSimulator/SeaWall 2 | { 3 | receive_shadows off 4 | technique 5 | { 6 | pass 7 | { 8 | ambient 0.616687 0.90461 1 0.7 9 | diffuse 0.616687 0.90461 1 0.7 10 | specular 1 1 1 1 20 11 | emissive 0.0 0.0 0.0 1.0 12 | scene_blend alpha_blend 13 | 14 | } 15 | } 16 | } 17 | 18 | 19 | material UUVSimulator/StaticWater 20 | { 21 | technique 22 | { 23 | pass 24 | { 25 | ambient 0.5 0.5 0.5 0.8 26 | diffuse 0.5 0.5 0.5 0.8 27 | 28 | scene_blend alpha_blend 29 | 30 | texture_unit 31 | { 32 | texture water_water_0076_03_s.jpg 33 | filtering anistropic 34 | max_anisotropy 16 35 | scale 1.0 1.0 36 | } 37 | } 38 | } 39 | } 40 | 41 | material UUVSimulator/StaticDeepWater 42 | { 43 | technique 44 | { 45 | pass 46 | { 47 | ambient 0.5 0.5 0.5 0.8 48 | diffuse 0.5 0.5 0.5 0.8 49 | 50 | scene_blend alpha_blend 51 | 52 | texture_unit 53 | { 54 | texture water_water_0046_01.jpg 55 | filtering anistropic 56 | max_anisotropy 16 57 | scale 1.0 1.0 58 | } 59 | } 60 | } 61 | } 62 | 63 | material UUVSimulator/StaticTurquoiseWater 64 | { 65 | technique 66 | { 67 | pass 68 | { 69 | ambient 0.5 0.5 0.5 0.8 70 | diffuse 0.5 0.5 0.5 0.8 71 | 72 | scene_blend alpha_blend 73 | 74 | texture_unit 75 | { 76 | texture water_water_0051_01.jpg 77 | filtering anistropic 78 | max_anisotropy 16 79 | scale 1.0 1.0 80 | } 81 | } 82 | } 83 | } 84 | 85 | material UUVSimulator/StaticWaves 86 | { 87 | technique 88 | { 89 | pass 90 | { 91 | ambient 0.5 0.5 0.5 0.8 92 | diffuse 0.5 0.5 0.5 0.8 93 | 94 | scene_blend alpha_blend 95 | 96 | texture_unit 97 | { 98 | texture water_water_0093_01.jpg 99 | filtering anistropic 100 | max_anisotropy 16 101 | scale 1.0 1.0 102 | } 103 | } 104 | } 105 | } 106 | -------------------------------------------------------------------------------- /smarc_worlds/Media/materials/scripts/waves.material: -------------------------------------------------------------------------------- 1 | material UUVSimulator/SimpleWaves 2 | { 3 | technique GLSL 4 | { 5 | pass 6 | { 7 | scene_blend alpha_blend 8 | vertex_program_ref SimpleWavesVS 9 | { 10 | param_named_auto eyePos camera_position_object_space 11 | param_named_auto time time_0_x 100.0 12 | param_named rescale float 0.5 13 | param_named bumpScale float2 25 25 14 | param_named bumpSpeed float2 0.01 0.01 15 | param_named frequency float 0.01 16 | param_named amplitude float 0.01 17 | param_named steepness float 1.0 18 | } 19 | 20 | fragment_program_ref SimpleWavesFS 21 | { 22 | param_named deepColor float4 0 0.05 0.2 1.0 23 | param_named shallowColor float4 0 0.6 1 1.0 24 | param_named fresnelPower float 5 25 | param_named hdrMultiplier float 0.4 26 | param_named bumpMap int 0 27 | param_named cubeMap int 1 28 | } 29 | 30 | texture_unit 31 | { 32 | texture wave_normals.dds 33 | tex_coord_set 0 34 | scale 0.1 0.1 35 | filtering linear linear linear 36 | } 37 | 38 | texture_unit 39 | { 40 | cubic_texture clouds.jpg combinedUVW 41 | tex_address_mode clamp 42 | tex_coord_set 1 43 | filtering linear linear linear 44 | } 45 | } 46 | } 47 | } -------------------------------------------------------------------------------- /smarc_worlds/Media/materials/scripts/waves.program: -------------------------------------------------------------------------------- 1 | vertex_program SimpleWavesVS glsl 2 | { 3 | source SimpleWaves.vert 4 | } 5 | 6 | fragment_program SimpleWavesFS glsl 7 | { 8 | source SimpleWaves.frag 9 | } 10 | -------------------------------------------------------------------------------- /smarc_worlds/Media/materials/textures/License and source for textures.txt: -------------------------------------------------------------------------------- 1 | Texture taken from Texture library © 2013 Dmitriy Chugai, admin@texturelib.com 2 | 3 | Direct links: 4 | http://texturelib.com/texture/?path=/Textures/soil/sand/soil_sand_0045 5 | http://texturelib.com/texture/?path=/Textures/water/water/water_water_0076 6 | http://texturelib.com/texture/?path=/Textures/water/water/water_water_0093 7 | http://texturelib.com/texture/?path=/Textures/water/water/water_water_0046 8 | http://texturelib.com/texture/?path=/Textures/water/water/water_water_0051 9 | 10 | License according to http://texturelib.com/license/: 11 | 12 | All images are taken by my own cameras and edited by me. 13 | 14 | They are free for commercial and non-commercial use with the only limitation: you are not allowed to sell or distribute original or slightly modified images alone or in packs. You can only distribute them as an integral part of your product. 15 | 16 | Credit texturelib.com if it is possible. 17 | 18 | -------------------------------------------------------------------------------- /smarc_worlds/Media/materials/textures/clouds.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smarc-project/smarc_simulations/c584debee27b02c6a43d33b239c45ca14ba6490c/smarc_worlds/Media/materials/textures/clouds.jpg -------------------------------------------------------------------------------- /smarc_worlds/Media/materials/textures/clouds_bk.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smarc-project/smarc_simulations/c584debee27b02c6a43d33b239c45ca14ba6490c/smarc_worlds/Media/materials/textures/clouds_bk.jpg -------------------------------------------------------------------------------- /smarc_worlds/Media/materials/textures/clouds_dn.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smarc-project/smarc_simulations/c584debee27b02c6a43d33b239c45ca14ba6490c/smarc_worlds/Media/materials/textures/clouds_dn.jpg -------------------------------------------------------------------------------- /smarc_worlds/Media/materials/textures/clouds_fr.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smarc-project/smarc_simulations/c584debee27b02c6a43d33b239c45ca14ba6490c/smarc_worlds/Media/materials/textures/clouds_fr.jpg -------------------------------------------------------------------------------- /smarc_worlds/Media/materials/textures/clouds_lf.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smarc-project/smarc_simulations/c584debee27b02c6a43d33b239c45ca14ba6490c/smarc_worlds/Media/materials/textures/clouds_lf.jpg -------------------------------------------------------------------------------- /smarc_worlds/Media/materials/textures/clouds_rt.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smarc-project/smarc_simulations/c584debee27b02c6a43d33b239c45ca14ba6490c/smarc_worlds/Media/materials/textures/clouds_rt.jpg -------------------------------------------------------------------------------- /smarc_worlds/Media/materials/textures/clouds_up.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smarc-project/smarc_simulations/c584debee27b02c6a43d33b239c45ca14ba6490c/smarc_worlds/Media/materials/textures/clouds_up.jpg -------------------------------------------------------------------------------- /smarc_worlds/Media/materials/textures/fiducials/License and source for textures.txt: -------------------------------------------------------------------------------- 1 | Project: AprilTags Visual Fiducial System 2 | License: BSD 3 | Current Version: 2015-03-18 4 | Source Download: https://april.eecs.umich.edu/software/apriltag/ -------------------------------------------------------------------------------- /smarc_worlds/Media/materials/textures/fiducials/tag36_11_00000.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smarc-project/smarc_simulations/c584debee27b02c6a43d33b239c45ca14ba6490c/smarc_worlds/Media/materials/textures/fiducials/tag36_11_00000.jpg -------------------------------------------------------------------------------- /smarc_worlds/Media/materials/textures/soil_sand_0045_01.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smarc-project/smarc_simulations/c584debee27b02c6a43d33b239c45ca14ba6490c/smarc_worlds/Media/materials/textures/soil_sand_0045_01.jpg -------------------------------------------------------------------------------- /smarc_worlds/Media/materials/textures/water_water_0046_01.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smarc-project/smarc_simulations/c584debee27b02c6a43d33b239c45ca14ba6490c/smarc_worlds/Media/materials/textures/water_water_0046_01.jpg -------------------------------------------------------------------------------- /smarc_worlds/Media/materials/textures/water_water_0051_01.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smarc-project/smarc_simulations/c584debee27b02c6a43d33b239c45ca14ba6490c/smarc_worlds/Media/materials/textures/water_water_0051_01.jpg -------------------------------------------------------------------------------- /smarc_worlds/Media/materials/textures/water_water_0076_03_s.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smarc-project/smarc_simulations/c584debee27b02c6a43d33b239c45ca14ba6490c/smarc_worlds/Media/materials/textures/water_water_0076_03_s.jpg -------------------------------------------------------------------------------- /smarc_worlds/Media/materials/textures/water_water_0093_01.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smarc-project/smarc_simulations/c584debee27b02c6a43d33b239c45ca14ba6490c/smarc_worlds/Media/materials/textures/water_water_0093_01.jpg -------------------------------------------------------------------------------- /smarc_worlds/Media/materials/textures/wave_normals.dds: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smarc-project/smarc_simulations/c584debee27b02c6a43d33b239c45ca14ba6490c/smarc_worlds/Media/materials/textures/wave_normals.dds -------------------------------------------------------------------------------- /smarc_worlds/launch/ekf_localization_tests.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 23 | 24 | meshes: 25 | seafloor: 26 | plane: [500, 500, 0.1] 27 | pose: 28 | position: [0, 0, -100] 29 | north: 30 | plane: [0.1, 500, 100] 31 | pose: 32 | position: [250, 0, -50] 33 | south: 34 | plane: [0.1, 500, 100] 35 | pose: 36 | position: [-250, 0, -50] 37 | west: 38 | plane: [500, 0.1, 100] 39 | pose: 40 | position: [0, -250, -50] 41 | east: 42 | plane: [500, 0.1, 100] 43 | pose: 44 | position: [0, 250, -50] 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | -------------------------------------------------------------------------------- /smarc_worlds/launch/pipe_following.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 23 | 24 | meshes: 25 | heightmap: 26 | mesh: package://smarc_worlds/world_models/sand_heightmap_pipe/meshes/heightmap.dae 27 | model: sand_heightmap_pipe 28 | seafloor: 29 | plane: [2000, 2000, 0.1] 30 | pose: 31 | position: [0, 0, -100] 32 | north: 33 | plane: [0.1, 2000, 100] 34 | pose: 35 | position: [1000, 0, -50] 36 | south: 37 | plane: [0.1, 2000, 100] 38 | pose: 39 | position: [-1000, 0, -50] 40 | west: 41 | plane: [2000, 0.1, 100] 42 | pose: 43 | position: [0, -1000, -50] 44 | east: 45 | plane: [2000, 0.1, 100] 46 | pose: 47 | position: [0, 1000, -50] 48 | pipeline: 49 | mesh: package://smarc_worlds/world_models/pipeline/meshes/pipeline.dae 50 | model: pipeline 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /smarc_worlds/launch/pool_world.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 | -------------------------------------------------------------------------------- /smarc_worlds/launch/random_rocks.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 23 | 24 | meshes: 25 | seafloor: 26 | plane: [2000, 2000, 0.1] 27 | pose: 28 | position: [0, 0, -100] 29 | north: 30 | plane: [0.1, 2000, 100] 31 | pose: 32 | position: [1000, 0, -50] 33 | south: 34 | plane: [0.1, 2000, 100] 35 | pose: 36 | position: [-1000, 0, -50] 37 | west: 38 | plane: [2000, 0.1, 100] 39 | pose: 40 | position: [0, -1000, -50] 41 | east: 42 | plane: [2000, 0.1, 100] 43 | pose: 44 | position: [0, 1000, -50] 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | -------------------------------------------------------------------------------- /smarc_worlds/launch/skytteren.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 35 | 36 | -------------------------------------------------------------------------------- /smarc_worlds/launch/sonar_intensities.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 23 | 24 | meshes: 25 | heightmap: 26 | mesh: package://smarc_worlds/world_models/sand_heightmap_pipe/meshes/heightmap_pipe.dae 27 | model: sand_heightmap_pipe 28 | seafloor: 29 | plane: [2000, 2000, 0.1] 30 | pose: 31 | position: [0, 0, -100] 32 | north: 33 | plane: [0.1, 2000, 100] 34 | pose: 35 | position: [1000, 0, -50] 36 | south: 37 | plane: [0.1, 2000, 100] 38 | pose: 39 | position: [-1000, 0, -50] 40 | west: 41 | plane: [2000, 0.1, 100] 42 | pose: 43 | position: [0, -1000, -50] 44 | east: 45 | plane: [2000, 0.1, 100] 46 | pose: 47 | position: [0, 1000, -50] 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /smarc_worlds/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | smarc_worlds 4 | 0.0.0 5 | The underwater_envs package 6 | 7 | nacho 8 | 9 | 10 | TODO 11 | 12 | catkin 13 | 14 | gazebo_ros 15 | gazebo_ros_control 16 | message_to_tf 17 | uuv_sensor_plugins_ros 18 | uuv_gazebo_ros_plugins 19 | 20 | 21 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /smarc_worlds/pool/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 17 | 18 | 19 | 20 | true 21 | 22 | 23 | 24 | 0 0 0 0 0 0 25 | 26 | 27 | file://Media/models/sea_surface_1000m_x_1000m.dae 28 | 0.01 0.015 0.1 29 | 30 | 31 | 32 | 36 | 37 | 38 | 39 | 40 | false 41 | 0 0 -3.9 0 0 0 42 | 43 | 44 | 100 100 .1 45 | 46 | 47 | 48 | 52 | 53 | 54 | 55 | 56 | 0 0 -3.9 0 0 0 57 | 58 | 59 | 100 100 .1 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | -------------------------------------------------------------------------------- /smarc_worlds/world_models/duck/images/duckCM.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smarc-project/smarc_simulations/c584debee27b02c6a43d33b239c45ca14ba6490c/smarc_worlds/world_models/duck/images/duckCM.png -------------------------------------------------------------------------------- /smarc_worlds/world_models/duck/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 0 0 0.5 0 0 0 6 | 7 | 10.0 8 | 9 | 12 | 13 | 14 | 15 | 16 | model://duck/meshes/duck.dae 17 | 0.5 0.5 0.5 18 | 19 | 20 | 21 | 22 | 23 | 24 | model://duck/meshes/duck.dae 25 | 0.5 0.5 0.5 26 | 27 | 28 | 29 | 30 | 31 | 1025.0 32 | hydrodynamics/current_velocity 33 | 1 34 | 35 | 0.015625 36 | 0 37 | 0 0 0 38 | 39 | 0.25 40 | 0.25 41 | 0.25 42 | 43 | 44 | fossen 45 | 46 | 47 | 0.93 0 0 0 0 0 48 | 0 35.5 0 0 0 -1.93 49 | 0 0 35.5 0 1.93 0 50 | 0 0 0 0.0704 0 0 51 | 0 0 1.93 0 4.88 0 52 | 0 -1.93 0 0 0 4.88 53 | 54 | -5.8596 -38.2014808 -38.2014808 0 -4.09 -4.09 55 | -1.62 -1310 -1310 -0.13 -188 -188 56 | 57 | 58 | 59 | 60 | 61 | 62 | -------------------------------------------------------------------------------- /smarc_worlds/world_models/duckCM.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smarc-project/smarc_simulations/c584debee27b02c6a43d33b239c45ca14ba6490c/smarc_worlds/world_models/duckCM.png -------------------------------------------------------------------------------- /smarc_worlds/world_models/dummy_laser/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | Laser (Static) 4 | 1.0 5 | model.sdf 6 | Nils Bore 7 | Dummy static laser, to help us achieve desired frequency on vehicles 8 | 9 | -------------------------------------------------------------------------------- /smarc_worlds/world_models/dummy_laser/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 15 | 16 | 17 | 0 0 0.1 0 0 0 18 | 19 | 20 | 0.1 21 | 22 | 23 | 24 | 25 | 0.2 0.2 0.2 26 | 27 | 28 | 29 | 30 | 31 | 32 | 0.2 0.2 0.2 33 | 34 | 35 | 36 | 37 | 0.01 0 0.0175 0 -0 0 38 | 39 | 40 | 41 | 1 42 | 1 43 | -2.26889 44 | 2.268899 45 | 46 | 47 | 1 48 | 1 49 | -0.535 50 | 0.186132 51 | 52 | 53 | 54 | 0.1 55 | 10 56 | 0.01 57 | 58 | 59 | 60 | 1 61 | 30 62 | false 63 | 64 | 65 | 66 | 67 | -------------------------------------------------------------------------------- /smarc_worlds/world_models/large_rock/meshes/large_rock_collision.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smarc-project/smarc_simulations/c584debee27b02c6a43d33b239c45ca14ba6490c/smarc_worlds/world_models/large_rock/meshes/large_rock_collision.stl -------------------------------------------------------------------------------- /smarc_worlds/world_models/large_rock/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | Large rock (Static) 4 | 1.0 5 | model.sdf 6 | Nils Bore 7 | Static model of a sea floor rock 8 | 9 | -------------------------------------------------------------------------------- /smarc_worlds/world_models/large_rock/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 15 | 16 | 17 | true 18 | 19 | 20 | 0 0 0 0 0 0 21 | 22 | model://large_rock/meshes/large_rock_collision.stl 23 | 24 | 200 25 | 26 | 27 | 28 | true 29 | 0 0 0 0 0 0 30 | 31 | model://large_rock/meshes/large_rock.dae 32 | 33 | 200 34 | 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /smarc_worlds/world_models/pipeline/meshes/pipeline_collision.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smarc-project/smarc_simulations/c584debee27b02c6a43d33b239c45ca14ba6490c/smarc_worlds/world_models/pipeline/meshes/pipeline_collision.stl -------------------------------------------------------------------------------- /smarc_worlds/world_models/pipeline/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | Pipeline (Static) 4 | 1.0 5 | model.sdf 6 | Nils Bore 7 | Static model of a pipeline, for the ocean heightmap 8 | 9 | -------------------------------------------------------------------------------- /smarc_worlds/world_models/pipeline/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 15 | 16 | 17 | true 18 | 19 | 20 | 0 0 0 0 0 0 21 | 22 | model://pipeline/meshes/pipeline_collision.stl 23 | 24 | 200 25 | 26 | 27 | 28 | true 29 | 0 0 0 0 0 0 30 | 31 | model://pipeline/meshes/pipeline.dae 32 | 33 | 34 | 38 | 39 | 200 40 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /smarc_worlds/world_models/sand_heightmap_pipe/meshes/License and source for textures.txt: -------------------------------------------------------------------------------- 1 | Texture taken from Texture library © 2013 Dmitriy Chugai, admin@texturelib.com 2 | 3 | Direct links: 4 | http://texturelib.com/texture/?path=/Textures/soil/sand/soil_sand_0045_01 5 | 6 | License according to http://texturelib.com/license/: 7 | 8 | All images are taken by my own cameras and edited by me. 9 | 10 | They are free for commercial and non-commercial use with the only limitation: you are not allowed to sell or distribute original or slightly modified images alone or in packs. You can only distribute them as an integral part of your product. 11 | 12 | Credit texturelib.com if it is possible. 13 | 14 | -------------------------------------------------------------------------------- /smarc_worlds/world_models/sand_heightmap_pipe/meshes/rust.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smarc-project/smarc_simulations/c584debee27b02c6a43d33b239c45ca14ba6490c/smarc_worlds/world_models/sand_heightmap_pipe/meshes/rust.jpg -------------------------------------------------------------------------------- /smarc_worlds/world_models/sand_heightmap_pipe/meshes/soil_sand_0045_01.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smarc-project/smarc_simulations/c584debee27b02c6a43d33b239c45ca14ba6490c/smarc_worlds/world_models/sand_heightmap_pipe/meshes/soil_sand_0045_01.jpg -------------------------------------------------------------------------------- /smarc_worlds/world_models/sand_heightmap_pipe/model.config: -------------------------------------------------------------------------------- 1 | 2 | 8 | 9 | Sand Heightmap Pipe 10 | 1.0 11 | model.sdf 12 | 13 | Sebastian Scherer 14 | sebastian.scherer2@de.bosch.com 15 | 16 | Sandy sea floor heightmap 17 | 18 | -------------------------------------------------------------------------------- /smarc_worlds/world_models/sand_heightmap_pipe/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 34 | 35 | 36 | true 37 | 38 | 39 | 0 0 0 0 0 0 40 | 41 | model://sand_heightmap_pipe/meshes/heightmap.dae 42 | 43 | 50 44 | 45 | 46 | 47 | true 48 | 0 0 0 0 0 0 49 | 50 | model://sand_heightmap_pipe/meshes/heightmap.dae 51 | 52 | 53 | 57 | 58 | 50 59 | 60 | 61 | 62 | 63 | -------------------------------------------------------------------------------- /smarc_worlds/world_models/skytteren/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Skytteren shipwreck 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Nacho 10 | nacho 11 | 12 | 13 | 14 | The Skytteren shipwreck 15 | 16 | 17 | -------------------------------------------------------------------------------- /smarc_worlds/world_models/skytteren/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 15 | 16 | 17 | true 18 | 0 0 0 0 0 0 19 | 20 | 21 | 22 | 23 | 24 | model://skytteren/meshes/skytteren.dae 25 | 26 | 27 | false 28 | 29 | 30 | 31 | 32 | 33 | model://skytteren/meshes/skytteren.dae 34 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /smarc_worlds/world_models/small_ocean_box/model.config: -------------------------------------------------------------------------------- 1 | 2 | 8 | 9 | Small Ocean Box 10 | 0.0.1 11 | model.sdf 12 | 13 | Sebastian Scherer 14 | sebastian.scherer2@de.bosch.com 15 | 16 | 17 | An small box that simulates a rectangular ocean region with texturized waves. 18 | 19 | 20 | -------------------------------------------------------------------------------- /smarc_worlds/world_models/small_ocean_box/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 34 | 35 | 36 | true 37 | 38 | 39 | false 40 | 0 0 -100 0 0 0 41 | 42 | 43 | 500 500 .1 44 | 45 | 46 | 47 | 51 | 52 | 50 53 | 54 | 55 | 56 | 0 0 -100 0 0 0 57 | 58 | 59 | 500 500 .1 60 | 61 | 62 | 50 63 | 64 | 65 | 66 | false 67 | 0 0 0 0 0 0 68 | 69 | 70 | 500 500 .1 71 | 72 | 73 | 74 | 78 | 79 | 80 | 81 | 82 | false 83 | 250 0 -50 0 0 0 84 | 85 | 86 | .1 500 100 87 | 88 | 89 | 90 | 94 | 95 | 96 | 97 | 98 | false 99 | -250 0 -50 0 0 0 100 | 101 | 102 | .1 500 100 103 | 104 | 105 | 106 | 110 | 111 | 112 | 113 | 114 | false 115 | 0 -250 -50 0 0 0 116 | 117 | 118 | 500 .1 100 119 | 120 | 121 | 122 | 126 | 127 | 128 | 129 | 130 | false 131 | 0 250 -50 0 0 0 132 | 133 | 134 | 500 .1 100 135 | 136 | 137 | 138 | 142 | 143 | 144 | 145 | 146 | 147 | -------------------------------------------------------------------------------- /smarc_worlds/worlds/pipe_following.world: -------------------------------------------------------------------------------- 1 | 2 | 34 | 35 | 36 | 37 | 0.002 38 | 1 39 | 500 40 | 41 | 42 | 0.01 0.01 0.01 1.0 43 | 44 | 45 | 12 46 | 47 | 48 | 1 49 | 50 | 51 | 52 | 53 | 56.71897669633431 54 | 3.515625 55 | 56 | 57 | 58 | 59 | 50 0 150 0 0 0 60 | 1 1 1 1 61 | .1 .1 .1 1 62 | 0.3 0.3 -1 63 | false 64 | 65 | 66 | 67 | 68 | -50 0 -150 0 0 0 69 | 0.6 0.6 0.6 1 70 | 0 0 0 1 71 | -0.3 -0.3 -1 72 | false 73 | 74 | 75 | 76 | 77 | model://ned_frame 78 | 0 0 0 0 0 0 79 | 80 | 81 | 82 | 83 | model://ocean 84 | 0 0 0 0 0 0 85 | 86 | 87 | 88 | 89 | model://sand_heightmap_pipe 90 | 0 0 -95 0 0 0 91 | 92 | 93 | 94 | 95 | model://pipeline 96 | 0 0 -95 0 0 0 97 | pipeline 98 | 99 | 100 | 101 | 102 | model://large_rock 103 | 205 -23 -95 0 0 0 104 | beacon_0 105 | 106 | 107 | 108 | model://large_rock 109 | 170 -60 -95 0 0 0 110 | beacon_1 111 | 112 | 113 | 114 | model://large_rock 115 | 123 -78 -95 0 0 0 116 | beacon_2 117 | 118 | 119 | 120 | model://large_rock 121 | 66 -80 -95 0 0 0 122 | beacon_3 123 | 124 | 125 | 126 | model://large_rock 127 | -10 -80 -95 0 0 0 128 | beacon_4 129 | 130 | 131 | 132 | model://large_rock 133 | -98 -77 -95 0 0 0 134 | beacon_5 135 | 136 | 137 | 138 | model://large_rock 139 | -153 -81 -95 0 0 0 140 | beacon_6 141 | 142 | 143 | 144 | model://large_rock 145 | -198 -111 -95 0 0 0 146 | beacon_7 147 | 148 | 149 | 150 | model://large_rock 151 | -230 -152 -95 0 0 0 152 | beacon_8 153 | 154 | 155 | 156 | 157 | 158 | model://dummy_laser 159 | 0 0 0 0 0 0 160 | dummy_laser 161 | 162 | 163 | 164 | 165 | hydrodynamics 166 | 167 | current_velocity 168 | 169 | 0 170 | -5 171 | 5 172 | 0.0 173 | 0.0 174 | 175 | 176 | 177 | 0 178 | -1.5707963267948966 179 | 1.5707963267948966 180 | 0.0 181 | 0.0 182 | 183 | 184 | 185 | 0 186 | -1.5707963267948966 187 | 1.5707963267948966 188 | 0.0 189 | 0.0 190 | 191 | 192 | 193 | 194 | 195 | 196 | 34.6042 32.2383 -20.6633 0 0.528384 1.41245 197 | orbit 198 | perspective 199 | 200 | 201 | 202 | 203 | -------------------------------------------------------------------------------- /smarc_worlds/worlds/random_rocks.world: -------------------------------------------------------------------------------- 1 | 2 | 34 | 35 | 36 | 37 | 0.002 38 | 1 39 | 500 40 | 41 | 42 | quick 43 | 50 44 | 0.5 45 | 46 | 47 | 48 | 49 | 0.01 0.01 0.01 1.0 50 | 51 | 52 | 12 53 | 54 | 55 | 1 56 | 57 | 0.1 0.2 0.3 1.0 58 | linear 59 | 0.001 60 | 10 61 | 200 62 | 63 | 64 | 65 | 66 | 67 | 56.71897669633431 68 | 3.515625 69 | 70 | 71 | 72 | 73 | model://sun 74 | 75 | 76 | 77 | 78 | model://ned_frame 79 | 0 0 0 0 0 0 80 | 81 | 82 | 83 | 84 | model://small_ocean_box 85 | 0 0 0 0 0 0 86 | 87 | 88 | 89 | 90 | 91 | 92 | true 93 | model://large_rock 94 | 95 | 96 | 0 0 -100 0 0 0 97 | 98 | 500 500 0.1 99 | 100 | 250 101 | 102 | random 103 | 104 | 105 | 106 | 107 | hydrodynamics 108 | 109 | current_velocity 110 | 111 | 0 112 | -5 113 | 5 114 | 0.0 115 | 0.0 116 | 117 | 118 | 119 | 0 120 | -3.141592653589793238 121 | 3.141592653589793238 122 | 0.0 123 | 0.0 124 | 125 | 126 | 127 | 0 128 | -3.141592653589793238 129 | 3.141592653589793238 130 | 0.0 131 | 0.0 132 | 133 | 134 | 135 | 136 | 137 | 138 | -------------------------------------------------------------------------------- /smarc_worlds/worlds/rocks_random.world: -------------------------------------------------------------------------------- 1 | 2 | 34 | 35 | 36 | 37 | 0.002 38 | 1 39 | 500 40 | 41 | 42 | 0.01 0.01 0.01 1.0 43 | 44 | 45 | 12 46 | 47 | 48 | 1 49 | 50 | 51 | 52 | 53 | 56.71897669633431 54 | 3.515625 55 | 56 | 57 | 58 | 59 | 50 0 150 0 0 0 60 | 1 1 1 1 61 | .1 .1 .1 1 62 | 0.3 0.3 -1 63 | false 64 | 65 | 66 | 67 | 68 | -50 0 -150 0 0 0 69 | 0.6 0.6 0.6 1 70 | 0 0 0 1 71 | -0.3 -0.3 -1 72 | false 73 | 74 | 75 | 76 | 77 | model://ned_frame 78 | 0 0 0 0 0 0 79 | 80 | 81 | 82 | 83 | model://ocean 84 | 0 0 0 0 0 0 85 | 86 | 87 | 88 | 89 | 90 | true 91 | model://large_rock 92 | 93 | 94 | 0 0 -100 0 0 0 95 | 96 | 2000 2000 0.1 97 | 98 | 100 99 | 100 | random 101 | 102 | 103 | 104 | 118 | 119 | 134 | 135 | 136 | 137 | model://dummy_laser 138 | 0 0 0 0 0 0 139 | dummy_laser 140 | 141 | 142 | 143 | hydrodynamics 144 | 145 | current_velocity 146 | 147 | 0 148 | -5 149 | 5 150 | 0.0 151 | 0.0 152 | 153 | 154 | 155 | 0 156 | -1.5707963267948966 157 | 1.5707963267948966 158 | 0.0 159 | 0.0 160 | 161 | 162 | 163 | 0 164 | -1.5707963267948966 165 | 1.5707963267948966 166 | 0.0 167 | 0.0 168 | 169 | 170 | 171 | 172 | 173 | 174 | 34.6042 32.2383 -20.6633 0 0.528384 1.41245 175 | orbit 176 | perspective 177 | 178 | 179 | 180 | 181 | -------------------------------------------------------------------------------- /smarc_worlds/worlds/sonar_intensities.world: -------------------------------------------------------------------------------- 1 | 2 | 34 | 35 | 36 | 37 | 0.002 38 | 1 39 | 500 40 | 41 | 42 | 0.01 0.01 0.01 1.0 43 | 44 | 45 | 12 46 | 47 | 48 | 1 49 | 50 | 51 | 52 | 53 | 56.71897669633431 54 | 3.515625 55 | 56 | 57 | 58 | 59 | 50 0 150 0 0 0 60 | 1 1 1 1 61 | .1 .1 .1 1 62 | 0.3 0.3 -1 63 | false 64 | 65 | 66 | 67 | 68 | -50 0 -150 0 0 0 69 | 0.6 0.6 0.6 1 70 | 0 0 0 1 71 | -0.3 -0.3 -1 72 | false 73 | 74 | 75 | 76 | 77 | model://ned_frame 78 | 0 0 0 0 0 0 79 | 80 | 81 | 82 | 83 | model://ocean 84 | 0 0 0 0 0 0 85 | 86 | 87 | 88 | 89 | model://sand_heightmap_pipe 90 | 0 0 -95 0 0 0 91 | 92 | 93 | 94 | 95 | model://large_rock 96 | -172 32 -95 0 0 0 97 | large_rock_1 98 | 99 | 100 | 101 | 102 | model://dummy_laser 103 | 0 0 0 0 0 0 104 | dummy_laser 105 | 106 | 107 | 108 | hydrodynamics 109 | 110 | current_velocity 111 | 112 | 0 113 | -5 114 | 5 115 | 0.0 116 | 0.0 117 | 118 | 119 | 120 | 0 121 | -1.5707963267948966 122 | 1.5707963267948966 123 | 0.0 124 | 0.0 125 | 126 | 127 | 128 | 0 129 | -1.5707963267948966 130 | 1.5707963267948966 131 | 0.0 132 | 0.0 133 | 134 | 135 | 136 | 137 | 138 | 139 | 34.6042 32.2383 -20.6633 0 0.528384 1.41245 140 | orbit 141 | perspective 142 | 143 | 144 | 145 | 146 | --------------------------------------------------------------------------------