├── .gitignore ├── GETTING_STARTED.md ├── LICENSE ├── README.md ├── SIMULATION_TUTORIAL.md ├── TUTORIAL-01.md ├── TUTORIAL-08.md ├── bluerov2_ardusub ├── CMakeLists.txt ├── calibrations │ └── nose_cam.yaml ├── config │ ├── apm_config.yaml │ └── apm_pluginlists.yaml ├── envs │ ├── rov-env.sh │ └── shore-env.sh ├── launch │ ├── start_ardusub_sitl.launch │ └── start_ardusub_vehi.launch ├── package.xml └── rviz │ └── bluerov2_ardusub.rviz ├── bluerov2_bringup ├── CMakeLists.txt ├── launch │ ├── bringup_ardusub_sitl.launch │ ├── bringup_ardusub_vehi.launch │ ├── bringup_neptus_gazebo.launch │ ├── bringup_simple.launch │ └── record.launch ├── package.xml ├── startup_scripts │ ├── bringup_rov.sh │ ├── shutdown_rov.sh │ ├── start_recording.sh │ └── stop_recording.sh └── udev │ └── 99-joysticks.rules ├── bluerov2_control ├── CMakeLists.txt ├── action │ └── FollowWaypoints.action ├── cfg │ ├── marker_reconfig.cfg │ └── pid_reconfig.cfg ├── config │ ├── TAM.yaml │ ├── controllers │ │ ├── cascade │ │ │ ├── inertial.yaml │ │ │ ├── pos_pid_control.yaml │ │ │ ├── simple_los.yaml │ │ │ └── vel_pid_control.yaml │ │ └── pid │ │ │ └── params.yaml │ ├── inertial.yaml │ ├── teleop │ │ ├── f310_command_mappings.yaml │ │ ├── f310_mappings.yaml │ │ ├── f710_command_mappings.yaml │ │ ├── f710_mappings.yaml │ │ ├── key_joy_command_mappings.yaml │ │ ├── key_joy_mappings.yaml │ │ ├── switchpro_command_mappings.yaml │ │ └── switchpro_mappings.yaml │ ├── thruster_manager.yaml │ └── thrusters │ │ ├── T200-Public-Performance-Data-10-20V-September-2019.csv │ │ ├── calculate_polynomial_thrust.py │ │ └── t200_thrust2gain_map.yaml ├── launch │ ├── joy_velocity.launch │ ├── joystick_teleop.launch │ ├── keyboard_teleop.launch │ ├── los_controller.launch │ ├── position_hold.launch │ ├── rov_mb_fl_controller.launch │ ├── rov_pid_controller.launch │ ├── start_thruster_manager.launch │ └── switch_pro_teleop.launch ├── msg │ ├── Autopilot.msg │ └── ControlMode.msg ├── package.xml ├── rviz │ └── bluerov2_control.rviz ├── scripts │ ├── geodetic_to_local.py │ ├── import_iver_mis.py │ ├── keyboard_joy.py │ ├── mavros_interface.py │ ├── simple_los.py │ ├── sitl_model_state_publisher.py │ ├── sitl_teleop_manual_control.py │ ├── sitl_teleop_rc_override.py │ ├── trajectory_visualizer.py │ └── utm_to_world.py ├── srv │ ├── ConvertGeoPoints.srv │ └── SetControlMode.srv └── waypoints │ └── example_waypoints.yaml ├── bluerov2_description ├── CMakeLists.txt ├── launch │ └── upload.launch ├── meshes │ ├── README.md │ ├── bluerov2_noprop.dae │ ├── bluerov2_noprop.stl │ ├── bluerov2_propccw.dae │ ├── bluerov2_propcw.dae │ ├── ping1d.STL │ └── ping1d.dae ├── package.xml ├── robots │ ├── altimeter.xacro │ ├── altimeter_sss.xacro │ └── default.xacro └── urdf │ ├── actuators.xacro │ ├── base.xacro │ ├── gazebo.xacro │ ├── sensors.xacro │ └── snippets.xacro ├── bluerov2_executive ├── CMakeLists.txt ├── launch │ └── bluerov2_executive.launch ├── package.xml └── src │ ├── ardusub_executive.py │ ├── gazebo_executive.py │ └── imc_enums.py ├── bluerov2_gazebo ├── CMakeLists.txt ├── config │ └── disturbances.yaml ├── launch │ ├── ardusub_peberholm_world.launch │ ├── herkules_ship_wreck.launch │ ├── peberholm_bluerov2.launch │ ├── peberholm_world.launch │ ├── start_accel_controller_demo.launch │ ├── start_ardusub_sitl_demo.launch │ ├── start_disturbance_demo.launch │ └── start_trajectory_controller_demo.launch ├── package.xml ├── rviz │ └── bluerov2_gazebo.rviz └── worlds │ ├── ardusub-peberholm.world │ └── peberholm.world ├── bluerov2_neptus ├── CMakeLists.txt ├── launch │ └── neptus.launch ├── package.xml └── vehicle-defs │ └── 00-bluerov2-1.nvcl ├── bluerov2_state_estimation ├── CMakeLists.txt ├── launch │ └── ekf.launch ├── package.xml ├── params │ └── ekf.yaml └── src │ ├── fake_waterlink.py │ ├── geodetic_helpers.py │ ├── math_helpers.py │ └── tf_manager.py ├── doc ├── imgs │ ├── bluerov2_uuv_simulator.png │ ├── neptus_001.png │ ├── neptus_002.png │ ├── neptus_003.png │ ├── neptus_004.png │ └── neptus_005.png └── topics_and_data.md ├── waterlinked_gps ├── CMakeLists.txt ├── Makefile ├── README.sh ├── data │ └── 14-03-2018_data_for_lp_design │ │ ├── data_export.csv │ │ ├── export_csv.py │ │ ├── start_recording.sh │ │ └── waterlinked_data_0.bag ├── launch │ └── uwgps-bringup.launch ├── package.xml └── scripts │ ├── http_request_speed_tests │ ├── waterlinked_grequests_test.py │ ├── waterlinked_requests_futures_test.py │ └── waterlinked_requests_test.py │ ├── math_helpers.py │ ├── owens_logger │ ├── GPS-loggin.txt │ ├── ROV_GPS.py │ └── waterlinked_owen.py │ └── waterlinked_gps_node.py └── waterlinked_gps_msgs ├── CMakeLists.txt ├── REMORA_ws.sublime-project ├── REMORA_ws.sublime-workspace ├── msg ├── About.msg ├── AboutStatus.msg ├── AboutTemperature.msg ├── ConfigGeneric.msg ├── ConfigReceivers.msg ├── ExternalOrientation.msg ├── PositionAcousticFiltered.msg ├── PositionAcousticRaw.msg ├── PositionGlobal.msg ├── PositionMaster.msg └── Receiver.msg └── package.xml /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | share/python-wheels/ 24 | *.egg-info/ 25 | .installed.cfg 26 | *.egg 27 | MANIFEST 28 | 29 | # PyInstaller 30 | # Usually these files are written by a python script from a template 31 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 32 | *.manifest 33 | *.spec 34 | 35 | # Installer logs 36 | pip-log.txt 37 | pip-delete-this-directory.txt 38 | 39 | # Unit test / coverage reports 40 | htmlcov/ 41 | .tox/ 42 | .nox/ 43 | .coverage 44 | .coverage.* 45 | .cache 46 | nosetests.xml 47 | coverage.xml 48 | *.cover 49 | *.py,cover 50 | .hypothesis/ 51 | .pytest_cache/ 52 | cover/ 53 | 54 | # Translations 55 | *.mo 56 | *.pot 57 | 58 | # Django stuff: 59 | *.log 60 | local_settings.py 61 | db.sqlite3 62 | db.sqlite3-journal 63 | 64 | # Flask stuff: 65 | instance/ 66 | .webassets-cache 67 | 68 | # Scrapy stuff: 69 | .scrapy 70 | 71 | # Sphinx documentation 72 | docs/_build/ 73 | 74 | # PyBuilder 75 | .pybuilder/ 76 | target/ 77 | 78 | # Jupyter Notebook 79 | .ipynb_checkpoints 80 | 81 | # IPython 82 | profile_default/ 83 | ipython_config.py 84 | 85 | # pyenv 86 | # For a library or package, you might want to ignore these files since the code is 87 | # intended to run in multiple environments; otherwise, check them in: 88 | # .python-version 89 | 90 | # pipenv 91 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 92 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 93 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 94 | # install all needed dependencies. 95 | #Pipfile.lock 96 | 97 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow 98 | __pypackages__/ 99 | 100 | # Celery stuff 101 | celerybeat-schedule 102 | celerybeat.pid 103 | 104 | # SageMath parsed files 105 | *.sage.py 106 | 107 | # Environments 108 | .env 109 | .venv 110 | env/ 111 | venv/ 112 | ENV/ 113 | env.bak/ 114 | venv.bak/ 115 | 116 | # Spyder project settings 117 | .spyderproject 118 | .spyproject 119 | 120 | # Rope project settings 121 | .ropeproject 122 | 123 | # mkdocs documentation 124 | /site 125 | 126 | # mypy 127 | .mypy_cache/ 128 | .dmypy.json 129 | dmypy.json 130 | 131 | # Pyre type checker 132 | .pyre/ 133 | 134 | # pytype static type analyzer 135 | .pytype/ 136 | 137 | # Cython debug symbols 138 | cython_debug/ 139 | 140 | # Jetbrains 141 | .idea/ 142 | -------------------------------------------------------------------------------- /GETTING_STARTED.md: -------------------------------------------------------------------------------- 1 | # Autonomous Marine Systems Software Setup Guide # 2 | 3 | The following steps are a checklist for ensuring you are able to use the open-source software that is used within this course. 4 | 5 | ## Install Ubuntu ## 6 | 7 | You may skip this step if Ubuntu 18.04 is already installed on your machine. 8 | 9 | - OS: Ubuntu 18.04 (not Ubuntu 20.04), see [Download Page](https://releases.ubuntu.com/18.04/) and [This Installation Guide for Dual Boot](https://help.ubuntu.com/community/WindowsDualBoot) 10 | - Configure Wi-Fi: If using DTU's DTUSecure or Eduroam WiFi networks, then follow the configuration instructions [Here](https://itswiki.compute.dtu.dk/index.php/DTUsecure_WiFi) 11 | 12 | ## Install Robot Operating System ## 13 | 14 | - Melodic Morenia: Follow the installation instructions [Here](http://wiki.ros.org/melodic/installation/Ubuntu), make sure to specify ros-melodic-desktop-full when installing. 15 | 16 | ### ROS Packages ### 17 | 18 | - UUV Simulator: Follow the installation instructions [Here](https://uuvsimulator.github.io/installation/) 19 | - MAVROS: Follow the installation instructions [Here](https://github.com/mavlink/mavros/blob/master/mavros/README.md#installation) 20 | - NOTE: replace "kinetic" with "melodic" in the apt-get install commands. 21 | - NOTE: Make sure to install the GeographicLib datasets after installing mavros! You will have to execute ´´´sudo ./install_geographiclib_datasets.sh"´´´ 22 | - BlueROV2 Gazebo and ArduSub Modelling 23 | 1. Follow instructions to create a new ROS workspace [Here (choose Melodic)](http://wiki.ros.org/catkin/Tutorials/create_a_workspace). 24 | 2. Clone this repository into your workspace's src directory (called `catkin_ws/src` in this guide) `git clone https://github.com/FletcherFT/bluerov2.git` 25 | 3. Execute `catkin_make` in the top-level directory of `catkin_ws`. 26 | 27 | ## Install Ardusub Software-In-The-Loop ## 28 | 29 | - SITL: Follow the installation instructions [Starting Here, and Be Sure to Follow Links](https://ardupilot.org/dev/docs/setting-up-sitl-on-linux.html). 30 | - NOTE: Follow "Cloning with the command line" section for minimum fuss. When moving on to the [BUILD.md](https://github.com/ArduPilot/ardupilot/blob/master/BUILD.md) section, you do not have to clone ardupilot again. 31 | 32 | - ArduSub Configuration: run 33 | 34 | `./waf configure --board sitl` 35 | 36 | `./waf sub` 37 | 38 | ## Install QGroundControl ## 39 | 40 | - APP: Install the application [Here](https://docs.qgroundcontrol.com/master/en/getting_started/download_and_install.html), follow instructions for Ubuntu Linux. 41 | 42 | ## Test ArduSub SITL and QGroundControl ## 43 | 44 | NOTE: For this you will need access to a gamepad joystick (XBox, Logitech, Nintendo Switch Pro, Playstation etc.), or a joystick-keyboard emulator for linux such as [QJoyPad](http://qjoypad.sourceforge.net/) 45 | 46 | - ArduSub SITL: Follow the instructions [Here](https://www.ardusub.com/developers/sitl.html) and launch a SITL simulation. 47 | - QGroundControl: Double Click on APP, configure joystick. 48 | 49 | ## Test UUV Simulator ## 50 | 51 | - Open ~/.ignition/fuel/config.yaml, replace "api.ignitionfuel.org" with "fuel.ignitionrobotics.org" 52 | - Follow instructions [Here](https://uuvsimulator.github.io/quick_start) 53 | 54 | ## Test ArduSub SITL and MAVROS ## 55 | 56 | ** NOTE, this is still a work in progress, the base_link -> thruster TFs are a little strange for some reason but this is purely cosmetic.** 57 | 58 | - Launch an instance of ArduSub simulation 59 | 60 | `sim_vehicle.py -v ArduSub -l 55.60304,12.808937,0,0 --map --console` 61 | 62 | - Launch MAVROS 63 | 64 | - with joystick: `roslaunch bluerov2_bringup bringup_ardusub_sitl.launch` 65 | 66 | - with keyboard: `roslaunch bluerov2_bringup bringup_ardusub_sitl.launch use_joystick:=false` 67 | 68 | - With Gazebo Camera Simulation 69 | 70 | - with joystick: `roslaunch bluerov2_bringup bringup_ardusub_sitl.launch gazebo:=true` 71 | 72 | - with keyboard: `roslaunch bluerov2_gazebo start_ardusub_sitl_demo.launch gazebo:=true use_joystick:=false` 73 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # BlueROV2 ROS Simulation 2 | 3 | ## What is this Repository? ## 4 | 5 | This repository contains the robot description and necessary launch files to simulate the BlueROV2 (unmanned underwater vehicle) on [Unmanned Underwater Vehicle Simulator (UUV Simulator)](https://github.com/uuvsimulator/uuv_simulator). It's also possible to simulate the BlueROV2 using [ArduSub Software-In-The-Loop](https://ardupilot.org/dev/docs/sitl-simulator-software-in-the-loop.html) with [mavros](http://wiki.ros.org/mavros) and other ROS packages. joystick interaction and video streaming capture with opencv based on package from BlueRobotics. 6 | 7 | This work consolidates the contributions from [Ingeniarius, Lda.](http://ingeniarius.pt/) and [Instituite of Systems and Robotics University of Coimbra](https://www.isr.uc.pt/) within the scope of MS thesis "Localization of an unmanned underwater vehicle using multiple water surface robots, multilateration, and sensor data fusion". Source code is derived from [bluerov_ros_playground](https://github.com/patrickelectric/bluerov_ros_playground). 8 | 9 | The [Unmanned Underwater Vehicle Simulator](https://uuvsimulator.github.io/) (UUV-Sim) is also supported in this repository. The vehicle model generated at [bluerov2](https://github.com/fredvaz/bluerov2), has been adapted to work with Melodic relase of UUV-Sim 10 | 11 | This repository is intended as introductory marine vehicle simulation software for the 2021 Spring Special Course "Autonomous Marine Robotics" hosted at the Technical University of Denmark. 12 | 13 |

14 | 15 |

16 | 17 | 18 | ## Getting Started ## 19 | 20 | The repository was built and tested on a fresh installation of Ubuntu 18.04. The [Getting Started Guide](GETTING_STARTED.md) will assist with installation of dependencies and testing. 21 | Tutorials [01](TUTORIAL-01.md) and [08](TUTORIAL-08.md) are intended for students enrolled in Autonomous Marine Robotics special course at DTU. The [simulation tutorial](SIMULATION_TUTORIAL.md) gives a quick start guide for people interested in using the simulations. 22 | 23 | ## Additional Links ## 24 | 25 | - [MAVROS Documentation](https://github.com/mavlink/mavros/blob/master/mavros/README.md) 26 | - [UUV Simulator Documentation](https://uuvsimulator.github.io/packages/uuv_simulator/intro/) 27 | 28 | 29 | ## Topics 30 | 31 | If you need more information about the topics and what you can access, take a look [here](doc/topics_and_data.md). 32 | 33 | 34 | ## License 35 | 36 | BlueROV2 package is open-sourced under the Apache-2.0 license. See the 37 | [LICENSE](LICENSE) file for details. 38 | -------------------------------------------------------------------------------- /SIMULATION_TUTORIAL.md: -------------------------------------------------------------------------------- 1 | # Getting Started With Gazebo Simulation of BlueROV2 (with Neptus Support) # 2 | 3 | In [tutorial 1](TUTORIAL-01.md) you were introduced to the UUV Simulator plugins for Gazebo and the BlueROV2. 4 | In this tutorial we'll integrate the simulator with an industry standard mission planning framework: [Neptus](https://github.com/LSTS/neptus). 5 | 6 | ## Pre-requisites ## 7 | 8 | Please follow the setup instructions in [tutorial 8](TUTORIAL-08.md) 9 | 10 | ## Gazebo Quick Start ## 11 | 12 | From a terminal, run the following: 13 | 14 | ``` 15 | roslaunch bluerov2_bringup bringup_neptus_gazebo.launch rviz_on:=true mode:=altimeter 16 | ``` 17 | 18 | This will launch a gazebo simulation of the peberholm world with a BlueROV2 equipped with a downwards facing altimeter. 19 | 20 | ## ArduSub Quick Start ## 21 | 22 | From a terminal, start the ardusub sitl simulation: 23 | 24 | ``` 25 | sim_vehicle.py -v ArduSub -l 55.60304,12.808937,0,0 --map --console 26 | ``` 27 | 28 | In another terminal, run the following: 29 | 30 | ``` 31 | roslaunch bluerov2_bringup bringup_ardusub_sitl.launch 32 | ``` -------------------------------------------------------------------------------- /TUTORIAL-01.md: -------------------------------------------------------------------------------- 1 | # UUV Simulator Tutorial # 2 | 3 | ## Spawning a Basic World ## 4 | 5 | In a new terminal (ctrl + alt + t), start up a ROS server. 6 | 7 | `roscore` 8 | 9 | In a new terminal (ctrl + shift + t), launch an underwater world. 10 | 11 | `roslaunch uuv_gazebo_worlds herkules_ship_wreck.launch gui:=false paused:=true` 12 | 13 | Hint: you can add `--ros-args` to the command to get a list of arguments available. 14 | 15 | Note: if you omit the gui argument, or set it to true, then the gazebo frontend GUI will launch and display the world. 16 | 17 | Note: the paused argument sets the physics simulator to freeze (nothing is published), it can then be unfrozen when you're ready. 18 | 19 | In a new terminal, explore the topics currently available. 20 | 21 | `rostopic list` 22 | 23 | **/hydrodynamics/current_velocity**: the world referenced current. 24 | 25 | **/gazebo/model_states**: pose and velocity of any models in the simulation. 26 | 27 | **/gazebo/link_states**: pose and velocity of any links in the simulation. 28 | 29 | ## Spawning a BlueROV2 ## 30 | 31 | With a world active, you can now simulate a vehicle in it. We will upload the BlueROV2 into the world at 40 m depth. 32 | 33 | `roslaunch bluerov2_description upload_bluerov2.launch z:=-50 x:=-15` 34 | 35 | Hint: If you are viewing with Gazebo, you can right click on Models->bluerov2 in the left panel and click "move to", or "follow" to find the BlueROV2. 36 | 37 | ## Unpausing the simulation ## 38 | 39 | First, pull up an RVIZ window: 40 | 41 | `rosrun rviz rviz -d $(rospack find bluerov2_gazebo)/rviz/bluerov2.rviz` 42 | 43 | Once rviz starts, check the box marked Image on the left panel. 44 | 45 | Next, unpause the simulation: 46 | 47 | `rosservice call /gazebo/unpause_physics` 48 | 49 | You should see the camera view of the ROV in RVIZ, and the ROV starting to rise in the gazebo view. This makes sense as the BlueROV2 is positively buoyant! 50 | 51 | ### Activity ### 52 | 53 | 1. Explore the topics provided in /bluerov2/... 54 | 2. Run the rqt_publisher GUI: `rosrun rqt_publisher rqt_publisher` 55 | 3. Play around with publishing thrust commands to /blurov2/thrusters/[0-5]/input topics 56 | 4. See if you can get the ROV to turn on the spot, or hold depth. 57 | 5. Apply 0.0 to the thrusters to stop them. 58 | 59 | 60 | ## Configure BlueROV2 with Thruster Allocation Matrix Support ## 61 | 62 | At this point, you are able to publish thrust messages to each of the thrusters manually and cause the model to move. UUV simulator comes with support for a Thruster Allocation Matrix (TAM) manager that converts a commanded body-frame wrench (axial force + rotational moment) into individual thrusts, whose resultant should match the command (within limits). 63 | 64 | First, pause the simulation and delete the model. 65 | 66 | `rosservice call /gazebo/pause_physics` 67 | 68 | `rosservice call /gazebo/delete_model "model_name: 'bluerov2'"` 69 | 70 | If your gazebo GUI crashes at this point, you can restart it by running `gzclient` in a terminal. 71 | 72 | Next, upload another bluerov2 as you did before. Then run the following to launch the TAM manager: 73 | 74 | `roslaunch bluerov2_control start_thruster_manager.launch` 75 | 76 | Publish wrench topics to the /bluerov2/thruster_manager/input topic. 77 | 78 | ### Activity ### 79 | 80 | Try to get the ROV to hold depth (view the /bluerov2/pose_gt/twist topic to get a velocity reference.) 81 | 82 | ## Reconfiguring the BlueROV2 Payload ## 83 | 84 | The upload_bluerov2.launch script uses the information provided in `$(rospack find bluerov2_description)/robots/bluerov2_default.xacro` to spawn the vehicle with the correct payload and properties. This can be changed! 85 | 86 | The xacro layout allows for layers of objects to be loaded in, the robots/bluerov2_default.xacro simply specifies all of the files .xacro files to be used. 87 | 88 | 1. Pause and delete the BlueROV2. 89 | 2. Open the `bluerov2_description/robots/bluerov2_default.xacro` and save as a new file `bluerov2_description/robots/bluerov2_down_facing_camera.xacro`. 90 | 3. After the line ``, paste in the following: 91 | ` 92 | 93 | ` 94 | 4. Spawn the BlueROV2 with the new configuration: 95 | `roslaunch bluerov2_description upload_bluerov2.launch mode:=down_facing_camera` 96 | 97 | ### For your own work ### 98 | 99 | Extra sensors are defined in `$(rospack find bluerov2_description)/urdf/sensors.xacro`, `$(rospack find bluerov2_description)/urdf/snippets.xacro`, which inherit from (among other places) `$(rospack find uuv_sensor_ros_plugins)/urdf`. Try modifying and using the bluerov_altimeter snippet defined in `$(rospack find bluerov2_description)/urdf/snippets.xacro` to spawn a new robot `bluerov2_altimeter.xacro` that has a downward facing rangefinder like in the real model. 100 | 101 | -------------------------------------------------------------------------------- /TUTORIAL-08.md: -------------------------------------------------------------------------------- 1 | # Mission Planning Tutorial # 2 | 3 | In [tutorial 1](TUTORIAL-01.md) you were introduced to the UUV Simulator plugins for Gazebo and the BlueROV2. 4 | In this tutorial we'll integrate the simulator with an industry standard mission planning framework: [Neptus](https://github.com/LSTS/neptus). 5 | 6 | ## Pre-requisites ## 7 | 8 | Please make sure you are running Ubuntu 18.04 (VM or otherwise) with ROS Melodic Morena installed. 9 | Please note that the packages are still only compatible with Python 2.7, just make sure you have it installed and make sure python routes to python2.7: 10 | 11 | ``` 12 | sudo apt install ros-melodic-ros-base --reinstall 13 | ``` 14 | 15 | **Note**: You will probably need the following as well: 16 | 17 | ``` 18 | sudo apt install ros-melodic-geographic-info ros-melodic-mavros 19 | python -m pip install --upgrade scipy 20 | ``` 21 | 22 | ### Installing IMC to ROS Bridge ### 23 | 24 | The Inter Module Communication (IMC) API is a messaging protocol used by Neptus, this needs to be bridged to the ROS messaging protocol. 25 | There is one good bridge available from [SMarC](https://smarc.se/), please clone the fork into your ROS workspace 26 | 27 | Navigate to the ROS workspace (called **catkin_ws** here) where you want to install the bluerov2 stack, then clone another ROS package into `catkin_ws/src` and build the packages. 28 | 29 | ``` 30 | git clone -b noetic-devel https://github.com/FletcherFT/imc_ros_bridge.git 31 | cd .. 32 | catkin_make 33 | ``` 34 | 35 | There may also be some other missing packages, pay attention to errors in catkin_make or during run time. 36 | 37 | ### Updating your bluerov2 repository ### 38 | 39 | The bluerov2 repository has been updated substantially to run with neptus. Please pull changes as follows 40 | 41 | ``` 42 | roscd bluerov2_control/.. 43 | git pull origin master 44 | ``` 45 | 46 | ### Installing and Starting Neptus ### 47 | 48 | First, install dependencies: `sudo apt install openjdk-11-jdk`. 49 | Second, make sure there are no other Javas in your system, you might have a `jre-default` installed by default. Use `apt remove ...` to remove it (See Troubleshooting for more details). 50 | 51 | **Neptus is not a ROS package, so you clone into your home directory (or wherever you have write access)** 52 | 53 | Clone and build Neptus at a known-working commit: 54 | ``` 55 | git clone https://github.com/LSTS/neptus.git 56 | cd neptus 57 | git checkout 38c7f41a9885c6b059f79b38861edb4b7b67511b 58 | ./gradlew 59 | ``` 60 | 61 | The built Neptus executable is now available and can be executed via ```./neptus``` from the neptus root directory. 62 | 63 | Next you should include the bluerov vehicle definition file into Neptus so that it knows what the vehicle is capable of. 64 | 65 | ``` 66 | roscp bluerov2_neptus 00-bluerov2-1.nvcl ~/neptus/vehicles-defs/. 67 | ``` 68 | 69 | If you are already running neptus, restart the program to load the new vehicle profle. 70 | 71 | [Neptus Startup](doc/imgs/neptus_001.png) 72 | 73 | Adjust the map overlay by right-clicking somewhere on the map and choosing "Choose Visible World Map": 74 | 75 | [World Map](doc/imgs/neptus_002.png) 76 | 77 | Load in something fairly light-weight like OpenStreetMap: 78 | 79 | [OpenStreetMap](doc/imgs/neptus_003.png) 80 | 81 | The default mission is based in Portugal, first adjust the home point by right-clicking the GPS reference on the right panel 82 | and choosing "View/Edit Home Reference". Change latitude and longitude to 55.603036 and 12.8089368 respectively. 83 | 84 | [Edit Home Point](docs/imgs/neptus_004.png) 85 | 86 | Then right-click somewhere on the map and choose "Center map in -> Home Reference" 87 | 88 | ## Checking Your Setup ## 89 | 90 | ### ROS Check ### 91 | 92 | You can launch the stack from the bluerov2_bringup package: 93 | 94 | ``` 95 | roslaunch bluerov2_bringup bringup_neptus_gazebo.launch rviz_on:=True gui_on:=True 96 | ``` 97 | 98 | Any errors should hopefully turn up at this point, you may have to install some additional ros packages: 99 | 100 | - ros-melodic-geodesy 101 | - ros-melodic-geographic-msgs 102 | 103 | Gazebo will warn you that pymap3d is not installed, please **do not** install this package as it messes with the geodetic libraries used by uuv_simulator. 104 | 105 | ### Neptus Integration Check ### 106 | 107 | Run neptus: `~/neptus/neptus` 108 | 109 | Next, with the bluerov2 ROS stack running, check that ROS is receiving heartbeats from Neptus via IMC: 110 | 111 | ``` 112 | rostopic echo /bluerov2/imc/imc_heartbeat 113 | ``` 114 | 115 | Then, check the bluerov2 is visible on neptus near the home point and facing East: 116 | 117 | [bluerov2 visible](doc/imgs/neptus_005.png) 118 | 119 | You should then be good for the tutorial! 120 | 121 | ## Troubleshooting ## 122 | 123 | If Neptus does not seem to work, try these: 124 | On Ubuntu 18.04 and 20.04, you might have `openjdk-11-jre-headless` and `default-jre` remove them with `apt remove ...` manually and make sure that `apt list --installed | grep jre` only shows one jre, it should look like this: 125 | ``` 126 | $ apt list --installed | grep jre 127 | 128 | WARNING: apt does not have a stable CLI interface. Use with caution in scripts. 129 | 130 | openjdk-8-jre-headless/focal-updates,focal-security,now 8u282-b08-0ubuntu1~20.04 amd64 [installed,automatic] 131 | openjdk-8-jre/focal-updates,focal-security,now 8u282-b08-0ubuntu1~20.04 amd64 [installed,automatic] 132 | ``` 133 | This is a known-working setup as of Mar 2 2021. 134 | 135 | 136 | Additionally, if you get a "No VTK Java Packages Found" message in Neptus's console AND things are not working, do this(might need to change the java version to the version you have): 137 | 138 | `sudo vim /etc/java-8-openjdk/accessibility.properties` 139 | Comment out the following line: 140 | `assistive_technologies=org.GNOME.Accessibility.AtkWrapper` 141 | 142 | If you get an error that involves `iced-tea` in the Neptus terminal window when you try to open a console, check that you have openjdk-8-jre. Not _just_ the headless version of it, Neptus needs the head. -------------------------------------------------------------------------------- /bluerov2_ardusub/calibrations/nose_cam.yaml: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/FletcherFT/bluerov2/c416fc8530aac2b0ac52626001308d780347d639/bluerov2_ardusub/calibrations/nose_cam.yaml -------------------------------------------------------------------------------- /bluerov2_ardusub/config/apm_pluginlists.yaml: -------------------------------------------------------------------------------- 1 | plugin_blacklist: 2 | # common 3 | - 3dr_radio 4 | - actuator_control 5 | - adsb 6 | - altitude 7 | - cam_imu_sync 8 | - companion_process_status 9 | - debug_value 10 | - esc_status 11 | - fake_gps 12 | - ftp 13 | - gps_rtk 14 | - gps_status 15 | - hil 16 | - home_position 17 | - landing_target 18 | - log_transfer 19 | - mocap_pose_estimate 20 | - mount_control 21 | - obstacle_distance 22 | - odom 23 | - onboard_computer_status 24 | - param 25 | - play_tune 26 | - px4flow 27 | - rangefinder 28 | 29 | - safety_area 30 | - setpoint_* 31 | - trajectory 32 | - vf4_hud 33 | - vibration 34 | - vision_* 35 | - waypoint 36 | - wheel_odometry 37 | - wind_estimation 38 | 39 | 40 | plugin_whitelist: 41 | - 'sys_*' 42 | - rc_io 43 | -------------------------------------------------------------------------------- /bluerov2_ardusub/envs/rov-env.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | source /opt/ros/kinetic/setup.bash 3 | source /home/pi/mavros_catkin_ws/devel/setup.bash 4 | source /home/pi/aqua_catkin_ws/devel/setup.bash 5 | export ROS_MASTER_URI=http://192.168.2.1:11311 6 | export ROS_IP=192.168.2.2 7 | exec "$@" 8 | -------------------------------------------------------------------------------- /bluerov2_ardusub/envs/shore-env.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | source /opt/ros/melodic/setup.bash 3 | source /home/fft/ros_ws/devel/setup.bash 4 | export ROS_MASTER_URI=http://192.168.2.1:11311 5 | export ROS_IP=192.168.2.1 6 | export DISPLAY=:0 7 | exec "$@" 8 | -------------------------------------------------------------------------------- /bluerov2_ardusub/launch/start_ardusub_sitl.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /bluerov2_ardusub/launch/start_ardusub_vehi.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | -------------------------------------------------------------------------------- /bluerov2_ardusub/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | bluerov2_ardusub 4 | 0.0.0 5 | The bluerov2_ardusub package 6 | 7 | 8 | 9 | 10 | fft 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | mavros 53 | mavros 54 | mavros 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | -------------------------------------------------------------------------------- /bluerov2_bringup/launch/bringup_ardusub_sitl.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | [55.60304, 12.808937, 0] 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 | -------------------------------------------------------------------------------- /bluerov2_bringup/launch/bringup_ardusub_vehi.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | -------------------------------------------------------------------------------- /bluerov2_bringup/launch/bringup_neptus_gazebo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /bluerov2_bringup/launch/bringup_simple.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /bluerov2_bringup/launch/record.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /bluerov2_bringup/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | bluerov2_bringup 4 | 0.0.0 5 | The bluerov2_bringup package 6 | 7 | 8 | 9 | 10 | fft 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /bluerov2_bringup/startup_scripts/bringup_rov.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | mkdir -p $(rospack find bluerov2_bringup)/logs 3 | LOGFILE=$(rospack find bluerov2_bringup)/logs/rosrov.log 4 | screen -L -Logfile $LOGFILE -dmS rosrov roslaunch bluerov2_bringup bringup_ardusub_vehi.launch joystick_name:=f710 -------------------------------------------------------------------------------- /bluerov2_bringup/startup_scripts/shutdown_rov.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | rosnode kill -a 3 | screen -XS rosrov quit -------------------------------------------------------------------------------- /bluerov2_bringup/startup_scripts/start_recording.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | TS=$(date -u '+%Y%m%dT%H%M%S') 3 | FP=$HOME/Desktop/ROS_Recordings/$TS 4 | mkdir -p $FP 5 | mkdir -p $(rospack find bluerov2_bringup)/logs 6 | LOGFILE=$(rospack find bluerov2_bringup)/logs/rosrecord.log 7 | screen -L -Logfile $LOGFILE -dmS rosrecord roslaunch bluerov2_bringup record.launch bag_dir:=$FP -------------------------------------------------------------------------------- /bluerov2_bringup/startup_scripts/stop_recording.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | screen -XS rosrecord quit -------------------------------------------------------------------------------- /bluerov2_bringup/udev/99-joysticks.rules: -------------------------------------------------------------------------------- 1 | # microsoft xbox controller 2 | #SUBSYSTEM=="input", KERNEL=="js*", SUBSYSTEMS=="usb", DRIVERS=="xpad", ATTRS{bInterfaceProtocol}=="81", ATTRS{bInterfaceNumber}=="00", SYMLINK+="xboxcontroller" 3 | ACTION=="add", SUBSYSTEM=="input", KERNEL=="js?", ATTRS{name}=="Logitech Gamepad F710", SYMLINK+="f710" 4 | ACTION=="add", SUBSYSTEM=="input", KERNEL=="js?", ATTRS{name}=="Logitech Gamepad F310", SYMLINK+="f310" 5 | ACTION=="add", SUBSYSTEM=="input", KERNEL=="js?", ATTRS{idVendor}=="0f0d", ATTRS{idProduct}=="00c1", SYMLINK+="switchpro" 6 | -------------------------------------------------------------------------------- /bluerov2_control/action/FollowWaypoints.action: -------------------------------------------------------------------------------- 1 | # Define the goal 2 | uuv_control_msgs/WaypointSet waypoints 3 | --- 4 | # Define the result 5 | uint32 waypoints_completed 6 | --- 7 | # Define a feedback message 8 | float32 percentage_complete -------------------------------------------------------------------------------- /bluerov2_control/cfg/marker_reconfig.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "bluerov2_control" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | gen.add("scale_x", double_t, 0, "Marker Scale x", 1.0, 0, 400) 9 | gen.add("scale_y", double_t, 0, "Marker Scale y", 1.0, 0, 400) 10 | gen.add("scale_z", double_t, 0, "Marker Scale z", 1.0, 0, 400) 11 | 12 | gen.add("color_r", double_t, 0, "Marker Color r", 1.0, 0, 1) 13 | gen.add("color_g", double_t, 0, "Marker Color g", 0.0, 0, 1) 14 | gen.add("color_b", double_t, 0, "Marker Color b", 0.0, 0, 1) 15 | gen.add("color_a", double_t, 0, "Marker Opacity", 1.0, 0, 1) 16 | 17 | exit(gen.generate(PACKAGE, "trajectory_visualizer.py", "marker_reconfig")) -------------------------------------------------------------------------------- /bluerov2_control/cfg/pid_reconfig.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "bluerov2_control" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | gen.add("accel_x_kp", double_t, 0, "kp x Gain Acceleration", 1.0, -400, 400) 9 | gen.add("accel_x_ki", double_t, 0, "ki x Gain Acceleration", 0.0, -400, 400) 10 | gen.add("accel_x_kd", double_t, 0, "kd x Gain Acceleration", 0.0, -400, 400) 11 | gen.add("accel_x_sat", double_t, 0, "Surge Force Saturation (N)", 1.0, 0, 110) 12 | 13 | gen.add("accel_y_kp", double_t, 0, "kp y Gain Acceleration", 1.0, -400, 400) 14 | gen.add("accel_y_ki", double_t, 0, "ki y Gain Acceleration", 0.0, -400, 400) 15 | gen.add("accel_y_kd", double_t, 0, "kd y Gain Acceleration", 0.0, -400, 400) 16 | gen.add("accel_y_sat", double_t, 0, "Sway Force Saturation (N)", 1.0, 0, 110) 17 | 18 | gen.add("accel_z_kp", double_t, 0, "kp z Gain Acceleration", 1.0, -400, 400) 19 | gen.add("accel_z_ki", double_t, 0, "ki z Gain Acceleration", 0.0, -400, 400) 20 | gen.add("accel_z_kd", double_t, 0, "kd z Gain Acceleration", 0.0, -400, 400) 21 | gen.add("accel_z_sat", double_t, 0, "Heave Force Saturation (N)", 1.0, 0, 110) 22 | 23 | gen.add("accel_r_kp", double_t, 0, "kp r Gain Acceleration", 1.0, -400, 400) 24 | gen.add("accel_r_ki", double_t, 0, "ki r Gain Acceleration", 0.0, -400, 400) 25 | gen.add("accel_r_kd", double_t, 0, "kd r Gain Acceleration", 0.0, -400, 400) 26 | gen.add("accel_r_sat", double_t, 0, "Yaw Moment Saturation (N)", 1.0, 0, 40) 27 | 28 | gen.add("vel_x_kp", double_t, 0, "kp x Gain Velocity", 1.0, -400, 400) 29 | gen.add("vel_x_ki", double_t, 0, "ki x Gain Velocity", 0.0, -400, 400) 30 | gen.add("vel_x_kd", double_t, 0, "kd x Gain Velocity", 0.0, -400, 400) 31 | gen.add("vel_x_sat", double_t, 0, "x Acceleration SP Limits", 1.0, 0, 10) 32 | gen.add("vel_x_lim", double_t, 0, "x Velocity SP limit (Overrides Position Controller)", 1.0, 0.0, 2.0) 33 | 34 | gen.add("vel_y_kp", double_t, 0, "kp y Gain Velocity", 1.0, -400, 400) 35 | gen.add("vel_y_ki", double_t, 0, "ki y Gain Velocity", 0.0, -400, 400) 36 | gen.add("vel_y_kd", double_t, 0, "kd y Gain Velocity", 0.0, -400, 400) 37 | gen.add("vel_y_sat", double_t, 0, "y Acceleration SP Limits", 1.0, 0, 10) 38 | gen.add("vel_y_lim", double_t, 0, "y Velocity SP limit (Overrides Position Controller)", 1.0, 0.0, 2.0) 39 | 40 | gen.add("vel_z_kp", double_t, 0, "kp z Gain Velocity", 1.0, -400, 400) 41 | gen.add("vel_z_ki", double_t, 0, "ki z Gain Velocity", 0.0, -400, 400) 42 | gen.add("vel_z_kd", double_t, 0, "kd z Gain Velocity", 0.0, -400, 400) 43 | gen.add("vel_z_sat", double_t, 0, "z Acceleration SP Limits", 1.0, 0, 10) 44 | gen.add("vel_z_lim", double_t, 0, "z Velocity SP limit (Overrides Position Controller)", 1.0, 0.0, 2.0) 45 | 46 | gen.add("vel_r_kp", double_t, 0, "kp r Gain Velocity", 1.0, -400, 400) 47 | gen.add("vel_r_ki", double_t, 0, "ki r Gain Velocity", 0.0, -400, 400) 48 | gen.add("vel_r_kd", double_t, 0, "kd r Gain Velocity", 0.0, -400, 400) 49 | gen.add("vel_r_sat", double_t, 0, "r Acceleration SP Limits", 1.0, 0, 10) 50 | gen.add("vel_r_lim", double_t, 0, "r Velocity limit (Overrides Position Controller)", 1.0, 0.0, 0.5) 51 | 52 | gen.add("pos_x_kp", double_t, 0, "kp x Gain Position", 1.0, -400, 400) 53 | gen.add("pos_x_ki", double_t, 0, "ki x Gain Position", 0.0, -400, 400) 54 | gen.add("pos_x_kd", double_t, 0, "kd x Gain Position", 0.0, -400, 400) 55 | gen.add("pos_x_sat", double_t, 0, "x Velocity SP Limits", 1.0, 0, 2) 56 | 57 | gen.add("pos_y_kp", double_t, 0, "kp y Gain Position", 1.0, -400, 400) 58 | gen.add("pos_y_ki", double_t, 0, "ki y Gain Position", 0.0, -400, 400) 59 | gen.add("pos_y_kd", double_t, 0, "kd y Gain Position", 0.0, -400, 400) 60 | gen.add("pos_y_sat", double_t, 0, "y Velocity SP Limits", 1.0, 0, 2) 61 | 62 | gen.add("pos_z_kp", double_t, 0, "kp z Gain Position", 1.0, -400, 400) 63 | gen.add("pos_z_ki", double_t, 0, "ki z Gain Position", 0.0, -400, 400) 64 | gen.add("pos_z_kd", double_t, 0, "kd z Gain Position", 0.0, -400, 400) 65 | gen.add("pos_z_sat", double_t, 0, "z Velocity SP Limits", 1.0, 0, 2) 66 | 67 | gen.add("pos_r_kp", double_t, 0, "kp r Gain Position", 1.0, -400, 400) 68 | gen.add("pos_r_ki", double_t, 0, "ki r Gain Position", 0.0, -400, 400) 69 | gen.add("pos_r_kd", double_t, 0, "kd r Gain Position", 0.0, -400, 400) 70 | gen.add("pos_r_sat", double_t, 0, "yaw Velocity SP Limits", 1.0, 0, 0.5) 71 | 72 | exit(gen.generate(PACKAGE, "simple_los.py", "pid_reconfig")) -------------------------------------------------------------------------------- /bluerov2_control/config/TAM.yaml: -------------------------------------------------------------------------------- 1 | tam: 2 | - [0.7071067811847433, 0.7071067811847433, -0.7071067811919605, -0.7071067811919605, 3 | 0.0, 0.0] 4 | - [0.7071067811883519, -0.7071067811883519, 0.7071067811811348, -0.7071067811811348, 5 | 0.0, 0.0] 6 | - [0.0, 0.0, 0.0, 0.0, 1.0000000000000002, 1.0000000000000002] 7 | - [0.051265241636155506, -0.05126524163615552, 0.05126524163563227, -0.05126524163563227, 8 | -0.11050000000000001, 0.11050000000000003] 9 | - [-0.05126524163589389, -0.051265241635893896, 0.05126524163641713, 0.05126524163641713, 10 | -0.002499999999974481, -0.002499999999974481] 11 | - [0.16652364696949604, -0.16652364696949604, -0.17500892834341342, 0.17500892834341342, 12 | 0.0, 0.0] 13 | -------------------------------------------------------------------------------- /bluerov2_control/config/controllers/cascade/inertial.yaml: -------------------------------------------------------------------------------- 1 | pid: 2 | mass: 15.0 3 | inertial: 4 | ixx: 5.0 5 | ixy: 0 6 | ixz: 0 7 | iyy: 10.0 8 | iyz: 0 9 | izz: 10.0 10 | -------------------------------------------------------------------------------- /bluerov2_control/config/controllers/cascade/pos_pid_control.yaml: -------------------------------------------------------------------------------- 1 | position_control/pos_p: 0.8 2 | position_control/pos_i: 0.0005 3 | position_control/pos_d: 0.0005 4 | position_control/pos_sat: 1.0 5 | 6 | position_control/rot_p: 0.3 7 | position_control/rot_i: 0.001 8 | position_control/rot_d: 0.001 9 | position_control/rot_sat: 2.0 10 | -------------------------------------------------------------------------------- /bluerov2_control/config/controllers/cascade/simple_los.yaml: -------------------------------------------------------------------------------- 1 | accel_r_kd: 0.0 2 | accel_r_ki: 0.0 3 | accel_r_kp: 1.0 4 | accel_r_sat: 1.0 5 | accel_x_kd: 0.0 6 | accel_x_ki: 0.0 7 | accel_x_kp: 1.0 8 | accel_x_sat: 1.0 9 | accel_y_kd: 0.0 10 | accel_y_ki: 0.0 11 | accel_y_kp: 1.0 12 | accel_y_sat: 1.0 13 | accel_z_kd: 0.0 14 | accel_z_ki: 0.0 15 | accel_z_kp: 1.0 16 | accel_z_sat: 1.0 17 | pid: 18 | inertial: {ixx: 5.0, ixy: 0, ixz: 0, iyy: 10.0, iyz: 0, izz: 10.0} 19 | mass: 15.0 20 | pos_r_kd: 0.0 21 | pos_r_ki: 0.0 22 | pos_r_kp: 1.0 23 | pos_r_sat: 0.5 24 | pos_x_kd: 0.0 25 | pos_x_ki: 0.0 26 | pos_x_kp: 1.0 27 | pos_x_sat: 1.0 28 | pos_y_kd: 0.0 29 | pos_y_ki: 0.0 30 | pos_y_kp: 1.0 31 | pos_y_sat: 1.0 32 | pos_z_kd: 0.0 33 | pos_z_ki: 0.0 34 | pos_z_kp: 1.0 35 | pos_z_sat: 1.0 36 | use_accel_fb: false 37 | vel_r_kd: 0.01 38 | vel_r_ki: 2.0 39 | vel_r_kp: 1.0 40 | vel_r_lim: 0.5 41 | vel_r_sat: 10.0 42 | vel_x_kd: 0.01 43 | vel_x_ki: 2.0 44 | vel_x_kp: 1.0 45 | vel_x_lim: 1.0 46 | vel_x_sat: 10 47 | vel_y_kd: 0.01 48 | vel_y_ki: 2.0 49 | vel_y_kp: 1.0 50 | vel_y_lim: 1.0 51 | vel_y_sat: 10 52 | vel_z_kd: 0.01 53 | vel_z_ki: 2.0 54 | vel_z_kp: 1.0 55 | vel_z_lim: 1.0 56 | vel_z_sat: 10 57 | -------------------------------------------------------------------------------- /bluerov2_control/config/controllers/cascade/vel_pid_control.yaml: -------------------------------------------------------------------------------- 1 | velocity_control/linear_d: 0.0 2 | velocity_control/linear_i: 2.0 3 | velocity_control/linear_p: 10.0 4 | velocity_control/linear_sat: 20.0 5 | 6 | velocity_control/angular_d: 1.0 7 | velocity_control/angular_i: 2.0 8 | velocity_control/angular_p: 10.0 9 | velocity_control/angular_sat: 5.0 10 | -------------------------------------------------------------------------------- /bluerov2_control/config/controllers/pid/params.yaml: -------------------------------------------------------------------------------- 1 | Kp: [3300, 3300, 3300, 3300, 3300, 330] 2 | Kd: [1100, 1100, 1100, 1100, 1100, 1100] 3 | Ki: [0, 0, 0, 0, 0, 0] -------------------------------------------------------------------------------- /bluerov2_control/config/inertial.yaml: -------------------------------------------------------------------------------- 1 | pid: 2 | mass: 11000.0 3 | inertial: 4 | ixx: 11000.0 5 | ixy: 0 6 | ixz: 0 7 | iyy: 11000.0 8 | iyz: 0 9 | izz: 11000.0 10 | -------------------------------------------------------------------------------- /bluerov2_control/config/teleop/f310_command_mappings.yaml: -------------------------------------------------------------------------------- 1 | mappings: 2 | depth_hold: 2 3 | switch: 0 4 | manual: 1 5 | stabilize: 3 6 | mount_dec: 4 7 | mount_inc: 5 8 | shift: 10 9 | 10 | disarm: 6 11 | arm: 7 12 | mount_center: 9 13 | 14 | 15 | 16 | light_dec: 12 17 | light_inc: 11 18 | gain_dec: 14 19 | gain_inc: 13 20 | -------------------------------------------------------------------------------- /bluerov2_control/config/teleop/f310_mappings.yaml: -------------------------------------------------------------------------------- 1 | mappings: 2 | axes: 3 | - axes[1] 4 | - axes[0] * -1.0 5 | - axes[4] 6 | - axes[3] 7 | - axes[2] 8 | - axes[5] 9 | 10 | 11 | buttons: 12 | - buttons[0] 13 | - buttons[1] 14 | - buttons[2] 15 | - buttons[3] 16 | - buttons[4] 17 | - buttons[5] 18 | - buttons[6] 19 | - buttons[7] 20 | - buttons[8] 21 | - buttons[9] 22 | - buttons[10] 23 | - max(axes[6],0.0) 24 | - min(axes[6],0.0) * -1.0 25 | - min(axes[7],0.0) * -1.0 26 | - max(axes[7],0.0) 27 | -------------------------------------------------------------------------------- /bluerov2_control/config/teleop/f710_command_mappings.yaml: -------------------------------------------------------------------------------- 1 | mappings: 2 | depth_hold: 2 3 | switch: 0 4 | manual: 1 5 | stabilize: 3 6 | mount_dec: 4 7 | mount_inc: 5 8 | shift: 10 9 | 10 | disarm: 6 11 | arm: 7 12 | mount_center: 9 13 | 14 | 15 | 16 | light_dec: 12 17 | light_inc: 11 18 | gain_dec: 14 19 | gain_inc: 13 20 | -------------------------------------------------------------------------------- /bluerov2_control/config/teleop/f710_mappings.yaml: -------------------------------------------------------------------------------- 1 | mappings: 2 | axes: 3 | - axes[1] 4 | - axes[0] * -1.0 5 | - axes[4] 6 | - axes[3] 7 | - axes[2] 8 | - axes[5] 9 | 10 | 11 | buttons: 12 | - buttons[0] 13 | - buttons[1] 14 | - buttons[2] 15 | - buttons[3] 16 | - buttons[4] 17 | - buttons[5] 18 | - buttons[6] 19 | - buttons[7] 20 | - buttons[8] 21 | - buttons[9] 22 | - buttons[10] 23 | - max(axes[6],0.0) 24 | - min(axes[6],0.0) * -1.0 25 | - min(axes[7],0.0) * -1.0 26 | - max(axes[7],0.0) 27 | -------------------------------------------------------------------------------- /bluerov2_control/config/teleop/key_joy_command_mappings.yaml: -------------------------------------------------------------------------------- 1 | mappings: 2 | arm: 0 3 | disarm: 1 4 | depth_hold: 3 5 | manual: 2 6 | stabilize: 4 7 | light_inc: -1 8 | light_dec: -1 9 | gain_inc: -1 10 | gain_dec: -1 11 | mount_center: -1 12 | mount_inc: -1 13 | mount_dec: -1 14 | shift: -1 15 | input_hold: -1 16 | 17 | # arm: 512 (9) 18 | # disarm: 256 (8) 19 | # gain_inc 14 20 | # gain_dec 15 21 | # manual 2 22 | # alt_hold 0 23 | # stabilize 3 24 | -------------------------------------------------------------------------------- /bluerov2_control/config/teleop/key_joy_mappings.yaml: -------------------------------------------------------------------------------- 1 | mappings: 2 | axes: 3 | - axes[0] 4 | - axes[1] * -1.0 5 | - axes[2] * 0.5 + 0.5 6 | - axes[3] * -1.0 7 | buttons: 8 | - buttons[0] 9 | - buttons[1] 10 | - buttons[2] 11 | - buttons[3] 12 | - buttons[4] 13 | -------------------------------------------------------------------------------- /bluerov2_control/config/teleop/switchpro_command_mappings.yaml: -------------------------------------------------------------------------------- 1 | mappings: 2 | depth_hold: 0 3 | switch: 1 4 | manual: 2 5 | stabilize: 3 6 | mount_dec: 4 7 | mount_inc: 5 8 | shift: 6 9 | 10 | disarm: 8 11 | arm: 9 12 | mount_center: 10 13 | 14 | 15 | 16 | light_dec: 15 17 | light_inc: 14 18 | gain_dec: 17 19 | gain_inc: 16 20 | 21 | # arm: 512 (9) 22 | # disarm: 256 (8) 23 | # gain_inc 14 24 | # gain_dec 15 25 | # manual 2 26 | # alt_hold 0 27 | # stabilize 3 28 | -------------------------------------------------------------------------------- /bluerov2_control/config/teleop/switchpro_mappings.yaml: -------------------------------------------------------------------------------- 1 | mappings: 2 | axes: 3 | - axes[1] 4 | - axes[0] * -1.0 5 | # - axes[3] * 0.5 + 0.5 6 | - axes[3] 7 | - axes[2] * -1.0 8 | 9 | buttons: 10 | - buttons[0] 11 | - buttons[1] 12 | - buttons[2] 13 | - buttons[3] 14 | - buttons[4] 15 | - buttons[5] 16 | - buttons[6] 17 | - buttons[7] 18 | - buttons[8] 19 | - buttons[9] 20 | - buttons[10] 21 | - buttons[11] 22 | - buttons[12] 23 | - buttons[13] 24 | - max(axes[4],0.0) 25 | - min(axes[4],0.0) * -1.0 26 | - min(axes[5],0.0) * -1.0 27 | - max(axes[5],0.0) 28 | -------------------------------------------------------------------------------- /bluerov2_control/config/thruster_manager.yaml: -------------------------------------------------------------------------------- 1 | thruster_manager: 2 | tf_prefix: bluerov2 3 | base_link: base_link 4 | thruster_topic_prefix: thrusters/ 5 | thruster_topic_suffix: /input 6 | thruster_frame_base: thruster_ 7 | max_thrust: 1540.0 8 | timeout: -1 9 | update_rate: 10 10 | ################################################## 11 | # Options to set the thruster models below 12 | ################################################## 13 | 14 | # 1) If all thrusters have the same model (as described in the vehicle's robot description) 15 | # 16 | # 1.1) If the conversion function set for the thruster plugins is the following: 17 | # 18 | # Basic 19 | # 0.0 20 | # 21 | # You can set the conversion function to be: 22 | conversion_fcn: proportional 23 | conversion_fcn_params: 24 | gain: 0.026546960744430276 # 25 | 26 | # 1.2) If the conversion function set for the thruster plugins is the following: 27 | # 28 | # LinearInterp 29 | # 0 1 2 3 30 | # 0 1 2 3 31 | # 32 | # You can set the conversion function to be: 33 | # conversion_fcn: custom 34 | # conversion_fcn_params: 35 | # input: [0, 1, 2, 3] 36 | # output: [0, 1, 2, 3] 37 | 38 | # 2) If the vehicle has thrusters with different models, you can list the 39 | # models descriptions. Beware to set the list where the index in list 40 | # matches the thruster ID 41 | # conversion_fcn: 42 | # - proportional 43 | # - proportional 44 | # - custom 45 | # conversion_fcn_params: 46 | # - gain: rotorConstant0 47 | # - gain: rotorConstant1 48 | # - input: [0, 1, 2, 3] 49 | # output: [0, 1, 2, 3] 50 | -------------------------------------------------------------------------------- /bluerov2_control/config/thrusters/calculate_polynomial_thrust.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | 4 | file = "T200-Public-Performance-Data-10-20V-September-2019.csv" 5 | 6 | data = np.genfromtxt(file, dtype=float, delimiter=",", skip_header=1) 7 | gain = np.linspace(-1000,1000, data.shape[0], dtype=float) 8 | gainz = np.linspace(0, 1000, data.shape[0], dtype=float) 9 | # map from kg-force to newton 10 | N = data[:,5] * 9.80665 11 | 12 | tau_x = 2*np.linspace(-N.max(),N.max(),data.shape[0]) 13 | tx_polys = np.polyfit(tau_x, gain, 4) 14 | ty_polys = tx_polys.copy() 15 | tz_polys = np.polyfit(tau_x, gainz, 4) 16 | arm = np.sqrt(0.14**2+0.1**2) # approximate arm length of thruster 17 | Mz = 4 * np.linspace(-N.max(), N.max(),data.shape[0]) * arm 18 | tr_polys = np.polyfit(Mz, gain, 4) 19 | -------------------------------------------------------------------------------- /bluerov2_control/config/thrusters/t200_thrust2gain_map.yaml: -------------------------------------------------------------------------------- 1 | wrench_polynomial: 2 | x: [ -1.18497740e-20, 2.70365773e-19, 1.06905241e-16, 9.71158298e+00, 3 | -1.78594037e-13 ] 4 | y: [ -1.18497740e-20, 2.70365773e-19, 1.06905241e-16, 9.71158298e+00, 5 | -1.78594037e-13 ] 6 | z: [ 6.06202871e-22, 2.29748106e-19, -1.33953168e-17, 4.85579149e+00, 7 | 5.00000000e+02 ] 8 | r: [ -5.90335882e-19, -5.78490495e-17, 5.63026679e-16, 2.82237147e+01, 9 | -1.28176920e-13 ] -------------------------------------------------------------------------------- /bluerov2_control/launch/joy_velocity.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 17 | 18 | 19 | 20 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /bluerov2_control/launch/joystick_teleop.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /bluerov2_control/launch/keyboard_teleop.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /bluerov2_control/launch/los_controller.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /bluerov2_control/launch/position_hold.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 12 | 13 | 14 | 15 | 17 | 18 | 19 | 20 | 21 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /bluerov2_control/launch/rov_pid_controller.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 17 | 18 | 19 | 20 | 21 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 47 | 48 | 49 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | inertial_frame_id: world_ned 77 | 78 | 79 | 80 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | inertial_frame_id: world 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | saturation: $(arg saturation) 119 | Kp: [$(arg Kp)] 120 | Kd: [$(arg Kd)] 121 | Ki: [$(arg Ki)] 122 | inertial_frame_id: world_ned 123 | 124 | 125 | 126 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | saturation: $(arg saturation) 142 | Kp: [$(arg Kp)] 143 | Kd: [$(arg Kd)] 144 | Ki: [$(arg Ki)] 145 | inertial_frame_id: world 146 | 147 | 148 | 149 | 150 | 151 | 152 | -------------------------------------------------------------------------------- /bluerov2_control/launch/start_thruster_manager.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 17 | 18 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | -------------------------------------------------------------------------------- /bluerov2_control/launch/switch_pro_teleop.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /bluerov2_control/msg/Autopilot.msg: -------------------------------------------------------------------------------- 1 | uint8 DEPTH=0 2 | uint8 ALT=1 3 | uint8 NONE=2 4 | Header header 5 | float64 U # forward speed (m/s) 6 | float64 Z # vertical distance (m) 7 | float64 psi # Heading (ENU) (radians) 8 | uint8 reference # vertical distance reference type 9 | -------------------------------------------------------------------------------- /bluerov2_control/msg/ControlMode.msg: -------------------------------------------------------------------------------- 1 | uint8 OFF=0 2 | uint8 IDLE=1 3 | uint8 ACCELTELEOP=2 4 | uint8 VELTELEOP=3 5 | uint8 HOLDPOSITION=4 6 | uint8 AUTOPILOT=5 7 | uint8 LOSGUIDANCE=6 8 | uint8 ABORT=7 9 | 10 | uint8 mode -------------------------------------------------------------------------------- /bluerov2_control/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | bluerov2_control 4 | 0.0.0 5 | The bluerov2_control package 6 | 7 | Ingeniarius 8 | ISR-UC 9 | 10 | Apache-2.0 11 | 12 | catkin 13 | 14 | catkin 15 | diagnostic_msgs 16 | geometry_msgs 17 | mavros 18 | mavros_msgs 19 | nav_msgs 20 | rospy 21 | roscpp 22 | sensor_msgs 23 | std_msgs 24 | geographic_msgs 25 | message_generation 26 | geodesy 27 | dynamic_reconfigure 28 | uuv_control_msgs 29 | actionlib_msgs 30 | diagnostic_msgs 31 | geometry_msgs 32 | mavros 33 | mavros_msgs 34 | nav_msgs 35 | rospy 36 | roscpp 37 | sensor_msgs 38 | std_msgs 39 | geographic_msgs 40 | geodesy 41 | dynamic_reconfigure 42 | uuv_control_msgs 43 | actionlib_msgs 44 | diagnostic_msgs 45 | geometry_msgs 46 | mavros 47 | mavros_msgs 48 | nav_msgs 49 | rospy 50 | roscpp 51 | sensor_msgs 52 | std_msgs 53 | geographic_msgs 54 | message_runtime 55 | geodesy 56 | dynamic_reconfigure 57 | uuv_control_msgs 58 | actionlib_msgs 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | -------------------------------------------------------------------------------- /bluerov2_control/scripts/geodetic_to_local.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | 5 | from geodesy import utm 6 | from bluerov2_control.srv import ConvertGeoPoints, ConvertGeoPointsResponse 7 | import rospy 8 | 9 | """This Node Simply Provides a Service to Convert Geographical Points (Latitude, Longitude, Altitude) into UTM 10 | Points. """ 11 | 12 | 13 | def convert_points(req): 14 | utms = [utm.fromMsg(msg) for msg in req.geopoints] 15 | res = ConvertGeoPointsResponse() 16 | res.utmpoints = [point.toPoint() for point in utms] 17 | return res 18 | 19 | 20 | if __name__=="__main__": 21 | rospy.init_node("geodesy_to_local") 22 | serv = rospy.Service("convert_points", ConvertGeoPoints, convert_points) 23 | serv.spin() -------------------------------------------------------------------------------- /bluerov2_control/scripts/import_iver_mis.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | from uuv_control_msgs.srv import InitWaypointSet, InitWaypointSetRequest, InitWaypointSetResponse 5 | from uuv_control_msgs.msg import Waypoint 6 | from bluerov2_control.srv import ConvertGeoPointsRequest, ConvertGeoPoints 7 | from geometry_msgs.msg import PointStamped 8 | from std_msgs.msg import Header 9 | from geographic_msgs.msg import GeoPoint 10 | from std_msgs.msg import String, Time 11 | from tf2_ros import Buffer, TransformListener 12 | import sys 13 | import os 14 | import re 15 | import tf2_geometry_msgs # this is a necessary import, don't be fooled 16 | 17 | 18 | def parse_iver_format(string): 19 | values = re.split(', | ',string) 20 | return values 21 | 22 | 23 | def parse_request(mis, utm_frame_id="utm", radius_of_acceptance=5.0): 24 | with open(mis, "r") as f: 25 | while not f.readline().startswith("START"): 26 | pass 27 | geopoints = [] 28 | headings = [] 29 | speeds = [] 30 | line = f.readline().rstrip() 31 | while not line.startswith("END"): 32 | values = line.split(";") 33 | values = [value.strip() for value in values] 34 | params = parse_iver_format(values[5]) 35 | zvar = -float(params[0][1:])*0.3048 if params[0][0] == 'D' else float(params[0][1:])*0.3048 36 | geopoints.append(GeoPoint(float(values[1]),float(values[2]), zvar)) 37 | headings.append(float(values[4])) 38 | speeds.append(float(params[-1][1:])*0.5144) 39 | line = f.readline().rstrip() 40 | # while not f.readline().startswith("START VEHICLE"): 41 | # pass 42 | # line = f.readline().rstrip() 43 | # while not line.startswith("END VEHICLE"): 44 | # if line.startswith("UVC=WPRadius"): 45 | # roas.append(float(line.split("=")[-1])) 46 | # break 47 | 48 | req = ConvertGeoPointsRequest() 49 | req.geopoints = geopoints 50 | p = rospy.ServiceProxy("convert_points", ConvertGeoPoints) 51 | p.wait_for_service() 52 | res = p(req) 53 | wps = [] 54 | for point, heading, speed in zip(res.utmpoints, headings, speeds): 55 | wp = Waypoint() 56 | wp.point.x = point.x 57 | wp.point.y = point.y 58 | wp.point.z = point.z 59 | wp.header.stamp = rospy.Time.now() 60 | wp.header.frame_id = utm_frame_id 61 | wp.radius_of_acceptance = radius_of_acceptance 62 | wp.max_forward_speed = speed 63 | wp.use_fixed_heading = False 64 | wp.heading_offset = 0.0 65 | wps.append(wp) 66 | req = InitWaypointSetRequest() 67 | req.start_time = Time(rospy.Time.from_sec(0)) 68 | req.start_now = True 69 | req.waypoints = wps 70 | req.max_forward_speed = max(speeds) 71 | req.interpolator = String("lipb") 72 | req.heading_offset = 0 73 | req.max_forward_speed = max(speeds) 74 | req.waypoints = wps 75 | return req 76 | 77 | if __name__=="__main__": 78 | rospy.init_node("set_iver_waypoints") 79 | if len(sys.argv) < 2: 80 | rospy.logerr('Invalid number of parameters\nusage: ' 81 | './import_iver_mis.py ' 82 | 'iver_mission.mis (REQUIRED) ' 83 | '__ns:="name" (OPTIONAL) ' 84 | '_radius:="radius_of_acceptance (m)" (OPTIONAL) ' 85 | '_frame_id:="frame name of utm" (OPTIONAL)') 86 | sys.exit(0) 87 | else: 88 | mission_filename = sys.argv[1] if sys.argv[1].endswith(".mis") else sys.argv[1]+".mis" 89 | if not os.path.exists(mission_filename): 90 | rospy.logerr("File not found: {}".format(mission_filename)) 91 | else: 92 | rospy.loginfo("Parsing Iver Mission File: {}".format(mission_filename)) 93 | roa = rospy.get_param("~radius", 5.0) 94 | frame_id = rospy.get_param("~utm_frame_id", "utm") 95 | req = parse_request(mission_filename, utm_frame_id=frame_id, radius_of_acceptance=roa) 96 | proxy = rospy.ServiceProxy("load_waypoints", InitWaypointSet) 97 | try: 98 | proxy.wait_for_service(timeout=1.0) 99 | res = proxy(req) 100 | if res.success: 101 | rospy.loginfo("IVER Waypoints Set, Executing.") 102 | else: 103 | rospy.logerr("IVER Waypoints Error.") 104 | except Exception as e: 105 | rospy.logerr("{} | {}".format(rospy.get_name(), e.message)) 106 | -------------------------------------------------------------------------------- /bluerov2_control/scripts/keyboard_joy.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | # 4 | # Copyright (c) 2013 PAL Robotics SL. 5 | # Released under the BSD License. 6 | # 7 | # Authors: 8 | # * Siegfried-A. Gevatter 9 | 10 | import curses 11 | 12 | import rospy 13 | from sensor_msgs.msg import Joy 14 | 15 | 16 | class TextWindow(): 17 | 18 | _screen = None 19 | _window = None 20 | _num_lines = None 21 | 22 | def __init__(self, stdscr, lines=10): 23 | self._screen = stdscr 24 | self._screen.nodelay(True) 25 | curses.curs_set(0) 26 | 27 | self._num_lines = lines 28 | 29 | def read_key(self): 30 | keycode = self._screen.getch() 31 | return keycode if keycode != -1 else None 32 | 33 | def clear(self): 34 | self._screen.clear() 35 | 36 | def write_line(self, lineno, message): 37 | if lineno < 0 or lineno >= self._num_lines: 38 | raise ValueError('lineno out of bounds') 39 | height, width = self._screen.getmaxyx() 40 | y = int((height / self._num_lines) * lineno) 41 | x = 10 42 | for text in message.split('\n'): 43 | text = text.ljust(width) 44 | self._screen.addstr(y, x, text) 45 | y += 1 46 | 47 | def refresh(self): 48 | self._screen.refresh() 49 | 50 | def beep(self): 51 | curses.flash() 52 | 53 | 54 | class SimpleKeyTeleop(): 55 | def __init__(self, interface): 56 | rospy.init_node('key_teleop') 57 | self._interface = interface 58 | self._pub_cmd = rospy.Publisher('joy', Joy) 59 | self._hz = rospy.get_param('~hz', 10) 60 | self._last_pressed = {} 61 | self._gain = rospy.get_param('~gain', 0.25) 62 | self._axes_bindings = {ord('w'): (0, 1.0), 63 | ord('s'): (0, -1.0), 64 | ord('a'): (1, 1.0), 65 | ord('d'): (1, -1.0), 66 | ord(' '): (2, 1.0), 67 | ord('c'): (2, -1.0), 68 | curses.KEY_LEFT: (3, 1.0), 69 | curses.KEY_RIGHT: (3, -1.0)} 70 | self._button_bindings = {ord('1'): 0, 71 | ord('2'): 1, 72 | ord('3'): 2, 73 | ord('4'): 3, 74 | ord('5'): 4} 75 | self._axes = [0.0]*4 76 | self._buttons = [0]*5 77 | 78 | def run(self): 79 | rate = rospy.Rate(self._hz) 80 | self._running = True 81 | while self._running: 82 | while True: 83 | keycode = self._interface.read_key() 84 | if keycode is None: 85 | break 86 | self._key_pressed(keycode) 87 | self._set_joy() 88 | self._publish() 89 | rate.sleep() 90 | 91 | def _set_joy(self): 92 | stamp = rospy.Time.now() 93 | keys = [] 94 | self._buttons = [0]*5 95 | for a in self._last_pressed: 96 | if (stamp - self._last_pressed[a]).to_sec() < 0.05: 97 | keys.append(a) 98 | for k in keys: 99 | if k in self._axes_bindings: 100 | i, gain = self._axes_bindings[k] 101 | self._axes[i] = self._axes[i] + gain*self._gain 102 | self._axes[i] = 0.0 if abs(self._axes[i]) < self._gain else self._axes[i] 103 | else: 104 | self._buttons[self._button_bindings[k]] = 1 105 | self._axes = [min(1.0, a) for a in self._axes] 106 | self._axes = [max(-1.0, a) for a in self._axes] 107 | 108 | def _get_joy(self): 109 | joy = Joy() 110 | joy.header.frame_id = 'keyboard' 111 | joy.header.stamp = rospy.Time.now() 112 | joy.axes = self._axes 113 | joy.buttons = self._buttons 114 | return joy 115 | 116 | def _key_pressed(self, keycode): 117 | if keycode == ord('q'): 118 | self._running = False 119 | rospy.signal_shutdown('Bye') 120 | elif keycode in self._axes_bindings: 121 | self._last_pressed[keycode] = rospy.Time.now() 122 | elif keycode in self._button_bindings: 123 | self._last_pressed[keycode] = rospy.Time.now() 124 | 125 | def _publish(self): 126 | self._interface.clear() 127 | self._interface.write_line(2, 'Surge: {}, Sway: {}, Heave: {}, Yaw: {}'.format(*self._axes)) 128 | self._interface.write_line(5, 'X +/-: w/s, Y +/-: a/d, Z +/-: space/c, PSI +/-: LEFT/RIGHT') 129 | self._interface.write_line(7, 'Arm: 1, Disarm: 2, Manual: 3, Depth: 4, Stabilize: 5') 130 | self._interface.refresh() 131 | joy = self._get_joy() 132 | self._pub_cmd.publish(joy) 133 | 134 | 135 | def main(stdscr): 136 | app = SimpleKeyTeleop(TextWindow(stdscr)) 137 | app.run() 138 | 139 | if __name__ == '__main__': 140 | try: 141 | curses.wrapper(main) 142 | rospy.spin() 143 | except rospy.ROSInterruptException: 144 | pass -------------------------------------------------------------------------------- /bluerov2_control/scripts/sitl_model_state_publisher.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | """ 3 | 4 | From bluerov_ros_playground respository (https://github.com/patrickelectric/bluerov_ros_playground) 5 | Credits: patrickelectric 6 | 7 | """ 8 | 9 | import rospy 10 | from gazebo_msgs.msg import ModelState 11 | from mavros_msgs.srv import CommandBool, SetMode 12 | from geometry_msgs.msg import PoseStamped 13 | from tf import transformations 14 | 15 | 16 | class SITL(object): 17 | """Class to handle with SITL 18 | 19 | Attributes: 20 | pub (TYPE): ROS publisher 21 | sub (TYPE): ROS subscriber 22 | """ 23 | 24 | def __init__(self): 25 | super(SITL, self).__init__() 26 | self._pose = PoseStamped() 27 | 28 | rospy.Subscriber('mavros/local_position/pose', PoseStamped, self._pose_callback) 29 | self._pub = rospy.Publisher('/gazebo/set_model_state', ModelState) 30 | 31 | def _pose_callback(self, msg): 32 | self._pose = msg 33 | 34 | def run(self): 35 | """ Send SITL information to gazebo: Pose from Pixhawk 36 | """ 37 | rate = rospy.Rate(100) 38 | while not rospy.is_shutdown(): 39 | 40 | # Get ROV position and send it to gazebo 41 | try: 42 | 43 | model_state = ModelState() 44 | model_state.model_name = 'bluerov2' 45 | # Set position 46 | model_state.pose.position.x = self._pose.pose.position.x 47 | model_state.pose.position.y = self._pose.pose.position.y 48 | model_state.pose.position.z = self._pose.pose.position.z 49 | 50 | Q = [q for q in self._pose.pose.orientation.x, self._pose.pose.orientation.y, 51 | self._pose.pose.orientation.z, self._pose.pose.orientation.w] 52 | q1 = transformations.quaternion_from_euler(0,0,0,'sxyz') 53 | q = transformations.quaternion_multiply(Q, q1) 54 | 55 | model_state.pose.orientation.x = q[0] 56 | model_state.pose.orientation.y = q[1] 57 | model_state.pose.orientation.z = q[2] 58 | model_state.pose.orientation.w = q[3] 59 | 60 | self._pub.publish(model_state) 61 | except Exception as error: 62 | rospy.logerr('{}: Get data error {}'.format(rospy.get_name(), error)) 63 | 64 | 65 | if __name__ == "__main__": 66 | try: 67 | rospy.init_node('bluerov_sitl', log_level=rospy.INFO) 68 | except rospy.ROSInterruptException as error: 69 | rospy.logerr('{}: pubs error with ROS {}'.format(rospy.get_name(), error)) 70 | exit(1) 71 | bluerov_sitl = SITL() 72 | bluerov_sitl.run() 73 | -------------------------------------------------------------------------------- /bluerov2_control/scripts/sitl_teleop_manual_control.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | 5 | from sensor_msgs.msg import Joy 6 | from mavros_msgs.msg import ManualControl 7 | 8 | 9 | class SITLTeleop(object): 10 | 11 | """Class to handle with gazebo teleop 12 | Attributes: 13 | pub (TYPE): ROS publisher 14 | sub (TYPE): ROS subscriber 15 | """ 16 | 17 | def __init__(self): 18 | super(SITLTeleop, self).__init__() 19 | self.btn_remap = self.load_mappings() 20 | 21 | self.man = ManualControl() 22 | self.man.z = 500.0 23 | self.pub = rospy.Publisher("mavros/manual_control/send", ManualControl, queue_size=10) 24 | 25 | rospy.Subscriber('joy', Joy, self.joy_callback) 26 | 27 | def load_mappings(self): 28 | return rospy.get_param("~mappings", {}) 29 | 30 | def bool2int(self, bools): 31 | return 32 | 33 | def joy_callback(self, msg): 34 | self.man.header.frame_id = msg.header.frame_id 35 | self.man.x = msg.axes[0] * 1000.0 36 | self.man.y = msg.axes[1] * 1000.0 37 | self.man.z = msg.axes[2] * 1000.0 38 | self.man.r = msg.axes[3] * 1000.0 39 | btns = msg.buttons 40 | # booleans in LSB order, taken from default joystick mappings for ArduSub 41 | bools = [bool(i) for i in [btns[self.btn_remap['depth_hold']] if self.btn_remap['depth_hold'] >= 0 else 0, 42 | 0, 43 | btns[self.btn_remap['manual']] if self.btn_remap['manual'] >= 0 else 0, 44 | btns[self.btn_remap['stabilize']] if self.btn_remap['stabilize'] >= 0 else 0, 45 | btns[self.btn_remap['mount_dec']] if self.btn_remap['mount_dec'] >= 0 else 0, 46 | btns[self.btn_remap['mount_inc']] if self.btn_remap['mount_inc'] >= 0 else 0, 47 | 0, 48 | 0, 49 | btns[self.btn_remap['disarm']] if self.btn_remap['disarm'] >= 0 else 0, 50 | btns[self.btn_remap['arm']] if self.btn_remap['arm'] >= 0 else 0, 51 | btns[self.btn_remap['mount_center']] if self.btn_remap['mount_center'] >= 0 else 0, 52 | 0, 53 | btns[self.btn_remap['light_inc']] if self.btn_remap['light_inc'] >= 0 else 0, 54 | btns[self.btn_remap['light_dec']] if self.btn_remap['light_dec'] >= 0 else 0, 55 | btns[self.btn_remap['gain_inc']] if self.btn_remap['gain_inc'] >= 0 else 0, 56 | btns[self.btn_remap['gain_dec']] if self.btn_remap['gain_dec'] >= 0 else 0, 57 | ]] 58 | self.man.buttons = int(''.join(str(int(i)) for i in reversed(bools)), 2) 59 | rospy.logdebug('{} manual: {}'.format(rospy.get_name(), int(''.join(str(int(i)) for i in reversed(bools)), 2))) 60 | 61 | def run(self): 62 | """ Run Gazebo Teleop 63 | """ 64 | hz = rospy.Rate(10) 65 | while not rospy.is_shutdown(): 66 | try: 67 | self.man.header.stamp = rospy.Time.now() 68 | self.pub.publish(self.man) 69 | hz.sleep() 70 | except Exception as error: 71 | rospy.logerr('{} rc error: {}'.format(rospy.get_name(), error)) 72 | 73 | 74 | if __name__ == "__main__": 75 | try: 76 | rospy.init_node('sitl_teleop') 77 | except rospy.ROSInterruptException: 78 | exit(1) 79 | sitl_teleop = SITLTeleop() 80 | sitl_teleop.run() -------------------------------------------------------------------------------- /bluerov2_control/scripts/sitl_teleop_rc_override.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | 5 | from sensor_msgs.msg import Joy 6 | from mavros_msgs.msg import OverrideRCIn 7 | 8 | 9 | class SITLTeleop(object): 10 | 11 | """Class to handle with gazebo teleop 12 | Attributes: 13 | pub (TYPE): ROS publisher 14 | sub (TYPE): ROS subscriber 15 | """ 16 | 17 | def __init__(self): 18 | super(SITLTeleop, self).__init__() 19 | 20 | self.pwms = [1500,1500,1500,1500,1500,1500,1500,1100] 21 | self.dirs = [-1.0,1.0,-1.0,1.0,1.0,1.0] 22 | self.pub = rospy.Publisher('/mavros/rc/override', OverrideRCIn, queue_size=10) 23 | rospy.Subscriber('/joy',Joy,self.joy_callback) 24 | 25 | def joy_callback(self, msg): 26 | gains = msg.axes 27 | self.pwms = [sgn*gain*(1900-1100)+1500 for sgn, gain in zip(self.dirs, gains)] 28 | 29 | def run(self): 30 | """ Run Gazebo Teleop 31 | """ 32 | hz = rospy.Rate(5) 33 | while not rospy.is_shutdown(): 34 | # Try to get data 35 | try: 36 | # Get joystick data and send it to Gazebo model 37 | msg = OverrideRCIn() 38 | # [roll, pitch, heave, yaw, surge, sway, camera, lights] 39 | msg.channels = [1500,1500,self.pwms[3],self.pwms[2],self.pwms[1],self.pwms[0],1500,1100] 40 | 41 | self.pub.publish(msg) 42 | except Exception as error: 43 | rospy.logerr('{}: rc error {}'.format(rospy.get_name(), error)) 44 | 45 | 46 | if __name__ == "__main__": 47 | try: 48 | rospy.init_node('sitl_teleop') 49 | except rospy.ROSInterruptException as error: 50 | rospy.logerr('{} pubs error with ROS {}'.format(rospy.get_name(), error)) 51 | exit(1) 52 | sitl_teleop = SITLTeleop() 53 | sitl_teleop.run() -------------------------------------------------------------------------------- /bluerov2_control/scripts/trajectory_visualizer.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | 4 | import rospy 5 | from nav_msgs.msg import Path, Odometry 6 | from geometry_msgs.msg import PoseStamped, Pose, Quaternion, Vector3 7 | from visualization_msgs.msg import Marker, MarkerArray 8 | from uuv_control_msgs.msg import WaypointSet 9 | from std_msgs.msg import Header, ColorRGBA 10 | from collections import deque 11 | from bluerov2_control.cfg import marker_reconfigConfig 12 | from dynamic_reconfigure.server import Server 13 | 14 | 15 | class MarkerVisualization(object): 16 | def __init__(self): 17 | self._path_update_period = rospy.Duration.from_sec(rospy.get_param("~update_period", 1.0)) 18 | self._path_buffer_len = rospy.get_param("~buffer_len", 100) 19 | self._path = Path(Header(0, rospy.Time.now(), ""), deque(maxlen=self._path_buffer_len)) 20 | self._path_pub = rospy.Publisher("history", Path, queue_size=1) 21 | self._markers = MarkerArray() 22 | self._marker_pub = rospy.Publisher("waypoint_markers", MarkerArray, queue_size=1) 23 | self._dr_serv = Server(marker_reconfigConfig, self._reconfig) 24 | self._scale = Vector3(1,1,1) 25 | rospy.Subscriber("odometry", Odometry, self._update_vehicle_path) 26 | rospy.Subscriber("current_waypoints", WaypointSet, self._update_marker_list) 27 | 28 | def _reconfig(self, config, level): 29 | self._scale = Vector3(config["scale_x"], 30 | config["scale_y"], 31 | config["scale_z"]) 32 | self._colors = ColorRGBA(config["color_r"], 33 | config["color_g"], 34 | config["color_b"], 35 | config["color_a"]) 36 | self._modify_marker_list() 37 | return config 38 | 39 | def _modify_marker_list(self): 40 | for i, m in enumerate(self._markers.markers): 41 | m.action = m.MODIFY 42 | m.scale = self._scale 43 | m.color = self._colors 44 | self._send_markers() 45 | 46 | def _reset_markers(self): 47 | for m in self._markers.markers: 48 | m = Marker() 49 | m.action = m.DELETE 50 | self._send_markers() 51 | 52 | def _update_marker_list(self, msg): 53 | markers = MarkerArray() 54 | for i, wp in enumerate(msg.waypoints): 55 | m = Marker() 56 | m.header = wp.header 57 | m.pose = Pose(wp.point, Quaternion(0, 0, 0, 1)) 58 | m.type = m.SPHERE 59 | m.action = m.ADD 60 | m.id = i 61 | m.scale = self._scale 62 | m.color = self._colors 63 | markers.markers.append(m) 64 | self._markers = markers 65 | self._send_markers() 66 | 67 | def _send_markers(self): 68 | self._marker_pub.publish(self._markers) 69 | 70 | def _update_vehicle_path(self, msg): 71 | if (msg.header.stamp - self._path.header.stamp) > self._path_update_period: 72 | self._path.poses.append(PoseStamped(msg.header, msg.pose.pose)) 73 | self._path.header = msg.header 74 | self._path_pub.publish(self._path) 75 | 76 | 77 | if __name__=="__main__": 78 | rospy.init_node("marker_manager_node") 79 | o = MarkerVisualization() 80 | rospy.spin() 81 | -------------------------------------------------------------------------------- /bluerov2_control/scripts/utm_to_world.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | 4 | import rospy 5 | from tf2_ros import StaticTransformBroadcaster 6 | from geometry_msgs.msg import TransformStamped 7 | from geographic_msgs.msg import GeoPoint 8 | from geodesy import utm 9 | 10 | 11 | if __name__=="__main__": 12 | rospy.init_node("utm_to_world") 13 | datum = map(float, rospy.get_param("~datum", []).strip('][').split(',')) 14 | datum = GeoPoint(*map(float, datum)) 15 | utmpoint = utm.fromMsg(datum) 16 | broadcaster = StaticTransformBroadcaster() 17 | tf = TransformStamped() 18 | tf.header.stamp = rospy.Time.now() 19 | tf.header.frame_id = "utm" 20 | tf.child_frame_id = "world" 21 | tf.transform.translation.x = utmpoint.easting 22 | tf.transform.translation.y = utmpoint.northing 23 | tf.transform.translation.z = utmpoint.altitude 24 | tf.transform.rotation.w = 1.0 25 | broadcaster.sendTransform(tf) 26 | rospy.spin() 27 | -------------------------------------------------------------------------------- /bluerov2_control/srv/ConvertGeoPoints.srv: -------------------------------------------------------------------------------- 1 | geographic_msgs/GeoPoint[] geopoints 2 | --- 3 | geometry_msgs/Point[] utmpoints 4 | -------------------------------------------------------------------------------- /bluerov2_control/srv/SetControlMode.srv: -------------------------------------------------------------------------------- 1 | bluerov2_control/ControlMode mode 2 | --- 3 | bool success 4 | -------------------------------------------------------------------------------- /bluerov2_control/waypoints/example_waypoints.yaml: -------------------------------------------------------------------------------- 1 | inertial_frame_id: world 2 | waypoints: 3 | - 4 | point: [4, 2, -5] 5 | max_forward_speed: 0.4 6 | heading: 0 7 | use_fixed_heading: False 8 | - 9 | point: [16, 12, -5] 10 | max_forward_speed: 0.4 11 | heading: 0 12 | use_fixed_heading: False 13 | - 14 | point: [20, 12, -5] 15 | max_forward_speed: 0.4 16 | heading: 0 17 | use_fixed_heading: False 18 | - 19 | point: [25, 16, -5] 20 | max_forward_speed: 0.4 21 | heading: 0 22 | use_fixed_heading: False 23 | - 24 | point: [25, 20, -5] 25 | max_forward_speed: 0.4 26 | heading: 0 27 | use_fixed_heading: False 28 | - 29 | point: [30, 40, -5] 30 | max_forward_speed: 0.4 31 | heading: 0 32 | use_fixed_heading: False 33 | - 34 | point: [36, 45, -5] 35 | max_forward_speed: 0.4 36 | heading: 0 37 | use_fixed_heading: False 38 | - 39 | point: [60, 70, -5] 40 | max_forward_speed: 0.4 41 | heading: 0 42 | use_fixed_heading: False 43 | - 44 | point: [105, 56, -5] 45 | max_forward_speed: 0.4 46 | heading: 0 47 | use_fixed_heading: False 48 | - 49 | point: [100, -20, -5] 50 | max_forward_speed: 0.4 51 | heading: 0 52 | use_fixed_heading: False 53 | - 54 | point: [100, -20, -5] 55 | max_forward_speed: 0.4 56 | heading: 0 57 | use_fixed_heading: False 58 | -------------------------------------------------------------------------------- /bluerov2_description/launch/upload.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 43 | 44 | 45 | 46 | 51 | 52 | 53 | 54 | 56 | 57 | 58 | 59 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | -------------------------------------------------------------------------------- /bluerov2_description/meshes/README.md: -------------------------------------------------------------------------------- 1 | This folder is reserved to store the meshes for the vehicle, propeller and/or fins. 2 | 3 | In the files they are named: 4 | 5 | - vehicle.stl (vehicle's collision geometry) 6 | - vehicle.dae (vehicle's visual geometry) 7 | - propeller.dae (propeller mesh) 8 | - fin.dae (fin/rudder mesh) 9 | -------------------------------------------------------------------------------- /bluerov2_description/meshes/bluerov2_noprop.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/FletcherFT/bluerov2/c416fc8530aac2b0ac52626001308d780347d639/bluerov2_description/meshes/bluerov2_noprop.stl -------------------------------------------------------------------------------- /bluerov2_description/meshes/ping1d.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/FletcherFT/bluerov2/c416fc8530aac2b0ac52626001308d780347d639/bluerov2_description/meshes/ping1d.STL -------------------------------------------------------------------------------- /bluerov2_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | bluerov2_description 4 | 0.0.0 5 | The bluerov2_description package 6 | 7 | 8 | 9 | 10 | ingeniarius 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /bluerov2_description/robots/altimeter.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 14 | 15 | 16 | 1028.0 17 | hydrodynamics/current_velocity 18 | $(arg debug) 19 | 20 | 21 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /bluerov2_description/robots/altimeter_sss.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 14 | 15 | 16 | 1028.0 17 | hydrodynamics/current_velocity 18 | $(arg debug) 19 | 20 | 21 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | -------------------------------------------------------------------------------- /bluerov2_description/robots/default.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 14 | 15 | 16 | 1028.0 17 | hydrodynamics/current_velocity 18 | $(arg debug) 19 | 20 | 21 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /bluerov2_description/urdf/actuators.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 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 | 44 | 45 | 46 | -------------------------------------------------------------------------------- /bluerov2_description/urdf/base.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 32 | 33 | 34 | 35 | 41 | 42 | 43 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 58 | 59 | 60 | 61 | 62 | 66 | 67 | 68 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | false 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | -------------------------------------------------------------------------------- /bluerov2_description/urdf/gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 0 10 | 11 | ${volume} 12 | 15 | 16 | ${x_size} 17 | ${y_size} 18 | ${z_size} 19 | 20 | 21 | ${cob} 22 | 23 | 28 | 29 | fossen 30 | 31 | 32 | 1.7182 0 0 0 0 0 33 | 0 0 0 0 0 0 34 | 0 0 5.468 0 0 0 35 | 0 0 0 0 0 0 36 | 0 0 0 0 1.2481 0 37 | 0 0 0 0 0 0.4006 38 | 39 | 44 | 45 | -11.7391 -20 -31.8678 -25 -44.9085 -5 46 | 47 | 53 | 56 | 61 | 62 | 0 0 0 0 0 0 63 | 64 | 65 | 79 | 84 | 85 | 86 | 87 | 88 | -------------------------------------------------------------------------------- /bluerov2_description/urdf/sensors.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 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 | -------------------------------------------------------------------------------- /bluerov2_executive/launch/bluerov2_executive.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /bluerov2_executive/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | bluerov2_executive 4 | 0.0.0 5 | The bluerov2_executive package 6 | 7 | 8 | 9 | 10 | fft 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | imc_ros_bridge 53 | roscpp 54 | rospy 55 | sensor_msgs 56 | imc_ros_bridge 57 | roscpp 58 | rospy 59 | sensor_msgs 60 | imc_ros_bridge 61 | roscpp 62 | rospy 63 | sensor_msgs 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | -------------------------------------------------------------------------------- /bluerov2_executive/src/imc_enums.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | # vim:fenc=utf-8 4 | # Ozer Ozkahraman (ozero@kth.se) 5 | 6 | ######################## 7 | # IMC Enums 8 | # See https://github.com/LSTS/imc/blob/master/IMC.xml 9 | ######################## 10 | STATE_BLOCKED = 0 11 | STATE_READY = 1 12 | STATE_INITIALIZING = 2 13 | STATE_EXECUTING = 3 14 | 15 | OP_MODE_SERVICE = 0 16 | OP_MODE_CALIBRATION = 1 17 | OP_MODE_ERROR = 2 18 | OP_MODE_MANEUVER = 3 19 | OP_MODE_EXTERNAL = 4 20 | OP_MODE_BOOT = 5 21 | 22 | PLANDB_TYPE_REQUEST = 0 23 | PLANDB_TYPE_SUCCESS = 1 24 | PLANDB_TYPE_FAILURE = 2 25 | PLANDB_TYPE_IN_PROGRESS = 3 26 | PLANDB_OP_SET = 0 27 | PLANDB_OP_DEL = 1 28 | PLANDB_OP_GET = 2 29 | PLANDB_OP_GET_INFO = 3 30 | PLANDB_OP_CLEAR = 4 31 | PLANDB_OP_GET_STATE = 5 32 | PLANDB_OP_GET_DSTATE = 6 33 | PLANDB_OP_BOOT = 7 34 | 35 | # TODO eventually implement other types of maneuvers 36 | # see mission_plan -> read_plandb 37 | MANEUVER_GOTO = 450 38 | MANEUVER_SAMPLE = 489 39 | 40 | 41 | # Speed units 42 | SPEED_UNIT_MPS = 0 43 | SPEED_UNIT_RPM = 1 44 | SPEED_UNIT_PERCENTAGE = 2 45 | 46 | # These are the same in smarc_msgs/GotoWaypoint.action 47 | # Z units 48 | Z_NONE = 0 49 | Z_DEPTH = 1 50 | Z_ALTITUDE = 2 51 | Z_HEIGHT = 3 52 | 53 | 54 | 55 | # a list of actions that we will consider 56 | # as the imc 'executing' state 57 | EXECUTING_ACTION_NAMES =[ 58 | 'A_GotoWaypoint', 59 | 'A_FollowLeader', 60 | 'A_SetNextPlanAction' #so we dont spam service/maneuver when going tru a lot of waypoints quickly 61 | ] 62 | # same thing for the 'blocked' state 63 | BLOCKED_ACTION_NAMES =[ 64 | 'A_EmergencySurface' 65 | ] -------------------------------------------------------------------------------- /bluerov2_gazebo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | 2 | cmake_minimum_required(VERSION 2.8.3) 3 | project(bluerov2_gazebo) 4 | 5 | find_package(catkin REQUIRED) 6 | 7 | catkin_package() 8 | 9 | install(DIRECTORY launch 10 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 11 | PATTERN "*~" EXCLUDE) 12 | -------------------------------------------------------------------------------- /bluerov2_gazebo/config/disturbances.yaml: -------------------------------------------------------------------------------- 1 | disturbances: 2 | - 3 | type: current 4 | starting_time: 5 5 | velocity: 0.5 6 | horizontal_angle: 0 7 | vertical_angle: 0 8 | duration: -1 9 | - 10 | type: wrench 11 | starting_time: 100 12 | duration: 10 13 | force: 14 | - 500 15 | - 100 16 | - 0 17 | torque: 18 | - 0 19 | - 0 20 | - 0 21 | - 22 | type: thruster_state 23 | starting_time: 60 24 | duration: 20 25 | is_on: 0 26 | thruster_id: 2 27 | - 28 | type: thrust_efficiency 29 | starting_time: 120 30 | duration: 30 31 | efficiency: 0.4 32 | thruster_id: 0 33 | - 34 | type: propeller_efficiency 35 | starting_time: 80 36 | duration: 10 37 | efficiency: 0.7 38 | thruster_id: 7 39 | -------------------------------------------------------------------------------- /bluerov2_gazebo/launch/ardusub_peberholm_world.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 26 | 27 | meshes: 28 | herkules_seabed: 29 | mesh: package://uuv_gazebo_worlds/models/herkules_seabed/meshes/herkules_seabed.dae 30 | pose: 31 | position: [0, 0, $(arg peberholm_depth)] 32 | orientation: [0.0,0,0] 33 | scale: [4, 4, 1] 34 | 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /bluerov2_gazebo/launch/herkules_ship_wreck.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 24 | 25 | meshes: 26 | herkules_seabed: 27 | mesh: package://uuv_gazebo_worlds/models/herkules_seabed/meshes/herkules_seabed.dae 28 | pose: 29 | position: [0, 0, -60] 30 | scale: [4, 4, 1] 31 | herkules_ship_wreck: 32 | mesh: package://uuv_gazebo_worlds/models/herkules_ship_wreck/meshes/herkules.dae 33 | pose: 34 | position: [0, 0, -60] 35 | orientation: [0, 0, 1.57] 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /bluerov2_gazebo/launch/peberholm_bluerov2.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /bluerov2_gazebo/launch/peberholm_world.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 26 | 27 | meshes: 28 | herkules_seabed: 29 | mesh: package://uuv_gazebo_worlds/models/herkules_seabed/meshes/herkules_seabed.dae 30 | pose: 31 | position: [0, 0, $(arg peberholm_depth)] 32 | orientation: [0.0349066,0,0] 33 | scale: [4, 4, 1] 34 | 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /bluerov2_gazebo/launch/start_accel_controller_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | -------------------------------------------------------------------------------- /bluerov2_gazebo/launch/start_ardusub_sitl_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /bluerov2_gazebo/launch/start_disturbance_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /bluerov2_gazebo/launch/start_trajectory_controller_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /bluerov2_gazebo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | bluerov2_gazebo 4 | 0.0.0 5 | Package with launch files for demonstrations with the BlueROV 2 vehicle 6 | 7 | Ingeniarius 8 | ISR-UC 9 | 10 | Apache-2.0 11 | 12 | catkin 13 | 14 | bluerov2_description 15 | bluerov2_control 16 | 17 | 18 | -------------------------------------------------------------------------------- /bluerov2_gazebo/worlds/ardusub-peberholm.world: -------------------------------------------------------------------------------- 1 | 2 | 17 | 18 | 19 | 20 | 0.01 21 | 1 22 | 100 23 | 24 | 25 | quick 26 | 50 27 | 1.2 28 | 29 | 30 | 31 | 32 | 0.01 0.01 0.01 1.0 33 | 34 | 35 | 12 36 | 37 | 38 | 1 39 | 40 | 0.1 0.2 0.3 1.0 41 | linear 42 | 0.1 43 | 10 44 | 40 45 | 46 | 47 | 48 | 51 | 52 | -55.603036 53 | -12.8089368 54 | 55 | 56 | 57 | 58 | -200 -200 100 0 0 0 59 | 0.6 0.6 0.6 1 60 | 0 0 0 1 61 | -0.3 -0.3 -1 62 | false 63 | 64 | 65 | 66 | -100 50 200 0 0 0 67 | 0.6 0.6 0.6 1 68 | 0 0 0 1 69 | -0.3 -0.3 -1 70 | false 71 | 72 | 73 | 74 | -150 -130 50 0 0 0 75 | 0.6 0.6 0.6 1 76 | 0.2 0.2 0.2 1 77 | 0.5 0.5 -1 78 | false 79 | 80 | 81 | 82 | 83 | model://herkules_seabed 84 | 0 0 8 0 0 0 85 | 86 | 119 | 120 | 121 | -25 25 -5 0 0 -0.8 122 | orbit 123 | perspective 124 | 125 | 126 | 127 | 128 | -------------------------------------------------------------------------------- /bluerov2_gazebo/worlds/peberholm.world: -------------------------------------------------------------------------------- 1 | 2 | 17 | 18 | 19 | 20 | 0.01 21 | 1 22 | 100 23 | 24 | 25 | quick 26 | 50 27 | 1.2 28 | 29 | 30 | 31 | 32 | 0.01 0.01 0.01 1.0 33 | 34 | 35 | 12 36 | 37 | 38 | 1 39 | 40 | 0.1 0.2 0.3 1.0 41 | linear 42 | 0.1 43 | 10 44 | 40 45 | 46 | 47 | 48 | 51 | 52 | -55.603036 53 | -12.8089368 54 | 55 | 56 | 57 | 58 | -200 -200 100 0 0 0 59 | 0.6 0.6 0.6 1 60 | 0 0 0 1 61 | -0.3 -0.3 -1 62 | true 63 | 64 | 65 | 66 | -100 50 200 0 0 0 67 | 0.6 0.6 0.6 1 68 | 0 0 0 1 69 | -0.3 -0.3 -1 70 | true 71 | 72 | 73 | 74 | -150 -130 50 0 0 0 75 | 0.6 0.6 0.6 1 76 | 0.2 0.2 0.2 1 77 | 0.5 0.5 -1 78 | true 79 | 80 | 81 | 82 | 83 | model://herkules_seabed 84 | 0 0 54 0.0349066 0 0 85 | 86 | 87 | 88 | hydrodynamics 89 | 90 | current_velocity 91 | 92 | 0 93 | 0 94 | 5 95 | 0.0 96 | 0.0 97 | 98 | 99 | 100 | 0 101 | -3.141592653589793238 102 | 3.141592653589793238 103 | 0.0 104 | 0.0 105 | 106 | 107 | 108 | 0 109 | -3.141592653589793238 110 | 3.141592653589793238 111 | 0.0 112 | 0.0 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | -25 25 -5 0 0 -0.8 122 | orbit 123 | perspective 124 | 125 | 126 | 127 | 128 | -------------------------------------------------------------------------------- /bluerov2_neptus/launch/neptus.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /bluerov2_neptus/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | bluerov2_neptus 4 | 0.0.0 5 | The bluerov2_neptus package 6 | 7 | 8 | 9 | 10 | fft 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /bluerov2_neptus/vehicle-defs/00-bluerov2-1.nvcl: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 00-bluerov2-1 5 | 00-bluerov2-1 6 | ROV 7 | 8 | 0.457 9 | 0.338 10 | 0.254 11 | ../vehicles-files/adamastor/adamastor_top.png 12 | ../vehicles-files/adamastor/adamastor_side.png 13 | ../vehicles-files/adamastor/adamastor_presentation.png 14 | ../vehicles-files/adamastor/adamastor.j3d 15 | 16 | 222 17 | 201 18 | 95 19 | 20 | 21 | 22 | altitude, depth 23 | 24 | SNAME 25 | 26 | 27 | 28 | 29 | 30 | No documentation available 31 | pt.lsts.neptus.mp.maneuvers.Unconstrained 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | GotoDestination 40 | GotoDestination 41 | 42 | 0N 43 | 0E 44 | 0.0 45 | 46 | 47 | 5 48 | 49 | 2 50 | 51 | 5 52 | 53 | 54 | Move to a given point 55 | pt.lsts.neptus.mp.maneuvers.Goto 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 0N 66 | 0E 67 | 0.0 68 | 69 | 70 | 0.0 71 | 72 | 100.0 73 | 200.0 74 | 0.0 75 | 27.0 76 | 0.0 77 | 0.0 78 | 1.0 79 | 80 | No documentation available 81 | pt.lsts.neptus.mp.maneuvers.RowsManeuver 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 0N 92 | 0E 93 | 0.0 94 | 95 | 96 | 0.0 97 | 98 | 100.0 99 | 20.0 100 | 0.0 101 | false 102 | 1.0 103 | 104 | No documentation available 105 | pt.lsts.neptus.mp.maneuvers.CrossHatchPattern 106 | 107 | 108 | 109 | 110 | 111 | 112 | ethernet 113 | ethernet 114 | 10.0.10.120 115 | imc 116 | 117 | 118 | 119 | 6002 120 | 6002 121 | true 122 | true 123 | 00:08 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | ../conf/consoles/lauv.ncon 133 | 134 | 135 | 136 | -------------------------------------------------------------------------------- /bluerov2_state_estimation/launch/ekf.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /bluerov2_state_estimation/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | bluerov2_state_estimation 4 | 0.0.0 5 | The bluerov2_state_estimation package 6 | 7 | 8 | 9 | 10 | fft 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | geodesy 53 | geographic_msgs 54 | geometry_msgs 55 | robot_localization 56 | rospy 57 | sensor_msgs 58 | waterlinked_gps_msgs 59 | geodesy 60 | geographic_msgs 61 | geometry_msgs 62 | robot_localization 63 | rospy 64 | sensor_msgs 65 | waterlinked_gps_msgs 66 | geodesy 67 | geographic_msgs 68 | geometry_msgs 69 | robot_localization 70 | rospy 71 | sensor_msgs 72 | waterlinked_gps_msgs 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | -------------------------------------------------------------------------------- /bluerov2_state_estimation/src/fake_waterlink.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | 4 | import rospy 5 | from tf2_ros import Buffer, TransformStamped, TransformListener, StaticTransformBroadcaster 6 | from geometry_msgs.msg import Quaternion, PointStamped, PoseWithCovarianceStamped, Transform, Point 7 | from scipy.spatial.transform import Rotation 8 | from std_msgs.msg import Header 9 | from sensor_msgs.msg import NavSatFix, NavSatStatus 10 | from geodetic_helpers import lla_to_utm 11 | import tf2_geometry_msgs 12 | 13 | 14 | def handle_gps(msg, args): 15 | """ 16 | Publish the GPS position in waterlinked frame. 17 | """ 18 | pub, tf = args 19 | e, n, a, zone, band = lla_to_utm(msg.latitude, msg.longitude, 0.0, radians=False) 20 | rospy.logdebug("utm: {} {} {}".format(e, n, a)) 21 | p_utm = PointStamped(Header(0, rospy.Time.from_sec(0.0), "utm"), Point(e, n, a)) 22 | # tf = Buffer() 23 | try: 24 | p_wl = tf.transform(p_utm, "waterlinked") 25 | rospy.logdebug("waterlinked: {} {} {}".format(p_wl.point.x, p_wl.point.y, p_wl.point.z)) 26 | except Exception as e: 27 | rospy.logerr_throttle(5.0, "{} | {}".format(rospy.get_name(), e.message)) 28 | return 29 | msg_pose_with_cov_stamped = PoseWithCovarianceStamped() 30 | var_xyz = pow(1.5, 2) # calculate variance from standard deviation 31 | msg_pose_with_cov_stamped.header.stamp = msg.header.stamp 32 | msg_pose_with_cov_stamped.header.frame_id = p_wl.header.frame_id 33 | msg_pose_with_cov_stamped.pose.pose.position.x = p_wl.point.x 34 | msg_pose_with_cov_stamped.pose.pose.position.y = p_wl.point.y 35 | msg_pose_with_cov_stamped.pose.pose.position.z = p_wl.point.z 36 | msg_pose_with_cov_stamped.pose.pose.orientation = Quaternion(0,0,0,1) 37 | msg_pose_with_cov_stamped.pose.covariance = [var_xyz, 0, 0, 0, 0, 0, 38 | 0, var_xyz, 0, 0, 0, 0, 39 | 0, 0, var_xyz, 0, 0, 0, 40 | 0, 0, 0, 0, 0, 0, 41 | 0, 0, 0, 0, 0, 0, 42 | 0, 0, 0, 0, 0, 0] 43 | pub.publish(msg_pose_with_cov_stamped) 44 | 45 | 46 | if __name__=="__main__": 47 | rospy.init_node("fake_waterlinked", log_level=rospy.INFO) 48 | heading_offset = rospy.get_param("~heading") # heading is given by waterlinked GPS in degrees CW from North 49 | master_datum = rospy.get_param("~datum") # Master located (latitude, longitude) 50 | lat, lon, alt = master_datum + [0.0] if len(master_datum) < 3 else master_datum 51 | master_gps = rospy.Publisher('gps_datum', NavSatFix, queue_size=5) 52 | master_msg = NavSatFix() 53 | master_msg.altitude = alt 54 | master_msg.longitude = lon 55 | master_msg.latitude = lat 56 | master_msg.status = NavSatStatus() 57 | master_msg.status.status = master_msg.status.STATUS_FIX 58 | master_msg.status.service = master_msg.status.SERVICE_GPS 59 | master_msg.position_covariance_type = master_msg.COVARIANCE_TYPE_UNKNOWN 60 | master_msg.position_covariance = [-1, 0, 0, 0, 0, 0, 0, 0, 0] 61 | pose_pub = rospy.Publisher('waterlinked/pose_with_cov_stamped', PoseWithCovarianceStamped, queue_size=5) 62 | buff = Buffer() 63 | TransformListener(buff) 64 | rospy.Subscriber("mavros/global_position/global", NavSatFix, handle_gps, (pose_pub, buff)) 65 | map_to_waterlink = TransformStamped(Header(0, rospy.Time.now(), 'map'), 'waterlinked', 66 | Transform(None, 67 | Quaternion(*Rotation.from_euler('xyz', [0, 0, 90 - heading_offset], 68 | degrees=True).as_quat().tolist()))) 69 | StaticTransformBroadcaster().sendTransform(map_to_waterlink) 70 | rate = rospy.Rate(4.0) 71 | while not rospy.is_shutdown(): 72 | master_msg.header.seq += 1 73 | master_msg.header.stamp = rospy.Time.now() 74 | master_gps.publish(master_msg) 75 | rate.sleep() 76 | -------------------------------------------------------------------------------- /bluerov2_state_estimation/src/geodetic_helpers.py: -------------------------------------------------------------------------------- 1 | from math_helpers import * 2 | import pyproj 3 | from scipy.spatial.transform import Rotation 4 | from geodesy.utm import gridZone 5 | import string 6 | 7 | 8 | def grid_convergence(lat, lon, radians=False): 9 | """ 10 | Given the latitude and longitude of a position, calculate the grid convergence 11 | Args: 12 | lat: latitude (degrees or radians) 13 | lon: longitude (degrees or radians) 14 | radians: true if lat/lon in radians 15 | 16 | Returns: gamma, the grid convergence angle in radians or degrees 17 | 18 | """ 19 | lon0, lat0, _ = utm_origin_lla(lat, lon, radians=radians) 20 | if radians: 21 | return atan(tan(lon - lon0)*sin(lat)) 22 | else: 23 | return rad2deg(atan(tand(lon - lon0)*sind(lat))) 24 | 25 | 26 | def lla_to_ecef(lat, lon, alt=0.0, radians=False): 27 | """ 28 | Convert Latitude, Longitude and altitude into ECEF coordinates 29 | Args: 30 | lat: latitude (degrees or radians) 31 | lon: longitude (degrees or radians) 32 | alt: altitude (metres) 33 | radians: true if lat/lon in radians 34 | 35 | Returns: 36 | x, y, z coordinates in ECEF (m) 37 | 38 | """ 39 | lla = pyproj.Proj(proj='latlong', ellps='WGS84', datum='WGS84') 40 | ecef = pyproj.Proj(proj='geocent', ellps='WGS84', datum='WGS84') 41 | return pyproj.transform(lla, ecef, lon, lat, alt, radians=radians) 42 | 43 | def ecef_to_lla(x, y, z, radians=False): 44 | """ 45 | Convert ECEF x, y and z into longitude, latitude, altitude 46 | Args: 47 | x: ECEF X (m) 48 | y: ECEF Y (m) 49 | z: ECEF Z (m) 50 | radians: true if lat/lon in radians 51 | 52 | Returns: 53 | longitude, latitude, altitude 54 | 55 | """ 56 | lla = pyproj.Proj(proj='latlong', ellps='WGS84', datum='WGS84') 57 | ecef = pyproj.Proj(proj='geocent', ellps='WGS84', datum='WGS84') 58 | return pyproj.transform(ecef, lla, x, y, z, radians=radians) 59 | 60 | 61 | def latlong_ecef_enu_rotation(lat, lon, radians=False): 62 | """ 63 | Determine rotation from ECEF frame to ENU frame given latitude and longitude 64 | 65 | Args: 66 | lat: latitude (degrees or radians) 67 | lon: longitude (degrees or radians) 68 | radians: true if lat/lon in radians 69 | 70 | Returns: Quaternion rotation from ECEF to ENU 71 | 72 | """ 73 | if not radians: 74 | return Rotation.from_dcm([[-sind(lon), cosd(lon), 0], 75 | [-cosd(lon) * sind(lat), -sind(lon) * sind(lat), cosd(lat)], 76 | [cosd(lon) * cosd(lat), sind(lon) * cosd(lat), sind(lat)]]).as_quat().tolist() 77 | else: 78 | return Rotation.from_dcm([[-sin(lon), cos(lon), 0], 79 | [-cos(lon) * sin(lat), -sin(lon) * sin(lat), cos(lat)], 80 | [cos(lon) * cos(lat), sin(lon) * cos(lat), sin(lat)]]).as_quat().tolist() 81 | 82 | 83 | def is_south(band): 84 | """ 85 | Return if band is in southern hemisphere 86 | """ 87 | alpha = {c: num for num, c in enumerate(string.ascii_uppercase)} 88 | return alpha[band.upper()] < 12 89 | 90 | 91 | def lla_to_utm(lat, lon, alt=0.0, radians=False): 92 | """ 93 | Given Latitude Longitude, Altitude, return the UTM position 94 | Args: 95 | lat: latitude (degrees or radians) 96 | lon: longitude (degrees or radians) 97 | alt: altitude (m) 98 | radians: true if lat/lon in radians 99 | 100 | Returns: Eastings (m), Northings (m), Altitude (m), Zone, Band 101 | 102 | """ 103 | lla = pyproj.Proj(proj='latlong', ellps='WGS84', datum='WGS84') 104 | zone, band = gridZone(lat, lon) 105 | utm = pyproj.Proj(proj='utm', ellps='WGS84', datum='WGS84', zone=zone, south=is_south(band)) 106 | return pyproj.transform(lla, utm, lon, lat, alt, radians=radians) + (zone, band) 107 | 108 | 109 | def ecef_to_utm(x, y, z): 110 | """ 111 | Given ECEF coordinates of a position, return the UTM position and rotation from ECEF frame to ENU frame 112 | Args: 113 | x: ECEF X coordinate (m) 114 | y: ECEF Y coordinate (m) 115 | z: ECEF Z coordinate (m) 116 | 117 | Returns: 118 | eastings, northings, altitude, quaternion ECEF to ENU (qx, qy, qz, qw) 119 | 120 | """ 121 | ecef = pyproj.Proj(proj='geocent', ellps='WGS84', datum='WGS84') 122 | lla = pyproj.Proj(proj='latlong', ellps='WGS84', datum='WGS84') 123 | lon, lat, alt = pyproj.transform(ecef, lla, x, y, z, radians=False) 124 | zone, band = gridZone(lat, lon) 125 | utm = pyproj.Proj(proj='utm', ellps='WGS84', datum='WGS84', zone=zone, south=is_south(band)) 126 | utmpos = pyproj.transform(ecef, utm, x, y, z) 127 | return utmpos + tuple(latlong_ecef_enu_rotation(lat, lon)) 128 | 129 | def utm_origin_lla(lat, lon, radians=False): 130 | """ 131 | Given latitude and longitude, return the origin of the UTM frame in latitude and longitude 132 | Args: 133 | lat: latitude (degrees or radians) 134 | lon: longitude (degrees or radians) 135 | radians: true if lat/lon in radians 136 | 137 | Returns: 138 | utm origin: (longitude, latitude, altitude) 139 | 140 | """ 141 | origin = [500000.0, 0.0, 0.0] 142 | zone, band = gridZone(lat, lon) 143 | utm = pyproj.Proj(proj='utm', ellps='WGS84', datum='WGS84', zone=zone, south=is_south(band)) 144 | lla = pyproj.Proj(proj='latlong', ellps='WGS84', datum='WGS84') 145 | return pyproj.transform(utm, lla, *origin, radians=radians) 146 | 147 | 148 | def utm_origin_ecef(x, y, z): 149 | """ 150 | Given ECEF coordinates, return origin of UTM zone in ECEF coordinates 151 | Args: 152 | x: ECEF X coordinate (m) 153 | y: ECEF Y coordinate (m) 154 | z: ECEF Z coordinate (m) 155 | 156 | Returns: 157 | UTM origin in ECEF (X, Y, Z), metres 158 | 159 | """ 160 | lon, lat, alt = ecef_to_lla(x, y, z) 161 | lon, lat, alt = utm_origin_lla(lat, lon) 162 | return lla_to_ecef(lat, lon, alt) 163 | 164 | 165 | if __name__=="__main__": 166 | # Reference lat, lon, alt ground truths 167 | latitude, longitude, altitude = [55.605312, 12.799501, 0.0] 168 | # ECEF ground truths 169 | XT, YT, ZT = 3521455, 800023, 5239744 170 | # UTM ground truths 171 | EastT, NorthT, AltT = 361377, 6164350, 0.0 172 | # ECEF calculation 173 | X, Y, Z = lla_to_ecef(latitude, longitude, altitude, radians=False) 174 | # UTM and Rotation Calculation 175 | East, North, Alt, QX, QY, QZ, QW = ecef_to_utm(X, Y, Z) 176 | # UTM origin calculation 177 | lonO, latO, altO = utm_origin_lla(latitude, longitude) 178 | XO, YO, ZO = utm_origin_ecef(X, Y, Z) 179 | 180 | latitude1, longitude1, altitude1 = [55.605312, 12.8, 0.0] 181 | X1, Y1, Z1 = lla_to_ecef(latitude1, longitude1, altitude1, radians=False) 182 | East1, North1, Alt1, QX1, QY1, QZ1, QW1 = ecef_to_utm(X1, Y1, Z1) 183 | dist = sqrt((Z1-Z)**2+(X1-X)**2+(Y1-Y)**2) 184 | utmdist = sqrt((East1-East)**2+(North1-North)**2+(Alt1-Alt)**2) 185 | 186 | 187 | -------------------------------------------------------------------------------- /bluerov2_state_estimation/src/math_helpers.py: -------------------------------------------------------------------------------- 1 | from math import * 2 | 3 | def sind(x): 4 | return sin(x*pi / 180.0) 5 | 6 | def cosd(x): 7 | return cos(x*pi/180.0) 8 | 9 | def tand(x): 10 | return tan(x*pi/180.0) 11 | 12 | def deg2rad(x): 13 | return x*pi/180.0 14 | 15 | def rad2deg(x): 16 | return x*180.0/pi -------------------------------------------------------------------------------- /bluerov2_state_estimation/src/tf_manager.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | 4 | import rospy 5 | import tf2_geometry_msgs 6 | import geodetic_helpers 7 | from sensor_msgs.msg import NavSatFix 8 | from robot_localization.srv import SetDatum 9 | from tf2_ros import TransformBroadcaster, TransformStamped, Buffer, TransformListener, StaticTransformBroadcaster 10 | from geometry_msgs.msg import Quaternion, Transform, Vector3, PoseWithCovarianceStamped, PointStamped, Vector3Stamped 11 | from std_msgs.msg import Header 12 | from std_srvs.srv import Trigger, TriggerResponse 13 | from scipy.spatial.transform import Rotation 14 | 15 | 16 | class TFManager(object): 17 | """ 18 | Listens to datum GPS or service and broadcasts: 19 | 1. ECEF TO UTM 20 | 2. UTM TO MAP 21 | 3. MAP TO ODOM (fixed or updated by a pose with covariance stamped topic). 22 | """ 23 | def __init__(self): 24 | self._datum = rospy.get_param("~datum", None) 25 | if self._datum is not None and len(self._datum) < 3: 26 | self._datum += [0.0] 27 | self._use_datum = self._datum is not None 28 | self._earth_frame_id = rospy.get_param("~earth_frame_id", "earth") 29 | self._utm_frame_id = rospy.get_param("~utm_frame_id", "utm") 30 | self._map_frame_id = rospy.get_param("~map_frame_id", "map") 31 | self._odom_frame_id = rospy.get_param("~odom_frame_id", "odom") 32 | self._body_frame_id = rospy.get_param("~base_frame_id", "base_link") # currently unused 33 | self._tf_broadcast_rate = rospy.get_param("~broadcast_rate", 10.0) 34 | self._serv = rospy.Service("~set_datum", SetDatum, self._set_datum) 35 | self._tf_buff = Buffer() 36 | TransformListener(self._tf_buff) 37 | self._tf_bcast = TransformBroadcaster() 38 | self._last_datum = None 39 | self._static_map_odom = rospy.get_param("~static_map_odom", False) 40 | self._odom_updated = False 41 | self._update_odom_serv = rospy.Service("~set_odom", Trigger, self._handle_set_odom) 42 | if not self._use_datum: 43 | rospy.Subscriber("gps_datum", NavSatFix, self._handle_datum) 44 | if not self._static_map_odom: 45 | rospy.Subscriber("waterlinked/pose_with_cov_stamped", PoseWithCovarianceStamped, self._handle_odom_tf) 46 | else: 47 | StaticTransformBroadcaster().sendTransform(TransformStamped(Header(0, rospy.Time.now(), self._map_frame_id), 48 | self._odom_frame_id, 49 | None, 50 | Quaternion(0,0,0,1))) 51 | self._map_odom_tf = None 52 | rospy.Timer(rospy.Duration.from_sec(1.0 / self._tf_broadcast_rate), self._send_tf) 53 | 54 | def _handle_set_odom(self, req): 55 | self._odom_updated = False 56 | res = TriggerResponse(True, "map -> odom tf set.") 57 | return res 58 | 59 | def _handle_odom_tf(self, msg): 60 | # Given the pose of the vehicle in waterlinked frame, output the map -> odom tf transformation 61 | if not self._odom_updated: 62 | point = PointStamped(Header(0, rospy.Time.from_sec(0.0), msg.header.frame_id), 63 | msg.pose.pose.position) 64 | try: 65 | point_map = self._tf_buff.transform(point, self._map_frame_id) 66 | except Exception as e: 67 | rospy.logerr_throttle(5, "{} | {}".format(rospy.get_name(), e.message)) 68 | return 69 | # Odom is always at same depth as map 70 | self._map_odom_tf = TransformStamped(Header(0, rospy.Time.now(), self._map_frame_id), 71 | self._odom_frame_id, 72 | Transform(Vector3(point_map.point.x, point_map.point.y, 0), 73 | Quaternion(0, 0, 0, 1))) 74 | self._odom_updated = True 75 | 76 | def _set_datum(self, req): 77 | self._datum = [req.geo_pose.position.latitude, req.geo_pose.position.longitude, req.geo_pose.position.altitude] 78 | return 79 | 80 | def _get_coords(self, latitude, longitude): 81 | # Get ECEF translation to UTM and Rotation to UTM from latitude and longitude (assuming 0.0 altitude) 82 | x, y, z = geodetic_helpers.utm_origin_ecef(*geodetic_helpers.lla_to_ecef(latitude, longitude)) 83 | q = geodetic_helpers.latlong_ecef_enu_rotation(latitude, longitude) 84 | # Get UTM translation to ENU and rotation to ENU from latitude and longitude (assuming 0.0 altitude) 85 | utm_current = geodetic_helpers.lla_to_utm(latitude, longitude) 86 | Y = geodetic_helpers.grid_convergence(latitude, longitude, radians=False) 87 | Y = 0 # The grid convergence seems to be already accounted for? 88 | enu_rotation = Rotation.from_euler('xyz', [0.0, 0.0, -Y], degrees=True).as_quat().tolist() 89 | return (x, y, z) + tuple(q), utm_current[:3] + tuple(enu_rotation) 90 | 91 | def _get_tfs(self, data): 92 | if type(data) is NavSatFix: 93 | earth, utm = self._get_coords(data.latitude, data.longitude) 94 | header_earth = Header(data.header.seq, data.header.stamp, self._earth_frame_id) 95 | header_utm = Header(data.header.seq, data.header.stamp, self._utm_frame_id) 96 | else: 97 | earth, utm = self._get_coords(self._datum[0], self._datum[1]) 98 | header_earth = Header(0, rospy.Time.now(), self._earth_frame_id) 99 | header_utm = Header(0, rospy.Time.now(), self._utm_frame_id) 100 | # Broadcast datum's latitude and longitude 101 | # Map from ECEF Frame 102 | earth_to_utm = TransformStamped(header_earth, 103 | self._utm_frame_id, 104 | Transform(Vector3(*earth[:3]), 105 | Quaternion(*earth[3:]))) 106 | # UTM from ECEF Frame 107 | utm_to_map = TransformStamped(header_utm, 108 | self._map_frame_id, 109 | Transform(Vector3(*utm[:3]), 110 | Quaternion(*utm[3:]))) 111 | return earth_to_utm, utm_to_map 112 | 113 | def _handle_datum(self, msg): 114 | self._last_datum = msg 115 | 116 | def _send_tf(self, event): 117 | if self._use_datum: 118 | tf_utm, tf_map = self._get_tfs(None) 119 | elif self._last_datum is not None: 120 | tf_utm, tf_map = self._get_tfs(self._last_datum) 121 | else: 122 | return 123 | self._tf_bcast.sendTransform(tf_utm) 124 | self._tf_bcast.sendTransform(tf_map) 125 | if self._map_odom_tf is not None: 126 | tf = self._map_odom_tf 127 | tf.header.stamp = rospy.Time.now() 128 | self._tf_bcast.sendTransform(tf) 129 | 130 | 131 | if __name__=="__main__": 132 | rospy.init_node("tf_manager") 133 | try: 134 | manager = TFManager() 135 | rospy.spin() 136 | except rospy.ROSInterruptException: 137 | pass -------------------------------------------------------------------------------- /doc/imgs/bluerov2_uuv_simulator.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/FletcherFT/bluerov2/c416fc8530aac2b0ac52626001308d780347d639/doc/imgs/bluerov2_uuv_simulator.png -------------------------------------------------------------------------------- /doc/imgs/neptus_001.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/FletcherFT/bluerov2/c416fc8530aac2b0ac52626001308d780347d639/doc/imgs/neptus_001.png -------------------------------------------------------------------------------- /doc/imgs/neptus_002.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/FletcherFT/bluerov2/c416fc8530aac2b0ac52626001308d780347d639/doc/imgs/neptus_002.png -------------------------------------------------------------------------------- /doc/imgs/neptus_003.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/FletcherFT/bluerov2/c416fc8530aac2b0ac52626001308d780347d639/doc/imgs/neptus_003.png -------------------------------------------------------------------------------- /doc/imgs/neptus_004.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/FletcherFT/bluerov2/c416fc8530aac2b0ac52626001308d780347d639/doc/imgs/neptus_004.png -------------------------------------------------------------------------------- /doc/imgs/neptus_005.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/FletcherFT/bluerov2/c416fc8530aac2b0ac52626001308d780347d639/doc/imgs/neptus_005.png -------------------------------------------------------------------------------- /waterlinked_gps/README.sh: -------------------------------------------------------------------------------- 1 | Author: Andreas Baldur Nørregård Hansen 2 | 3 | The library cpp-netlib is used to make http requests to the waterlinked system. 4 | It can be installed using the guide here: 5 | 6 | http://cpp-netlib.org/0.12.0/getting_started.html#getting-started 7 | -------------------------------------------------------------------------------- /waterlinked_gps/data/14-03-2018_data_for_lp_design/export_csv.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import rosbag 3 | bag = rosbag.Bag('waterlinked_data_0.bag') 4 | t_arr=[] 5 | x_arr=[] 6 | y_arr=[] 7 | first_loop = True 8 | t0 = 0 9 | file = open("data_export.csv","w") 10 | file.write("t[s],x[m],y[m]\n") 11 | for topic, msg, t in bag.read_messages(topics=['/waterlinked/position/acoustic/filtered']): 12 | if first_loop: 13 | t0 = t.to_sec() 14 | first_loop = False 15 | file.write(str(t.to_sec()-t0) + "," + str(msg.x) + "," + str(msg.y) + "\n") 16 | file.close() 17 | bag.close() -------------------------------------------------------------------------------- /waterlinked_gps/data/14-03-2018_data_for_lp_design/start_recording.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | rosbag record /waterlinked/position/acoustic/filtered \ 3 | /waterlinked/position/acoustic/raw \ 4 | -O $1 -------------------------------------------------------------------------------- /waterlinked_gps/data/14-03-2018_data_for_lp_design/waterlinked_data_0.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/FletcherFT/bluerov2/c416fc8530aac2b0ac52626001308d780347d639/waterlinked_gps/data/14-03-2018_data_for_lp_design/waterlinked_data_0.bag -------------------------------------------------------------------------------- /waterlinked_gps/launch/uwgps-bringup.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /waterlinked_gps/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | waterlinked_gps 4 | 0.0.0 5 | The waterlinked_gps package 6 | 7 | 8 | 9 | 10 | pi 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | message_generation 41 | waterlinked_gps_msgs 42 | waterlinked_gps_msgs 43 | 44 | 45 | 46 | 47 | 48 | message_runtime 49 | 50 | 51 | 52 | 53 | 54 | catkin 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | -------------------------------------------------------------------------------- /waterlinked_gps/scripts/http_request_speed_tests/waterlinked_grequests_test.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | import rospy 3 | import grequests 4 | import time 5 | #import sys 6 | 7 | # The base URL is the one specified through: http://192.168.2.2:2770/waterlinked 8 | base_url = 'http://192.168.2.94:80/api/v1' 9 | 10 | # The complete API can be found here: http://192.168.2.94/swagger/ 11 | api_list = ['/about', '/about/status', '/about/temperature', '/position/master', 12 | '/config/generic', '/config/receivers', 13 | '/external/orientation', 14 | '/position/acoustic/filtered', '/position/acoustic/raw', '/position/global', '/position/master'] 15 | 16 | urls = [base_url + api for api in api_list] 17 | #print urls 18 | 19 | def main_func(): 20 | rospy.init_node('waterlinked_gps_node', anonymous=True) 21 | rate = rospy.Rate(50) 22 | 23 | t0 = time.time() 24 | is_first_iteration = True 25 | f_cum = 0 26 | n = 0 27 | 28 | rs = [grequests.get(url) for url in urls] 29 | 30 | while not rospy.is_shutdown(): 31 | 32 | # Get timing 33 | tnow = time.time() 34 | if is_first_iteration: 35 | is_first_iteration = False 36 | else: 37 | f = 1/(tnow-t0) 38 | f_cum += f 39 | n+=1 40 | f_avg = f_cum / n 41 | print('n = ' + str(n) + ', f_avg = ' + str(f_avg)) 42 | t0 = tnow 43 | 44 | res=grequests.map(rs) 45 | for resp in res: 46 | print resp.content 47 | 48 | 49 | #url = "http://192.168.2.94:80/api/v1/position/master" 50 | #r = requests.get(url) 51 | #print(url) 52 | #print r.content 53 | #print('loop') 54 | rate.sleep() 55 | 56 | if __name__ == '__main__': 57 | try: 58 | main_func() 59 | except rospy.ROSInterruptException: 60 | pass 61 | -------------------------------------------------------------------------------- /waterlinked_gps/scripts/http_request_speed_tests/waterlinked_requests_futures_test.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | import rospy 3 | import time 4 | from requests_futures.sessions import FuturesSession 5 | 6 | # The base URL is the one specified through: http://192.168.2.2:2770/waterlinked 7 | base_url = 'http://192.168.2.94:80/api/v1' 8 | 9 | # The complete API can be found here: http://192.168.2.94/swagger/ 10 | api_list = ['/about', '/about/status', '/about/temperature', 11 | '/config/generic', '/config/receivers', 12 | '/external/orientation', 13 | '/position/acoustic/filtered', '/position/acoustic/raw', '/position/global', '/position/master'] 14 | 15 | urls = [base_url + api for api in api_list] 16 | 17 | # Use 10 workers as there are 10 unique APIs 18 | session = FuturesSession(max_workers=10) 19 | 20 | def main_func(): 21 | rospy.init_node('waterlinked_requests_thread', anonymous=False) 22 | rate = rospy.Rate(20) 23 | 24 | t0 = time.time() 25 | is_first_iteration = True 26 | f_cum = 0 27 | n = 0 28 | 29 | while not rospy.is_shutdown(): 30 | 31 | # Get timing 32 | tnow = time.time() 33 | if is_first_iteration: 34 | is_first_iteration = False 35 | else: 36 | f = 1/(tnow-t0) 37 | f_cum += f 38 | n+=1 39 | f_avg = f_cum / n 40 | print('n = ' + str(n) + ', f_avg = ' + str(f_avg)) 41 | t0 = tnow 42 | 43 | future_list = [session.get(url) for url in urls] 44 | for future in future_list: 45 | resp = future.result() 46 | #print(resp.content) 47 | 48 | rate.sleep() 49 | 50 | if __name__ == '__main__': 51 | try: 52 | main_func() 53 | except rospy.ROSInterruptException: 54 | pass 55 | -------------------------------------------------------------------------------- /waterlinked_gps/scripts/http_request_speed_tests/waterlinked_requests_test.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | import requests 3 | import rospy 4 | import time 5 | 6 | # The base URL is the one specified through: http://192.168.2.2:2770/waterlinked 7 | base_url = 'http://192.168.2.94:80/api/v1' 8 | 9 | # The complete API can be found here: http://192.168.2.94/swagger/ 10 | api_list = ['/about', '/about/status', '/about/temperature', '/position/master', 11 | # '/config/generic', '/config/receivers', 12 | # '/external/orientation', 13 | # '/position/acoustic/filtered', '/position/acoustic/raw', '/position/global', '/position/master'] 14 | ] 15 | 16 | urls = [base_url + api for api in api_list] 17 | 18 | def main_func(): 19 | rospy.init_node('waterlinked_requests', anonymous=True) 20 | rate = rospy.Rate(50) 21 | 22 | t0 = time.time() 23 | is_first_iteration = True 24 | f_cum = 0 25 | n = 0 26 | while not rospy.is_shutdown(): 27 | 28 | # Get timing 29 | tnow = time.time() 30 | if is_first_iteration: 31 | is_first_iteration = False 32 | else: 33 | f = 1/(tnow-t0) 34 | f_cum += f 35 | n+=1 36 | f_avg = f_cum / n 37 | print('n = ' + str(n) + ', f_avg = ' + str(f_avg)) 38 | t0 = tnow 39 | 40 | resp_list = [requests.get(url) for url in urls] 41 | print('---') 42 | for resp in resp_list: 43 | print resp.content 44 | 45 | rate.sleep() 46 | 47 | if __name__ == '__main__': 48 | try: 49 | main_func() 50 | except rospy.ROSInterruptException: 51 | pass 52 | -------------------------------------------------------------------------------- /waterlinked_gps/scripts/math_helpers.py: -------------------------------------------------------------------------------- 1 | from math import * 2 | 3 | def sind(x): 4 | return sin(x*pi / 180.0) 5 | 6 | def cosd(x): 7 | return cos(x*pi/180.0) 8 | 9 | def tand(x): 10 | return tan(x*pi/180.0) 11 | 12 | def deg2rad(x): 13 | return x*pi/180.0 14 | 15 | def rad2deg(x): 16 | return x*180.0/pi 17 | -------------------------------------------------------------------------------- /waterlinked_gps/scripts/owens_logger/ROV_GPS.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | import requests 3 | import time 4 | 5 | # The base URL is the one specified through: http://192.168.2.2:2770/waterlinked 6 | base_url = 'http://192.168.2.94:80/api/v1' 7 | ​ 8 | # The complete API can be found here: http://192.168.2.94/swagger/ 9 | api_list = ['/about', '/about/status', '/about/temperature', '/position/master', 10 | '/config/generic', '/config/receivers', 11 | '/external/orientation', 12 | '/position/acoustic/filtered', '/position/acoustic/raw', '/position/global', '/position/master'] 13 | ​ 14 | urls = [base_url + api for api in api_list] 15 | ​ 16 | def main_func(): 17 | ​ 18 | n = 0 19 | while True: 20 | resp_list = [requests.get(url) for url in urls] 21 | print(n) 22 | for resp in resp_list: 23 | print resp.content 24 | 25 | ##my data logging 26 | 27 | 28 | f = open("GPS-logging.txt", "a") #will need to create txt file firts 29 | 30 | f.write(str(resp.content)) 31 | 32 | f.close() 33 | 34 | ## 35 | 36 | 37 | n += 1 38 | ​ 39 | if __name__ == '__main__': 40 | main_func() 41 | -------------------------------------------------------------------------------- /waterlinked_gps/scripts/owens_logger/waterlinked_owen.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | import requests 3 | import time 4 | 5 | # The base URL is the one specified through: http://192.168.2.2:2770/waterlinked 6 | base_url = 'http://192.168.2.94:80/api/v1' 7 | 8 | # The complete API can be found here: http://192.168.2.94/swagger/ 9 | api_list = ['/about', '/about/status', '/about/temperature', '/position/master', 10 | '/config/generic', '/config/receivers', 11 | '/external/orientation', 12 | '/position/acoustic/filtered', '/position/acoustic/raw', '/position/global', '/position/master'] 13 | 14 | urls = [base_url + api for api in api_list] 15 | 16 | def main_func(): 17 | 18 | n = 0 19 | while True: 20 | resp_list = [requests.get(url) for url in urls] 21 | print(n) 22 | for resp in resp_list: 23 | print resp.content 24 | f = open("GPS-loggin.txt",'a') 25 | f.write(str(resp.content)) 26 | f.close() 27 | n += 1 28 | 29 | if __name__ == '__main__': 30 | main_func() 31 | -------------------------------------------------------------------------------- /waterlinked_gps_msgs/REMORA_ws.sublime-project: -------------------------------------------------------------------------------- 1 | { 2 | "folders": 3 | [ 4 | { 5 | "path": "/home/fft/REMORA_ws/src" 6 | } 7 | ] 8 | } 9 | -------------------------------------------------------------------------------- /waterlinked_gps_msgs/msg/About.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | # Example 4 | # {"chipid":"0xf1440630630608","version":"1.2.1 (317.5a4809a)"} 5 | 6 | string chipid # Chip identifier 7 | string version # Web Server version string 8 | -------------------------------------------------------------------------------- /waterlinked_gps_msgs/msg/AboutStatus.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | # Example: 4 | # {"gps":0,"imu":0} 5 | 6 | uint8 gps # GPS lock status 7 | uint8 imu # IMU calibration status 8 | -------------------------------------------------------------------------------- /waterlinked_gps_msgs/msg/AboutTemperature.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | # Example 4 | # {"board":49.125} 5 | 6 | float32 board # Board temperature 7 | -------------------------------------------------------------------------------- /waterlinked_gps_msgs/msg/ConfigGeneric.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | # Example: 4 | # {"carrier_frequency":150,"compass":"onboard","gps":"static","max_range":100,"static_lat":0,"static_lon":0,"static_orientation":0,"use_external_depth":true} 5 | 6 | uint16 carrier_frequency # Carrier frequency (kHz) 7 | string compass # Compass provider setting [ onboard, static, external ] 8 | string gps # GPS provider setting 9 | int16 range_max_x # Max range (meters) 10 | int16 range_max_y # Max range (meters) 11 | int16 range_max_z # Max range (meters) 12 | int16 range_min_x # Min range (meters) 13 | int16 range_min_y # Min range (meters) 14 | float32 static_lat # Latitude to use in static mode 15 | float32 static_lon # Longitude to use in static mode 16 | uint16 static_orientation # Orientation/compass reading to use in static mode (degrees) 17 | bool use_external_depth # Use external depth. Required to be true when using Locator-A1. 18 | -------------------------------------------------------------------------------- /waterlinked_gps_msgs/msg/ConfigReceivers.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | # Example 4 | # [{"id":0,"x":0,"y":2.49,"z":1.02},{"id":1,"x":2.52,"y":2.52,"z":1.12},{"id":2,"x":2.52,"y":0,"z":1.29},{"id":3,"x":0,"y":0,"z":0.9}] 5 | 6 | waterlinked_gps_msgs/Receiver[] receivers 7 | -------------------------------------------------------------------------------- /waterlinked_gps_msgs/msg/ExternalOrientation.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | # Current orientation/compass heading (degrees). -1 means no orientation set 4 | int16 orientation 5 | -------------------------------------------------------------------------------- /waterlinked_gps_msgs/msg/PositionAcousticFiltered.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | float32 std # Current accoustic position accuracy (meter) 4 | float32 temp # Current accoustic temperature deg. C 5 | float32 x # Current accoustic x position relative to master electronics (meter) 6 | float32 y # Current accoustic y position relative to master electronics (meter) 7 | float32 z # Current accoustic z position (depth) relative to master electronics (meter) 8 | -------------------------------------------------------------------------------- /waterlinked_gps_msgs/msg/PositionAcousticRaw.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | float32 std # Current accoustic position accuracy (meter) 4 | float32 temp # Current accoustic temperature deg. C 5 | float32 x # Current accoustic x position relative to master electronics (meter) 6 | float32 y # Current accoustic y position relative to master electronics (meter) 7 | float32 z # Current accoustic z position (depth) relative to master electronics (meter) 8 | -------------------------------------------------------------------------------- /waterlinked_gps_msgs/msg/PositionGlobal.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | float32 lat # Current Latitude 4 | float32 lon # Current Longitude 5 | -------------------------------------------------------------------------------- /waterlinked_gps_msgs/msg/PositionMaster.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | int16 cog # Course over ground (degrees). -1 means no data. 4 | float32 hdop # Horizontal dilution of precision. -1 means no data. 5 | float32 lat # Current Latitude 6 | float32 lon # Current Longitude 7 | int16 numsats # Number of satellites. -1 means no data. 8 | int16 orientation # Current orientation/compass heading (degrees). -1 means no data. 9 | float32 sog # Speed over ground (km/h). -1 means no data. 10 | -------------------------------------------------------------------------------- /waterlinked_gps_msgs/msg/Receiver.msg: -------------------------------------------------------------------------------- 1 | # Example 2 | # {"id":3,"x":0,"y":0,"z":0.9} 3 | 4 | uint64 id # Unique receiver identifier 5 | float32 x # Configured X position relative to master electronics (meter) 6 | float32 y # Configured Y position relative to master electronics (meter) 7 | float32 z # Configured Z position relative to master electronics (meter) 8 | -------------------------------------------------------------------------------- /waterlinked_gps_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | waterlinked_gps_msgs 4 | 0.0.0 5 | The waterlinked_gps_msgs package 6 | 7 | 8 | 9 | 10 | Andreas Baldur Nørregård Hansen 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | std_msgs 41 | message_generation 42 | 43 | 44 | 45 | 46 | 47 | message_runtime 48 | 49 | 50 | 51 | 52 | 53 | catkin 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | --------------------------------------------------------------------------------