├── README.md ├── setup └── install_geographiclib_datasets.sh ├── underwater_sensor_msgs ├── CHANGELOG.rst ├── CMakeLists.txt ├── msg │ ├── DVL.msg │ ├── LedLight.msg │ └── Pressure.msg ├── package.xml └── srv │ └── SpawnMarker.srv ├── underwater_vehicle_dynamics ├── CHANGELOG.rst ├── CMakeLists.txt ├── Makefile ├── config │ └── dynamics_g500.yaml ├── launch │ ├── UWSim_g500_dynamics.launch │ └── dynamics.launch ├── mainpage.dox ├── package.xml └── src │ ├── dynamics.py │ └── keyboard.py └── uwsim ├── CHANGELOG.rst ├── CMakeLists.txt ├── cmake └── Modules │ ├── CheckGeographicLibDatasets.cmake │ └── FindGeographicLib.cmake ├── data ├── scenes │ ├── .vscode │ │ ├── c_cpp_properties.json │ │ └── settings.json │ ├── UWSimScene.dtd │ ├── cirs.xml │ ├── cirs_sls.xml │ ├── cirs_stereo.xml │ ├── common.xacro │ ├── deviceLibrary.xacro │ ├── g500ARM5-rviz.urdf │ ├── g500ARM5.urdf │ ├── g500ARM5.urdf.xacro │ ├── g500ARM5.xml │ ├── g500ARM5_laser.urdf │ ├── installScene │ ├── launch │ │ ├── .gitignore │ │ ├── cuba_aruco.launch │ │ ├── cuba_aruco.rviz │ │ ├── demo-hil.launch │ │ ├── demo-sitl.launch │ │ └── netsim_cirs.launch │ ├── objectLibrary.xacro │ ├── r2d2.urdf │ ├── shapesTest.xml │ ├── ship.xml │ ├── ship_scene.xml │ ├── shipwreck.xml │ ├── ujione-rviz.urdf │ ├── ujione.urdf │ ├── ujione.xml │ ├── vehicleLibrary.xacro │ └── xacroExample.xml.xacro └── shaders │ ├── default_scene.frag │ └── default_scene.vert ├── include └── uwsim │ ├── AcousticCommsChannel.h │ ├── AcousticCommsDevice.h │ ├── BulletPhysics.h │ ├── CommsDevice.h │ ├── ConfigXMLParser.h │ ├── CustomCommsChannel.h │ ├── CustomCommsDevice.h │ ├── CustomWidget.h │ ├── DVLSensor.h │ ├── DredgeTool.h │ ├── EventHandler.h │ ├── ForceSensor.h │ ├── GPSSensor.h │ ├── HUDCamera.h │ ├── InertialMeasurementUnit.h │ ├── KinematicChain.h │ ├── LedArray.h │ ├── MultibeamSensor.h │ ├── NED.h │ ├── NetSim.h │ ├── ObjectPicker.h │ ├── PhysicsBuilder.h │ ├── PressureSensor.h │ ├── ROSInterface.h │ ├── ROSSceneBuilder.h │ ├── SceneBuilder.h │ ├── SimDev_Echo.h │ ├── SimulatedDevice.h │ ├── SimulatedDevices.h │ ├── SimulatedIAUV.h │ ├── SimulatorConfig.h.cmake │ ├── SkyDome.h │ ├── SphereSegment.h │ ├── TextHUD.h │ ├── TrajectoryVisualization.h │ ├── URDFRobot.h │ ├── UWSimUtils.h │ ├── ViewBuilder.h │ ├── VirtualCamera.h │ ├── VirtualRangeSensor.h │ ├── VirtualSLSProjector.h │ ├── osgOceanScene.h │ └── osgPCDLoader.h ├── interface_examples ├── followMarker.cpp ├── gotoAbsolutePose.cpp ├── gotoRelativePose.cpp ├── makeVehicleSurvey.cpp ├── setJointPosition.cpp ├── setJointVelocity.cpp ├── setVehicleActuators.cpp ├── setVehicleDisturbances.cpp ├── setVehiclePose.cpp ├── setVehiclePosition.cpp ├── setVehicleTwist.cpp ├── setVehicleVelocity.cpp └── spawnMarker.cpp ├── license.txt ├── package.xml ├── saveDataFile.sh ├── simdev_plugins.xml └── src ├── AcousticCommsChannel.cpp ├── AcousticCommsDevice.cpp ├── BulletPhysics.cpp ├── CommsDevice.cpp ├── ConfigXMLParser.cpp ├── CustomCommsChannel.cpp ├── CustomCommsDevice.cpp ├── DVLSensor.cpp ├── DredgeTool.cpp ├── ForceSensor.cpp ├── GPSSensor.cpp ├── HUDCamera.cpp ├── InertialMeasurementUnit.cpp ├── KinematicChain.cpp ├── LedArray.cpp ├── MultibeamSensor.cpp ├── NED.cpp ├── NetSim.cpp ├── ObjectPicker.cpp ├── PhysicsBuilder.cpp ├── PressureSensor.cpp ├── ROSInterface.cpp ├── ROSSceneBuilder.cpp ├── SceneBuilder.cpp ├── SimDev_Echo.cpp ├── SimulatedDevices.cpp ├── SimulatedIAUV.cpp ├── SkyDome.cpp ├── SphereSegment.cpp ├── URDFRobot.cpp ├── UWSimUtils.cpp ├── ViewBuilder.cpp ├── VirtualCamera.cpp ├── VirtualRangeSensor.cpp ├── VirtualSLSProjector.cpp ├── main.cpp ├── osgOceanScene.cpp ├── osgPCDLoader.cpp └── uwsim /README.md: -------------------------------------------------------------------------------- 1 | 2 | # UWSim-NET: UWSim with Network Simulator 3 | [![Build Status](http://build.ros.org/job/Mbin_uB64__uwsim__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros.org/job/Mbin_uB64__uwsim__ubuntu_bionic_amd64__binary/) 4 | 5 | The new version of UWSim intregates a Network Simulator to be used along with the dccomms API. This simulator uses the NS3 libraries and the AquaSimNG as a NS3 module. The documentation related to simulate communications is a work in progress and will be included in the Wiki as soon as we can (please, be patience). 6 | 7 | ### Wiki 8 | http://www.irs.uji.es/uwsim/wiki/index.php?title=Main_Page 9 | 10 | ### Citations 11 | - Prats, M.; Perez, J.; Fernandez, J.J.; Sanz, P.J., "An open source tool for simulation and supervision of underwater intervention missions", 2012 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 2577-2582, 7-12 Oct. 2012 12 | - Centelles, D.; Soriano-Asensi, A.; Martí, J.V.; Marín, R.; Sanz, P.J. Underwater Wireless Communications for Cooperative Robotics with UWSim-NET. Appl. Sci. 2019, 9, 3526. 13 | 14 | ### Issues 15 | 16 | #### Exception GeographicLib::GeographicErr when starting uwsim 17 | 18 | If you are getting the following error output when executing rosrun uwsim uwsim: 19 | ``` 20 | ... 21 | data/objects/gun.osg 22 | Starting UWSim... 23 | Loading SimulatedDevices plugin: 'AcousticCommsDevice_Factory' 24 | Loading SimulatedDevices plugin: 'CustomCommsDevice_Factory' 25 | Loading SimulatedDevices plugin: 'DredgeTool_Factory' 26 | Loading SimulatedDevices plugin: 'ForceSensor_Factory' 27 | Loading SimulatedDevices plugin: 'SimDev_Echo_Factory' 28 | . Setting localized world: 7.1e-05s 29 | terminate called after throwing an instance of 'GeographicLib::GeographicErr' 30 | what(): File not readable /usr/share/GeographicLib/geoids/egm96-5.pgm 31 | /opt/ros/melodic/lib/uwsim/uwsim: line 23: 1452 Aborted (core dumped) rosrun uwsim uwsim_binary --dataPath ~/.uwsim/data $@ 32 | ``` 33 | 34 | Run the following commands to fix it: 35 | ``` 36 | wget https://raw.githubusercontent.com/mavlink/mavros/master/mavros/scripts/install_geographiclib_datasets.sh 37 | chmod u+x install_geographiclib_datasets.sh 38 | sudo ./install_geographiclib_datasets.sh 39 | ``` 40 | 41 | 42 | -------------------------------------------------------------------------------- /setup/install_geographiclib_datasets.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # Script to install the model datasets required 3 | # to GeographicLib apply certain conversions 4 | 5 | if [[ $UID != 0 ]]; then 6 | echo "This script require root privileges!" 1>&2 7 | exit 1 8 | fi 9 | 10 | # Install datasets 11 | run_get() { 12 | local dir="$1" 13 | local tool="$2" 14 | local model="$3" 15 | 16 | files=$(shopt -s nullglob dotglob; echo /usr/share/GeographicLib/$dir/$model* /usr/local/share/GeographicLib/$dir/$model*) 17 | if (( ${#files} )); then 18 | echo "GeographicLib $tool dataset $model already exists, skipping" 19 | return 20 | fi 21 | 22 | echo "Installing GeographicLib $tool $model" 23 | geographiclib-get-$tool $model >/dev/null 2>&1 24 | } 25 | 26 | # check which command script is available 27 | if hash geographiclib-get-geoids; then 28 | run_get geoids geoids egm96-5 29 | run_get gravity gravity egm96 30 | run_get magnetic magnetic emm2015 31 | elif hash geographiclib-datasets-download; then # only allows install the goid model dataset 32 | geographiclib-datasets-download egm96_5; 33 | else 34 | echo "OS not supported! Check GeographicLib page for supported OS and lib versions." 1>&2 35 | fi 36 | -------------------------------------------------------------------------------- /underwater_sensor_msgs/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package underwater_sensor_msgs 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.4.1 (2015-12-14) 6 | ------------------ 7 | 8 | 1.4.0 (2015-09-23) 9 | ------------------ 10 | * Added marker service to spawn and modify geometry. 11 | * Contributors: perezsolerj 12 | -------------------------------------------------------------------------------- /underwater_sensor_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(underwater_sensor_msgs) 3 | 4 | find_package(catkin REQUIRED COMPONENTS message_generation roscpp std_msgs visualization_msgs) 5 | 6 | add_message_files( 7 | FILES 8 | DVL.msg 9 | Pressure.msg 10 | LedLight.msg 11 | ) 12 | 13 | add_service_files( 14 | FILES 15 | SpawnMarker.srv 16 | ) 17 | 18 | generate_messages( 19 | DEPENDENCIES roscpp std_msgs visualization_msgs 20 | ) 21 | 22 | catkin_package( 23 | CATKIN_DEPENDS message_runtime roscpp 24 | ) 25 | 26 | 27 | -------------------------------------------------------------------------------- /underwater_sensor_msgs/msg/DVL.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | string header_dvl 3 | string date 4 | float64 salinity 5 | float64 temperature # TODO::ARNAU ha de ser generic ? 6 | float64 depth # TODO::ARNAU ha de ser generic 7 | float64 sound_speed 8 | int32 test 9 | #WATER-MASS, INSTRUMENT-REFERENCED VELOCITY DATA 10 | float64 wi_x_axis 11 | float64 wi_y_axis 12 | float64 wi_z_axis 13 | float64 wi_error 14 | string wi_status 15 | #BOTTOM-TRACK, INSTRUMENT-REFERENCED VELOCITY DATA 16 | float64 bi_x_axis 17 | float64 bi_y_axis 18 | float64 bi_z_axis 19 | float64 bi_error 20 | string bi_status 21 | #WATER-MASS, SHIP-REFERENCED VELOCITY DATA 22 | float64 ws_transverse 23 | float64 ws_longitudinal 24 | float64 ws_normal 25 | string ws_status 26 | #BOTTOM-TRACK, SHIP-REFERENCED VELOCITY DATA 27 | float64 bs_transverse 28 | float64 bs_longitudinal 29 | float64 bs_normal 30 | string bs_status 31 | #WATER-MASS, EARTH-REFERENCED VELOCITY DATA 32 | float64 we_east 33 | float64 we_north 34 | float64 we_upwards 35 | string we_status 36 | #BOTTOM-TRACK, EARTH-REFERENCED VELOCITY DATA 37 | float64 be_east 38 | float64 be_north 39 | float64 be_upwards 40 | string be_status 41 | # WATER-MASS, EARTH-REFERENCED DISTANCE DATA 42 | float64 wd_east 43 | float64 wd_north 44 | float64 wd_upwards 45 | float64 wd_range 46 | float64 wd_time 47 | #BOTTOM-TRACK, EARTH-REFERENCED DISTANCE DATA 48 | float64 bd_east 49 | float64 bd_north 50 | float64 bd_upwards 51 | float64 bd_range 52 | float64 bd_time 53 | #RAW DATA 54 | string raw_data 55 | -------------------------------------------------------------------------------- /underwater_sensor_msgs/msg/LedLight.msg: -------------------------------------------------------------------------------- 1 | duration duration 2 | -------------------------------------------------------------------------------- /underwater_sensor_msgs/msg/Pressure.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | float32 pressure 3 | -------------------------------------------------------------------------------- /underwater_sensor_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | underwater_sensor_msgs 4 | 1.4.2 5 | Common messages for underwater robotics 6 | 7 | Mario Prats 8 | Mario Prats 9 | Javier Perez 10 | Diego Centelles 11 | 12 | BSD 13 | 14 | roscpp 15 | message_generation 16 | std_msgs 17 | visualization_msgs 18 | roscpp 19 | message_runtime 20 | std_msgs 21 | visualization_msgs 22 | 23 | catkin 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /underwater_sensor_msgs/srv/SpawnMarker.srv: -------------------------------------------------------------------------------- 1 | visualization_msgs/Marker marker # Marker to spawn/modify/delete 2 | --- 3 | bool success # Returns True if the request was successfull 4 | string status_message # Comments on the request 5 | -------------------------------------------------------------------------------- /underwater_vehicle_dynamics/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package underwater_vehicle_dynamics 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.4.1 (2015-12-14) 6 | ------------------ 7 | 8 | 1.4.0 (2015-09-23) 9 | ------------------ 10 | -------------------------------------------------------------------------------- /underwater_vehicle_dynamics/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(underwater_vehicle_dynamics) 3 | 4 | find_package(catkin REQUIRED COMPONENTS rospy std_msgs nav_msgs geometry_msgs sensor_msgs tf tf_conversions 5 | #python_orocos_kdl 6 | ) 7 | 8 | catkin_package( 9 | CATKIN_DEPENDS rospy std_msgs nav_msgs geometry_msgs sensor_msgs tf tf_conversions 10 | #python_orocos_kdl 11 | ) 12 | 13 | install(PROGRAMS src/dynamics.py 14 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 15 | 16 | install(PROGRAMS src/keyboard.py 17 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 18 | 19 | install(DIRECTORY launch/ 20 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch 21 | PATTERN ".svn" EXCLUDE) 22 | 23 | install(DIRECTORY config/ 24 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config 25 | PATTERN ".svn" EXCLUDE) 26 | 27 | -------------------------------------------------------------------------------- /underwater_vehicle_dynamics/Makefile: -------------------------------------------------------------------------------- 1 | include $(shell rospack find mk)/cmake.mk -------------------------------------------------------------------------------- /underwater_vehicle_dynamics/config/dynamics_g500.yaml: -------------------------------------------------------------------------------- 1 | # Dynamics model parameters for G500 2 | 3 | g500/num_actuators: 5 4 | g500/dynamics/period: 0.001 5 | g500/dynamics/uwsim_period: 0.001 6 | g500/dynamics/mass: 98.0 7 | g500/dynamics/gravity_center: [0.0, 0.0, 0.05] 8 | g500/dynamics/g: 9.81 9 | g500/dynamics/radius: 0.286 10 | 11 | g500/dynamics/ctf: 0.00006835 12 | g500/dynamics/ctb: 0.00006835 13 | 14 | #Actuator tau for first order actuator dynamic model 15 | g500/dynamics/actuators_tau: [0.2, 0.2, 0.2, 0.2, 0.2] 16 | #Inputs higher than actuators_maxsat will saturate to actuators_maxsat 17 | g500/dynamics/actuators_maxsat: [1, 1, 1, 1, 1] 18 | #Inputs below actuators_minsat will saturate to actuators_minsat 19 | g500/dynamics/actuators_minsat: [-1, -1, -1, -1, -1] 20 | #Inputs to actuators will be scaled to actuators_gain, 21 | g500/dynamics/actuators_gain: [1500, 1500, 1500, 1500, 1500] 22 | 23 | g500/dynamics/dzv: 0.05 24 | g500/dynamics/dv: 0.35 25 | g500/dynamics/dh: 0.4 26 | g500/dynamics/density: 1000.0 27 | 28 | g500/dynamics/tensor: [8.0, 0.0, 0.0, 0.0, 8.0, 0.0, 0.0, 0.0, 8.0] 29 | g500/dynamics/damping: [.0, .0, .0, -130.0, -130.0, -130.0] 30 | g500/dynamics/quadratic_damping: [-148.0, -148.0, -148.0, -180.0, -180.0, -180.0] 31 | 32 | g500/dynamics/Mrb: [ 98.0, 0.0, 0.0, 0.0, 4.9, -0.0, 33 | 0.0, 98.0, 0.0, -4.9, 0.0, 0.0, 34 | 0.0, 0.0, 98.0, 0.0, -0.0, 0.0, 35 | 0.0, -4.9, 0.0, 8.0, 0.0, 0.0, 36 | 4.9, 0.0, -0.0, 0.0, 8.0, 0.0, 37 | -0.0, 0.0, 0.0, 0.0, 0.0, 8.0 ] 38 | 39 | g500/dynamics/Ma: [ 49.0, 0.0, 0.0, 0.0, 0.0, 0.0, 40 | 0.0, 49.0, 0.0, 0.0, 0.0, 0.0, 41 | 0.0, 0.0, 49.0, 0.0, 0.0, 0.0, 42 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 43 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 44 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] 45 | 46 | #G500 thrusters. 47 | #Expresion evaluated at each iteration by the python interpreter 48 | #ct is a vector internally defined such that if u[i]>0 then ct[i]=ctf, else ct[i]=ctb, i>=0 and i 2 | 3 | 4 | 5 | 6 | g500/ForceSensor 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /underwater_vehicle_dynamics/launch/dynamics.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /underwater_vehicle_dynamics/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b underwater_vehicle_dynamics is ... 6 | 7 | 10 | 11 | 12 | \section codeapi Code API 13 | 14 | 24 | 25 | 26 | */ 27 | -------------------------------------------------------------------------------- /underwater_vehicle_dynamics/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | underwater_vehicle_dynamics 4 | 1.4.2 5 | An underwater dynamics module 6 | 7 | Arnau Carrera 8 | Narcis Palomeras 9 | Mario Prats 10 | Mario Prats 11 | Javier Perez 12 | Diego Centelles 13 | 14 | BSD 15 | 16 | rospy 17 | std_msgs 18 | nav_msgs 19 | geometry_msgs 20 | sensor_msgs 21 | tf 22 | tf_conversions 23 | 24 | 25 | rospy 26 | std_msgs 27 | nav_msgs 28 | geometry_msgs 29 | sensor_msgs 30 | tf 31 | tf_conversions 32 | 33 | 34 | catkin 35 | 36 | 37 | -------------------------------------------------------------------------------- /underwater_vehicle_dynamics/src/keyboard.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from std_msgs.msg import Float64MultiArray 4 | import termios, fcntl, sys, os 5 | import rospy 6 | 7 | #import services 8 | from std_srvs.srv import Empty 9 | 10 | if len(sys.argv) != 4: 11 | sys.exit("Usage: "+sys.argv[0]+" ") 12 | 13 | 14 | thrusters_topic=sys.argv[1] 15 | 16 | fd = sys.stdin.fileno() 17 | oldterm = termios.tcgetattr(fd) 18 | newattr = termios.tcgetattr(fd) 19 | newattr[3] = newattr[3] & ~termios.ICANON & ~termios.ECHO 20 | termios.tcsetattr(fd, termios.TCSANOW, newattr) 21 | 22 | oldflags = fcntl.fcntl(fd, fcntl.F_GETFL) 23 | fcntl.fcntl(fd, fcntl.F_SETFL, oldflags | os.O_NONBLOCK) 24 | 25 | pub = rospy.Publisher(thrusters_topic, Float64MultiArray) 26 | rospy.init_node('keyboard') 27 | rospy.wait_for_service('/dynamics/reset') 28 | reset=rospy.ServiceProxy('/dynamics/reset', Empty) 29 | try: 30 | while not rospy.is_shutdown(): 31 | thrusters=[0,0,0,0,0] 32 | msg = Float64MultiArray() 33 | try: 34 | c = sys.stdin.read(1) 35 | ##print "Got character", repr(c) 36 | if c=='w': 37 | thrusters[0]=thrusters[1]=0.4 38 | elif c=='s': 39 | thrusters[0]=thrusters[1]=-0.4 40 | elif c=='a': 41 | thrusters[4]=0.4 42 | elif c=='d': 43 | thrusters[4]=-0.4 44 | elif c==' ': 45 | reset() 46 | elif c=='\x1b': 47 | c2= sys.stdin.read(1) 48 | c2= sys.stdin.read(1) 49 | if c2=='A': 50 | thrusters[2]=thrusters[3]=0.4 51 | elif c2=='B': 52 | thrusters[2]=thrusters[3]=-0.4 53 | elif c2=='C': 54 | thrusters[0]=-0.4 55 | thrusters[1]=0.4 56 | elif c2=='D': 57 | thrusters[0]=0.4 58 | thrusters[1]=-0.4 59 | else: 60 | print 'wrong key pressed' 61 | while c!='': 62 | c = sys.stdin.read(1) 63 | except IOError: pass 64 | msg.data = thrusters 65 | pub.publish(msg) 66 | rospy.sleep(0.1) 67 | finally: 68 | termios.tcsetattr(fd, termios.TCSAFLUSH, oldterm) 69 | fcntl.fcntl(fd, fcntl.F_SETFL, oldflags) 70 | -------------------------------------------------------------------------------- /uwsim/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package uwsim 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.4.1 (2015-12-14) 6 | ------------------ 7 | * Can use package:// to resolve ROS package location for meshes 8 | * Can use package:// to pull a urdf from a ROS package 9 | * Fixed transform publishing to fit the new robotpublisher interface 10 | * Contributors: Bence Magyar, perezsolerj 11 | 12 | 1.4.0 (2015-09-23) 13 | ------------------ 14 | * Initial support for markers and interactive markers issue #1 15 | * MultibeamSensors now use multiple cameras when the FOV is high in order to avoid eyefish distortions and allow FOV >180º. 16 | * Added xacro support for scene building, some xacro macros created as example 17 | * Added lightRate configuration option, shader projection is no longer affected by lighting 18 | * Support for buried objects and dredging 19 | * Added ARMask to Augmented reality elements such as trails, axis, rangeSensor debug, pointclouds and multibeamSensor debug so they are not showed on virtual cameras 20 | * Added debug visible tag to multibeam sensors (similar to range sensor) 21 | * Added gaussian random noise to camera output through shaders 22 | * Underwater particles are no longer present on multibeamSensor, although it can be activated through XML 23 | * Added ROSPointCloudLoader interface that allows to visualize 3D pointclouds on demand. 24 | * Major update on shader management: 25 | * added timeWindow and restarter (y key) to trajectory trail. 26 | * Bugs and fixes. 27 | * Contributors: David Fornas, Miguel Ribeiro, perezsolerj 28 | -------------------------------------------------------------------------------- /uwsim/cmake/Modules/CheckGeographicLibDatasets.cmake: -------------------------------------------------------------------------------- 1 | ## 2 | # This module verifies the installation of the GeographicLib datasets and warns 3 | # if it doesn't detect them. 4 | ## 5 | 6 | find_path(GEOGRAPHICLIB_GEOID_PATH NAMES geoids PATH_SUFFIXES share/GeographicLib share/geographiclib) 7 | find_path(GEOGRAPHICLIB_GRAVITY_PATH_ NAMES gravity PATH_SUFFIXES share/GeographicLib) 8 | find_path(GEOGRAPHICLIB_MAGNETIC_PATH_ NAMES magnetic PATH_SUFFIXES share/GeographicLib) 9 | 10 | if(NOT GEOGRAPHICLIB_GEOID_PATH) 11 | message(STATUS "No geoid model datasets found. This will result on a SIGINT! Please execute the script install_geographiclib_dataset.sh in /mavros/scripts") 12 | else() 13 | message(STATUS "Geoid model datasets found in: " ${GEOGRAPHICLIB_GEOID_PATH}/geoid) 14 | set(GEOGRAPHICLIB_GEOID_PATH ${GEOGRAPHICLIB_GEOID_PATH}/geoid) 15 | endif() 16 | if(NOT GEOGRAPHICLIB_GRAVITY_PATH_) 17 | message(STATUS "No gravity field model datasets found. Please execute the script install_geographiclib_dataset.sh in /mavros/scripts") 18 | else() 19 | message(STATUS "Gravity Field model datasets found in: " ${GEOGRAPHICLIB_GRAVITY_PATH_}/gravity) 20 | set(GEOGRAPHICLIB_GRAVITY_PATH ${GEOGRAPHICLIB_GRAVITY_PATH_}/gravity) 21 | endif() 22 | if(NOT GEOGRAPHICLIB_MAGNETIC_PATH_) 23 | message(STATUS "No magnetic field model datasets found. Please execute the script install_geographiclib_dataset.sh in /mavros/scripts") 24 | else() 25 | message(STATUS "Magnetic Field model datasets found in: " ${GEOGRAPHICLIB_MAGNETIC_PATH_}/magnetic) 26 | set(GEOGRAPHICLIB_MAGNETIC_PATH ${GEOGRAPHICLIB_MAGNETIC_PATH_}/magnetic) 27 | endif() 28 | -------------------------------------------------------------------------------- /uwsim/cmake/Modules/FindGeographicLib.cmake: -------------------------------------------------------------------------------- 1 | # Look for GeographicLib 2 | # 3 | # Set 4 | # GEOGRAPHICLIB_FOUND = TRUE 5 | # GeographicLib_INCLUDE_DIRS = /usr/local/include 6 | # GeographicLib_LIBRARIES = /usr/local/lib/libGeographic.so 7 | # GeographicLib_LIBRARY_DIRS = /usr/local/lib 8 | 9 | find_path (GeographicLib_INCLUDE_DIRS NAMES GeographicLib/Config.h) 10 | 11 | find_library (GeographicLib_LIBRARIES NAMES Geographic) 12 | 13 | include (FindPackageHandleStandardArgs) 14 | find_package_handle_standard_args (GeographicLib DEFAULT_MSG 15 | GeographicLib_LIBRARIES GeographicLib_INCLUDE_DIRS) 16 | mark_as_advanced (GeographicLib_LIBRARIES GeographicLib_INCLUDE_DIRS) 17 | 18 | #message(WARNING "GL: F:${GeographicLib_FOUND} L:${GeographicLib_LIBRARIES} I:${GeographicLib_INCLUDE_DIRS}") 19 | -------------------------------------------------------------------------------- /uwsim/data/scenes/.vscode/c_cpp_properties.json: -------------------------------------------------------------------------------- 1 | { 2 | "configurations": [ 3 | { 4 | "browse": { 5 | "databaseFilename": "", 6 | "limitSymbolsToIncludedHeaders": true 7 | }, 8 | "includePath": [ 9 | "/home/irs/programming/uwsim_ws/devel/include", 10 | "/home/irs/programming/uwsim_deps_ws/install_isolated/include", 11 | "/opt/ros/melodic/include", 12 | "/home/irs/programming/uwsim_ws/src/dccomms_ros_pkgs/dccomms_ros_msgs/include", 13 | "/home/irs/programming/uwsim_ws/src/underwater_simulation/uwsim/include", 14 | "/usr/include" 15 | ], 16 | "name": "Linux", 17 | "intelliSenseMode": "gcc-x64" 18 | } 19 | ], 20 | "version": 4 21 | } -------------------------------------------------------------------------------- /uwsim/data/scenes/.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "python.autoComplete.extraPaths": [ 3 | "/home/irs/programming/uwsim_ws/devel/lib/python2.7/dist-packages", 4 | "/opt/ros/melodic/lib/python2.7/dist-packages" 5 | ] 6 | } -------------------------------------------------------------------------------- /uwsim/data/scenes/deviceLibrary.xacro: -------------------------------------------------------------------------------- 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 | -------------------------------------------------------------------------------- /uwsim/data/scenes/g500ARM5-rviz.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | -------------------------------------------------------------------------------- /uwsim/data/scenes/g500ARM5.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 0.04 8 | 0.04 9 | 12 10 | 1000 11 | 0.35 12 | 1e-7 13 | 0 14 | 2.5 15 | 2.2 16 | 0 17 | 18 | 0.1 19 | 20 | 0 21 | 0.05 22 | 0.3 23 | 24 | 25 | 26 | 27 | 0 28 | 640 29 | 480 30 | 31 | 32 | 33 | 0 34 | girona500/base_link 35 | 60 36 | 1.33 37 | 1 38 | 10000 39 | 40 | 0 41 | 2.5 42 | 0 43 | 44 | 45 | 0 46 | 0 47 | 0 48 | 49 | 50 | 51 | 52 | girona500 53 | data/scenes/g500ARM5.urdf 54 | 55 | 0 56 | 0.7 57 | 1.57 58 | 0 59 | 0 60 | 61 | 62 | 0 63 | 0 64 | -3.5 65 | 66 | 67 | 3.1415 68 |

0

69 | 0 70 | 71 | 72 | bowtech 73 | base_link 74 | 160 75 | 120 76 | 77 | -0.5 78 | 1 79 | -0.2 80 | 81 | 82 | 83 | 84 | 85 | 86 | /uwsim/joint_state 87 | girona500 88 | 89 | 90 | /uwsim/joint_state_command 91 | girona500 92 | 93 | 94 | 95 | 96 | 97 | -------------------------------------------------------------------------------- /uwsim/data/scenes/g500ARM5_laser.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 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 | -------------------------------------------------------------------------------- /uwsim/data/scenes/installScene: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | scriptDir=`dirname $0` 4 | server="http://www.irs.uji.es/uwsim/files" 5 | force="-k" 6 | metaScene=false 7 | clean=false 8 | 9 | while [[ $# > 0 ]] 10 | do 11 | key="$1" 12 | 13 | case $key in 14 | -f|--force) 15 | force="--overwrite" 16 | ;; 17 | 18 | -s|--scene) 19 | metaScene=true 20 | ;; 21 | 22 | -c|--clean) 23 | clean=true 24 | ;; 25 | 26 | *) 27 | filename=$1 # unknown option 28 | ;; 29 | esac 30 | shift # past argument or value 31 | done 32 | 33 | if [ "$metaScene" = true ] 34 | then 35 | wget $server/scenes/metaScenes/$filename -O ./scene.uws 36 | filename="./scene.uws" 37 | else 38 | if [ ! -f $filename ] 39 | then 40 | echo "Can't find $filename file" 41 | exit 42 | fi 43 | fi 44 | 45 | if [ ! -f $scriptDir/UWSimScene.dtd ] 46 | then 47 | echo "Directory where installScene is doesn't seem a UWSim scenes folder (UWSimScene.dtd does not exist)." 48 | exit 49 | fi 50 | 51 | if [ -d $HOME/.uwsim/data ] 52 | then 53 | echo "Installing..." 54 | while read -r line 55 | do 56 | words=( $line ) 57 | location="${words[1]/#\~/$HOME}" #This expands "~" safely. 58 | if [[ "$location" = /* ]] ##Checks if it is an absolute path (we consider relative paths to be in the scriptDir) 59 | then 60 | wget $server/scenes/${words[0]} -O ~/.uwsim/downloaded_scene_data.tgz && tar -zxf ~/.uwsim/downloaded_scene_data.tgz $force -C $location 61 | mkdir -p $scriptDir/$location 62 | else 63 | wget $server/scenes/${words[0]} -O ~/.uwsim/downloaded_scene_data.tgz && tar -zxf ~/.uwsim/downloaded_scene_data.tgz $force -C $scriptDir/$location 64 | mkdir -p $scriptDir/$location/launch 65 | fi 66 | rm ~/.uwsim/downloaded_scene_data.tgz 67 | done < "$filename" 68 | 69 | else 70 | echo "Data directory doesn't exist. Run UWSim for the first time to download basic scene data." 71 | exit 72 | fi 73 | 74 | if [ "$metaScene" = true ] || [ "$clean" = true ] 75 | then 76 | rm "$filename" 77 | fi 78 | -------------------------------------------------------------------------------- /uwsim/data/scenes/launch/.gitignore: -------------------------------------------------------------------------------- 1 | !.gitignore 2 | -------------------------------------------------------------------------------- /uwsim/data/scenes/launch/cuba_aruco.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /uwsim/data/scenes/launch/demo-hil.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /uwsim/data/scenes/launch/demo-sitl.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /uwsim/data/scenes/launch/netsim_cirs.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /uwsim/data/scenes/objectLibrary.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /uwsim/data/scenes/shapesTest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 0.04 8 | 0.04 9 | 12 10 | 1000 11 | 0.35 12 | 1e-7 13 | 0 14 | 2.5 15 | 2.2 16 | 0 17 | 18 | 0.1 19 | 20 | 0 21 | 0.05 22 | 0.3 23 | 24 | 25 | 26 | 27 | 0 28 | 640 29 | 480 30 | 31 | 32 | 33 | 0 34 | r2d2 35 | 60 36 | 1.33 37 | 1 38 | 10000 39 | 40 | 0 41 | 0 42 | 2 43 | 44 | 45 | 1.57 46 | 0 47 | 0 48 | 49 | 50 | 51 | 52 | 53 | r2d2 54 | data/scenes/r2d2.urdf 55 | 56 | 0 57 | 0 58 | -3.5 59 | 60 | 61 | 1.57 62 |

0

63 | 0 64 | 65 | 66 | 67 | 68 | 69 | -------------------------------------------------------------------------------- /uwsim/data/scenes/ship.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 0.04 8 | 0.04 9 | 12 10 | 1000 11 | 0.35 12 | 1e-7 13 | 0 14 | 2.5 15 | 2.2 16 | 0 17 | 18 | 0.1 19 | 20 | 0 21 | 0.05 22 | 0.3 23 | 24 | 25 | 26 | 0.0 27 | 0.05 28 | 0.3 29 | 30 | 31 | 0.015 32 | 0.0075 33 | 0.005 34 | 35 | 36 | 37 | 0 38 | 640 39 | 480 40 | 41 | 42 | 0 43 | ship 44 | 60 45 | 1.33 46 | 1 47 | 10000 48 | 49 | 25 50 | -25 51 | 25 52 | 53 | 54 | 0 55 | 0 56 | 0 57 | 58 | 59 | 60 | 61 | 62 | ship 63 | objects/ship.ive 64 | 65 | 0 66 | 0 67 | 0 68 | 69 | 70 | 0 71 |

0

72 | 0 73 | 74 | 75 | 76 | 77 | 78 | -------------------------------------------------------------------------------- /uwsim/data/scenes/ship_scene.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 0.04 8 | 0.04 9 | 12 10 | 1000 11 | 0.35 12 | 1e-7 13 | 0 14 | 2.5 15 | 2.2 16 | 0 17 | 18 | 0.1 19 | 20 | 0 21 | 0.05 22 | 0.3 23 | 24 | 25 | 26 | 0.0 27 | 0.05 28 | 0.3 29 | 30 | 31 | 0.015 32 | 0.0075 33 | 0.005 34 | 35 | 36 | 37 | 0 38 | 640 39 | 480 40 | 41 | 42 | 0 43 | ship 44 | 60 45 | 1.33 46 | 1 47 | 10000 48 | 49 | 25 50 | -25 51 | 25 52 | 53 | 54 | 0 55 | 0 56 | 0 57 | 58 | 59 | 60 | 61 | 62 | ship 63 | objects/ship.ive 64 | 65 | 0 66 | 0 67 | 0 68 | 69 | 70 | 0 71 |

0

72 | 0 73 | 74 | 75 | 76 | 77 | 78 | -------------------------------------------------------------------------------- /uwsim/data/scenes/shipwreck.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 1.0 8 | 1.0 9 | 12 10 | 10000 11 | 0.35 12 | 1e-8 13 | 0 14 | -2.5 15 | 2.2 16 | 0 17 | 18 | 0.1 19 | 20 | 0.017 21 | 0.027 22 | 0.079 23 | 24 | 25 | 26 | 0.017 27 | 0.027 28 | 0.079 29 | 30 | 31 | 0.015 32 | 0.0075 33 | 0.005 34 | 35 | 36 | 37 | 38 | 0 39 | 800 40 | 600 41 | 42 | 43 | 44 | 1 45 | girona500_RAUVI/base_link 46 | 60 47 | 1.33 48 | 1 49 | 10000 50 | 51 | 16 52 | 9 53 | -7 54 | 55 | 56 | 5 57 | 7 58 | -10 59 | 60 | 61 | 62 | 63 | girona500_RAUVI 64 | data/scenes/g500ARM5.urdf 65 | 66 | 0 67 | M_PI_4 68 | M_PI_4 69 | 0 70 | -0.3 71 | 72 | 73 | 5 74 | 7 75 | -10 76 | 77 | 78 | 3.1415 79 |

0

80 | 0 81 | 82 | 83 | 84 | 85 | girona500_TRIDENT 86 | data/scenes/g500ARM5.urdf 87 | 88 | 0 89 | M_PI_4 90 | M_PI_4 91 | 0 92 | -0.3 93 | 94 | 95 | 7 96 | 5 97 | -11 98 | 99 | 100 | 3.1415 101 |

0

102 | 1.57 103 | 104 | 105 | 106 | 107 | terrain 108 | terrain/shipwreck/terrain.ive 109 | 110 | 0 111 | 0 112 | 0 113 | 114 | 115 | 0 116 |

0

117 | 0 118 | 119 | 120 | 0 121 | trimesh 122 | 123 | 124 | 125 | 126 | 127 | /dataNavigator_G500RAUVI 128 | girona500_RAUVI 129 | 130 | 131 | /dataNavigator_G500TRIDENT 132 | girona500_TRIDENT 133 | 134 | 135 | /uwsim/girona500_odom_RAUVI 136 | girona500_RAUVI 137 | 138 | 139 | /uwsim/girona500_odom_TRIDENT 140 | girona500_TRIDENT 141 | 142 | 143 | world 144 | 0 145 | 10 146 | 147 | 148 | 149 | 150 | 151 | -------------------------------------------------------------------------------- /uwsim/data/scenes/ujione-rviz.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | -------------------------------------------------------------------------------- /uwsim/data/scenes/ujione.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | -------------------------------------------------------------------------------- /uwsim/data/scenes/ujione.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 0.04 8 | 0.04 9 | 12 10 | 1000 11 | 0.35 12 | 1e-7 13 | 0 14 | 2.5 15 | 2.2 16 | 0 17 | 18 | 0.1 19 | 20 | 0 21 | 0.05 22 | 0.3 23 | 24 | 25 | 26 | 27 | 0 28 | 640 29 | 480 30 | 31 | 32 | 33 | 0 34 | ujione/ujione_jaw_inner 35 | 60 36 | 1.33 37 | 0.05 38 | 10000 39 | 40 | 0 41 | 1.5 42 | 0 43 | 44 | 45 | 0 46 | 0 47 | 0 48 | 49 | 50 | 51 | 52 | ujione 53 | data/scenes/ujione.urdf 54 | 55 | -0.4 56 | 57 | 58 | 0 59 | 0 60 | -3.5 61 | 62 | 63 | 0 64 |

0

65 | 0 66 | 67 | 68 | 69 | 70 | 71 | -------------------------------------------------------------------------------- /uwsim/data/scenes/vehicleLibrary.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /uwsim/data/scenes/xacroExample.xml.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 0 20 | 0 21 | 2.2 22 | 0 23 | 0.3 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /uwsim/data/shaders/default_scene.vert: -------------------------------------------------------------------------------- 1 | // osgOcean Uniforms 2 | // ----------------- 3 | uniform mat4 osg_ViewMatrixInverse; 4 | uniform float osgOcean_WaterHeight; 5 | uniform vec3 osgOcean_Eye; 6 | uniform vec3 osgOcean_UnderwaterAttenuation; 7 | uniform vec4 osgOcean_UnderwaterDiffuse; 8 | uniform bool osgOcean_EnableUnderwaterScattering; 9 | // ----------------- 10 | 11 | varying vec3 vExtinction; 12 | varying vec3 vInScattering; 13 | 14 | varying vec3 vNormal; 15 | varying vec3 vLightDir; 16 | varying vec3 vEyeVec; 17 | varying float vWorldHeight; 18 | varying vec4 color; 19 | 20 | //Laser variables 21 | // ----------------- 22 | uniform mat4 LightModelViewProjectionMatrix; 23 | 24 | varying vec4 ShadowCoord; 25 | varying vec4 textureCoord; 26 | // ----------------- 27 | 28 | void computeScattering( in vec3 eye, in vec3 worldVertex, out vec3 extinction, out vec3 inScattering ) 29 | { 30 | float viewDist = length(eye-worldVertex); 31 | 32 | float depth = max(osgOcean_WaterHeight-worldVertex.z, 0.0); 33 | 34 | extinction = exp(-osgOcean_UnderwaterAttenuation*viewDist*2.0); 35 | 36 | // Need to compute accurate kd constant. 37 | // const vec3 kd = vec3(0.001, 0.001, 0.001); 38 | inScattering = osgOcean_UnderwaterDiffuse.rgb * (1.0-extinction*exp(-depth*vec3(0.001))); 39 | } 40 | 41 | void main(void) 42 | { 43 | gl_TexCoord[0] = gl_MultiTexCoord0; 44 | gl_Position = ftransform(); 45 | gl_FogFragCoord = gl_Position.z; 46 | gl_ClipVertex = gl_ModelViewMatrix * gl_Vertex; // for reflections 47 | 48 | vNormal = gl_NormalMatrix * gl_Normal; 49 | vLightDir = gl_LightSource[osgOcean_LightID].position.xyz; 50 | vEyeVec = -vec3(gl_ModelViewMatrix*gl_Vertex); 51 | 52 | vec4 worldVertex = (osg_ViewMatrixInverse*gl_ModelViewMatrix) * gl_Vertex; 53 | 54 | //Laser computations 55 | ShadowCoord = LightModelViewProjectionMatrix*worldVertex; 56 | // ----------------- 57 | 58 | if (osgOcean_EnableUnderwaterScattering) 59 | computeScattering( osgOcean_Eye, worldVertex.xyz, vExtinction, vInScattering); 60 | 61 | 62 | vWorldHeight = worldVertex.z; 63 | color=gl_FrontMaterial.diffuse; 64 | } 65 | -------------------------------------------------------------------------------- /uwsim/include/uwsim/AcousticCommsChannel.h: -------------------------------------------------------------------------------- 1 | #ifndef UWSIM_ACOUSTICCOMMSCHANNEL_ 2 | #define UWSIM_ACOUSTICCOMMSCHANNEL_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace uwsim { 9 | 10 | class AcousticCommsChannel { 11 | public: 12 | AcousticCommsChannelConfig config; 13 | AcousticCommsChannel(AcousticCommsChannelConfig cfg); 14 | private: 15 | bool _AddToNetSim(); 16 | 17 | }; 18 | } 19 | #endif 20 | -------------------------------------------------------------------------------- /uwsim/include/uwsim/AcousticCommsDevice.h: -------------------------------------------------------------------------------- 1 | #ifndef AcousticCommsDevice_ECHO_H_ 2 | #define AcousticCommsDevice_ECHO_H_ 3 | #include "ConfigXMLParser.h" 4 | #include "ROSInterface.h" 5 | #include "SimulatedDevice.h" 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | using namespace uwsim; 15 | 16 | class AcousticCommsDevice_Config : public CommsDevice_Config { 17 | 18 | public: 19 | // XML members 20 | double range, pT, frequency, L, K, turnOnEnergy, turnOffEnergy, preamble, 21 | pTConsume, pRConsume, pIdle, codingEff, bitErrorRate, energyLevel; 22 | uint32_t symbolsPerSecond; 23 | // constructor 24 | AcousticCommsDevice_Config(std::string type_) 25 | : CommsDevice_Config(type_), range(100), pT(0.2818), frequency(25), 26 | L(0.0), K(2.0), turnOnEnergy(0.0), turnOffEnergy(0.0), preamble(0.0), 27 | pTConsume(0.660), pRConsume(0.395), pIdle(0.0) {} 28 | }; 29 | 30 | class AcousticCommsDevice : public UWSimCommsDevice { 31 | public: 32 | AcousticCommsDevice_Config *config; 33 | AcousticCommsDevice(AcousticCommsDevice_Config *cfg, 34 | osg::ref_ptr target, SimulatedIAUV *auv); 35 | void AddToNetSim(); 36 | static uint32_t nDevsReady; 37 | static uint32_t nDevs; 38 | void SetConfig(CommsDevice_Config *cfg); 39 | CommsDevice_Config *GetConfig(); 40 | 41 | protected: 42 | bool _AddToNetSim(); 43 | 44 | private: 45 | ros::ServiceClient _addService, _linkToChannelService; 46 | }; 47 | 48 | /* You will need to add your code HERE */ 49 | class AcousticCommsDevice_Factory : public CommsDevice_Factory { 50 | public: 51 | // this is the only place the device/interface type is set 52 | AcousticCommsDevice_Factory(std::string type_ = "AcousticCommsDevice") 53 | : CommsDevice_Factory(type_){}; 54 | UWSimCommsDevice *Create(CommsDevice_Config *cfg, 55 | osg::ref_ptr target, SimulatedIAUV *auv); 56 | SimulatedDeviceConfig::Ptr processConfig(const xmlpp::Node *node, 57 | ConfigFile *config); 58 | }; 59 | 60 | class AcousticCommsDevice_ROSPublisher : public CommsDevice_ROSPublisher { 61 | public: 62 | AcousticCommsDevice_ROSPublisher(AcousticCommsDevice *dev, std::string topic, 63 | int rate) 64 | : CommsDevice_ROSPublisher(dev, topic, rate) {} 65 | 66 | ~AcousticCommsDevice_ROSPublisher() {} 67 | }; 68 | 69 | #endif 70 | -------------------------------------------------------------------------------- /uwsim/include/uwsim/BulletPhysics.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013 University of Jaume-I. 3 | * All rights reserved. This program and the accompanying materials 4 | * are made available under the terms of the GNU Public License v3.0 5 | * which accompanies this distribution, and is available at 6 | * http://www.gnu.org/licenses/gpl.html 7 | * 8 | * Contributors: 9 | * Mario Prats 10 | * Javier Perez 11 | */ 12 | 13 | #ifndef BULLETPHYSICS_H_ 14 | #define BULLETPHYSICS_H_ 15 | 16 | #include "SimulatorConfig.h" 17 | #include "UWSimUtils.h" 18 | 19 | #include 20 | #include 21 | #include 22 | 23 | #include 24 | #include 25 | 26 | #include 27 | 28 | //#include 29 | 30 | #define UWSIM_DEFAULT_GRAVITY btVector3(0,0,-1) 31 | 32 | // Define filter groups 33 | enum CollisionTypes 34 | { 35 | COL_NOTHING = 0x00000000, COL_OBJECTS = 0x00000001, COL_VEHICLE = 0x00000010, COL_EVERYTHING = 0x11111111, 36 | }; 37 | 38 | /*class NodeDataType : public osg::Referenced{ 39 | public: 40 | NodeDataType(btRigidBody * rigidBody,int catcha){ catchable=catcha; rb=rigidBody;}; 41 | int catchable; 42 | btRigidBody * rb; 43 | 44 | };*/ 45 | 46 | class CollisionDataType : public osg::Referenced 47 | { 48 | public: 49 | CollisionDataType(std::string nam, std::string vehName, int isVehi) 50 | { 51 | vehicleName = vehName; 52 | name = nam; 53 | isVehicle = isVehi; 54 | } 55 | ; 56 | std::string getObjectName() 57 | { 58 | if (isVehicle) 59 | return vehicleName; 60 | else 61 | return name; 62 | } 63 | ; 64 | std::string name, vehicleName; 65 | int isVehicle; 66 | 67 | }; 68 | 69 | //Adds tick callback manager which will do all stuff needed in pretick callback 70 | void preTickCallback(btDynamicsWorld *world, btScalar timeStep); 71 | 72 | //Adds tick callback manager which will do all stuff needed in posttick callback 73 | void postTickCallback(btDynamicsWorld *world, btScalar timeStep); 74 | 75 | class BulletPhysics : public osg::Referenced 76 | { 77 | 78 | public: 79 | typedef enum 80 | { 81 | SHAPE_BOX, SHAPE_SPHERE, SHAPE_TRIMESH, SHAPE_COMPOUND_TRIMESH, SHAPE_COMPOUND_BOX, SHAPE_COMPOUND_CYLINDER 82 | } collisionShapeType_t; 83 | 84 | btDiscreteDynamicsWorld * dynamicsWorld; 85 | //osgbCollision::GLDebugDrawer debugDrawer; 86 | 87 | BulletPhysics(PhysicsConfig physicsConfig, osgOcean::OceanTechnique* oceanSurf); 88 | 89 | void setGravity(btVector3 g) 90 | { 91 | dynamicsWorld->setGravity(g); 92 | } 93 | btRigidBody* addObject(osg::MatrixTransform *root, osg::Node *node, CollisionDataType * data, 94 | std::shared_ptr pp, osg::Node * colShape = NULL); 95 | 96 | void stepSimulation(btScalar timeStep, int maxSubSteps, btScalar fixedTimeStep); 97 | void printManifolds(); 98 | 99 | int getNumCollisions(); 100 | 101 | btRigidBody* copyObject(btRigidBody * copied); 102 | int physicsStep; 103 | 104 | btPersistentManifold * getCollision(int i); 105 | 106 | ~BulletPhysics() 107 | { 108 | } 109 | ; 110 | 111 | //This class stores information related to different sources that need to be called in internal tick callbacks. 112 | //To know force sensors. 113 | class TickCallbackManager 114 | { 115 | private: 116 | //Force sensor structures; 117 | struct ForceSensorcbInfo 118 | { 119 | btRigidBody * copy, * target; 120 | btVector3 linInitial, angInitial,linFinal, angFinal; 121 | }; 122 | std::vector forceSensors; 123 | 124 | void preTickForceSensors(); 125 | void postTickForceSensors(); 126 | public: 127 | 128 | int substep; //Tracks number of substep; 129 | 130 | TickCallbackManager(){}; 131 | int addForceSensor(btRigidBody * copy, btRigidBody * target); 132 | void getForceSensorSpeed(int forceSensor, double linSpeed[3],double angSpeed[3]); 133 | void physicsInternalPreProcessCallback(btScalar timeStep); 134 | void physicsInternalPostProcessCallback(btScalar timeStep); 135 | }; 136 | 137 | 138 | TickCallbackManager * callbackManager; 139 | 140 | private: 141 | btDefaultCollisionConfiguration * collisionConfiguration; 142 | btCollisionDispatcher * dispatcher; 143 | btConstraintSolver * solver; 144 | btBroadphaseInterface * inter; 145 | 146 | osgOcean::OceanTechnique* oceanSurface; 147 | 148 | void cleanManifolds(); 149 | btCollisionShape* GetCSFromOSG(osg::Node * node, collisionShapeType_t ctype); 150 | 151 | }; 152 | 153 | 154 | #endif 155 | 156 | -------------------------------------------------------------------------------- /uwsim/include/uwsim/CommsDevice.h: -------------------------------------------------------------------------------- 1 | #ifndef CommsDevice_ECHO_H_ 2 | #define CommsDevice_ECHO_H_ 3 | #include "ConfigXMLParser.h" 4 | #include "ROSInterface.h" 5 | #include "SimulatedDevice.h" 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | using namespace uwsim; 13 | 14 | class CommsDevice_Config : public SimulatedDeviceConfig { 15 | 16 | public: 17 | // XML members 18 | std::string relativeTo, tfId, relativeTfId, dccommsId, logLevel; 19 | PacketBuilderConfig txPacketBuilderConfig, rxPacketBuilderConfig; 20 | unsigned int mac, channelId, txFifoSize, maxBackoffSlots; 21 | double position[3]{0, 0, 0}, orientation[3]{0, 0, 0}; 22 | Mesh mesh; 23 | int disable = 0; 24 | std::string macProtocol; 25 | double macDistance; 26 | 27 | CommsDevice_Config(std::string type_) : SimulatedDeviceConfig(type_) {} 28 | }; 29 | 30 | class UWSimCommsDevice : public SimulatedDevice { 31 | public: 32 | osg::Node *parent; 33 | SimulatedIAUV *auv; 34 | osg::ref_ptr target; 35 | ros::NodeHandle node; 36 | std::string targetTfId, tfId; 37 | bool render; 38 | dccomms::PacketBuilderPtr txPacketBuilder, rxPacketBuilder; 39 | 40 | UWSimCommsDevice(CommsDevice_Config *cfg, osg::ref_ptr target, 41 | SimulatedIAUV *auv); 42 | 43 | void Init(CommsDevice_Config *cfg, osg::ref_ptr target, 44 | SimulatedIAUV *auv); 45 | 46 | void AddToNetSim(); 47 | virtual CommsDevice_Config *GetConfig() = 0; 48 | virtual void SetConfig(CommsDevice_Config *cfg) = 0; 49 | void SetPacketBuilder(PACKET_TYPE pType, CommsDevice_Config *cfg, 50 | PacketBuilderConfig *pbcfg); 51 | 52 | protected: 53 | virtual bool _AddToNetSim() = 0; 54 | 55 | private: 56 | ros::ServiceClient _checkService, _rmService; 57 | bool _Check(); 58 | bool _Remove(); 59 | }; 60 | 61 | /* You will need to add your code HERE */ 62 | class CommsDevice_Factory : public SimulatedDeviceFactory { 63 | public: 64 | // this is the only place the device/interface type is set 65 | CommsDevice_Factory(std::string type_ = "CommsDevice") 66 | : SimulatedDeviceFactory(type_){}; 67 | virtual UWSimCommsDevice *Create(CommsDevice_Config *cfg, 68 | osg::ref_ptr target, 69 | SimulatedIAUV *auv) = 0; 70 | 71 | virtual void processCommonConfig(const xmlpp::Node *node, ConfigFile *config, 72 | CommsDevice_Config *commonConfig); 73 | 74 | // Methods from SimulatedDevice 75 | virtual bool applyConfig(SimulatedIAUV *auv, Vehicle &vehicleChars, 76 | SceneBuilder *sceneBuilder, size_t iteration); 77 | virtual std::vector> 78 | getInterface(ROSInterfaceInfo &rosInterface, 79 | std::vector> &iauvFile); 80 | virtual SimulatedDeviceConfig::Ptr processConfig(const xmlpp::Node *node, 81 | ConfigFile *config) = 0; 82 | }; 83 | 84 | class CommsDevice_ROSPublisher : public ROSPublisherInterface { 85 | public: 86 | CommsDevice_ROSPublisher(UWSimCommsDevice *dev, std::string topic, int rate) 87 | : ROSPublisherInterface(topic, rate), dev(dev) {} 88 | 89 | UWSimCommsDevice *dev; 90 | virtual void createPublisher(ros::NodeHandle &nh); 91 | virtual void publish(); 92 | 93 | ~CommsDevice_ROSPublisher() {} 94 | 95 | private: 96 | tf::TransformBroadcaster _tfBr; 97 | }; 98 | 99 | #endif 100 | -------------------------------------------------------------------------------- /uwsim/include/uwsim/CustomCommsChannel.h: -------------------------------------------------------------------------------- 1 | #ifndef UWSIM_CUSTOMCOMMSCHANNEL_ 2 | #define UWSIM_CUSTOMCOMMSCHANNEL_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace uwsim { 9 | 10 | class CustomCommsChannel { 11 | public: 12 | CustomCommsChannelConfig config; 13 | CustomCommsChannel(CustomCommsChannelConfig cfg); 14 | private: 15 | bool _AddToNetSim(); 16 | 17 | }; 18 | } 19 | #endif 20 | -------------------------------------------------------------------------------- /uwsim/include/uwsim/CustomCommsDevice.h: -------------------------------------------------------------------------------- 1 | #ifndef CustomCommsDevice_ECHO_H_ 2 | #define CustomCommsDevice_ECHO_H_ 3 | #include "ConfigXMLParser.h" 4 | #include "ROSInterface.h" 5 | #include "SimulatedDevice.h" 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | using namespace uwsim; 16 | 17 | class CustomCommsDevice_Config : public CommsDevice_Config { 18 | 19 | public: 20 | // XML members 21 | unsigned int txChannelId, rxChannelId; 22 | double bitrate; 23 | double txJitter, rxJitter; 24 | double intrinsicDelay; 25 | double maxDistance, minDistance; 26 | double pktErrRatioIncPerMeter, minPktErrRatio; 27 | std::string errorRateExpr, errorUnit; 28 | 29 | // constructor 30 | CustomCommsDevice_Config(std::string type_) : CommsDevice_Config(type_) {} 31 | }; 32 | 33 | class CustomCommsDevice : public UWSimCommsDevice { 34 | public: 35 | CustomCommsDevice_Config *config; 36 | CustomCommsDevice(CustomCommsDevice_Config *cfg, 37 | osg::ref_ptr target, SimulatedIAUV *auv); 38 | void AddToNetSim(); 39 | static uint32_t nDevsReady; 40 | static uint32_t nDevs; 41 | void SetConfig(CommsDevice_Config *cfg); 42 | CommsDevice_Config *GetConfig(); 43 | 44 | protected: 45 | bool _AddToNetSim(); 46 | 47 | private: 48 | ros::ServiceClient _addService, _linkToChannelService; 49 | }; 50 | 51 | /* You will need to add your code HERE */ 52 | class CustomCommsDevice_Factory : public CommsDevice_Factory { 53 | public: 54 | // this is the only place the device/interface type is set 55 | CustomCommsDevice_Factory(std::string type_ = "CustomCommsDevice") 56 | : CommsDevice_Factory(type_){}; 57 | UWSimCommsDevice *Create(CommsDevice_Config *cfg, 58 | osg::ref_ptr target, SimulatedIAUV *auv); 59 | SimulatedDeviceConfig::Ptr processConfig(const xmlpp::Node *node, 60 | ConfigFile *config); 61 | }; 62 | 63 | class CustomCommsDevice_ROSPublisher : public CommsDevice_ROSPublisher { 64 | public: 65 | CustomCommsDevice_ROSPublisher(CustomCommsDevice *dev, std::string topic, 66 | int rate) 67 | : CommsDevice_ROSPublisher(dev, topic, rate) {} 68 | 69 | ~CustomCommsDevice_ROSPublisher() {} 70 | }; 71 | 72 | #endif 73 | -------------------------------------------------------------------------------- /uwsim/include/uwsim/CustomWidget.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013 University of Jaume-I. 3 | * All rights reserved. This program and the accompanying materials 4 | * are made available under the terms of the GNU Public License v3.0 5 | * which accompanies this distribution, and is available at 6 | * http://www.gnu.org/licenses/gpl.html 7 | * 8 | * Contributors: 9 | * Mario Prats 10 | * Javier Perez 11 | */ 12 | 13 | #ifndef CUSTOMWIDGET 14 | #define CUSTOMWIDGET 15 | 16 | #include "SimulatorConfig.h" 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | class CustomWidget 23 | { 24 | public: 25 | CustomWidget() 26 | { 27 | } 28 | 29 | virtual osg::ref_ptr getWidgetWindow()=0; 30 | 31 | ~CustomWidget() 32 | { 33 | } 34 | }; 35 | 36 | #endif 37 | 38 | -------------------------------------------------------------------------------- /uwsim/include/uwsim/DVLSensor.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013 University of Jaume-I. 3 | * All rights reserved. This program and the accompanying materials 4 | * are made available under the terms of the GNU Public License v3.0 5 | * which accompanies this distribution, and is available at 6 | * http://www.gnu.org/licenses/gpl.html 7 | * 8 | * Contributors: 9 | * Mario Prats 10 | * Javier Perez 11 | */ 12 | 13 | #ifndef DVLSENSOR_H_ 14 | #define DVLSENSOR_H_ 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | #include 22 | 23 | #include 24 | #include 25 | 26 | class DVLSensor 27 | { 28 | 29 | public: 30 | std::string name,parentLinkName; 31 | 32 | /** Constructor 33 | * @param name the name of the dvl sensor 34 | * @param parentName the name of the link that holds the DVL sensor 35 | * @param parent the node of the scene graph that holds the sensor 36 | * @param rMl the sensor measures are given with respect to the root (r). Use rMl to transform them to another frame ('l' is the new frame, typically the localized world) 37 | * @param std the standard deviation on the sensor measures 38 | */ 39 | DVLSensor(std::string sensor_name, std::string parentName, osg::Node *parent, osg::Matrixd rMl, double std = 0) : 40 | name(sensor_name) ,parentLinkName(parentName), parent_(parent), rMl_(rMl), std_(std) 41 | { 42 | node_ = new osg::Node(); 43 | parent->asGroup()->addChild(node_); 44 | tprevious_ = ros::Time::now(); 45 | } 46 | 47 | osg::Vec3d getMeasurement(); 48 | int getTFTransform(tf::Pose & pose, std::string & parent); 49 | 50 | double getStandardDeviation() 51 | { 52 | return std_; 53 | } 54 | 55 | virtual ~DVLSensor() 56 | { 57 | } 58 | 59 | private: 60 | osg::ref_ptr parent_; 61 | osg::Matrixd rMl_; 62 | double std_; 63 | osg::ref_ptr node_; 64 | ros::Time tprevious_; 65 | osg::Vec3d xprevious_; 66 | 67 | boost::mt19937 rng_; ///< Boost random number generator 68 | }; 69 | 70 | #endif /* DVLSENSOR_H_ */ 71 | -------------------------------------------------------------------------------- /uwsim/include/uwsim/DredgeTool.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013 University of Jaume-I. 3 | * All rights reserved. This program and the accompanying materials 4 | * are made available under the terms of the GNU Public License v3.0 5 | * which accompanies this distribution, and is available at 6 | * http://www.gnu.org/licenses/gpl.html 7 | * 8 | * Contributors: 9 | * Mario Prats 10 | * Javier Perez 11 | */ 12 | 13 | 14 | #ifndef DREDGE_TOOL_H_ 15 | #define DREDGE_TOOL_H_ 16 | #include "SimulatedDevice.h" 17 | using namespace uwsim; 18 | 19 | //PARTICLE SYSTEM FUNCTIONS 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | #include 26 | #include 27 | 28 | //This operator attracts the particles to a sink (dredge tool). 29 | //This class could be in UWSimUtils as it is not specific for Dredge Tool, but I left it here because I think 30 | // it will not be used anywhere else 31 | class AttractOperator : public osgParticle::Operator 32 | { 33 | public: 34 | AttractOperator() : osgParticle::Operator(), _magnitude(1.0f), _killSink(true){} 35 | AttractOperator( const AttractOperator& copy, const osg::CopyOp& copyop = osg::CopyOp::SHALLOW_COPY ) 36 | : osgParticle::Operator(copy, copyop), _magnitude(copy._magnitude), _killSink(copy._killSink) 37 | {} 38 | META_Object( spin, AttractOperator ); 39 | 40 | /// Set the acceleration scale 41 | void setMagnitude( float mag ) { _magnitude = mag; } 42 | /// Get the acceleration scale 43 | float getMagnitude() const { return _magnitude; } 44 | /// Set whether the attractor kills the particles once they arrive 45 | void setKillSink( bool kill ) { _killSink = kill; } 46 | /// Get whether the attractor kills the particles once they arrive 47 | bool getKillSink() const { return _killSink; } 48 | /// Apply attraction to a particle. Do not call this method manually. 49 | inline void operate( osgParticle::Particle* P, double dt ); 50 | 51 | protected: 52 | virtual ~AttractOperator() {} 53 | AttractOperator& operator=( const AttractOperator& ) { return *this; } 54 | float _magnitude; 55 | bool _killSink; 56 | }; 57 | 58 | //Creates a particle system 59 | osgParticle::ParticleSystem* createSmokeParticles( osg::Group* parent, osgParticle::RandomRateCounter * rrc); 60 | 61 | 62 | //Driver/ROSInterface configuration 63 | class DredgeTool_Config : public SimulatedDeviceConfig 64 | { 65 | public: 66 | //XML members 67 | std::string target; 68 | double offsetp[3]; 69 | double offsetr[3]; 70 | //constructor 71 | DredgeTool_Config(std::string type_) : 72 | SimulatedDeviceConfig(type_) 73 | { 74 | } 75 | }; 76 | 77 | //Driver/ROSInterface factory class 78 | class DredgeTool_Factory : public SimulatedDeviceFactory 79 | { 80 | public: 81 | //this is the only place the device/interface type is set 82 | DredgeTool_Factory(std::string type_ = "DredgeTool") : 83 | SimulatedDeviceFactory(type_) 84 | { 85 | } 86 | ; 87 | 88 | SimulatedDeviceConfig::Ptr processConfig(const xmlpp::Node* node, ConfigFile * config); 89 | bool applyConfig(SimulatedIAUV * auv, Vehicle &vehicleChars, SceneBuilder *sceneBuilder, size_t iteration); 90 | }; 91 | 92 | //can be a sparate header file for actual implementation classes... 93 | 94 | #include "ConfigXMLParser.h" 95 | #include "ROSInterface.h" 96 | #include 97 | #include "UWSimUtils.h" 98 | 99 | //Driver class 100 | class DredgeTool : public SimulatedDevice, public AbstractDredgeTool 101 | { 102 | osg::ref_ptr target; 103 | osgParticle::ParticleSystem* smoke; 104 | osgParticle::RandomRateCounter * rrc; 105 | 106 | //Number of particles in the system (this should decrease slowly) 107 | int particles; 108 | 109 | void applyPhysics(BulletPhysics * bulletPhysics) 110 | { 111 | } 112 | public: 113 | std::string info; //Device's property 114 | 115 | DredgeTool(DredgeTool_Config * cfg, osg::ref_ptr target); 116 | 117 | virtual std::shared_ptr getDredgePosition(); 118 | 119 | void dredgedParticles(int nparticles); 120 | 121 | 122 | }; 123 | 124 | 125 | #endif 126 | -------------------------------------------------------------------------------- /uwsim/include/uwsim/ForceSensor.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013 University of Jaume-I. 3 | * All rights reserved. This program and the accompanying materials 4 | * are made available under the terms of the GNU Public License v3.0 5 | * which accompanies this distribution, and is available at 6 | * http://www.gnu.org/licenses/gpl.html 7 | * 8 | * Contributors: 9 | * Mario Prats 10 | * Javier Perez 11 | */ 12 | 13 | #ifndef FORCESENSOR_H_ 14 | #define FORCESENSOR_H_ 15 | #include "SimulatedDevice.h" 16 | using namespace uwsim; 17 | 18 | class ForceSensor_Config : public SimulatedDeviceConfig 19 | { 20 | public: 21 | //XML members 22 | std::string target; 23 | double offsetp[3]; 24 | double offsetr[3]; 25 | //constructor 26 | ForceSensor_Config(std::string type_) : 27 | SimulatedDeviceConfig(type_) 28 | { 29 | } 30 | }; 31 | 32 | //Driver/ROSInterface factory class 33 | class ForceSensor_Factory : public SimulatedDeviceFactory 34 | { 35 | public: 36 | //this is the only place the device/interface type is set 37 | ForceSensor_Factory(std::string type_ = "ForceSensor") : 38 | SimulatedDeviceFactory(type_) 39 | { 40 | } 41 | ; 42 | 43 | SimulatedDeviceConfig::Ptr processConfig(const xmlpp::Node* node, ConfigFile * config); 44 | bool applyConfig(SimulatedIAUV * auv, Vehicle &vehicleChars, SceneBuilder *sceneBuilder, size_t iteration); 45 | std::vector > getInterface(ROSInterfaceInfo & rosInterface, 46 | std::vector > & iauvFile); 47 | }; 48 | 49 | //can be a sparate header file for actual implementation classes... 50 | 51 | #include "ConfigXMLParser.h" 52 | #include "ROSInterface.h" 53 | #include 54 | #include 55 | #include "BulletPhysics.h" 56 | 57 | //Driver class 58 | 59 | class ForceSensor : public SimulatedDevice 60 | { 61 | void applyPhysics(BulletPhysics * bulletPhysics); 62 | double lastTimeStep; 63 | int CBreference; 64 | public: 65 | BulletPhysics * physics; 66 | btRigidBody * copy, * btTarget; //Rigid object copy with physical reaction 67 | osg::ref_ptr target; 68 | double offsetp[3]; 69 | osg::Matrixd offset; //We only need rotation as traslation goes to bullet directly 70 | int physicsApplied; 71 | 72 | ForceSensor(ForceSensor_Config * cfg, osg::ref_ptr target); 73 | void getForceTorque(double force[3], double torque[3]); 74 | }; 75 | 76 | 77 | 78 | //ROS publishers and subscribers work exactly as before, no subclassing is needed 79 | class ForceSensor_ROSPublisher : public ROSPublisherInterface 80 | { 81 | //this is just an example, use a pointer to SimulatedIAUV, if only ROSInterface is implemented 82 | //pointer to a device 83 | ForceSensor * dev; 84 | public: 85 | ForceSensor_ROSPublisher(ForceSensor *dev, std::string topic, int rate) : 86 | ROSPublisherInterface(topic, rate), dev(dev) 87 | { 88 | } 89 | 90 | void createPublisher(ros::NodeHandle &nh); 91 | void publish(); 92 | 93 | ~ForceSensor_ROSPublisher() 94 | { 95 | } 96 | }; 97 | 98 | #endif 99 | -------------------------------------------------------------------------------- /uwsim/include/uwsim/GPSSensor.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013 University of Jaume-I. 3 | * All rights reserved. This program and the accompanying materials 4 | * are made available under the terms of the GNU Public License v3.0 5 | * which accompanies this distribution, and is available at 6 | * http://www.gnu.org/licenses/gpl.html 7 | * 8 | * Contributors: 9 | * Mario Prats 10 | * Javier Perez 11 | */ 12 | 13 | #ifndef GPSSENSOR_H_ 14 | #define GPSSENSOR_H_ 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | #include 22 | #include "osgOceanScene.h" 23 | #include 24 | 25 | class GPSSensor 26 | { 27 | 28 | public: 29 | std::string name,parentLinkName; 30 | 31 | /** Constructor 32 | * @param name the name of the GPS sensor 33 | * @param parentName the name of the link that holds the GPS sensor 34 | * @param parent the node of the scene graph that holds the sensor 35 | * @param rMl the sensor measures are given with respect to the root (r). Use rMl to transform them to another frame ('l' is the new frame, typically the localized world) 36 | * @param std the standard deviation on the sensor measures 37 | */ 38 | GPSSensor(osgOceanScene *oscene, std::string sensor_name, std::string parentName, osg::Node *parent, osg::Matrixd rMl, double std = 0) : 39 | name(sensor_name),parentLinkName(parentName), oscene_(oscene), parent_(parent), rMl_(rMl), std_(std) 40 | { 41 | node_ = new osg::Node(); 42 | parent->asGroup()->addChild(node_); 43 | } 44 | 45 | osg::Vec3d getMeasurement(); 46 | int getTFTransform(tf::Pose & pose, std::string & parent); 47 | 48 | double getStandardDeviation() 49 | { 50 | return std_; 51 | } 52 | 53 | /** Returns the depth of the sensor (in meters) under the water*/ 54 | double depthBelowWater(); 55 | 56 | virtual ~GPSSensor() 57 | { 58 | } 59 | 60 | private: 61 | osg::ref_ptr oscene_; 62 | osg::ref_ptr parent_; 63 | osg::Matrixd rMl_; 64 | double std_; 65 | osg::ref_ptr node_; 66 | 67 | boost::mt19937 rng_; ///< Boost random number generator 68 | }; 69 | 70 | #endif /* GPSSENSOR_H_ */ 71 | -------------------------------------------------------------------------------- /uwsim/include/uwsim/HUDCamera.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013 University of Jaume-I. 3 | * All rights reserved. This program and the accompanying materials 4 | * are made available under the terms of the GNU Public License v3.0 5 | * which accompanies this distribution, and is available at 6 | * http://www.gnu.org/licenses/gpl.html 7 | * 8 | * Contributors: 9 | * Mario Prats 10 | * Javier Perez 11 | */ 12 | 13 | #ifndef HUDCAMERA_H_ 14 | #define HUDCAMERA_H_ 15 | 16 | #include "SimulatorConfig.h" 17 | #include "CustomWidget.h" 18 | 19 | #include 20 | 21 | /** A ROS Camera subscriber that can be displayed as a widget */ 22 | class HUDCamera : public CustomWidget 23 | { 24 | osg::ref_ptr widget; 25 | 26 | class widgetUpdateCallback : public osg::Drawable::UpdateCallback 27 | { 28 | osg::ref_ptr image; 29 | public: 30 | widgetUpdateCallback(osg::Image *i) : 31 | osg::Drawable::UpdateCallback() 32 | { 33 | this->image = i; 34 | } 35 | virtual void update(osg::NodeVisitor *nv, osg::Drawable *d) 36 | { 37 | (static_cast(d))->setImage(image, true, false); 38 | } 39 | }; 40 | 41 | public: 42 | unsigned int width, height; ///< Width and height in pixels of the input image 43 | unsigned int posx, posy; ///< Default position of the widget, given in pixels wrt bottom-left corner (X to the right, Y upwards) 44 | double scale; ///< Percentage of default widget scaling (0..1) 45 | 46 | osg::ref_ptr osg_image; //The osg::Image object where to store the ROS image 47 | bool ready_; //true if images have been acquired 48 | 49 | /** Constructor from the image and info topics */ 50 | HUDCamera(unsigned int width, unsigned int height, unsigned int posx = 0, unsigned int posy = 0, double scale = 1, 51 | int blackWhite = 0); 52 | 53 | bool ready() 54 | { 55 | return ready_; 56 | } 57 | 58 | //Interface to be implemented by widgets. Build a widget window with the data to be displayed 59 | osg::ref_ptr getWidgetWindow(); 60 | 61 | ~HUDCamera(); 62 | }; 63 | 64 | #endif /* HUDCAMERA_H_ */ 65 | -------------------------------------------------------------------------------- /uwsim/include/uwsim/InertialMeasurementUnit.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013 University of Jaume-I. 3 | * All rights reserved. This program and the accompanying materials 4 | * are made available under the terms of the GNU Public License v3.0 5 | * which accompanies this distribution, and is available at 6 | * http://www.gnu.org/licenses/gpl.html 7 | * 8 | * Contributors: 9 | * Mario Prats 10 | * Javier Perez 11 | */ 12 | 13 | #ifndef INERTIALMEASUREMENTUNIT_H_ 14 | #define INERTIALMEASUREMENTUNIT_H_ 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | #include 22 | #include 23 | 24 | class InertialMeasurementUnit 25 | { 26 | 27 | public: 28 | std::string name,parentLinkName; 29 | 30 | /** Constructor 31 | * @param imu_name the name of the IMU 32 | * @param parentName the name of the link that holds the IMU 33 | * @param imu_parent the node of the scene graph that holds the imu 34 | * @param rMl the imu measures are given with respect to the root (r). Use rMl to transform them to another frame ('l' is the new frame, typically the localized world) 35 | * @param imu_std the standard deviation on the imu measures 36 | */ 37 | InertialMeasurementUnit(std::string imu_name, std::string parentName, osg::Node *imu_parent, osg::Matrixd rMl, double imu_std = 0) : 38 | name(imu_name),parentLinkName(parentName), parent_(imu_parent), rMl_(rMl), std_(imu_std) 39 | { 40 | imu_node_ = new osg::Node(); 41 | imu_parent->asGroup()->addChild(imu_node_); 42 | } 43 | 44 | int getTFTransform(tf::Pose & pose, std::string & parent); 45 | osg::Quat getMeasurement(); 46 | 47 | double getStandardDeviation() 48 | { 49 | return std_; 50 | } 51 | 52 | virtual ~InertialMeasurementUnit() 53 | { 54 | } 55 | 56 | private: 57 | osg::ref_ptr parent_; 58 | osg::Matrixd rMl_; 59 | double std_; 60 | osg::ref_ptr imu_node_; 61 | 62 | boost::mt19937 rng_; ///< Boost random number generator 63 | }; 64 | 65 | #endif /* INERTIALMEASUREMENTUNIT_H_ */ 66 | -------------------------------------------------------------------------------- /uwsim/include/uwsim/KinematicChain.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013 University of Jaume-I. 3 | * All rights reserved. This program and the accompanying materials 4 | * are made available under the terms of the GNU Public License v3.0 5 | * which accompanies this distribution, and is available at 6 | * http://www.gnu.org/licenses/gpl.html 7 | * 8 | * Contributors: 9 | * Mario Prats 10 | * Javier Perez 11 | */ 12 | 13 | #ifndef KINEMATICCHAIN_H_ 14 | #define KINEMATICCHAIN_H_ 15 | 16 | #include "SimulatorConfig.h" 17 | #include 18 | #include 19 | 20 | #include 21 | #include 22 | #include "BulletPhysics.h" 23 | 24 | struct MimicArm 25 | { 26 | int joint; 27 | double offset, multiplier; 28 | int sliderCrank; 29 | }; 30 | 31 | /** Abstract Kinematic Chain class for holding articulated 3D Models 32 | */ 33 | class KinematicChain 34 | { 35 | public: 36 | std::vector > link; ///< pointers to link models 37 | std::vector q; ///< Joint values 38 | std::vector > limits; /// names; //Names of the joints 40 | 41 | std::vector mimic; //Mimic joints info 42 | std::vector jointType; //type of joints 0 fixed, 1 rotation, 2 prismatic 43 | std::vector > joints; ///< pointers to transforms between links 44 | std::vector > zerojoints; /// baseTransform; /// tool; ///< Pointer to the tool osg node 49 | 50 | std::vector qLastSafe; ///< Last joint values not colliding 51 | BulletPhysics * physics; /// Bullet physics pointer to query 52 | 53 | ros::WallTime last; 54 | int started; 55 | 56 | KinematicChain(int nlinks, int njoints); 57 | 58 | void setJointPosition(double *q, int n); 59 | void setJointVelocity(double *qdot, int n); 60 | void setJointPosition(std::vector &q, std::vector names = std::vector()); 61 | void setJointVelocity(std::vector &qdot, std::vector names = std::vector()); 62 | std::vector getJointPosition(); 63 | std::vector getJointName(); 64 | std::map getFullJointMap(); //Builds a map: name -> jointValue with all non-fixed joints (even mimic joints) 65 | 66 | /** Get the number of links */ 67 | int getNumberOfLinks() 68 | { 69 | return link.size(); 70 | } 71 | /** Get the number of joints*/ 72 | int getNumberOfJoints() 73 | { 74 | return q.size(); 75 | } 76 | 77 | ~KinematicChain(); 78 | 79 | protected: 80 | virtual void updateJoints(std::vector &q)=0; ///< Implemented by childs for joint position update 81 | }; 82 | 83 | #endif /* KINEMATICCHAIN_H_ */ 84 | -------------------------------------------------------------------------------- /uwsim/include/uwsim/LedArray.h: -------------------------------------------------------------------------------- 1 | #ifndef UWSIM_LEDARRAY_ 2 | #define UWSIM_LEDARRAY_ 3 | 4 | #include 5 | //#include 6 | //#include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | namespace uwsim { 18 | 19 | class LedArray { 20 | public: 21 | enum ledType { RED_LED, GREEN_LED }; 22 | LedArray(osg::ref_ptr sceneRoot, LedArrayConfig config); 23 | osg::ref_ptr GetOSGNode(); 24 | void StartAnimationTest(); 25 | void UpdateLetState(ledType, bool); 26 | 27 | private: 28 | ros::NodeHandle nh; 29 | osg::ref_ptr sceneRoot; 30 | osg::ref_ptr vMRedLight; 31 | osg::ref_ptr vMGreenLight; 32 | osg::ref_ptr node; 33 | static uint32_t numLedLights; 34 | osg::ref_ptr greenLightSource, redLightSource; 35 | osg::ref_ptr greenLightMaterial, redLightMaterial; 36 | uint32_t redLightNum, greenLightNum; 37 | ros::Subscriber redLedSubscriber; 38 | ros::Subscriber greenLedSubscriber; 39 | std::thread ledWorker, testWorker; 40 | LedArrayConfig config; 41 | 42 | struct Action { 43 | ros::Time offTime; 44 | }; 45 | 46 | //std::mutex actionsMutex; 47 | //std::condition_variable actionsCond; 48 | bool redStateOn, greenStateOn; 49 | Action redAction, greenAction; 50 | 51 | void InitROSInterface(); 52 | void HandleNewLedState(underwater_sensor_msgs::LedLightConstPtr msg, ledType); 53 | void CheckAndUpdateLed(const ros::Time & now, ledType type, bool , const Action &); 54 | }; 55 | } 56 | #endif 57 | -------------------------------------------------------------------------------- /uwsim/include/uwsim/MultibeamSensor.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013 University of Jaume-I. 3 | * All rights reserved. This program and the accompanying materials 4 | * are made available under the terms of the GNU Public License v3.0 5 | * which accompanies this distribution, and is available at 6 | * http://www.gnu.org/licenses/gpl.html 7 | * 8 | * Contributors: 9 | * Mario Prats 10 | * Javier Perez 11 | */ 12 | 13 | #ifndef MULTIBEAMSENSOR_H_ 14 | #define MULTIBEAMSENSOR_H_ 15 | 16 | #include "VirtualCamera.h" 17 | 18 | class MultibeamSensor 19 | { 20 | struct Remap 21 | { 22 | int pixel1, pixel2; 23 | double weight1, weight2; 24 | double distort; 25 | }; 26 | 27 | public: 28 | std::vector vcams; //Virtual Cameras 29 | std::string name, parentLinkName; 30 | int numpixels, camPixels, nCams; 31 | double range, initAngle, finalAngle, angleIncr, camsFOV; 32 | osg::ref_ptr geode; //Geometry node that draws the beam 33 | std::vector remapVector; 34 | osg::Node *trackNode; 35 | 36 | MultibeamSensor(osg::Group *uwsim_root, std::string name, std::string parentName, osg::Node *trackNode, double initAngle, double finalAngle, 37 | double alpha, double range, unsigned int mask, int visible,unsigned int ARMask); 38 | void preCalcTable(); 39 | int getTFTransform(tf::Pose & pose, std::string & parent); 40 | }; 41 | 42 | #endif 43 | -------------------------------------------------------------------------------- /uwsim/include/uwsim/NED.h: -------------------------------------------------------------------------------- 1 | #ifndef UWSIM_NED_ 2 | #define UWSIM_NED_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | namespace uwsim { 10 | 11 | class NED : public cpplogging::Logger{ 12 | public: 13 | static void SetOrigin(double lat, double lon, double alt); 14 | static void GetNED(double lat, double lon, double alt, double &x, double &y, double &z); 15 | 16 | private: 17 | static GeographicLib::LocalCartesian localCartesian; 18 | static GeographicLib::Geocentric earth; 19 | static std::mutex localCartesian_mutex; 20 | 21 | }; 22 | 23 | } 24 | 25 | #endif 26 | -------------------------------------------------------------------------------- /uwsim/include/uwsim/NetSim.h: -------------------------------------------------------------------------------- 1 | #ifndef UWSIM_NETSIM_ 2 | #define UWSIM_NETSIM_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace uwsim { 9 | using namespace dccomms_ros; 10 | 11 | class NetSimTracing : public cpplogging::Logger { 12 | public: 13 | NetSimTracing(); 14 | void Run(); 15 | virtual void Configure(); 16 | virtual void DoRun(); 17 | }; 18 | 19 | typedef std::shared_ptr NetSimTracingPtr; 20 | class NetSim { 21 | public: 22 | static ns3::Ptr GetSim(); 23 | static void LoadDefaultTracingScript(); 24 | static void LoadTracingScript(const std::string &className, 25 | const std::string &libPath); 26 | static NetSimTracingPtr GetScript(); 27 | private: 28 | static NetSimTracingPtr _script; 29 | static std::shared_ptr _loader; 30 | 31 | }; 32 | 33 | } 34 | 35 | #endif 36 | -------------------------------------------------------------------------------- /uwsim/include/uwsim/ObjectPicker.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013 University of Jaume-I. 3 | * All rights reserved. This program and the accompanying materials 4 | * are made available under the terms of the GNU Public License v3.0 5 | * which accompanies this distribution, and is available at 6 | * http://www.gnu.org/licenses/gpl.html 7 | * 8 | * Contributors: 9 | * Mario Prats 10 | * Javier Perez 11 | */ 12 | 13 | #ifndef OBJECTPICKER_H_ 14 | #define OBJECTPICKER_H_ 15 | 16 | #include "VirtualRangeSensor.h" 17 | #include "UWSimUtils.h" 18 | #include "URDFRobot.h" 19 | #include 20 | 21 | //Node tracker that updates the ray coordinates from the tracked node position, computes intersections and 'picks' nodes 22 | class ObjectPickerUpdateCallback : public IntersectorUpdateCallback 23 | { 24 | virtual void operator()(osg::Node *node, osg::NodeVisitor *nv) 25 | { 26 | osg::Matrixd mStart, mEnd; 27 | mStart = osg::computeLocalToWorld(nv->getNodePath()); 28 | traverse(node, nv); 29 | 30 | //update ray and compute intersections. Checks intersections along X axis of the local frame 31 | mEnd = mStart; 32 | mEnd.preMultTranslate(osg::Vec3d(range, 0, 0)); 33 | 34 | intersector->reset(); 35 | intersector->setStart(mStart.getTrans()); 36 | intersector->setEnd(mEnd.getTrans()); 37 | 38 | root->accept(intersectVisitor); 39 | 40 | if (intersector->containsIntersections() && !picked) 41 | { 42 | osgUtil::LineSegmentIntersector::Intersection intersection = intersector->getFirstIntersection(); 43 | osg::Vec3d worldIntPoint = intersection.getWorldIntersectPoint(); 44 | distance_to_obstacle = (worldIntPoint - mStart.getTrans()).length(); 45 | impact = intersection.nodePath; 46 | 47 | //search for catchable objects in nodepath 48 | for (osg::NodePath::iterator i = impact.begin(); i != impact.end(); ++i) 49 | { 50 | osg::ref_ptr data = dynamic_cast(i[0]->getUserData()); 51 | if (data != NULL && data->catchable) 52 | { 53 | 54 | std::cerr << "Picking object up." << std::endl; 55 | //physics: set static object flag 56 | if (data->rigidBody) 57 | data->rigidBody->setCollisionFlags( 58 | data->rigidBody->getCollisionFlags() | btCollisionObject::CF_STATIC_OBJECT); 59 | 60 | //add link to kinematic chain 61 | urdf->addToKinematicChain(i[0], data->rigidBody); 62 | 63 | osg::Node * objectTransf = i[0]->getParent(0)->getParent(0); //Object->linkBaseTransform->transform 64 | 65 | //Get coordinates to change them when changing position in graph 66 | std::shared_ptr originalpos = getWorldCoords(objectTransf); 67 | std::shared_ptr hand = getWorldCoords(trackNode); 68 | hand->invert(*hand); 69 | 70 | //ADD node in hand, remove object from original position. 71 | trackNode->asTransform()->addChild(objectTransf); 72 | objectTransf->getParent(0)->asGroup()->removeChild(objectTransf); 73 | 74 | osg::Matrixd matrix = *originalpos * *hand; 75 | objectTransf->asTransform()->asMatrixTransform()->setMatrix(matrix); 76 | 77 | picked = true; 78 | } 79 | } 80 | } 81 | else if (!picked) 82 | distance_to_obstacle = range; 83 | } 84 | public: 85 | osg::NodePath impact; 86 | osg::Node *trackNode; 87 | std::shared_ptr urdf; 88 | bool picked; 89 | 90 | ObjectPickerUpdateCallback(osg::Node *trackNode, double range, bool visible, osg::Node *root, 91 | std::shared_ptr urdf) : 92 | IntersectorUpdateCallback(range, visible, root) 93 | { 94 | this->trackNode = trackNode; 95 | picked = false; 96 | this->urdf = urdf; 97 | } 98 | }; 99 | 100 | class ObjectPicker : public VirtualRangeSensor 101 | { 102 | public: 103 | ObjectPicker(std::string name, osg::Node *root, osg::Node *trackNode, double range, bool visible, 104 | std::shared_ptr urdf,unsigned int mask); 105 | ObjectPicker(); 106 | 107 | void init(std::string name, osg::Node *root, osg::Node *trackNode, double range, bool visible, 108 | std::shared_ptr urdf,unsigned int mask); 109 | }; 110 | 111 | #endif 112 | -------------------------------------------------------------------------------- /uwsim/include/uwsim/PhysicsBuilder.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013 University of Jaume-I. 3 | * All rights reserved. This program and the accompanying materials 4 | * are made available under the terms of the GNU Public License v3.0 5 | * which accompanies this distribution, and is available at 6 | * http://www.gnu.org/licenses/gpl.html 7 | * 8 | * Contributors: 9 | * Mario Prats 10 | * Javier Perez 11 | */ 12 | 13 | #ifndef PHYSICSBUILDER_H 14 | #define PHYSICSBUILDER_H 15 | 16 | #include "SceneBuilder.h" 17 | #include "BulletPhysics.h" 18 | #include "ConfigXMLParser.h" 19 | 20 | class PhysicsBuilder 21 | { 22 | public: 23 | osg::ref_ptr physics; 24 | public: 25 | 26 | PhysicsBuilder(SceneBuilder * scene_builder, ConfigFile config); 27 | PhysicsBuilder() 28 | { 29 | } 30 | void loadPhysics(SceneBuilder * scene_builder, ConfigFile config); 31 | 32 | }; 33 | 34 | #endif 35 | -------------------------------------------------------------------------------- /uwsim/include/uwsim/PressureSensor.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013 University of Jaume-I. 3 | * All rights reserved. This program and the accompanying materials 4 | * are made available under the terms of the GNU Public License v3.0 5 | * which accompanies this distribution, and is available at 6 | * http://www.gnu.org/licenses/gpl.html 7 | * 8 | * Contributors: 9 | * Mario Prats 10 | * Javier Perez 11 | */ 12 | 13 | #ifndef PRESSURESENSOR_H_ 14 | #define PRESSURESENSOR_H_ 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | #include 22 | #include 23 | 24 | class PressureSensor 25 | { 26 | 27 | public: 28 | std::string name, parentLinkName; 29 | 30 | /** Constructor 31 | * @param name the name of the pressure sensor 32 | * @param parentName the name of the link that holds the IMU 33 | * @param parent the node of the scene graph that holds the sensor 34 | * @param rMl the sensor measures are given with respect to the root (r). Use rMl to transform them to another frame ('l' is the new frame, typically the localized world) 35 | * @param std the standard deviation on the sensor measures 36 | */ 37 | PressureSensor(std::string sensor_name, std::string parentName, osg::Node *parent, osg::Matrixd rMl, double std = 0) : 38 | name(sensor_name),parentLinkName(parentName) , parent_(parent), rMl_(rMl), std_(std) 39 | { 40 | node_ = new osg::Node(); 41 | parent->asGroup()->addChild(node_); 42 | } 43 | 44 | double getMeasurement(); 45 | int getTFTransform(tf::Pose & pose, std::string & parent); 46 | 47 | double getStandardDeviation() 48 | { 49 | return std_; 50 | } 51 | 52 | virtual ~PressureSensor() 53 | { 54 | } 55 | 56 | private: 57 | osg::ref_ptr parent_; 58 | osg::Matrixd rMl_; 59 | double std_; 60 | osg::ref_ptr node_; 61 | 62 | boost::mt19937 rng_; ///< Boost random number generator 63 | }; 64 | 65 | #endif /* PRESSURESENSOR_H_ */ 66 | -------------------------------------------------------------------------------- /uwsim/include/uwsim/ROSSceneBuilder.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013 University of Jaume-I. 3 | * All rights reserved. This program and the accompanying materials 4 | * are made available under the terms of the GNU Public License v3.0 5 | * which accompanies this distribution, and is available at 6 | * http://www.gnu.org/licenses/gpl.html 7 | * 8 | * Contributors: 9 | * Mario Prats 10 | * Javier Perez 11 | */ 12 | 13 | 14 | #ifndef ROSSCENEBUILDER_H_ 15 | #define ROSSCENEBUILDER_H_ 16 | 17 | #include 18 | 19 | #include 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | 26 | //This class extends the SceneBuilder with ROS interfaces to modify and interact with the scene geometry. 27 | 28 | class ROSSceneBuilder : public SceneBuilder 29 | { 30 | 31 | public: 32 | 33 | ROSSceneBuilder(std::shared_ptr args); 34 | bool loadScene(ConfigFile config); 35 | 36 | void updateIM(); 37 | 38 | private: 39 | 40 | ros::NodeHandle n; 41 | ros::ServiceServer markerService; 42 | osg::Group * markers; 43 | std::shared_ptr frame_manager; 44 | std::shared_ptr marker_cli; 45 | 46 | ros::WallTime last_wall_time; 47 | ros::Time last_ros_time; 48 | 49 | typedef std::list< std::shared_ptr > MarkerList; 50 | MarkerList markerList; 51 | 52 | //This callback will receibe marker requests and create/modify/delete objects in the scene. 53 | bool markerSRVCallback(underwater_sensor_msgs::SpawnMarker::Request &req, underwater_sensor_msgs::SpawnMarker::Response &res); 54 | 55 | 56 | }; 57 | 58 | #endif 59 | -------------------------------------------------------------------------------- /uwsim/include/uwsim/SceneBuilder.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013 University of Jaume-I. 3 | * All rights reserved. This program and the accompanying materials 4 | * are made available under the terms of the GNU Public License v3.0 5 | * which accompanies this distribution, and is available at 6 | * http://www.gnu.org/licenses/gpl.html 7 | * 8 | * Contributors: 9 | * Mario Prats 10 | * Javier Perez 11 | */ 12 | 13 | #ifndef SCENEBUILDER_H 14 | #define SCENEBUILDER_H 15 | 16 | #include 17 | #include 18 | 19 | #include "osgOceanScene.h" 20 | #include "HUDCamera.h" 21 | #include "ROSInterface.h" 22 | #include "SimulatedIAUV.h" 23 | #include "ConfigXMLParser.h" 24 | #include 25 | #include 26 | #include 27 | 28 | class SceneBuilder 29 | { 30 | public: 31 | std::shared_ptr arguments; 32 | osg::ref_ptr scene; 33 | std::vector > iauvFile; 34 | std::vector > objects; 35 | std::vector > trajectories; 36 | 37 | osg::ref_ptr root; 38 | 39 | std::vector > realcams; 40 | std::vector > ROSInterfaces; 41 | std::vector > camWidgets; 42 | std::vector> customCommsChannels; 43 | std::vector> acousticCommsChannels; 44 | std::vector> ledArrays; 45 | 46 | public: 47 | SceneBuilder(); 48 | SceneBuilder(int *argc, char **argv); 49 | SceneBuilder(std::shared_ptr args); 50 | 51 | bool loadScene(std::string xml_file); 52 | bool loadScene(ConfigFile config); 53 | 54 | osg::Group* getRoot() 55 | { 56 | return root.get(); 57 | } 58 | osgOceanScene* getScene() 59 | { 60 | return scene.get(); 61 | } 62 | 63 | ~SceneBuilder(); 64 | }; 65 | 66 | #endif 67 | 68 | -------------------------------------------------------------------------------- /uwsim/include/uwsim/SimDev_Echo.h: -------------------------------------------------------------------------------- 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////// 2 | //"Echo" example, SimulatedDevice_Echo.h 3 | #ifndef SIMULATEDDEVICE_ECHO_H_ 4 | #define SIMULATEDDEVICE_ECHO_H_ 5 | #include "SimulatedDevice.h" 6 | using namespace uwsim; 7 | 8 | /* 9 | * Example header of driver/rosinterface configuration/factory 10 | * 11 | * Included in SimulatedDevices.cpp 12 | */ 13 | 14 | //Driver/ROSInterface configuration 15 | class SimDev_Echo_Config : public SimulatedDeviceConfig 16 | { 17 | public: 18 | //XML members 19 | std::string info; 20 | //constructor 21 | SimDev_Echo_Config(std::string type_) : 22 | SimulatedDeviceConfig(type_) 23 | { 24 | } 25 | }; 26 | 27 | //Driver/ROSInterface factory class 28 | class SimDev_Echo_Factory : public SimulatedDeviceFactory 29 | { 30 | public: 31 | //this is the only place the device/interface type is set 32 | SimDev_Echo_Factory(std::string type_ = "echo") : 33 | SimulatedDeviceFactory(type_) 34 | { 35 | } 36 | ; 37 | 38 | SimulatedDeviceConfig::Ptr processConfig(const xmlpp::Node* node, ConfigFile * config); 39 | bool applyConfig(SimulatedIAUV * auv, Vehicle &vehicleChars, SceneBuilder *sceneBuilder, size_t iteration); 40 | std::vector > getInterface(ROSInterfaceInfo & rosInterface, 41 | std::vector > & iauvFile); 42 | }; 43 | 44 | //can be a sparate header file for actual implementation classes... 45 | 46 | #include "ConfigXMLParser.h" 47 | #include "ROSInterface.h" 48 | #include 49 | #include 50 | 51 | //Driver class 52 | class SimDev_Echo : public SimulatedDevice 53 | { 54 | void applyPhysics(BulletPhysics * bulletPhysics) 55 | { 56 | } 57 | public: 58 | std::string info; //Device's property 59 | 60 | SimDev_Echo(SimDev_Echo_Config * cfg); 61 | }; 62 | 63 | //ROS publishers and subscribers work exactly as before, no subclassing is needed 64 | class SimDev_Echo_ROSPublisher : public ROSPublisherInterface 65 | { 66 | //this is just an example, use a pointer to SimulatedIAUV, if only ROSInterface is implemented 67 | //pointer to a device 68 | SimDev_Echo * dev; 69 | public: 70 | SimDev_Echo_ROSPublisher(SimDev_Echo *dev, std::string topic, int rate) : 71 | ROSPublisherInterface(topic, rate), dev(dev) 72 | { 73 | } 74 | 75 | void createPublisher(ros::NodeHandle &nh); 76 | void publish(); 77 | 78 | ~SimDev_Echo_ROSPublisher() 79 | { 80 | } 81 | }; 82 | 83 | #endif 84 | -------------------------------------------------------------------------------- /uwsim/include/uwsim/SimulatedDevice.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013 Tallinn University of Technology. 3 | * All rights reserved. This program and the accompanying materials 4 | * are made available under the terms of the GNU Public License v3.0 5 | * which accompanies this distribution, and is available at 6 | * http://www.gnu.org/licenses/gpl.html 7 | * 8 | * Contributors: 9 | * Yuri Gavshin 10 | */ 11 | 12 | #ifndef SIMULATEDDEVICE_H_ 13 | #define SIMULATEDDEVICE_H_ 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | struct ROSInterfaceInfo; 22 | struct SimulatedIAUV; 23 | struct ROSInterface; 24 | struct ConfigFile; 25 | struct Vehicle; 26 | struct SceneBuilder; 27 | struct BulletPhysics; 28 | struct ViewBuilder; 29 | 30 | namespace uwsim 31 | { 32 | struct SimulatedDevice; 33 | //Base class for device's XML configuration 34 | class SimulatedDeviceConfig 35 | { 36 | //device/rosinterface type identifier for both "XML config" and a "factory" 37 | std::string type; 38 | 39 | public: 40 | typedef std::shared_ptr Ptr; 41 | //common XML properties: 42 | std::string name; 43 | std::string getType() 44 | { 45 | return type; 46 | } 47 | 48 | SimulatedDeviceConfig(std::string type); 49 | virtual ~SimulatedDeviceConfig() 50 | { 51 | } 52 | ; 53 | }; 54 | 55 | //Base class for device/rosinterface "factory" 56 | class SimulatedDeviceFactory 57 | { 58 | //device/rosinterface type identifier for both "XML config" and a "factory" 59 | std::string type; 60 | public: 61 | typedef std::shared_ptr Ptr; 62 | std::string getType() 63 | { 64 | return type; 65 | } 66 | SimulatedDeviceFactory(std::string type); 67 | 68 | //DRIVER: parses XML and returns "XML config", executed first 69 | virtual SimulatedDeviceConfig::Ptr processConfig(const xmlpp::Node* node, ConfigFile * config) = 0; 70 | //DRIVER: checks parsed XML configurations and sets SimulatedAUV's data, executed second 71 | //Executed multiple times (to allow dependent devices work independent from order of ), until all factories return true 72 | //normally, configuration should occur only on iteration 0 73 | virtual bool applyConfig(SimulatedIAUV * auv, Vehicle &vehicleChars, SceneBuilder *oscene, size_t iteration) = 0; 74 | //ROSINTERFACE: returns configured ROSInterfaces, executed third 75 | virtual std::vector > getInterface( 76 | ROSInterfaceInfo & rosInterface, std::vector > & iauvFile) 77 | { 78 | std::vector < std::shared_ptr > ifaces; 79 | return ifaces; 80 | } 81 | ; 82 | 83 | virtual ~SimulatedDeviceFactory() 84 | { 85 | } 86 | ; 87 | }; 88 | 89 | //Base class for a simulated device 90 | class SimulatedDevice 91 | { 92 | std::string type; //driver/rosinterface type 93 | public: 94 | std::string name; //common property 95 | std::string getType() 96 | { 97 | return type; 98 | } 99 | typedef std::shared_ptr Ptr; 100 | SimulatedDevice(SimulatedDeviceConfig * cfg); 101 | virtual void applyPhysics(BulletPhysics * bulletPhysics) 102 | { 103 | } 104 | virtual void setViewBuilder(ViewBuilder * viewBuilder) 105 | { 106 | } 107 | 108 | virtual ~SimulatedDevice() 109 | { 110 | } 111 | }; 112 | } 113 | ; 114 | 115 | #endif /* SIMULATEDDEVICES_H_ */ 116 | -------------------------------------------------------------------------------- /uwsim/include/uwsim/SimulatedDevices.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013 Tallinn University of Technology. 3 | * All rights reserved. This program and the accompanying materials 4 | * are made available under the terms of the GNU Public License v3.0 5 | * which accompanies this distribution, and is available at 6 | * http://www.gnu.org/licenses/gpl.html 7 | * 8 | * Contributors: 9 | * Yuri Gavshin 10 | */ 11 | 12 | #ifndef SIMULATEDDEVICES_H_ 13 | #define SIMULATEDDEVICES_H_ 14 | #include "SimulatedDevice.h" 15 | 16 | //Class added to SimulatedIAUV as devices->, put your data into into this class members or use all vector to store your device 17 | class SimulatedDevices 18 | { 19 | public: 20 | //Object members 21 | std::vector all; 22 | 23 | SimulatedDevices(); 24 | 25 | //Applies parsed XML configurations by calling appropriate methods 26 | //on all registered factories (set in initFactories() in SimulatedDevices.cpp) 27 | void applyConfig(SimulatedIAUV * auv, Vehicle &vehicleChars, SceneBuilder *oscene); 28 | 29 | //Factory methods 30 | 31 | //returns configured ROSInterface based on given XML configuration 32 | static std::vector > getInterfaces( 33 | ROSInterfaceInfo & rosInterface, std::vector > & iauvFile); 34 | 35 | //Parses driver's XML configuration 36 | static std::vector processConfig(const xmlpp::Node* node, ConfigFile * config, 37 | bool isDevice = false); 38 | }; 39 | #endif /* SIMULATEDDEVICES_H_ */ 40 | -------------------------------------------------------------------------------- /uwsim/include/uwsim/SimulatedIAUV.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013 University of Jaume-I. 3 | * All rights reserved. This program and the accompanying materials 4 | * are made available under the terms of the GNU Public License v3.0 5 | * which accompanies this distribution, and is available at 6 | * http://www.gnu.org/licenses/gpl.html 7 | * 8 | * Contributors: 9 | * Mario Prats 10 | * Javier Perez 11 | */ 12 | 13 | #ifndef SIMULATEDIAUV_H_ 14 | #define SIMULATEDIAUV_H_ 15 | 16 | #include "osgOceanScene.h" 17 | #include "URDFRobot.h" 18 | #include "VirtualCamera.h" 19 | #include "ConfigXMLParser.h" 20 | #include "VirtualRangeSensor.h" 21 | #include "VirtualSLSProjector.h" 22 | #include "ObjectPicker.h" 23 | #include "InertialMeasurementUnit.h" 24 | #include "PressureSensor.h" 25 | #include "GPSSensor.h" 26 | #include "DVLSensor.h" 27 | #include "MultibeamSensor.h" 28 | #include 29 | #include 30 | 31 | class SceneBuilder; 32 | 33 | /* An I-AUV */ 34 | class SimulatedIAUV 35 | { 36 | public: 37 | std::vector camview; 38 | std::vector range_sensors; 39 | std::vector sls_projectors; 40 | std::vector object_pickers; 41 | std::vector imus; 42 | std::vector pressure_sensors; 43 | std::vector gps_sensors; 44 | std::vector dvl_sensors; 45 | std::vector multibeam_sensors; 46 | std::shared_ptr devices; 47 | osg::ref_ptr root; 48 | std::shared_ptr ledArray; 49 | 50 | typedef enum 51 | { 52 | ARM5, PA10 53 | } arm_t; 54 | 55 | std::string name; ///< Vehicle name 56 | std::shared_ptr urdf; ///< URDF I-AUV 57 | //osg::LightSource* lightSource; ///< vehicle lamp 58 | osg::ref_ptr baseTransform; 59 | std::mutex baseTransform_mutex; 60 | osg::Vec3d scale; //Vehicle scale factor 61 | 62 | SimulatedIAUV(SceneBuilder *oscene, Vehicle vehicle); 63 | 64 | //void setVirtualCamera(std::string name, osg::Transform* transform, osg::Vec3d eye, osg::Vec3d orientation, osg::Vec3d upVector, int width, int height); 65 | 66 | //setPosition 67 | void setVehiclePosition(double x, double y, double z, double yaw) 68 | { 69 | setVehiclePosition(x, y, z, 0, 0, yaw); 70 | } 71 | void setVehiclePosition(double x, double y, double z, double roll, double pitch, double yaw); 72 | void setVehiclePosition(double p[6]) 73 | { 74 | setVehiclePosition(p[0], p[1], p[2], p[3], p[4], p[5]); 75 | } 76 | void setVehiclePosition(osg::Matrixd m); 77 | 78 | unsigned int getNumCams() 79 | { 80 | return camview.size(); 81 | } 82 | unsigned int getNumRangeSensors() 83 | { 84 | return range_sensors.size(); 85 | } 86 | unsigned int getNumObjectPickers() 87 | { 88 | return object_pickers.size(); 89 | } 90 | 91 | ~SimulatedIAUV() 92 | { 93 | OSG_DEBUG << "Simulated IAUV destructor" << std::endl; 94 | } 95 | }; 96 | 97 | #endif /* SIMULATEDIAUV_H_ */ 98 | -------------------------------------------------------------------------------- /uwsim/include/uwsim/SimulatorConfig.h.cmake: -------------------------------------------------------------------------------- 1 | #ifndef SIMULATORCONFIG_H 2 | #define SIMULATORCONFIG_H 3 | 4 | #ifndef M_PI 5 | #define M_PI 3.14159265358979323846 6 | #endif 7 | #ifndef M_PI_2 8 | #define M_PI_2 M_PI/2 9 | #endif 10 | #ifndef M_PI_4 11 | #define M_PI_4 M_PI/4 12 | #endif 13 | 14 | #include 15 | 16 | #ifndef OSG_FATAL 17 | #define OSG_FATAL osg::notify(osg::FATAL) 18 | #endif 19 | 20 | #ifndef OSG_ERROR 21 | #define OSG_ERROR osg::notify(osg::ERROR) 22 | #endif 23 | 24 | #ifndef OSG_WARN 25 | #define OSG_WARN osg::notify(osg::WARN) 26 | #endif 27 | 28 | #ifndef OSG_INFO 29 | #define OSG_INFO osg::notify(osg::INFO) 30 | #endif 31 | 32 | #ifndef OSG_DEBUG 33 | #define OSG_DEBUG osg::notify(osg::DEBUG_INFO) 34 | #endif 35 | 36 | //Build ROS interfaces 37 | #cmakedefine BUILD_ROS_INTERFACES ${BUILD_ROS_INTERFACES} 38 | 39 | //Build Bullet physics 40 | #cmakedefine BUILD_BULLET_PHYSICS ${BUILD_BULLET_PHYSICS} 41 | 42 | //UWSim root folder 43 | #cmakedefine UWSIM_ROOT_PATH "${UWSIM_ROOT_PATH}" 44 | 45 | //Operating system 46 | #cmakedefine WIN32 ${WIN32} 47 | #cmakedefine UNIX ${UNIX} 48 | #cmakedefine APPLE ${APPLE} 49 | 50 | #endif 51 | 52 | -------------------------------------------------------------------------------- /uwsim/include/uwsim/SkyDome.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This source file is part of the osgOcean library 3 | * 4 | * Copyright (C) 2009 Kim Bale 5 | * Copyright (C) 2009 The University of Hull, UK 6 | * 7 | * This program is free software; you can redistribute it and/or modify it under 8 | * the terms of the GNU Lesser General Public License as published by the Free Software 9 | * Foundation; either version 3 of the License, or (at your option) any later 10 | * version. 11 | 12 | * This program is distributed in the hope that it will be useful, but WITHOUT 13 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 14 | * FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more details. 15 | * http://www.gnu.org/copyleft/lesser.txt. 16 | */ 17 | 18 | #pragma once 19 | #include "SphereSegment.h" 20 | #include 21 | #include 22 | #include 23 | 24 | class SkyDome : public SphereSegment 25 | { 26 | public: 27 | SkyDome(void); 28 | SkyDome(const SkyDome& copy, const osg::CopyOp& copyop = osg::CopyOp::SHALLOW_COPY); 29 | SkyDome(float radius, unsigned int longSteps, unsigned int latSteps, osg::TextureCubeMap* cubemap); 30 | 31 | protected: 32 | ~SkyDome(void); 33 | 34 | public: 35 | void setupStateSet(osg::TextureCubeMap* cubemap); 36 | void create(float radius, unsigned int latSteps, unsigned int longSteps, osg::TextureCubeMap* cubemap); 37 | 38 | inline void setCubeMap(osg::TextureCubeMap* cubemap) 39 | { 40 | getOrCreateStateSet()->setTextureAttributeAndModes(0, cubemap, osg::StateAttribute::ON); 41 | } 42 | 43 | private: 44 | osg::ref_ptr createShader(void); 45 | 46 | }; 47 | -------------------------------------------------------------------------------- /uwsim/include/uwsim/SphereSegment.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This source file is part of the osgOcean library 3 | * 4 | * Copyright (C) 2009 Kim Bale 5 | * Copyright (C) 2009 The University of Hull, UK 6 | * 7 | * This program is free software; you can redistribute it and/or modify it under 8 | * the terms of the GNU Lesser General Public License as published by the Free Software 9 | * Foundation; either version 3 of the License, or (at your option) any later 10 | * version. 11 | 12 | * This program is distributed in the hope that it will be useful, but WITHOUT 13 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 14 | * FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more details. 15 | * http://www.gnu.org/copyleft/lesser.txt. 16 | */ 17 | 18 | #pragma once 19 | #include 20 | #include 21 | 22 | class SphereSegment : public osg::Geode 23 | { 24 | public: 25 | SphereSegment(void); 26 | 27 | SphereSegment(float radius, unsigned int longitudeSteps, unsigned int lattitudeSteps, float longStart, float longEnd, 28 | float latStart, float latEnd); 29 | 30 | SphereSegment(const SphereSegment& copy, const osg::CopyOp& copyop = osg::CopyOp::SHALLOW_COPY); 31 | 32 | protected: 33 | ~SphereSegment(void); 34 | 35 | public: 36 | // 0 >= longStart/longEnd <= 180 37 | // 0 >= latStart/latEnd <= 360 38 | void compute(float radius, unsigned int longitudeSteps, unsigned int lattitudeSteps, float longStart, float longEnd, 39 | float latStart, float latEnd); 40 | private: 41 | osg::Vec2 sphereMap(osg::Vec3& vertex, float radius); 42 | 43 | inline unsigned int idx(unsigned int r, unsigned int c, unsigned int row_len) 44 | { 45 | return c + r * row_len; 46 | } 47 | }; 48 | -------------------------------------------------------------------------------- /uwsim/include/uwsim/TextHUD.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013 University of Jaume-I. 3 | * All rights reserved. This program and the accompanying materials 4 | * are made available under the terms of the GNU Public License v3.0 5 | * which accompanies this distribution, and is available at 6 | * http://www.gnu.org/licenses/gpl.html 7 | * 8 | * Contributors: 9 | * Mario Prats 10 | * Javier Perez 11 | */ 12 | 13 | // ---------------------------------------------------- 14 | // Text HUD Class 15 | // ---------------------------------------------------- 16 | #include 17 | 18 | class TextHUD : public osg::Referenced 19 | { 20 | private: 21 | osg::ref_ptr _camera; 22 | osg::ref_ptr _modeText; 23 | osg::ref_ptr _cameraModeText; 24 | 25 | public: 26 | TextHUD(void) 27 | { 28 | _camera = createCamera(); 29 | _camera->addChild(createText()); 30 | } 31 | 32 | osg::Camera* createCamera(void) 33 | { 34 | osg::ref_ptr camera = new osg::Camera; 35 | 36 | camera->setViewport(0, 0, 1024, 768); 37 | camera->setReferenceFrame(osg::Transform::ABSOLUTE_RF); 38 | camera->setProjectionMatrixAsOrtho2D(0, 1024, 0, 768); 39 | camera->setRenderOrder(osg::Camera::POST_RENDER); 40 | camera->getOrCreateStateSet()->setMode(GL_LIGHTING, osg::StateAttribute::OFF); 41 | camera->setClearMask(GL_DEPTH_BUFFER_BIT); 42 | // camera->setProjectionResizePolicy(osg::Camera::VERTICAL); 43 | 44 | return camera.release(); 45 | } 46 | 47 | osg::Node* createText(void) 48 | { 49 | osg::Geode* textGeode = new osg::Geode; 50 | 51 | osgText::Text* title = new osgText::Text; 52 | //title->setFont("fonts/arial.ttf"); 53 | title->setCharacterSize(24); 54 | title->setLineSpacing(0.4f); 55 | 56 | std::string versionText = std::string("\nPress 'h' for options"); 57 | 58 | title->setText(versionText); 59 | textGeode->addDrawable(title); 60 | 61 | _modeText = new osgText::Text; 62 | //_modeText->setFont("fonts/arial.ttf"); 63 | _modeText->setCharacterSize(24); 64 | _modeText->setPosition(osg::Vec3f(0.f, -40.f, 0.f)); 65 | _modeText->setDataVariance(osg::Object::DYNAMIC); 66 | textGeode->addDrawable(_modeText.get()); 67 | 68 | _cameraModeText = new osgText::Text; 69 | //_cameraModeText->setFont("fonts/arial.ttf"); 70 | _cameraModeText->setCharacterSize(24); 71 | _cameraModeText->setPosition(osg::Vec3f(0.f, -60.f, 0.f)); 72 | _cameraModeText->setDataVariance(osg::Object::DYNAMIC); 73 | //textGeode->addDrawable( _cameraModeText.get() ); 74 | 75 | osg::PositionAttitudeTransform* titlePAT = new osg::PositionAttitudeTransform; 76 | titlePAT->setPosition(osg::Vec3f(10, 70, 0.f)); 77 | //titlePAT->addChild(textGeode); 78 | 79 | return titlePAT; 80 | } 81 | 82 | void setSceneText(const std::string& preset) 83 | { 84 | _modeText->setText("\nWeather: " + preset + "\n"); 85 | } 86 | 87 | void setCameraText(const std::string& mode) 88 | { 89 | _cameraModeText->setText("Camera: " + mode); 90 | } 91 | 92 | osg::Camera* getHudCamera(void) 93 | { 94 | return _camera.get(); 95 | } 96 | }; 97 | -------------------------------------------------------------------------------- /uwsim/include/uwsim/URDFRobot.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013 University of Jaume-I. 3 | * All rights reserved. This program and the accompanying materials 4 | * are made available under the terms of the GNU Public License v3.0 5 | * which accompanies this distribution, and is available at 6 | * http://www.gnu.org/licenses/gpl.html 7 | * 8 | * Contributors: 9 | * Mario Prats 10 | * Javier Perez 11 | */ 12 | 13 | #ifndef URDFROBOT_H_ 14 | #define URDFROBOT_H_ 15 | 16 | #include "SimulatorConfig.h" 17 | #include "KinematicChain.h" 18 | #include "ConfigXMLParser.h" 19 | 20 | #include 21 | #include 22 | 23 | #include 24 | #include 25 | 26 | class URDFRobot : public KinematicChain 27 | { 28 | 29 | public: 30 | 31 | std::vector joint_axis; 32 | std::string URDFFile; 33 | 34 | URDFRobot(osgOcean::OceanScene *oscene, Vehicle vehicle); 35 | void addToKinematicChain(osg::Node * link, btRigidBody* body); 36 | 37 | ~URDFRobot(); 38 | 39 | protected: 40 | 41 | void updateJoints(std::vector &q); 42 | void updateJoints(std::vector &q, int startJoint, int numJoints); 43 | 44 | private: 45 | void moveJoints(std::vector &q); 46 | 47 | }; 48 | 49 | #endif /* URDFROBOT_H_ */ 50 | -------------------------------------------------------------------------------- /uwsim/include/uwsim/ViewBuilder.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013 University of Jaume-I. 3 | * All rights reserved. This program and the accompanying materials 4 | * are made available under the terms of the GNU Public License v3.0 5 | * which accompanies this distribution, and is available at 6 | * http://www.gnu.org/licenses/gpl.html 7 | * 8 | * Contributors: 9 | * Mario Prats 10 | * Javier Perez 11 | */ 12 | 13 | #ifndef VIEWBUILDER_H 14 | #define VIEWBUILDER_H 15 | 16 | #include 17 | #include 18 | 19 | #include "osgOceanScene.h" 20 | #include "HUDCamera.h" 21 | #include "ROSInterface.h" 22 | #include "SimulatedIAUV.h" 23 | #include "ConfigXMLParser.h" 24 | #include "SceneBuilder.h" 25 | #include 26 | 27 | class ViewBuilder 28 | { 29 | public: 30 | osg::ref_ptr viewer; 31 | std::shared_ptr arguments; 32 | int fullScreenNum; 33 | 34 | public: 35 | osg::ref_ptr wm; 36 | ViewBuilder(ConfigFile &config, SceneBuilder *scene_builder); 37 | ViewBuilder(ConfigFile &config, SceneBuilder *scene_builder, int *argc, char **argv); 38 | ViewBuilder(ConfigFile &config, SceneBuilder *scene_builder, std::shared_ptr args); 39 | 40 | osgViewer::View* getView() 41 | { 42 | return viewer.get(); 43 | } 44 | osgViewer::Viewer* getViewer() 45 | { 46 | return viewer.get(); 47 | } 48 | void init(); 49 | 50 | ~ViewBuilder() 51 | { 52 | } 53 | 54 | protected: 55 | bool init(ConfigFile &config, SceneBuilder *scene_builder); 56 | }; 57 | 58 | #endif 59 | 60 | -------------------------------------------------------------------------------- /uwsim/include/uwsim/VirtualSLSProjector.h: -------------------------------------------------------------------------------- 1 | /* 2 | * VirtualStructuredLightProjector.h 3 | * 4 | * Created on: 06/02/2013 5 | * Author: Miquel Massot 6 | * Modified by: Javier Perez 7 | * 8 | */ 9 | 10 | #ifndef VirtualSLSProjector_H 11 | #define VirtualSLSProjector_H 12 | 13 | #include 14 | 15 | #include "VirtualCamera.h" 16 | 17 | /** Virtual Structured Light sensor that Projects a light (laser or not) in the scene */ 18 | class VirtualSLSProjector 19 | { 20 | public: 21 | std::string name; 22 | std::string image_name; 23 | osg::ref_ptr node; 24 | osg::ref_ptr root; 25 | double range; ///< Max projection range 26 | double fov; ///< Field of view 27 | unsigned int textureUnit; 28 | osg::Texture2D* dbgDepthTexture; 29 | VirtualCamera camera; 30 | 31 | VirtualSLSProjector(std::string name,std::string parentName, osg::Node *root, osg::Node *node, std::string image_name, double fov, 32 | bool laser); 33 | VirtualSLSProjector(); 34 | 35 | virtual void init(std::string name,std::string parentName, osg::Node *root, osg::Node *node, std::string image_name, double range, 36 | double fov, bool laser); 37 | }; 38 | 39 | #endif 40 | -------------------------------------------------------------------------------- /uwsim/include/uwsim/osgOceanScene.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This source file is part of the osgOcean library 3 | * 4 | * Copyright (C) 2009 Kim Bale 5 | * Copyright (C) 2009 The University of Hull, UK 6 | * 7 | * This program is free software; you can redistribute it and/or modify it under 8 | * the terms of the GNU Lesser General Public License as published by the Free Software 9 | * Foundation; either version 3 of the License, or (at your option) any later 10 | * version. 11 | 12 | * This program is distributed in the hope that it will be useful, but WITHOUT 13 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 14 | * FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more details. 15 | * http://www.gnu.org/copyleft/lesser.txt. 16 | * 17 | * Adapted to UWSim by Mario Prats 18 | */ 19 | 20 | #pragma once 21 | #include 22 | #include 23 | 24 | #include 25 | 26 | #include 27 | #include 28 | #include 29 | 30 | #include "SkyDome.h" 31 | #include "ConfigXMLParser.h" 32 | #include 33 | 34 | enum DrawMask 35 | { 36 | CAST_SHADOW = (0x1 << 30), RECEIVE_SHADOW = (0x1 << 29), 37 | }; 38 | 39 | class osgOceanScene : public osg::Referenced 40 | { 41 | public: 42 | enum SCENE_TYPE 43 | { 44 | CLEAR, DUSK, CLOUDY 45 | }; 46 | osg::ref_ptr localizedWorld; 47 | 48 | private: 49 | SCENE_TYPE _sceneType; 50 | bool _useVBO; 51 | 52 | osg::ref_ptr _modeText; 53 | osg::ref_ptr _scene; 54 | 55 | osg::ref_ptr _oceanScene; 56 | osg::ref_ptr _FFToceanSurface; 57 | osg::ref_ptr _cubemap; 58 | osg::ref_ptr _skyDome; 59 | 60 | std::vector _cubemapDirs; 61 | std::vector _lightColors; 62 | std::vector _fogColors; 63 | std::vector _underwaterAttenuations; 64 | std::vector _underwaterDiffuse; 65 | 66 | osg::ref_ptr _light; 67 | 68 | std::vector _sunPositions; 69 | std::vector _sunDiffuse; 70 | std::vector _waterFogColors; 71 | 72 | osg::ref_ptr _islandSwitch; 73 | 74 | public: 75 | osgOceanScene(double offsetp[3], double offsetr[3], const osg::Vec2f& windDirection = osg::Vec2f(1.0f, 1.0f), 76 | float windSpeed = 12.f, float depth = 10000.f, float reflectionDamping = 0.35f, float scale = 1e-8, 77 | bool isChoppy = true, float choppyFactor = -2.5f, float crestFoamHeight = 2.2f, bool useVBO = false, 78 | const std::string& terrain_shader_basename = "terrain"); 79 | 80 | void build(double offsetp[3], double offsetr[3], const osg::Vec2f& windDirection, float windSpeed, float depth, 81 | float reflectionDamping, float waveScale, bool isChoppy, float choppyFactor, float crestFoamHeight, 82 | bool useVBO, const std::string& terrain_shader_basename); 83 | 84 | void changeScene(SCENE_TYPE type); 85 | 86 | // Load the islands model 87 | // Here we attach a custom shader to the model. 88 | // This shader overrides the default shader applied by OceanScene but uses uniforms applied by OceanScene. 89 | // The custom shader is needed to add multi-texturing and bump mapping to the terrain. 90 | osg::Node* loadIslands(const std::string& terrain_shader_basename); 91 | 92 | osg::ref_ptr loadCubeMapTextures(const std::string& dir); 93 | 94 | osg::Geode* sunDebug(const osg::Vec3f& position); 95 | 96 | inline osg::Vec4f intColor(unsigned r, unsigned g, unsigned b, unsigned a = 255) 97 | { 98 | float div = 1.f / 255.f; 99 | return osg::Vec4f(div * (float)r, div * (float)g, div * float(b), div * (float)a); 100 | } 101 | 102 | inline osgOcean::OceanScene::EventHandler* getOceanSceneEventHandler() 103 | { 104 | return _oceanScene->getEventHandler(); 105 | } 106 | 107 | inline osgOcean::OceanTechnique* getOceanSurface(void) 108 | { 109 | return _FFToceanSurface.get(); 110 | } 111 | 112 | inline osg::Group* getScene(void) 113 | { 114 | return _scene.get(); 115 | } 116 | 117 | inline osgOcean::OceanScene* getOceanScene() 118 | { 119 | return _oceanScene.get(); 120 | } 121 | 122 | osg::Light* getLight() 123 | { 124 | return _light.get(); 125 | } 126 | 127 | osg::Node* addObject(osg::Transform *transform, std::string filename, Object *o = NULL); 128 | 129 | void addObject(osg::Transform *transform); 130 | 131 | }; 132 | -------------------------------------------------------------------------------- /uwsim/interface_examples/gotoAbsolutePose.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013 University of Jaume-I. 3 | * All rights reserved. This program and the accompanying materials 4 | * are made available under the terms of the GNU Public License v3.0 5 | * which accompanies this distribution, and is available at 6 | * http://www.gnu.org/licenses/gpl.html 7 | * 8 | * Contributors: 9 | * Mario Prats 10 | * Javier Perez 11 | */ 12 | 13 | 14 | 15 | /* Move a vehicle with a velocity proportional to the distance 16 | * between the current pose and a desired absolute pose 17 | */ 18 | 19 | #include 20 | #include 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | bool firstpass=false; 29 | osg::Quat initialQ, goalQ, currentQ; 30 | osg::Vec3d initialT, goalT, currentT; 31 | double totalDistance, currentDistance; 32 | 33 | void vehiclePoseCallback(const nav_msgs::Odometry& odom) { 34 | if (!firstpass) { 35 | initialT.set(odom.pose.pose.position.x,odom.pose.pose.position.y,odom.pose.pose.position.z); 36 | initialQ.set(odom.pose.pose.orientation.x, odom.pose.pose.orientation.y, odom.pose.pose.orientation.z, odom.pose.pose.orientation.w); 37 | totalDistance=(goalT-initialT).length(); 38 | firstpass=true; 39 | } 40 | currentT.set(odom.pose.pose.position.x,odom.pose.pose.position.y,odom.pose.pose.position.z); 41 | currentDistance=(goalT-currentT).length(); 42 | } 43 | 44 | int main(int argc, char **argv) { 45 | 46 | 47 | if (argc!=9) { 48 | std::cerr << "USAGE: " << argv[0] << " " << std::endl; 49 | std::cerr << "units are meters and radians." << std::endl; 50 | return 0; 51 | } 52 | 53 | std::string poseTopic(argv[1]); 54 | std::string controlTopic(argv[2]); 55 | double x=atof(argv[3]); 56 | double y=atof(argv[4]); 57 | double z=atof(argv[5]); 58 | double roll=atof(argv[6]); 59 | double pitch=atof(argv[7]); 60 | double yaw=atof(argv[8]); 61 | 62 | std::string nodeName=controlTopic; 63 | nodeName.replace(0,1,"a"); 64 | ros::init(argc, argv, nodeName); 65 | ros::NodeHandle nh; 66 | 67 | osg::Matrixd T, Rx, Ry, Rz, transform; 68 | T.makeTranslate(x,y,z); 69 | Rx.makeRotate(roll,1,0,0); 70 | Ry.makeRotate(pitch,0,1,0); 71 | Rz.makeRotate(yaw,0,0,1); 72 | transform=Rz*Ry*Rx*T; 73 | goalT=transform.getTrans(); 74 | goalQ=transform.getRotate(); 75 | 76 | ros::Publisher position_pub=nh.advertise(controlTopic,1); 77 | ros::Subscriber position_sub = nh.subscribe(poseTopic, 1, vehiclePoseCallback); 78 | 79 | ros::Rate r(30); 80 | while (ros::ok()) { 81 | if (firstpass) { 82 | osg::Vec3d vT=(goalT-currentT)*0.15; 83 | double vScale=(vT.length()>0.1) ? 0.1/vT.length() : 1; 84 | /* 85 | std::cerr << "Initial Q: " << initialQ.x() << " " << initialQ.y() << " " << initialQ.z() << " " << initialQ.w() << std::endl; 86 | std::cerr << "Goal Q: " << goalQ.x() << " " << goalQ.y() << " " << goalQ.z() << " " << goalQ.w() << std::endl; 87 | std::cerr << "Goal Q: " << currentQ.x() << " " << currentQ.y() << " " << currentQ.z() << " " << currentQ.w() << std::endl; 88 | std::cerr << "Goal T: " << goalT.x() << " " << goalT.y() << " " << goalT.z() << std::endl; 89 | std::cerr << "Current T: " << currentT.x() << " " << currentT.y() << " " << currentT.z() << std::endl; 90 | std::cerr << "Goal-Current" << (goalT-currentT).x() << " " << (goalT-currentT).y() << " " << (goalT-currentT).z() << std::endl; 91 | std::cerr << "Current dist: " << currentDistance << " Total: " << totalDistance << std::endl; 92 | std::cerr << "current/total: " << currentDistance/totalDistance << std::endl; 93 | std::cerr << "Slerp: " << 1 - currentDistance/totalDistance << std::endl; 94 | std::cerr << std::endl; 95 | */ 96 | currentQ.slerp(1-currentDistance/totalDistance,initialQ, goalQ); 97 | nav_msgs::Odometry odom; 98 | odom.pose.pose.position.x=currentT.x()+vT.x()*vScale; 99 | odom.pose.pose.position.y=currentT.y()+vT.y()*vScale; 100 | odom.pose.pose.position.z=currentT.z()+vT.z()*vScale; 101 | odom.pose.pose.orientation.x=currentQ.x(); 102 | odom.pose.pose.orientation.y=currentQ.y(); 103 | odom.pose.pose.orientation.z=currentQ.z(); 104 | odom.pose.pose.orientation.w=currentQ.w(); 105 | 106 | odom.twist.twist.linear.x=0; 107 | odom.twist.twist.linear.y=0; 108 | odom.twist.twist.linear.z=0; 109 | odom.twist.twist.angular.x=0; 110 | odom.twist.twist.angular.y=0; 111 | odom.twist.twist.angular.z=0; 112 | for (int i=0; i<36; i++) { 113 | odom.twist.covariance[i]=0; 114 | odom.pose.covariance[i]=0; 115 | } 116 | position_pub.publish(odom); 117 | } 118 | ros::spinOnce(); 119 | r.sleep(); 120 | } 121 | 122 | return 0; 123 | } 124 | -------------------------------------------------------------------------------- /uwsim/interface_examples/gotoRelativePose.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013 University of Jaume-I. 3 | * All rights reserved. This program and the accompanying materials 4 | * are made available under the terms of the GNU Public License v3.0 5 | * which accompanies this distribution, and is available at 6 | * http://www.gnu.org/licenses/gpl.html 7 | * 8 | * Contributors: 9 | * Mario Prats 10 | * Javier Perez 11 | */ 12 | 13 | 14 | /* Move a vehicle with a velocity proportional to the distance 15 | * between the current pose and a desired pose relative to the current one (given in the global coordinate system) 16 | */ 17 | 18 | #include 19 | #include 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | bool firstpass=false; 28 | osg::Quat initialQ, goalQ, currentQ; 29 | osg::Vec3d initialT, goalT, currentT; 30 | double totalDistance, currentDistance; 31 | double x, y, z, roll, pitch, yaw; 32 | 33 | void vehiclePoseCallback(const nav_msgs::Odometry& odom) { 34 | if (!firstpass) { 35 | initialT.set(odom.pose.pose.position.x,odom.pose.pose.position.y,odom.pose.pose.position.z); 36 | initialQ.set(odom.pose.pose.orientation.x, odom.pose.pose.orientation.y, odom.pose.pose.orientation.z, odom.pose.pose.orientation.w); 37 | osg::Matrixd transform; 38 | transform.setTrans(initialT); 39 | transform.setRotate(initialQ); 40 | 41 | osg::Matrixd T, Rx, Ry, Rz, transform_rel; 42 | T.makeTranslate(x,y,z); 43 | Rx.makeRotate(roll,1,0,0); 44 | Ry.makeRotate(pitch,0,1,0); 45 | Rz.makeRotate(yaw,0,0,1); 46 | transform_rel=Rz*Ry*Rx*T; 47 | transform=transform_rel*transform; 48 | goalQ=transform.getRotate(); 49 | goalT=transform.getTrans(); 50 | 51 | totalDistance=(goalT-initialT).length(); 52 | firstpass=true; 53 | } 54 | currentT.set(odom.pose.pose.position.x,odom.pose.pose.position.y,odom.pose.pose.position.z); 55 | currentDistance=(goalT-currentT).length(); 56 | } 57 | 58 | int main(int argc, char **argv) { 59 | 60 | ros::init(argc, argv, "gotoRelativePose"); 61 | ros::NodeHandle nh; 62 | 63 | if (argc!=9) { 64 | std::cerr << "USAGE: " << argv[0] << " " << std::endl; 65 | std::cerr << "units are meters and radians." << std::endl; 66 | return 0; 67 | } 68 | 69 | std::string poseTopic(argv[1]); 70 | std::string controlTopic(argv[2]); 71 | x=atof(argv[3]); 72 | y=atof(argv[4]); 73 | z=atof(argv[5]); 74 | roll=atof(argv[6]); 75 | pitch=atof(argv[7]); 76 | yaw=atof(argv[8]); 77 | 78 | ros::Publisher position_pub=nh.advertise(controlTopic,1); 79 | ros::Subscriber position_sub = nh.subscribe(poseTopic, 1, vehiclePoseCallback); 80 | 81 | ros::Rate r(30); 82 | while (ros::ok()) { 83 | if (firstpass) { 84 | osg::Vec3d vT=(goalT-currentT)*0.15; 85 | double vScale=(vT.length()>0.05) ? 0.05/vT.length() : 1; 86 | currentQ.slerp(1-currentDistance/totalDistance,initialQ, goalQ); 87 | nav_msgs::Odometry odom; 88 | odom.pose.pose.position.x=currentT.x()+vT.x()*vScale; 89 | odom.pose.pose.position.y=currentT.y()+vT.y()*vScale; 90 | odom.pose.pose.position.z=currentT.z()+vT.z()*vScale; 91 | odom.pose.pose.orientation.x=currentQ.x(); 92 | odom.pose.pose.orientation.y=currentQ.y(); 93 | odom.pose.pose.orientation.z=currentQ.z(); 94 | odom.pose.pose.orientation.w=currentQ.w(); 95 | 96 | odom.twist.twist.linear.x=0; 97 | odom.twist.twist.linear.y=0; 98 | odom.twist.twist.linear.z=0; 99 | odom.twist.twist.angular.x=0; 100 | odom.twist.twist.angular.y=0; 101 | odom.twist.twist.angular.z=0; 102 | for (int i=0; i<36; i++) { 103 | odom.twist.covariance[i]=0; 104 | odom.pose.covariance[i]=0; 105 | } 106 | position_pub.publish(odom); 107 | } 108 | ros::spinOnce(); 109 | r.sleep(); 110 | } 111 | 112 | return 0; 113 | } 114 | -------------------------------------------------------------------------------- /uwsim/interface_examples/makeVehicleSurvey.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013 University of Jaume-I. 3 | * All rights reserved. This program and the accompanying materials 4 | * are made available under the terms of the GNU Public License v3.0 5 | * which accompanies this distribution, and is available at 6 | * http://www.gnu.org/licenses/gpl.html 7 | * 8 | * Contributors: 9 | * Mario Prats 10 | * Javier Perez 11 | */ 12 | 13 | #include 14 | #include 15 | 16 | //ROS 17 | #include 18 | #include 19 | 20 | #include 21 | #include 22 | 23 | int main(int argc, char **argv) { 24 | 25 | 26 | if (argc!=8) { 27 | std::cerr << "USAGE: " << argv[0] << " " << std::endl; 28 | std::cerr << "units in meters and radians" << std::endl; 29 | return 0; 30 | } 31 | 32 | std::string topic(argv[1]); 33 | double x=atof(argv[2]); 34 | double y=atof(argv[3]); 35 | double z=atof(argv[4]); 36 | // TODO FIXME yaw is not used below! 37 | //double yaw=atof(argv[5]); 38 | double amp=atof(argv[6]); 39 | double step=atof(argv[7]); 40 | 41 | std::string nodeName=topic; 42 | nodeName=nodeName.replace(0,1,"a"); 43 | std::cerr << "NodeName: " << nodeName << std::endl; 44 | ros::init(argc, argv, nodeName); 45 | ros::NodeHandle nh; 46 | 47 | ros::Publisher position_pub; 48 | position_pub=nh.advertise(topic,1); 49 | 50 | double angle=0; 51 | ros::Rate r(20); 52 | while (ros::ok()) { 53 | nav_msgs::Odometry odom; 54 | odom.pose.pose.position.x=x+amp*sin(angle); 55 | odom.pose.pose.position.y=y+angle*step; 56 | odom.pose.pose.position.z=z; 57 | 58 | double rz=0; 59 | if (step>0) { 60 | rz=M_PI_2-M_PI_2*cos(angle); 61 | } else { 62 | rz=-M_PI_2+M_PI_2*cos(angle); 63 | } 64 | osg::Quat rot(0, osg::Vec3d(1,0,0), 0, osg::Vec3d(0,1,0), rz, osg::Vec3d(0,0,1)); 65 | odom.pose.pose.orientation.x=rot.x(); 66 | odom.pose.pose.orientation.y=rot.y(); 67 | odom.pose.pose.orientation.z=rot.z(); 68 | odom.pose.pose.orientation.w=rot.w(); 69 | 70 | odom.twist.twist.linear.x=0; 71 | odom.twist.twist.linear.y=0; 72 | odom.twist.twist.linear.z=0; 73 | odom.twist.twist.angular.x=0; 74 | odom.twist.twist.angular.y=0; 75 | odom.twist.twist.angular.z=0; 76 | for (int i=0; i<36; i++) { 77 | odom.twist.covariance[i]=0; 78 | odom.pose.covariance[i]=0; 79 | } 80 | position_pub.publish(odom); 81 | 82 | ros::spinOnce(); 83 | r.sleep(); 84 | angle+=0.01; 85 | } 86 | 87 | return 0; 88 | } 89 | -------------------------------------------------------------------------------- /uwsim/interface_examples/setJointPosition.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013 University of Jaume-I. 3 | * All rights reserved. This program and the accompanying materials 4 | * are made available under the terms of the GNU Public License v3.0 5 | * which accompanies this distribution, and is available at 6 | * http://www.gnu.org/licenses/gpl.html 7 | * 8 | * Contributors: 9 | * Mario Prats 10 | * Javier Perez 11 | */ 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | int main(int argc, char **argv) { 18 | if (argc != 7) { 19 | std::cerr << "Usage: " << argv[0] << " " << std::endl; 20 | std::cerr << "Units are radians" << std::endl; 21 | exit(0); 22 | } 23 | std::string topic(argv[1]); 24 | 25 | ros::init(argc, argv, "setJointPosition"); 26 | ros::NodeHandle nh; 27 | ros::Publisher position_pub; 28 | position_pub=nh.advertise(topic,1); 29 | ros::Rate rate(30); 30 | 31 | double q[5]; 32 | for (int i=0; i<5; i++) q[i]=atof(argv[i+2]); 33 | 34 | while (ros::ok()) { 35 | 36 | sensor_msgs::JointState js; 37 | js.name.push_back(std::string("Slew")); 38 | js.position.push_back(q[0]); 39 | js.name.push_back(std::string("Shoulder")); 40 | js.position.push_back(q[1]); 41 | js.name.push_back(std::string("Elbow")); 42 | js.position.push_back(q[2]); 43 | js.name.push_back(std::string("JawRotate")); 44 | js.position.push_back(q[3]); 45 | js.name.push_back(std::string("JawOpening")); 46 | js.position.push_back(q[4]); 47 | 48 | position_pub.publish(js); 49 | 50 | ros::spinOnce(); 51 | rate.sleep(); 52 | } 53 | 54 | return 0; 55 | } 56 | -------------------------------------------------------------------------------- /uwsim/interface_examples/setJointVelocity.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013 University of Jaume-I. 3 | * All rights reserved. This program and the accompanying materials 4 | * are made available under the terms of the GNU Public License v3.0 5 | * which accompanies this distribution, and is available at 6 | * http://www.gnu.org/licenses/gpl.html 7 | * 8 | * Contributors: 9 | * Mario Prats 10 | * Javier Perez 11 | */ 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | int main(int argc, char **argv) { 18 | if (argc != 7) { 19 | std::cerr << "Usage: " << argv[0] << " " << std::endl; 20 | std::cerr << "Units are radians/simulated_time. Time scale to be implemented." << std::endl; 21 | exit(0); 22 | } 23 | std::string topic(argv[1]); 24 | 25 | ros::init(argc, argv, "setJointVelocity"); 26 | ros::NodeHandle nh; 27 | ros::Publisher velocity_pub; 28 | velocity_pub=nh.advertise(topic,1); 29 | ros::Rate rate(30); 30 | 31 | double qdot[5]; 32 | for (int i=0; i<5; i++) qdot[i]=atof(argv[i+2]); 33 | 34 | while (ros::ok()) { 35 | 36 | sensor_msgs::JointState js; 37 | js.name.push_back(std::string("Slew")); 38 | js.velocity.push_back(qdot[0]); 39 | js.name.push_back(std::string("Shoulder")); 40 | js.velocity.push_back(qdot[1]); 41 | js.name.push_back(std::string("Elbow")); 42 | js.velocity.push_back(qdot[2]); 43 | js.name.push_back(std::string("JawRotate")); 44 | js.velocity.push_back(qdot[3]); 45 | js.name.push_back(std::string("JawOpening")); 46 | js.velocity.push_back(qdot[4]); 47 | 48 | velocity_pub.publish(js); 49 | 50 | ros::spinOnce(); 51 | rate.sleep(); 52 | } 53 | 54 | return 0; 55 | } 56 | -------------------------------------------------------------------------------- /uwsim/interface_examples/setVehicleActuators.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013 University of Jaume-I. 3 | * All rights reserved. This program and the accompanying materials 4 | * are made available under the terms of the GNU Public License v3.0 5 | * which accompanies this distribution, and is available at 6 | * http://www.gnu.org/licenses/gpl.html 7 | * 8 | * Contributors: 9 | * Mario Prats 10 | * Javier Perez 11 | */ 12 | 13 | /** Sends a control signal to the vehicle actuators, needs the dynamics module running 14 | */ 15 | 16 | #include 17 | #include 18 | #include 19 | 20 | int main(int argc, char **argv) { 21 | if (argc < 3) { 22 | std::cerr << "Usage: " << argv[0] << " ... " << std::endl; 23 | exit(0); 24 | } 25 | std::string topic(argv[1]); 26 | 27 | ros::init(argc, argv, "setVehicleActuators"); 28 | ros::NodeHandle nh; 29 | 30 | ros::Publisher u_pub; 31 | u_pub=nh.advertise(topic,1); 32 | ros::Rate rate(5); 33 | 34 | double u[argc-2]; 35 | for (int i=2; i 14 | #include 15 | 16 | //ROS 17 | #include 18 | #include 19 | //OSG 20 | #include 21 | #include 22 | #include 23 | 24 | bool firstpass=false; 25 | osg::Quat initialQ; 26 | osg::Vec3d initialT; 27 | osg::Matrix wMv_initial; 28 | 29 | void vehiclePoseCallback(const nav_msgs::Odometry& odom) { 30 | if (!firstpass) { 31 | initialT.set(odom.pose.pose.position.x,odom.pose.pose.position.y,odom.pose.pose.position.z); 32 | initialQ.set(odom.pose.pose.orientation.x, odom.pose.pose.orientation.y, odom.pose.pose.orientation.z, odom.pose.pose.orientation.w); 33 | wMv_initial.setTrans(initialT); 34 | wMv_initial.setRotate(initialQ); 35 | firstpass=true; 36 | } 37 | } 38 | 39 | int main(int argc, char **argv) { 40 | 41 | if (argc!=9) { 42 | std::cerr << "USAGE: " << argv[0] << " " << std::endl; 43 | std::cerr << " is the maximum desired variation on the vehicle pose" << std::endl; 44 | std::cerr << "units in meters and radians" << std::endl; 45 | std::cerr << "Example: " << argv[0] << "/dataNavigator 0.2 0.2 0.2 0.2" << std::endl; 46 | return 0; 47 | } 48 | 49 | std::string poseTopic(argv[1]); 50 | std::string controlTopic(argv[2]); 51 | double ax=atof(argv[3]); 52 | double ay=atof(argv[4]); 53 | double az=atof(argv[5]); 54 | double aroll=atof(argv[6]); 55 | double apitch=atof(argv[7]); 56 | double ayaw=atof(argv[8]); 57 | 58 | std::string nodeName=controlTopic; 59 | nodeName.replace(0,1,"a"); 60 | ros::init(argc, argv, nodeName); 61 | ros::NodeHandle nh; 62 | ros::Publisher position_pub; 63 | position_pub=nh.advertise(controlTopic,1); 64 | ros::Subscriber position_sub = nh.subscribe(poseTopic, 1, vehiclePoseCallback); 65 | 66 | double angle=0; 67 | ros::Rate r(25); 68 | while (ros::ok()) { 69 | if (firstpass) { 70 | osg::Matrixd T, Rx, Ry, Rz, transform; 71 | T.makeTranslate(ax*sin(2*angle),ay*sin(angle),az*sin(angle)); 72 | Rx.makeRotate(aroll*sin(angle),1,0,0); 73 | Ry.makeRotate(apitch*sin(angle),0,1,0); 74 | Rz.makeRotate(ayaw*sin(angle),0,0,1); 75 | transform=Rz*Ry*Rx*T*wMv_initial; 76 | 77 | osg::Vec3d currentT=transform.getTrans(); 78 | osg::Quat currentQ=transform.getRotate(); 79 | 80 | nav_msgs::Odometry odom; 81 | odom.pose.pose.position.x=currentT.x(); 82 | odom.pose.pose.position.y=currentT.y(); 83 | odom.pose.pose.position.z=currentT.z(); 84 | odom.pose.pose.orientation.x=currentQ.x(); 85 | odom.pose.pose.orientation.y=currentQ.y(); 86 | odom.pose.pose.orientation.z=currentQ.z(); 87 | odom.pose.pose.orientation.w=currentQ.w(); 88 | angle+=0.01; 89 | 90 | odom.twist.twist.linear.x=0; 91 | odom.twist.twist.linear.y=0; 92 | odom.twist.twist.linear.z=0; 93 | odom.twist.twist.angular.x=0; 94 | odom.twist.twist.angular.y=0; 95 | odom.twist.twist.angular.z=0; 96 | for (int i=0; i<36; i++) { 97 | odom.twist.covariance[i]=0; 98 | odom.pose.covariance[i]=0; 99 | } 100 | position_pub.publish(odom); 101 | } 102 | ros::spinOnce(); 103 | r.sleep(); 104 | 105 | } 106 | 107 | return 0; 108 | } 109 | -------------------------------------------------------------------------------- /uwsim/interface_examples/setVehiclePose.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013 University of Jaume-I. 3 | * All rights reserved. This program and the accompanying materials 4 | * are made available under the terms of the GNU Public License v3.0 5 | * which accompanies this distribution, and is available at 6 | * http://www.gnu.org/licenses/gpl.html 7 | * 8 | * Contributors: 9 | * Mario Prats 10 | * Javier Perez 11 | */ 12 | 13 | #include 14 | #include 15 | 16 | //ROS 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | 24 | int main(int argc, char **argv) { 25 | 26 | ros::init(argc, argv, "setVehiclePose"); 27 | ros::NodeHandle nh; 28 | 29 | if (argc!=8) { 30 | std::cerr << "USAGE: " << argv[0] << " " << std::endl; 31 | std::cerr << "units are in meters and radians." << std::endl; 32 | return 0; 33 | } 34 | 35 | std::string topic(argv[1]); 36 | double x=atof(argv[2]); 37 | double y=atof(argv[3]); 38 | double z=atof(argv[4]); 39 | double roll=atof(argv[5]); 40 | double pitch=atof(argv[6]); 41 | double yaw=atof(argv[7]); 42 | 43 | ros::Publisher position_pub; 44 | position_pub=nh.advertise(topic,1); 45 | 46 | 47 | osg::Matrixd T, Rx, Ry, Rz, transform; 48 | T.makeTranslate(x,y,z); 49 | Rx.makeRotate(roll,1,0,0); 50 | Ry.makeRotate(pitch,0,1,0); 51 | Rz.makeRotate(yaw,0,0,1); 52 | transform=Rz*Ry*Rx*T; 53 | osg::Vec3d trans=transform.getTrans(); 54 | osg::Quat rot=transform.getRotate(); 55 | 56 | 57 | ros::Rate r(25); 58 | while (ros::ok()) { 59 | geometry_msgs::Pose pose; 60 | 61 | pose.position.x=trans.x(); 62 | pose.position.y=trans.y(); 63 | pose.position.z=trans.z(); 64 | pose.orientation.x=rot.x(); 65 | pose.orientation.y=rot.y(); 66 | pose.orientation.z=rot.z(); 67 | pose.orientation.w=rot.w(); 68 | 69 | position_pub.publish(pose); 70 | 71 | ros::spinOnce(); 72 | r.sleep(); 73 | } 74 | 75 | return 0; 76 | } 77 | -------------------------------------------------------------------------------- /uwsim/interface_examples/setVehiclePosition.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013 University of Jaume-I. 3 | * All rights reserved. This program and the accompanying materials 4 | * are made available under the terms of the GNU Public License v3.0 5 | * which accompanies this distribution, and is available at 6 | * http://www.gnu.org/licenses/gpl.html 7 | * 8 | * Contributors: 9 | * Mario Prats 10 | * Javier Perez 11 | */ 12 | 13 | #include 14 | #include 15 | 16 | //ROS 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | int main(int argc, char **argv) { 24 | 25 | ros::init(argc, argv, "setVehiclePosition"); 26 | ros::NodeHandle nh; 27 | 28 | if (argc!=8) { 29 | std::cerr << "USAGE: " << argv[0] << " " << std::endl; 30 | std::cerr << "units in meters and radians" << std::endl; 31 | return 0; 32 | } 33 | 34 | std::string topic(argv[1]); 35 | double x=atof(argv[2]); 36 | double y=atof(argv[3]); 37 | double z=atof(argv[4]); 38 | double roll=atof(argv[5]); 39 | double pitch=atof(argv[6]); 40 | double yaw=atof(argv[7]); 41 | 42 | ros::Publisher position_pub; 43 | position_pub=nh.advertise(topic,1); 44 | 45 | osg::Matrixd T, Rx, Ry, Rz, transform; 46 | T.makeTranslate(x,y,z); 47 | Rx.makeRotate(roll,1,0,0); 48 | Ry.makeRotate(pitch,0,1,0); 49 | Rz.makeRotate(yaw,0,0,1); 50 | transform=Rz*Ry*Rx*T; 51 | osg::Vec3d trans=transform.getTrans(); 52 | osg::Quat rot=transform.getRotate(); 53 | 54 | ros::Rate r(10); 55 | while (ros::ok()) { 56 | nav_msgs::Odometry odom; 57 | odom.pose.pose.position.x=trans.x(); 58 | odom.pose.pose.position.y=trans.y(); 59 | odom.pose.pose.position.z=trans.z(); 60 | odom.pose.pose.orientation.x=rot.x(); 61 | odom.pose.pose.orientation.y=rot.y(); 62 | odom.pose.pose.orientation.z=rot.z(); 63 | odom.pose.pose.orientation.w=rot.w(); 64 | 65 | odom.twist.twist.linear.x=0; 66 | odom.twist.twist.linear.y=0; 67 | odom.twist.twist.linear.z=0; 68 | odom.twist.twist.angular.x=0; 69 | odom.twist.twist.angular.y=0; 70 | odom.twist.twist.angular.z=0; 71 | for (int i=0; i<36; i++) { 72 | odom.twist.covariance[i]=0; 73 | odom.pose.covariance[i]=0; 74 | } 75 | position_pub.publish(odom); 76 | 77 | ros::spinOnce(); 78 | r.sleep(); 79 | } 80 | 81 | return 0; 82 | } 83 | -------------------------------------------------------------------------------- /uwsim/interface_examples/setVehicleTwist.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013 University of Jaume-I. 3 | * All rights reserved. This program and the accompanying materials 4 | * are made available under the terms of the GNU Public License v3.0 5 | * which accompanies this distribution, and is available at 6 | * http://www.gnu.org/licenses/gpl.html 7 | * 8 | * Contributors: 9 | * Mario Prats 10 | * Javier Perez 11 | */ 12 | 13 | #include 14 | #include 15 | 16 | //ROS 17 | #include 18 | #include 19 | 20 | 21 | int main(int argc, char **argv) { 22 | 23 | 24 | if (argc!=8) { 25 | std::cerr << "USAGE: " << argv[0] << " " << std::endl; 26 | std::cerr << "units are displacement/simulated_time. Time scale to be implemented." << std::endl; 27 | return 0; 28 | } 29 | 30 | std::string topic(argv[1]); 31 | double x=atof(argv[2]); 32 | double y=atof(argv[3]); 33 | double z=atof(argv[4]); 34 | double roll=atof(argv[5]); 35 | double pitch=atof(argv[6]); 36 | double yaw=atof(argv[7]); 37 | 38 | std::string nodeName=topic; 39 | nodeName.replace(0,1,"a"); 40 | ros::init(argc, argv, "setVehicleVelocity"); 41 | ros::NodeHandle nh; 42 | ros::Publisher position_pub; 43 | position_pub=nh.advertise(topic,1); 44 | 45 | ros::Rate r(25); 46 | while (ros::ok()) { 47 | geometry_msgs::TwistStamped twist; 48 | 49 | twist.twist.linear.x=x; 50 | twist.twist.linear.y=y; 51 | twist.twist.linear.z=z; 52 | twist.twist.angular.x=roll; 53 | twist.twist.angular.y=pitch; 54 | twist.twist.angular.z=yaw; 55 | 56 | position_pub.publish(twist); 57 | 58 | ros::spinOnce(); 59 | r.sleep(); 60 | } 61 | 62 | return 0; 63 | } 64 | -------------------------------------------------------------------------------- /uwsim/interface_examples/setVehicleVelocity.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013 University of Jaume-I. 3 | * All rights reserved. This program and the accompanying materials 4 | * are made available under the terms of the GNU Public License v3.0 5 | * which accompanies this distribution, and is available at 6 | * http://www.gnu.org/licenses/gpl.html 7 | * 8 | * Contributors: 9 | * Mario Prats 10 | * Javier Perez 11 | */ 12 | 13 | #include 14 | #include 15 | 16 | //ROS 17 | #include 18 | #include 19 | 20 | 21 | int main(int argc, char **argv) { 22 | 23 | 24 | if (argc!=8) { 25 | std::cerr << "USAGE: " << argv[0] << " " << std::endl; 26 | std::cerr << "units are displacement/simulated_time. Time scale to be implemented." << std::endl; 27 | return 0; 28 | } 29 | 30 | std::string topic(argv[1]); 31 | double x=atof(argv[2]); 32 | double y=atof(argv[3]); 33 | double z=atof(argv[4]); 34 | double roll=atof(argv[5]); 35 | double pitch=atof(argv[6]); 36 | double yaw=atof(argv[7]); 37 | 38 | //std::string nodeName=topic; 39 | //nodeName.replace(0,1,"a"); 40 | ros::init(argc, argv, "setVehicleVelocity"); 41 | ros::NodeHandle nh; 42 | ros::Publisher position_pub; 43 | position_pub=nh.advertise(topic,1); 44 | 45 | ros::Rate r(25); 46 | while (ros::ok()) { 47 | nav_msgs::Odometry odom; 48 | odom.pose.pose.position.x=0.0; 49 | odom.pose.pose.position.y=0.0; 50 | odom.pose.pose.position.z=0.0; 51 | odom.pose.pose.orientation.x=0.0; 52 | odom.pose.pose.orientation.y=0.0; 53 | odom.pose.pose.orientation.z=0.0; 54 | odom.pose.pose.orientation.w=1; 55 | 56 | odom.twist.twist.linear.x=x; 57 | odom.twist.twist.linear.y=y; 58 | odom.twist.twist.linear.z=z; 59 | odom.twist.twist.angular.x=roll; 60 | odom.twist.twist.angular.y=pitch; 61 | odom.twist.twist.angular.z=yaw; 62 | for (int i=0; i<36; i++) { 63 | odom.twist.covariance[i]=0; 64 | odom.pose.covariance[i]=0; 65 | } 66 | position_pub.publish(odom); 67 | 68 | ros::spinOnce(); 69 | r.sleep(); 70 | } 71 | 72 | return 0; 73 | } 74 | -------------------------------------------------------------------------------- /uwsim/interface_examples/spawnMarker.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013 University of Jaume-I. 3 | * All rights reserved. This program and the accompanying materials 4 | * are made available under the terms of the GNU Public License v3.0 5 | * which accompanies this distribution, and is available at 6 | * http://www.gnu.org/licenses/gpl.html 7 | * 8 | * Contributors: 9 | * Mario Prats 10 | * Javier Perez 11 | */ 12 | 13 | /* Spawns / deletes a Mesh marker in the uwsim scene through a marker service */ 14 | //Arguments: 15 | // action (add/modify, del), name, id, mesh, x ,y ,z , r, p, y 16 | 17 | #include 18 | #include 19 | #include 20 | 21 | 22 | 23 | int main( int argc, char** argv ) 24 | { 25 | ros::init(argc, argv, "spawnMarker"); 26 | ros::NodeHandle n; 27 | 28 | if (argc!=11) 29 | { 30 | std::cerr << "USAGE: " << argv[0] << " " << std::endl; 31 | std::cerr << "units in meters and radians" << std::endl; 32 | return 0; 33 | } 34 | 35 | ros::ServiceClient client = n.serviceClient("SpawnMarker"); 36 | underwater_sensor_msgs::SpawnMarker srv; 37 | 38 | // UWSim does not use frame_id (TODO), it will add the mark in localizedWorld. 39 | srv.request.marker.header.frame_id = "/localizedWorld"; 40 | srv.request.marker.header.stamp = ros::Time::now(); 41 | 42 | // Set the namespace and id for this marker. This serves to create a unique ID 43 | // Any marker sent with the same namespace and id will overwrite the old one 44 | srv.request.marker.ns = (std::string) argv[2]; 45 | srv.request.marker.id = atoi(argv[3]); 46 | 47 | // Set the marker type. 48 | srv.request.marker.type = visualization_msgs::Marker::MESH_RESOURCE; 49 | srv.request.marker.mesh_resource="package://uwsim/" + (std::string) argv[4]; 50 | 51 | // Set the marker action. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL) 52 | if((std::string)argv[1]=="add") 53 | srv.request.marker.action = visualization_msgs::Marker::ADD; 54 | else if ((std::string)argv[1]=="del") 55 | srv.request.marker.action = visualization_msgs::Marker::DELETE; 56 | 57 | // Set the pose of the marker. This is a full 6DOF pose relative to the frame/time specified in the header 58 | srv.request.marker.pose.position.x = atof(argv[5]); 59 | srv.request.marker.pose.position.y = atof(argv[6]); 60 | srv.request.marker.pose.position.z = atof(argv[7]); 61 | 62 | tf::Quaternion quat; 63 | quat.setRPY( atof(argv[8]),atof(argv[9]),atof(argv[10])); 64 | quat.normalize(); 65 | 66 | srv.request.marker.pose.orientation.x = quat.getX(); 67 | srv.request.marker.pose.orientation.y = quat.getY(); 68 | srv.request.marker.pose.orientation.z = quat.getZ(); 69 | srv.request.marker.pose.orientation.w = quat.getW(); 70 | 71 | // Set the scale of the marker -- 1x1x1 here means 1m on a side 72 | srv.request.marker.scale.x = 1.0; 73 | srv.request.marker.scale.y = 1.0; 74 | srv.request.marker.scale.z = 1.0; 75 | 76 | // Set the color -- be sure to set alpha to something non-zero! 77 | srv.request.marker.color.r = 1.0f; 78 | srv.request.marker.color.g = 1.0f; 79 | srv.request.marker.color.b = 1.0f; 80 | srv.request.marker.color.a = 1.0; 81 | 82 | if (client.call(srv)) 83 | { 84 | ROS_INFO("Operation exited with %d", srv.response.success); 85 | std::cout< 2 | 3 | uwsim 4 | 1.4.2 5 | 6 | UWSim is an UnderWater SIMulator for marine robotics research and development. UWSim visualizes an underwater virtual scenario that can be configured using standard modeling software. Controllable underwater vehicles, surface vessels and robotic manipulators, as well as simulated sensors, can be added to the scene and accessed externally through ROS interfaces. This allows to easily integrate the visualization tool with existing control architectures. 7 | 8 | 9 | Mario Prats 10 | Javier Perez 11 | Mario Prats 12 | Javier Perez 13 | Diego Centelles 14 | 15 | GPL 16 | http://www.irs.uji.es/uwsim/ 17 | 18 | catkin 19 | 20 | boost 21 | geometry_msgs 22 | image_transport 23 | interactive_markers 24 | kdl_parser 25 | libopenscenegraph 26 | libxml++-2.6 27 | nav_msgs 28 | opengl 29 | osg_interactive_markers 30 | osg_markers 31 | osg_utils 32 | pcl_ros 33 | pluginlib 34 | resource_retriever 35 | robot_state_publisher 36 | roscpp 37 | sensor_msgs 38 | tf 39 | urdf 40 | underwater_sensor_msgs 41 | uwsim_bullet 42 | uwsim_osgbullet 43 | uwsim_osgocean 44 | uwsim_osgworks 45 | xacro 46 | dccomms_ros 47 | 48 | libxml++-2.6 49 | muparser 50 | libopenscenegraph 51 | libfftw3 52 | geographiclib-tools 53 | geographiclib 54 | 55 | boost 56 | geometry_msgs 57 | image_transport 58 | interactive_markers 59 | kdl_parser 60 | libopenscenegraph 61 | libxml++-2.6 62 | nav_msgs 63 | opengl 64 | osg_interactive_markers 65 | osg_markers 66 | osg_utils 67 | pcl_ros 68 | pluginlib 69 | resource_retriever 70 | robot_state_publisher 71 | roscpp 72 | sensor_msgs 73 | tf 74 | urdf 75 | underwater_sensor_msgs 76 | uwsim_bullet 77 | uwsim_osgbullet 78 | uwsim_osgocean 79 | uwsim_osgworks 80 | xacro 81 | dccomms_ros 82 | 83 | libxml++-2.6 84 | muparser 85 | libopenscenegraph 86 | libfftw3 87 | geographiclib-tools 88 | geographiclib 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | -------------------------------------------------------------------------------- /uwsim/saveDataFile.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | tar czvf UWSim-data-mine.tgz data/robot data/objects data/textures data/terrain 3 | 4 | -------------------------------------------------------------------------------- /uwsim/simdev_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | Example plugin 7 | 8 | 9 | 10 | 12 | 13 | Force Sensor 14 | 15 | 16 | 17 | 19 | 20 | Drege Tool 21 | 22 | 23 | 24 | 26 | 27 | Creates a comms device for dccomms_ros simulator 28 | 29 | 30 | 31 | 33 | 34 | Creates a comms device for dccomms_ros simulator 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /uwsim/src/AcousticCommsChannel.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | namespace uwsim { 7 | 8 | AcousticCommsChannel::AcousticCommsChannel(AcousticCommsChannelConfig cfg) { 9 | config = cfg; 10 | _AddToNetSim(); 11 | } 12 | 13 | bool AcousticCommsChannel::_AddToNetSim() { 14 | dccomms_ros_msgs::AddAcousticChannelRequest srv; 15 | srv.id = config.id; 16 | srv.noiseLvl = config.noise; 17 | srv.salinity = config.salinity; 18 | srv.temperature = config.temperature; 19 | srv.bandwidth = config.bandwidth; 20 | srv.logLevel = config.logLevel; 21 | 22 | auto netsim = NetSim::GetSim(); 23 | netsim->AddAcousticChannel(srv); 24 | } 25 | } 26 | -------------------------------------------------------------------------------- /uwsim/src/CustomCommsChannel.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | namespace uwsim { 7 | 8 | CustomCommsChannel::CustomCommsChannel(CustomCommsChannelConfig cfg) { 9 | config = cfg; 10 | _AddToNetSim(); 11 | } 12 | 13 | bool CustomCommsChannel::_AddToNetSim() { 14 | dccomms_ros_msgs::AddCustomChannelRequest srv; 15 | srv.id = config.id; 16 | srv.minPrTime = config.minPropTime; 17 | srv.prTimeIncPerMeter = config.propTimeIncPerMeter; 18 | srv.logLevel = config.logLevel; 19 | 20 | auto netsim = NetSim::GetSim(); 21 | netsim->AddCustomChannel(srv); 22 | } 23 | } 24 | -------------------------------------------------------------------------------- /uwsim/src/DVLSensor.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013 University of Jaume-I. 3 | * All rights reserved. This program and the accompanying materials 4 | * are made available under the terms of the GNU Public License v3.0 5 | * which accompanies this distribution, and is available at 6 | * http://www.gnu.org/licenses/gpl.html 7 | * 8 | * Contributors: 9 | * Mario Prats 10 | * Javier Perez 11 | */ 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #include 19 | 20 | osg::Vec3d DVLSensor::getMeasurement() 21 | { 22 | //Should get world coords and then transform to the localizedWorld 23 | std::shared_ptr rMs = getWorldCoords(node_); 24 | osg::Matrixd lMs = *rMs * osg::Matrixd::inverse(rMl_); 25 | osg::Vec3d x = lMs.getTrans(); 26 | 27 | ros::Time tnow = ros::Time::now(); 28 | ros::Duration ellapsed_time = tnow - tprevious_; 29 | tprevious_ = tnow; 30 | 31 | //TODO: should compute the velocity at a higher rate inside an update callback? 32 | osg::Vec3d v = (x - xprevious_) / ellapsed_time.toSec(); 33 | xprevious_ = x; 34 | 35 | //v is given wrt to the localized world. Need to rotate to the dvl frame 36 | osg::Matrixd sRl = osg::Matrixd::inverse(lMs); 37 | sRl.setTrans(0, 0, 0); 38 | osg::Vec4d vh(v.x(), v.y(), v.z(), 1); 39 | osg::Vec4d vdvl = vh * sRl; 40 | 41 | //Now add some gaussian noise 42 | static boost::normal_distribution<> normal(0, std_); 43 | static boost::variate_generator > var_nor(rng_, normal); 44 | vdvl[0] += var_nor(); 45 | vdvl[1] += var_nor(); 46 | vdvl[2] += var_nor(); 47 | 48 | return osg::Vec3d(vdvl.x(), vdvl.y(), vdvl.z()); 49 | } 50 | 51 | int DVLSensor::getTFTransform(tf::Pose & pose, std::string & parent){ 52 | parent=parentLinkName; 53 | pose.setOrigin(tf::Vector3(parent_->asTransform()->asPositionAttitudeTransform()->getPosition().x(), 54 | parent_->asTransform()->asPositionAttitudeTransform()->getPosition().y(), 55 | parent_->asTransform()->asPositionAttitudeTransform()->getPosition().z())); 56 | pose.setRotation( tf::Quaternion(parent_->asTransform()->asPositionAttitudeTransform()->getAttitude().x(), 57 | parent_->asTransform()->asPositionAttitudeTransform()->getAttitude().y(), 58 | parent_->asTransform()->asPositionAttitudeTransform()->getAttitude().z(), 59 | parent_->asTransform()->asPositionAttitudeTransform()->getAttitude().w())); 60 | return 1; 61 | 62 | } 63 | 64 | -------------------------------------------------------------------------------- /uwsim/src/GPSSensor.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013 University of Jaume-I. 3 | * All rights reserved. This program and the accompanying materials 4 | * are made available under the terms of the GNU Public License v3.0 5 | * which accompanies this distribution, and is available at 6 | * http://www.gnu.org/licenses/gpl.html 7 | * 8 | * Contributors: 9 | * Mario Prats 10 | * Javier Perez 11 | */ 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | 19 | osg::Vec3d GPSSensor::getMeasurement() 20 | { 21 | //Should get world coords and then transform to the localizedWorld 22 | std::shared_ptr rMs = getWorldCoords(node_); 23 | osg::Matrixd lMs = *rMs * osg::Matrixd::inverse(rMl_); 24 | 25 | //Now add some gaussian noise 26 | static boost::normal_distribution<> normal(0, std_); 27 | static boost::variate_generator > var_nor(rng_, normal); 28 | 29 | return lMs.getTrans() + osg::Vec3d(var_nor(), var_nor(), var_nor()); 30 | } 31 | 32 | double GPSSensor::depthBelowWater() 33 | { 34 | std::shared_ptr rMs = getWorldCoords(node_); 35 | return -(rMs->getTrans().z() - oscene_->getOceanScene()->getOceanSurfaceHeight()); 36 | } 37 | 38 | int GPSSensor::getTFTransform(tf::Pose & pose, std::string & parent){ 39 | parent=parentLinkName; 40 | pose.setOrigin(tf::Vector3(parent_->asTransform()->asPositionAttitudeTransform()->getPosition().x(), 41 | parent_->asTransform()->asPositionAttitudeTransform()->getPosition().y(), 42 | parent_->asTransform()->asPositionAttitudeTransform()->getPosition().z())); 43 | pose.setRotation( tf::Quaternion(parent_->asTransform()->asPositionAttitudeTransform()->getAttitude().x(), 44 | parent_->asTransform()->asPositionAttitudeTransform()->getAttitude().y(), 45 | parent_->asTransform()->asPositionAttitudeTransform()->getAttitude().z(), 46 | parent_->asTransform()->asPositionAttitudeTransform()->getAttitude().w())); 47 | return 1; 48 | 49 | } 50 | -------------------------------------------------------------------------------- /uwsim/src/HUDCamera.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013 University of Jaume-I. 3 | * All rights reserved. This program and the accompanying materials 4 | * are made available under the terms of the GNU Public License v3.0 5 | * which accompanies this distribution, and is available at 6 | * http://www.gnu.org/licenses/gpl.html 7 | * 8 | * Contributors: 9 | * Mario Prats 10 | * Javier Perez 11 | */ 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | HUDCamera::HUDCamera(unsigned int width, unsigned int height, unsigned int posx, unsigned int posy, double scale, 18 | int blackWhite) 19 | { 20 | this->width = width; 21 | this->height = height; 22 | this->posx = posx; 23 | this->posy = posy; 24 | this->scale = scale; 25 | osg_image = new osg::Image(); 26 | if (blackWhite) 27 | { 28 | osg_image->allocateImage(width, height, 1, GL_LUMINANCE, GL_UNSIGNED_BYTE); 29 | memset(osg_image->data(), 0, width * height * 1 * sizeof(unsigned char)); 30 | } 31 | else 32 | { 33 | osg_image->allocateImage(width, height, 1, GL_RGB, GL_UNSIGNED_BYTE); 34 | memset(osg_image->data(), 0, width * height * 3 * sizeof(unsigned char)); 35 | } 36 | 37 | ready_ = false; 38 | //OSG_INFO << "HUDCamera::HUDCamera Constructor finished " << info_topic << std::endl; 39 | } 40 | 41 | osg::ref_ptr HUDCamera::getWidgetWindow() 42 | { 43 | osg::ref_ptr < osgWidget::Box > box = new osgWidget::Box("HUDCameraBox", osgWidget::Box::HORIZONTAL, true); 44 | widget = new osgWidget::Widget("HUDCameraWidget", width, height); 45 | widget->setUpdateCallback(new widgetUpdateCallback(osg_image)); 46 | //widget->setImage(osg_image,true,false); 47 | box->addWidget(widget); 48 | box->setX(posx); 49 | box->setY(posy); 50 | box->setScale(scale); 51 | box->getBackground()->setColor(1.0f, 0.0f, 0.0f, 0.8f); 52 | box->attachMoveCallback(); 53 | box->attachScaleCallback(); 54 | return box; 55 | } 56 | 57 | HUDCamera::~HUDCamera() 58 | { 59 | } 60 | 61 | -------------------------------------------------------------------------------- /uwsim/src/InertialMeasurementUnit.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013 University of Jaume-I. 3 | * All rights reserved. This program and the accompanying materials 4 | * are made available under the terms of the GNU Public License v3.0 5 | * which accompanies this distribution, and is available at 6 | * http://www.gnu.org/licenses/gpl.html 7 | * 8 | * Contributors: 9 | * Mario Prats 10 | * Javier Perez 11 | */ 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | osg::Quat InertialMeasurementUnit::getMeasurement() 18 | { 19 | //Should get world coords and then transform to the localizedWorld 20 | std::shared_ptr rMi = getWorldCoords(imu_node_); 21 | osg::Matrixd lMi = *rMi * osg::Matrixd::inverse(rMl_); 22 | 23 | //Now add some gaussian noise 24 | static boost::normal_distribution<> normal(0, std_); 25 | static boost::variate_generator > var_nor(rng_, normal); 26 | 27 | return lMi.getRotate() 28 | * osg::Quat(var_nor(), osg::Vec3d(1, 0, 0), var_nor(), osg::Vec3d(0, 1, 0), var_nor(), osg::Vec3d(0, 0, 1)); 29 | } 30 | 31 | int InertialMeasurementUnit::getTFTransform(tf::Pose & pose, std::string & parent){ 32 | parent=parentLinkName; 33 | pose.setOrigin(tf::Vector3(parent_->asTransform()->asPositionAttitudeTransform()->getPosition().x(), 34 | parent_->asTransform()->asPositionAttitudeTransform()->getPosition().y(), 35 | parent_->asTransform()->asPositionAttitudeTransform()->getPosition().z())); 36 | pose.setRotation( tf::Quaternion(parent_->asTransform()->asPositionAttitudeTransform()->getAttitude().x(), 37 | parent_->asTransform()->asPositionAttitudeTransform()->getAttitude().y(), 38 | parent_->asTransform()->asPositionAttitudeTransform()->getAttitude().z(), 39 | parent_->asTransform()->asPositionAttitudeTransform()->getAttitude().w())); 40 | return 1; 41 | 42 | } 43 | -------------------------------------------------------------------------------- /uwsim/src/NED.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace uwsim { 4 | 5 | GeographicLib::LocalCartesian NED::localCartesian; 6 | GeographicLib::Geocentric NED::earth; 7 | std::mutex NED::localCartesian_mutex; 8 | 9 | void NED::SetOrigin(double lat, double lon, double alt) { 10 | localCartesian_mutex.lock(); 11 | earth = GeographicLib::Geocentric(GeographicLib::Constants::WGS84_a(), 12 | GeographicLib::Constants::WGS84_f()); 13 | localCartesian = GeographicLib::LocalCartesian(lat, lon, alt, earth); 14 | localCartesian_mutex.unlock(); 15 | } 16 | 17 | void NED::GetNED(double lat, double lon, double alt, double &x, double &y, 18 | double &z) { 19 | localCartesian_mutex.lock(); 20 | //https://github.com/mavlink/mavros/issues/216 21 | double enux, enuy, enuz; 22 | localCartesian.Forward(lat, lon, alt, enux, enuy, enuz); 23 | x = enuy; 24 | y = enux; 25 | z = -enuz; 26 | localCartesian_mutex.unlock(); 27 | } 28 | } 29 | -------------------------------------------------------------------------------- /uwsim/src/NetSim.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | namespace uwsim { 5 | 6 | NetSimTracingPtr NetSim::_script; 7 | std::shared_ptr NetSim::_loader; 8 | 9 | ns3::Ptr NetSim::GetSim() { 10 | static ns3::Ptr ptr = 0; 11 | if (ptr == 0) { 12 | ptr = ns3::CreateObject(); 13 | auto pb = dccomms::CreateObject( 14 | dccomms::DataLinkFrame::crc16); 15 | ptr->SetDefaultPacketBuilder(pb); 16 | } 17 | return ptr; 18 | } 19 | 20 | void NetSim::LoadTracingScript(const std::string &className, 21 | const std::string &libPath) { 22 | _loader = std::shared_ptr(new class_loader::ClassLoader(libPath)); 23 | std::vector classes = 24 | _loader->getAvailableClasses(); 25 | for (unsigned int c = 0; c < classes.size(); ++c) { 26 | if (classes[c] == className) { 27 | NetSimTracing *tr = 28 | _loader->createUnmanagedInstance(classes[c]); 29 | _script = std::shared_ptr(tr); 30 | _script->Configure(); 31 | break; 32 | } 33 | } 34 | } 35 | 36 | void NetSim::LoadDefaultTracingScript() { 37 | _script = NetSimTracingPtr(new NetSimTracing()); 38 | _script->Configure(); 39 | } 40 | 41 | NetSimTracingPtr NetSim::GetScript() { return _script; } 42 | 43 | NetSimTracing::NetSimTracing() { 44 | SetLogFormatter(std::make_shared("%v")); 45 | LogToConsole(false); 46 | } 47 | 48 | void NetSimTracing::Configure() { 49 | SetLogName("uwsim_netsim"); 50 | 51 | /* 52 | In order to use an object created outside the lambda function 53 | we have to make it global or capture the object. However, capturing 54 | does not work if the lambda function is going to be converted to a 55 | function pointer. So we make all this objects static. 56 | From the std 57 | (https://stackoverflow.com/questions/28746744/passing-lambda-as-function-pointer): 58 | "The closure type for a lambda-expression with no lambda-capture 59 | has a public non-virtual non-explicit const conversion function to 60 | pointer to function having the same parameter and return types as the 61 | closure type’s function call operator. The value returned by this 62 | conversion function shall be the address of a function that, when invoked, 63 | has the same effect as invoking the closure type’s function call operator." 64 | */ 65 | 66 | static NetSimTracing *tracing = this; 67 | ROSCommsDevice::PacketTransmittingCallback txcb = 68 | [](std::string path, ROSCommsDevicePtr dev, ns3ConstPacketPtr pkt) { 69 | // tracing->Info("{}: (ID: {} ; MAC: {}) Transmitting packet", path, 70 | // dev->GetDccommsId(), dev->GetMac()); 71 | }; 72 | 73 | ROSCommsDevice::PacketReceivedCallback rxcb = []( 74 | std::string path, ROSCommsDevicePtr dev, ns3ConstPacketPtr pkt) { 75 | // NetsimHeader header; 76 | // pkt->PeekHeader(header); 77 | // tracing->Info("{}: (ID: {} ; MAC: {}) Packet received from {} ({} bytes)", 78 | // path, dev->GetDccommsId(), dev->GetMac(), header.GetSrc(), 79 | // header.GetPacketSize()); 80 | }; 81 | 82 | ns3::Config::Connect("/ROSDeviceList/*/PacketTransmitting", 83 | ns3::MakeCallback(txcb)); 84 | 85 | ns3::Config::Connect("/ROSDeviceList/*/PacketReceived", 86 | ns3::MakeCallback(rxcb)); 87 | } 88 | 89 | void NetSimTracing::Run() { DoRun(); } 90 | void NetSimTracing::DoRun() {} 91 | } 92 | -------------------------------------------------------------------------------- /uwsim/src/ObjectPicker.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013 University of Jaume-I. 3 | * All rights reserved. This program and the accompanying materials 4 | * are made available under the terms of the GNU Public License v3.0 5 | * which accompanies this distribution, and is available at 6 | * http://www.gnu.org/licenses/gpl.html 7 | * 8 | * Contributors: 9 | * Mario Prats 10 | * Javier Perez 11 | */ 12 | 13 | #include 14 | #include 15 | 16 | ObjectPicker::ObjectPicker() : 17 | VirtualRangeSensor() 18 | { 19 | } 20 | 21 | ObjectPicker::ObjectPicker(std::string name, osg::Node *root, osg::Node *trackNode, double range, bool visible, 22 | std::shared_ptr urdf,unsigned int mask) 23 | { 24 | init(name, root, trackNode, range, visible, urdf, mask); 25 | } 26 | 27 | void ObjectPicker::init(std::string name, osg::Node *root, osg::Node *trackNode, double range, bool visible, 28 | std::shared_ptr urdf,unsigned int mask) 29 | { 30 | this->name = name; 31 | this->root = root; 32 | 33 | this->trackNode = trackNode; 34 | //Add a switchable frame geometry on the sensor frame 35 | osg::ref_ptr < osg::Node > axis = UWSimGeometry::createSwitchableFrame(); 36 | //Add label to switchable frame 37 | axis->asGroup()->addChild(UWSimGeometry::createLabel(name)); 38 | this->trackNode->asGroup()->addChild(axis); 39 | 40 | this->range = range; 41 | this->visible = visible; 42 | 43 | //make this virtual ray track the node 44 | node_tracker = new ObjectPickerUpdateCallback(trackNode, range, visible, root, urdf); 45 | trackNode->setUpdateCallback((ObjectPickerUpdateCallback*)(node_tracker.get())); 46 | trackNode->asGroup()->addChild(node_tracker->geode); 47 | 48 | if(node_tracker->geode) 49 | node_tracker->geode->setNodeMask(mask); 50 | } 51 | 52 | /* 53 | bool Hand::closeHand(){ 54 | 55 | for(osg::NodePath::iterator i=node_tracker->impact.begin();i!=node_tracker->impact.end();++i){ 56 | osg::ref_ptr data = dynamic_cast (i[0]->getUserData()); 57 | if(data!=NULL && data->catchable){ //Search for "catchable" objects in nodepath 58 | //Get coordinates to change them when changing position in graph 59 | osg::Matrixd *originalpos=getWorldCoords(i[0]); 60 | osg::Matrixd *hand = getWorldCoords(trackNode); 61 | hand->invert(*hand); 62 | 63 | //Turn object to Kinematic state, add callback to move collision shape 64 | if(data->rb) 65 | data->rb->setCollisionFlags( data->rb->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT ); 66 | catched_callback=new BulletPhysics::MirrorTransformCallback(data->rb); 67 | i[0]->setUpdateCallback(catched_callback); 68 | 69 | //ADD node in hand, remove object from original position. 70 | trackNode->asTransform()->addChild(i[0]); 71 | i[0]->getParent(0)->asGroup()->removeChild(i[0]); 72 | osg::Matrixd matrix=*originalpos * *hand; 73 | i[0]->asTransform()->asMatrixTransform()->setMatrix(matrix); 74 | freeHand=0; 75 | catched=i[0]; 76 | return 1; 77 | } 78 | 79 | } 80 | return 0; 81 | } 82 | 83 | 84 | void Hand::openHand(){ 85 | osg::ref_ptr data = dynamic_cast (catched->getUserData()); 86 | 87 | osg::Matrixd *originalpos=getWorldCoords(catched); 88 | osg::Matrixd *world = getWorldCoords(root); 89 | world->invert(*world); 90 | 91 | root->asGroup()->addChild(catched); 92 | trackNode->asGroup()->removeChild(catched); 93 | osg::Matrixd matrix=*originalpos * *world; 94 | catched->asTransform()->asMatrixTransform()->setMatrix(matrix); 95 | 96 | //Get collision shape to current position, destroy callback and turn object to Dynamic state again. 97 | catched->removeUpdateCallback(catched_callback); 98 | catched_callback=NULL; 99 | data->rb->setWorldTransform( osgbCollision::asBtTransform(matrix) ); 100 | if(data->rb) 101 | data->rb->setCollisionFlags( data->rb->getCollisionFlags() & ~btCollisionObject::CF_KINEMATIC_OBJECT ); 102 | freeHand=1; 103 | catched=NULL; 104 | 105 | } 106 | */ 107 | 108 | -------------------------------------------------------------------------------- /uwsim/src/PressureSensor.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013 University of Jaume-I. 3 | * All rights reserved. This program and the accompanying materials 4 | * are made available under the terms of the GNU Public License v3.0 5 | * which accompanies this distribution, and is available at 6 | * http://www.gnu.org/licenses/gpl.html 7 | * 8 | * Contributors: 9 | * Mario Prats 10 | * Javier Perez 11 | */ 12 | 13 | #include 14 | #include 15 | 16 | #include 17 | 18 | double PressureSensor::getMeasurement() 19 | { 20 | //Should get world coords and then transform to the localizedWorld 21 | std::shared_ptr rMs = getWorldCoords(node_); 22 | osg::Matrixd lMs = *rMs * osg::Matrixd::inverse(rMl_); 23 | 24 | //Now add some gaussian noise 25 | static boost::normal_distribution<> normal(0, std_); 26 | static boost::variate_generator > var_nor(rng_, normal); 27 | 28 | return lMs.getTrans().z() + var_nor(); 29 | } 30 | 31 | int PressureSensor::getTFTransform(tf::Pose & pose, std::string & parent){ 32 | parent=parentLinkName; 33 | pose.setOrigin(tf::Vector3(parent_->asTransform()->asPositionAttitudeTransform()->getPosition().x(), 34 | parent_->asTransform()->asPositionAttitudeTransform()->getPosition().y(), 35 | parent_->asTransform()->asPositionAttitudeTransform()->getPosition().z())); 36 | pose.setRotation( tf::Quaternion(parent_->asTransform()->asPositionAttitudeTransform()->getAttitude().x(), 37 | parent_->asTransform()->asPositionAttitudeTransform()->getAttitude().y(), 38 | parent_->asTransform()->asPositionAttitudeTransform()->getAttitude().z(), 39 | parent_->asTransform()->asPositionAttitudeTransform()->getAttitude().w())); 40 | return 1; 41 | 42 | } 43 | -------------------------------------------------------------------------------- /uwsim/src/SimDev_Echo.cpp: -------------------------------------------------------------------------------- 1 | //"Echo" example, SimulatedDevice_Echo.cpp 2 | 3 | #include 4 | #include 5 | 6 | SimDev_Echo::SimDev_Echo(SimDev_Echo_Config * cfg) : 7 | SimulatedDevice(cfg) 8 | { 9 | this->info = cfg->info; 10 | } 11 | 12 | SimulatedDeviceConfig::Ptr SimDev_Echo_Factory::processConfig(const xmlpp::Node* node, ConfigFile * config) 13 | { 14 | SimDev_Echo_Config * cfg = new SimDev_Echo_Config(getType()); 15 | xmlpp::Node::NodeList list = node->get_children(); 16 | for (xmlpp::Node::NodeList::iterator iter = list.begin(); iter != list.end(); ++iter) 17 | { 18 | const xmlpp::Node* child = dynamic_cast(*iter); 19 | if (child->get_name() == "info") 20 | config->extractStringChar(child, cfg->info); 21 | } 22 | return SimulatedDeviceConfig::Ptr(cfg); 23 | } 24 | 25 | bool SimDev_Echo_Factory::applyConfig(SimulatedIAUV * auv, Vehicle &vehicleChars, SceneBuilder *sceneBuilder, 26 | size_t iteration) 27 | { 28 | if (iteration > 0) 29 | return true; 30 | for (size_t i = 0; i < vehicleChars.simulated_devices.size(); ++i) 31 | if (vehicleChars.simulated_devices[i]->getType() == this->getType()) 32 | { 33 | SimDev_Echo_Config * cfg = dynamic_cast(vehicleChars.simulated_devices[i].get()); 34 | if (cfg && cfg->info.length() > 0) 35 | { 36 | auv->devices->all.push_back(SimDev_Echo::Ptr(new SimDev_Echo(cfg))); 37 | } 38 | else 39 | OSG_FATAL << "SimDev_Echo device '" << vehicleChars.simulated_devices[i]->name << "' inside robot '" 40 | << vehicleChars.name << "' has empty info, discarding..." << std::endl; 41 | } 42 | return true; 43 | } 44 | 45 | std::vector > SimDev_Echo_Factory::getInterface( 46 | ROSInterfaceInfo & rosInterface, std::vector > & iauvFile) 47 | { 48 | std::vector < std::shared_ptr > ifaces; 49 | for (size_t i = 0; i < iauvFile.size(); ++i) 50 | for (size_t d = 0; d < iauvFile[i]->devices->all.size(); ++d) 51 | if (iauvFile[i]->devices->all[d]->getType() == this->getType() 52 | && iauvFile[i]->devices->all[d]->name == rosInterface.targetName) 53 | { 54 | ifaces.push_back( 55 | std::shared_ptr < ROSInterface 56 | > (new SimDev_Echo_ROSPublisher(dynamic_cast(iauvFile[i]->devices->all[d].get()), 57 | rosInterface.topic, rosInterface.rate))); 58 | //rosInterface.values are for new and non-standard xml configurations, but it looks like currently existing rosInterface fields are enough... 59 | //below is just an example how to get values, alternatively you can use rosInterface.values["name"] 60 | //for(std::map::iterator it = rosInterface.values.begin() ;it != rosInterface.values.end();++it) 61 | // ROS_INFO("rosInterface.values[%s]='%s'", it->first.c_str(), it->second.c_str()); 62 | } 63 | if (ifaces.size() == 0) 64 | ROS_WARN("Returning empty ROS interface for device %s...", rosInterface.targetName.c_str()); 65 | return ifaces; 66 | } 67 | 68 | void SimDev_Echo_ROSPublisher::createPublisher(ros::NodeHandle &nh) 69 | { 70 | ROS_INFO("SimDev_Echo_ROSPublisher on topic %s", topic.c_str()); 71 | pub_ = nh.advertise < std_msgs::String > (topic, 1); 72 | } 73 | 74 | void SimDev_Echo_ROSPublisher::publish() 75 | { 76 | std_msgs::String msg; 77 | if (dev != NULL) 78 | msg.data = dev->info; 79 | else 80 | msg.data = "dev==NULL"; 81 | pub_.publish(msg); 82 | } 83 | 84 | #if ROS_VERSION_MINIMUM(1, 9, 0) 85 | // new pluginlib API in Groovy and Hydro 86 | PLUGINLIB_EXPORT_CLASS(SimDev_Echo_Factory, uwsim::SimulatedDeviceFactory) 87 | #else 88 | PLUGINLIB_REGISTER_CLASS(SimDev_Echo_Factory, SimDev_Echo_Factory, uwsim::SimulatedDeviceFactory) 89 | #endif 90 | 91 | -------------------------------------------------------------------------------- /uwsim/src/SkyDome.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This source file is part of the osgOcean library 3 | * 4 | * Copyright (C) 2009 Kim Bale 5 | * Copyright (C) 2009 The University of Hull, UK 6 | * 7 | * This program is free software; you can redistribute it and/or modify it under 8 | * the terms of the GNU Lesser General Public License as published by the Free Software 9 | * Foundation; either version 3 of the License, or (at your option) any later 10 | * version. 11 | 12 | * This program is distributed in the hope that it will be useful, but WITHOUT 13 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 14 | * FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more details. 15 | * http://www.gnu.org/copyleft/lesser.txt. 16 | */ 17 | 18 | #include 19 | #include 20 | 21 | SkyDome::SkyDome(void) 22 | { 23 | 24 | } 25 | 26 | SkyDome::SkyDome(const SkyDome& copy, const osg::CopyOp& copyop) : 27 | SphereSegment(copy, copyop) 28 | { 29 | 30 | } 31 | 32 | SkyDome::SkyDome(float radius, unsigned int longSteps, unsigned int latSteps, osg::TextureCubeMap* cubemap) 33 | { 34 | compute(radius, longSteps, latSteps, 90.f, 180.f, 0.f, 360.f); 35 | setupStateSet(cubemap); 36 | } 37 | 38 | SkyDome::~SkyDome(void) 39 | { 40 | } 41 | 42 | void SkyDome::create(float radius, unsigned int latSteps, unsigned int longSteps, osg::TextureCubeMap* cubemap) 43 | { 44 | compute(radius, longSteps, latSteps, 90.f, 180.f, 0.f, 360.f); 45 | setupStateSet(cubemap); 46 | } 47 | 48 | void SkyDome::setupStateSet(osg::TextureCubeMap* cubemap) 49 | { 50 | osg::StateSet* ss = new osg::StateSet; 51 | 52 | ss->setMode(GL_LIGHTING, osg::StateAttribute::OFF); 53 | ss->setTextureAttributeAndModes(0, cubemap, osg::StateAttribute::ON); 54 | ss->setAttributeAndModes(createShader().get(), osg::StateAttribute::ON); 55 | ss->addUniform(new osg::Uniform("uEnvironmentMap", 0)); 56 | 57 | setStateSet(ss); 58 | } 59 | 60 | osg::ref_ptr SkyDome::createShader(void) 61 | { 62 | osg::ref_ptr < osg::Program > program = new osg::Program; 63 | 64 | // Do not use shaders if they were globally disabled. 65 | if (osgOcean::ShaderManager::instance().areShadersEnabled()) 66 | { 67 | char vertexSource[] = "varying vec3 vTexCoord;\n" 68 | "\n" 69 | "void main(void)\n" 70 | "{\n" 71 | " gl_Position = ftransform();\n" 72 | " vTexCoord = gl_Vertex.xyz;\n" 73 | "}\n"; 74 | 75 | char fragmentSource[] = "uniform samplerCube uEnvironmentMap;\n" 76 | "varying vec3 vTexCoord;\n" 77 | "\n" 78 | "void main(void)\n" 79 | "{\n" 80 | " vec3 texcoord = vec3(vTexCoord.x, vTexCoord.y, -vTexCoord.z);\n" 81 | " gl_FragData[0] = textureCube( uEnvironmentMap, texcoord.xzy );\n" 82 | " gl_FragData[0].a = 0.0;\n" 83 | " gl_FragData[1] = vec4(0.0);\n" 84 | "}\n"; 85 | 86 | program->setName("sky_dome_shader"); 87 | program->addShader(new osg::Shader(osg::Shader::VERTEX, vertexSource)); 88 | program->addShader(new osg::Shader(osg::Shader::FRAGMENT, fragmentSource)); 89 | } 90 | 91 | return program; 92 | } 93 | -------------------------------------------------------------------------------- /uwsim/src/SphereSegment.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This source file is part of the osgOcean library 3 | * 4 | * Copyright (C) 2009 Kim Bale 5 | * Copyright (C) 2009 The University of Hull, UK 6 | * 7 | * This program is free software; you can redistribute it and/or modify it under 8 | * the terms of the GNU Lesser General Public License as published by the Free Software 9 | * Foundation; either version 3 of the License, or (at your option) any later 10 | * version. 11 | 12 | * This program is distributed in the hope that it will be useful, but WITHOUT 13 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 14 | * FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more details. 15 | * http://www.gnu.org/copyleft/lesser.txt. 16 | */ 17 | 18 | #include 19 | 20 | SphereSegment::SphereSegment(void) 21 | { 22 | } 23 | 24 | SphereSegment::SphereSegment(float radius, unsigned int longitudeSteps, unsigned int lattitudeSteps, float longStart, 25 | float longEnd, float latStart, float latEnd) 26 | { 27 | compute(radius, longitudeSteps, lattitudeSteps, longStart, longEnd, latStart, latEnd); 28 | } 29 | 30 | SphereSegment::SphereSegment(const SphereSegment& copy, const osg::CopyOp& copyop) : 31 | osg::Geode(copy, copyop) 32 | { 33 | } 34 | 35 | SphereSegment::~SphereSegment(void) 36 | { 37 | } 38 | 39 | // 0 >= longStart/longEnd <= 180 40 | // 0 >= latStart/latEnd <= 360 41 | 42 | void SphereSegment::compute(float radius, unsigned int longitudeSteps, unsigned int lattitudeSteps, float longStart, 43 | float longEnd, float latStart, float latEnd) 44 | 45 | { 46 | removeDrawables(0, getNumDrawables()); 47 | 48 | osg::Vec3Array* vertices = new osg::Vec3Array(); 49 | osg::Vec2Array* texcoords = new osg::Vec2Array(); 50 | 51 | double x, y, z, t, p, sin_t, cos_t; 52 | 53 | double longInc = (longEnd - longStart) / (double)longitudeSteps; 54 | double latInc = (latEnd - latStart) / (double)lattitudeSteps; 55 | 56 | double theta = longStart, phi = latStart; 57 | 58 | float uScale = 1.f / longitudeSteps; 59 | float vScale = 1.f / lattitudeSteps; 60 | 61 | for (unsigned int i = 0; i <= longitudeSteps; ++i) 62 | { 63 | t = osg::DegreesToRadians(theta); 64 | sin_t = sin(t); 65 | cos_t = cos(t); 66 | 67 | for (unsigned int j = 0; j <= lattitudeSteps; ++j) 68 | { 69 | p = osg::DegreesToRadians(phi); 70 | 71 | x = radius * sin_t * cos(p); 72 | y = radius * sin_t * sin(p); 73 | z = radius * cos_t; 74 | 75 | vertices->push_back(osg::Vec3(x, y, z)); 76 | texcoords->push_back(osg::Vec2(j * vScale, i * uScale)); 77 | 78 | phi += latInc; 79 | } 80 | 81 | theta -= longInc; 82 | phi = latStart; 83 | } 84 | 85 | osg::ref_ptr < osg::Geometry > geom = new osg::Geometry(); 86 | 87 | for (unsigned int r = 0; r <= longitudeSteps - 1; r += 1) 88 | { 89 | osg::DrawElementsUInt* indices = new osg::DrawElementsUInt(osg::PrimitiveSet::TRIANGLE_STRIP, 0); 90 | 91 | for (unsigned int c = 0; c <= lattitudeSteps; c += 1) 92 | { 93 | indices->push_back(idx(r, c, lattitudeSteps + 1)); 94 | indices->push_back(idx(r + 1, c, lattitudeSteps + 1)); 95 | } 96 | 97 | geom->addPrimitiveSet(indices); 98 | } 99 | 100 | osg::Vec4Array* colors = new osg::Vec4Array(); 101 | colors->push_back(osg::Vec4(1.f, 1.f, 1.f, 1.f)); 102 | 103 | geom->setVertexArray(vertices); 104 | geom->setTexCoordArray(0, texcoords); 105 | geom->setColorArray(colors); 106 | geom->setColorBinding(osg::Geometry::BIND_OVERALL); 107 | 108 | addDrawable(geom.get()); 109 | } 110 | 111 | osg::Vec2 SphereSegment::sphereMap(osg::Vec3& vertex, float radius) 112 | { 113 | float u, v; 114 | 115 | float TWOPI = osg::PI * 2.f; 116 | 117 | v = acos(vertex.y() / radius) / osg::PI; 118 | 119 | if (vertex.z() >= 0.f) 120 | u = acos(vertex.x() / (radius * sin(osg::PI * v))) / TWOPI; 121 | else 122 | u = (osg::PI + acos(vertex.x() / (radius * sin(osg::PI * v)))) / TWOPI; 123 | 124 | return osg::Vec2(u, v); 125 | } 126 | -------------------------------------------------------------------------------- /uwsim/src/VirtualRangeSensor.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013 University of Jaume-I. 3 | * All rights reserved. This program and the accompanying materials 4 | * are made available under the terms of the GNU Public License v3.0 5 | * which accompanies this distribution, and is available at 6 | * http://www.gnu.org/licenses/gpl.html 7 | * 8 | * Contributors: 9 | * Mario Prats 10 | * Javier Perez 11 | */ 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | VirtualRangeSensor::VirtualRangeSensor() 19 | { 20 | } 21 | 22 | VirtualRangeSensor::VirtualRangeSensor(std::string name, std::string parentName, osg::Node *root, osg::Node *trackNode, double range, 23 | bool visible,unsigned int mask) 24 | { 25 | init(name,parentName, root, trackNode, range, visible,mask); 26 | } 27 | 28 | void VirtualRangeSensor::init(std::string name, std::string parentName, osg::Node *root, osg::Node *trackNode, double range, bool visible,unsigned int mask) 29 | { 30 | this->name = name; 31 | this->parentLinkName=parentName; 32 | this->root = root; 33 | 34 | this->trackNode = trackNode; 35 | //Add a switchable frame geometry on the sensor frame 36 | osg::ref_ptr < osg::Node > axis = UWSimGeometry::createSwitchableFrame(); 37 | //Add label to switchable frame 38 | axis->asGroup()->addChild(UWSimGeometry::createLabel(name)); 39 | this->trackNode->asGroup()->addChild(axis); 40 | 41 | this->range = range; 42 | this->visible = visible; 43 | 44 | //make this virtual ray track the node 45 | node_tracker = new IntersectorUpdateCallback(range, visible, root); 46 | trackNode->setUpdateCallback(node_tracker); 47 | trackNode->asGroup()->addChild(node_tracker->geode); 48 | 49 | if(node_tracker->geode) 50 | node_tracker->geode->setNodeMask(mask); 51 | } 52 | 53 | int VirtualRangeSensor::getTFTransform(tf::Pose & pose, std::string & parent){ 54 | parent=parentLinkName; 55 | pose.setOrigin(tf::Vector3(trackNode->asTransform()->asPositionAttitudeTransform()->getPosition().x(), 56 | trackNode->asTransform()->asPositionAttitudeTransform()->getPosition().y(), 57 | trackNode->asTransform()->asPositionAttitudeTransform()->getPosition().z())); 58 | pose.setRotation( tf::Quaternion(trackNode->asTransform()->asPositionAttitudeTransform()->getAttitude().x(), 59 | trackNode->asTransform()->asPositionAttitudeTransform()->getAttitude().y(), 60 | trackNode->asTransform()->asPositionAttitudeTransform()->getAttitude().z(), 61 | trackNode->asTransform()->asPositionAttitudeTransform()->getAttitude().w())); 62 | return 1; 63 | 64 | } 65 | 66 | -------------------------------------------------------------------------------- /uwsim/src/VirtualSLSProjector.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * VirtualStructuredLightProjector.cpp 3 | * 4 | * Created on: 06/02/2013 5 | * Author: Miquel Massot 6 | * Modified by: Javier Perez 7 | * 8 | */ 9 | 10 | #include 11 | 12 | class UpdateLMVPM : public osg::Uniform::Callback 13 | { 14 | public: 15 | UpdateLMVPM(osg::Camera* camera) : 16 | mCamera(camera) 17 | { 18 | } 19 | virtual void operator ()(osg::Uniform* u, osg::NodeVisitor*) 20 | { 21 | osg::Matrixd lmvpm = mCamera->getViewMatrix() * mCamera->getProjectionMatrix() * osg::Matrix::translate(1, 1, 1) 22 | * osg::Matrix::scale(0.5, 0.5, 0.5); 23 | 24 | u->set(lmvpm); 25 | } 26 | 27 | protected: 28 | osg::Camera* mCamera; 29 | }; 30 | 31 | VirtualSLSProjector::VirtualSLSProjector() 32 | { 33 | osg::ref_ptr < osg::Node > node = new osg::Node; 34 | osg::ref_ptr < osg::Node > root = new osg::Node; 35 | std::string name = "SLSprojector"; 36 | std::string image_name = "laser_texture.png"; 37 | double range = 0; 38 | double fov = 60.0; 39 | init(name,"base_link", root, node, image_name, range, fov, 0); 40 | } 41 | 42 | VirtualSLSProjector::VirtualSLSProjector(std::string name,std::string parentName, osg::Node *root, osg::Node *node, std::string image_name, 43 | double fov, bool laser) 44 | { 45 | double range = 0; 46 | init(name, parentName, root, node, image_name, range, fov, laser); 47 | } 48 | 49 | void VirtualSLSProjector::init(std::string name,std::string parentName, osg::Node *root, osg::Node *node, std::string image_name, double range, 50 | double fov, bool laser) 51 | { 52 | this->name = name; 53 | this->fov = fov; 54 | this->range = range; 55 | this->node = node; 56 | this->image_name = image_name; 57 | this->textureUnit = 3; // It shouldn't be fixed 58 | 59 | //Create projected texture 60 | osg::Texture2D* texture = new osg::Texture2D(); 61 | osg::Image* texture_to_project = osgDB::readImageFile(this->image_name); 62 | assert(texture_to_project); 63 | texture->setImage(texture_to_project); 64 | texture->setWrap(osg::Texture::WRAP_S, osg::Texture::CLAMP_TO_BORDER); // It makes texture not to repeat 65 | texture->setWrap(osg::Texture::WRAP_T, osg::Texture::CLAMP_TO_BORDER); // F.M.I: http://lucera-project.blogspot.com.es/2010/06/opengl-wrap.html 66 | texture->setWrap(osg::Texture::WRAP_R, osg::Texture::CLAMP_TO_BORDER); 67 | texture->setBorderColor(osg::Vec4d(0.0, 0.0, 0.0, 0.0)); 68 | root->getOrCreateStateSet()->setTextureAttributeAndModes(4, texture, osg::StateAttribute::ON); 69 | 70 | //Shadow camera 71 | camera = VirtualCamera(root->asGroup(), name,parentName, node, texture_to_project->s(), texture_to_project->t(), fov, 72 | texture_to_project->s() / (float)texture_to_project->t()); 73 | 74 | //Create depth texture for shadow mapping test 75 | dbgDepthTexture = new osg::Texture2D; 76 | dbgDepthTexture->setTextureSize(texture_to_project->s(), texture_to_project->t()); //CHECK: Maybe we can use a smaller texture? 77 | dbgDepthTexture->setInternalFormat(GL_DEPTH_COMPONENT); 78 | dbgDepthTexture->setFilter(osg::Texture::MIN_FILTER, osg::Texture::LINEAR); 79 | dbgDepthTexture->setFilter(osg::Texture::MAG_FILTER, osg::Texture::LINEAR); 80 | root->getOrCreateStateSet()->setTextureAttributeAndModes(3, dbgDepthTexture, osg::StateAttribute::ON); 81 | camera.textureCamera->attach(osg::Camera::DEPTH_BUFFER, dbgDepthTexture); 82 | 83 | //Uniform to update texture 84 | osg::Matrixd lmvpm = camera.textureCamera->getViewMatrix() * camera.textureCamera->getProjectionMatrix() 85 | * osg::Matrix::translate(1, 1, 1) * osg::Matrix::scale(0.5, 0.5, 0.5); 86 | osg::Uniform* u = new osg::Uniform(osg::Uniform::FLOAT_MAT4,"LightModelViewProjectionMatrix"); 87 | u->setUpdateCallback(new UpdateLMVPM(camera.textureCamera)); 88 | root->getOrCreateStateSet()->addUniform(u); 89 | 90 | // add Laser uniform to change from laser to light behaviours 91 | osg::Uniform* laserUniform = new osg::Uniform("isLaser", laser); 92 | root->getOrCreateStateSet()->addUniform(laserUniform); 93 | 94 | } 95 | 96 | -------------------------------------------------------------------------------- /uwsim/src/osgPCDLoader.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013 University of Jaume-I. 3 | * All rights reserved. This program and the accompanying materials 4 | * are made available under the terms of the GNU Public License v3.0 5 | * which accompanies this distribution, and is available at 6 | * http://www.gnu.org/licenses/gpl.html 7 | * 8 | * Contributors: 9 | * Mario Prats 10 | * Javier Perez 11 | */ 12 | 13 | #include "osgPCDLoader.h" 14 | 15 | -------------------------------------------------------------------------------- /uwsim/src/uwsim: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | #TODO: Check for ~/.uwsim/data, download if it doesn't exist 4 | 5 | DATAFILE=data.tgz 6 | 7 | if [ ! -d $HOME/.uwsim/data ] 8 | then 9 | echo -e "The UWSim data directory (~/.uwsim/data) does not exist. We need to download ~300Mb of data and place it under ~/.uwsim/data. Note this is required to run UWSim.\nContinue (Y/n) ? " 10 | read answer 11 | if [[ -z $answer || $answer == "Y" || $answer == "y" ]] 12 | then 13 | mkdir -p ~/.uwsim/data 14 | wget http://www.irs.uji.es/uwsim/files/$DATAFILE -O ~/.uwsim/UWSim-data.tgz && tar -zxvf ~/.uwsim/UWSim-data.tgz -C ~/.uwsim 15 | rm ~/.uwsim/UWSim-data.tgz 16 | else 17 | echo "Cannot run UWSim." 18 | exit 19 | fi 20 | fi 21 | 22 | echo "Starting UWSim..." 23 | rosrun uwsim uwsim_binary --dataPath ~/.uwsim/data $@ 24 | --------------------------------------------------------------------------------