├── rti_dvl ├── scripts │ ├── pynmea2 │ │ ├── _version.py │ │ ├── types │ │ │ ├── __init__.py │ │ │ └── proprietary │ │ │ │ ├── __init__.py │ │ │ │ ├── rdi.py │ │ │ │ ├── srf.py │ │ │ │ ├── grm.py │ │ │ │ ├── chr.py │ │ │ │ ├── ubx.py │ │ │ │ ├── tnl.py │ │ │ │ ├── sxn.py │ │ │ │ ├── ash.py │ │ │ │ └── rti.py │ │ ├── __init__.py │ │ ├── seatalk_utils.py │ │ ├── LICENSE │ │ ├── nmea_file.py │ │ ├── stream.py │ │ ├── nmea_utils.py │ │ └── nmea.py │ ├── minicom.sh │ └── rti_dvl_node.py ├── msg │ ├── DVL.msg │ ├── BottomTrack.msg │ └── Command.msg ├── CMakeLists.txt └── package.xml ├── bluerov_bridge ├── src │ └── bluerov_bridge │ │ ├── __init__.py │ │ └── bridge.py ├── CMakeLists.txt ├── setup.py └── package.xml ├── README.md ├── bluerov ├── CMakeLists.txt └── package.xml ├── bluerov_control ├── launch │ ├── params │ │ ├── empty.png │ │ ├── emptymap.yaml │ │ └── move_base │ │ │ ├── local_costmap_params.yaml │ │ │ ├── move_base_params.yaml │ │ │ ├── costmap_common_params.yaml │ │ │ ├── global_costmap_params.yaml │ │ │ ├── dwa_local_planner_params.yaml │ │ │ └── teb_local_planner_params.yaml │ ├── joy_control.launch │ └── move_base.launch ├── CMakeLists.txt ├── package.xml └── scripts │ └── joy_control.py ├── bluerov_launch ├── scripts │ ├── connect_rti_dvl.sh │ ├── connect_vn100_imu.sh │ ├── mavproxy.sh │ ├── qgc.sh │ ├── inspect_bag.py │ ├── dashboard.py │ └── video.py ├── CMakeLists.txt ├── README.md ├── package.xml └── launch │ └── bluerov_launch.launch ├── bar30_depth ├── msg │ └── Depth.msg ├── CMakeLists.txt ├── package.xml └── scripts │ └── bar30_depth_node.py ├── sonar_oculus ├── msg │ ├── OculusFire.msg │ └── OculusPing.msg ├── README.md ├── package.xml ├── launch │ └── sonar_oculus.launch ├── CMakeLists.txt ├── src │ ├── DataWrapper.h │ ├── OculusClient.h │ ├── Oculus.h │ └── sonar.cpp ├── cfg │ └── OculusParams.cfg └── scripts │ └── oculus_viewer.py ├── waterlinked_gps ├── CMakeLists.txt ├── package.xml └── scripts │ └── waterlinked_gps_node.py ├── imu_vn_100 ├── package.xml ├── src │ └── imu_vn_100_cont.cpp ├── CMakeLists.txt ├── vncpplib │ ├── LICENSE.txt │ └── include │ │ ├── vectornav.h │ │ ├── vn_math.h │ │ ├── vn_common.h │ │ ├── vnint.h │ │ ├── vn_kinematics.h │ │ ├── vn_linearAlgebra.h │ │ ├── vn_errorCodes.h │ │ └── vncp_services.h ├── .travis.yml ├── launch │ └── vn_100_cont.launch ├── include │ └── imu_vn_100 │ │ └── imu_vn_100.h ├── README.md └── LICENSE.txt ├── INSTALL.md ├── LICENSE └── CONFIG.md /rti_dvl/scripts/pynmea2/_version.py: -------------------------------------------------------------------------------- 1 | __version__ = '1.12.0' 2 | -------------------------------------------------------------------------------- /bluerov_bridge/src/bluerov_bridge/__init__.py: -------------------------------------------------------------------------------- 1 | from bridge import Bridge -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # BlueROV2 ROS Package 2 | 3 | - [Installation](./INSTALL.md) 4 | - [Configuration](./CONFIG.md) 5 | -------------------------------------------------------------------------------- /bluerov/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(bluerov) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() -------------------------------------------------------------------------------- /rti_dvl/msg/DVL.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | geometry_msgs/Vector3 velocity 4 | float64 temperature 5 | float64 altitude 6 | float64 time 7 | -------------------------------------------------------------------------------- /bluerov_control/launch/params/empty.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/bluerov/HEAD/bluerov_control/launch/params/empty.png -------------------------------------------------------------------------------- /bluerov_launch/scripts/connect_rti_dvl.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | socat pty,link=/tmp/rti_dvl,waitslave,raw,echo=0,ignoreeof tcp:192.168.2.2:14660 -------------------------------------------------------------------------------- /bar30_depth/msg/Depth.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | float32 time 4 | float32 pressure_abs 5 | float32 pressure_diff 6 | float32 temperature 7 | float32 depth -------------------------------------------------------------------------------- /bluerov_launch/scripts/connect_vn100_imu.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | socat pty,link=/tmp/vn100_imu,waitslave,raw,echo=0,ignoreeof tcp:192.168.2.2:14661 -------------------------------------------------------------------------------- /rti_dvl/scripts/pynmea2/types/__init__.py: -------------------------------------------------------------------------------- 1 | # pylint: disable=wildcard-import 2 | 3 | from .talker import * 4 | from .proprietary import * 5 | 6 | -------------------------------------------------------------------------------- /bluerov_control/launch/params/emptymap.yaml: -------------------------------------------------------------------------------- 1 | image: empty.png 2 | resolution: 10.0 3 | origin: [-500.0, -500.0, 0.0] 4 | negate: 1 5 | occupied_thresh: 0.5 6 | free_thresh: 0.5 7 | -------------------------------------------------------------------------------- /bluerov_launch/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(bluerov_launch) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | ) 6 | 7 | catkin_package() 8 | -------------------------------------------------------------------------------- /sonar_oculus/msg/OculusFire.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | uint8 mode 4 | uint8 gamma 5 | uint8 flags 6 | float64 range 7 | float64 gain 8 | float64 speed_of_sound 9 | float64 salinity -------------------------------------------------------------------------------- /waterlinked_gps/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(waterlinked_gps) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | rospy 6 | ) 7 | 8 | catkin_package() 9 | -------------------------------------------------------------------------------- /rti_dvl/scripts/pynmea2/types/proprietary/__init__.py: -------------------------------------------------------------------------------- 1 | from . import ash 2 | from . import grm 3 | from . import rdi 4 | from . import srf 5 | from . import sxn 6 | from . import tnl 7 | from . import ubx 8 | from . import rti 9 | from . import chr 10 | 11 | -------------------------------------------------------------------------------- /bluerov_bridge/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | project(bluerov_bridge) 3 | 4 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -O3") 5 | 6 | find_package(catkin REQUIRED COMPONENTS 7 | ) 8 | 9 | catkin_package( 10 | ) 11 | 12 | catkin_python_setup() 13 | -------------------------------------------------------------------------------- /bluerov_control/launch/params/move_base/local_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | global_frame: map 3 | robot_base_frame: base_link 4 | update_frequency: 5.0 5 | publish_frequency: 5.0 6 | width: 10 7 | height: 10 8 | resolution: 0.5 9 | static_map: false 10 | rolling_window: true 11 | -------------------------------------------------------------------------------- /rti_dvl/scripts/minicom.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | if ! [ -x "$(command -v minicom)" ]; then 4 | echo "minicom not found" 5 | sudo apt-get install minicom 6 | fi 7 | 8 | socat pty,link=/tmp/rti_dvl,waitslave,raw,echo=0,ignoreeof tcp:192.168.2.2:14660 & 9 | sleep 1 10 | minicom -D /tmp/rti_dvl 11 | killall socat 12 | -------------------------------------------------------------------------------- /bluerov_control/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | project(bluerov_control) 3 | 4 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -O3") 5 | 6 | find_package(catkin REQUIRED COMPONENTS 7 | rospy 8 | bluerov_bridge 9 | sensor_msgs 10 | geometry_msgs 11 | nav_core 12 | ) 13 | 14 | catkin_package() 15 | 16 | -------------------------------------------------------------------------------- /bluerov_control/launch/params/move_base/move_base_params.yaml: -------------------------------------------------------------------------------- 1 | shutdown_costmaps: false 2 | 3 | controller_frequency: 10.0 4 | controller_patience: 5.0 5 | 6 | planner_frequency: 3.0 7 | planner_patience: 10.0 8 | 9 | oscillation_timeout: 0.0 10 | oscillation_distance: 0.5 11 | 12 | recovery_behavior_enabled: true 13 | clearing_rotation_allowed: true -------------------------------------------------------------------------------- /bluerov_bridge/setup.py: -------------------------------------------------------------------------------- 1 | ## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD 2 | 3 | from distutils.core import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | # fetch values from package.xml 7 | setup_args = generate_distutils_setup( 8 | packages=['bluerov_bridge'], 9 | package_dir={'': 'src'}) 10 | 11 | setup(**setup_args) -------------------------------------------------------------------------------- /bar30_depth/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(bar30_depth) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | rospy 6 | std_msgs 7 | bluerov_bridge 8 | message_generation 9 | ) 10 | 11 | add_message_files( 12 | FILES 13 | Depth.msg 14 | ) 15 | 16 | generate_messages( 17 | DEPENDENCIES 18 | std_msgs 19 | ) 20 | 21 | catkin_package() 22 | -------------------------------------------------------------------------------- /rti_dvl/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(rti_dvl) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | rospy 6 | geometry_msgs 7 | message_generation 8 | ) 9 | 10 | add_message_files( 11 | FILES 12 | DVL.msg 13 | Command.msg 14 | BottomTrack.msg 15 | ) 16 | 17 | generate_messages( 18 | DEPENDENCIES 19 | geometry_msgs 20 | ) 21 | 22 | catkin_package() 23 | -------------------------------------------------------------------------------- /sonar_oculus/README.md: -------------------------------------------------------------------------------- 1 | # oculus_driver 2 | ROS device driver for Blueprint Subsea's Oculus multibeam sonars 3 | 4 | Screenshot_from_2018_04_25_17_18_36 5 | 6 | # Usage 7 | 8 | ``` 9 | roslaunch sonar_oculus sonar_oculus.launch 10 | 11 | # play bag file 12 | roslaunch sonar_oculus sonar_oculus.launch sonar:=false 13 | ``` -------------------------------------------------------------------------------- /sonar_oculus/msg/OculusPing.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | OculusFire fire_msg 4 | 5 | uint32 ping_id 6 | uint16 part_number 7 | uint32 start_time 8 | 9 | int16[] bearings # bearings of beams (bearing * PI / 18000) 10 | float64 range_resolution # length of a single range bin 11 | uint32 num_ranges # number of range lines in the image 12 | uint32 num_beams # number of bearings in the image 13 | 14 | sensor_msgs/Image ping 15 | -------------------------------------------------------------------------------- /rti_dvl/msg/BottomTrack.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | rti_dvl/Command command 3 | 4 | # Sample number 5 | uint64 sample 6 | # Start time of the sample in second 7 | float64 time 8 | # Velocity in m/s in body frame 9 | geometry_msgs/Vector3 velocity 10 | # Orientation (roll, pitch, yaw) at the sample 11 | geometry_msgs/Vector3 orientation 12 | # Temperature in Celsius 13 | float64 temperature 14 | # Depth below transducer in m 15 | float64 altitude 16 | # Pressure in BAR 17 | float64 pressure 18 | -------------------------------------------------------------------------------- /bluerov_bridge/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | bluerov_bridge 4 | 0.0.0 5 | Bridge between mavlink and ROS 6 | Jinkun Wang 7 | BSD 3-Clause License 8 | Jinkun Wang 9 | 10 | catkin 11 | rospy 12 | python-pymavlink 13 | 14 | -------------------------------------------------------------------------------- /bluerov_launch/README.md: -------------------------------------------------------------------------------- 1 | # bluerov_launch 2 | 3 | ## Installation 4 | 5 | ### [rqt_multiplot_plugin](https://github.com/ANYbotics/rqt_multiplot_plugin) 6 | 7 | - Install [variant](https://github.com/ANYbotics/variant). Check out [this](https://github.com/ANYbotics/variant/pull/7) pull request if error occurs. 8 | 9 | - Set `QWT_LIBRARIES` to `rqt_multiplot_plugin/rqt_multiplot/CMakeLists.txt` if not found. 10 | 11 | ``` 12 | set(QWT_LIBRARIES /usr/lib/libqwt-qt5.so.6) 13 | ``` 14 | 15 | 16 | -------------------------------------------------------------------------------- /waterlinked_gps/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | waterlinked_gps 4 | 0.0.1 5 | ROS Driver for the Waterlinked Underwater GPS 6 | 7 | Muthu Chandrasekaran 8 | Jinkun Wang 9 | BSD 3-Clause License 10 | catkin 11 | rospy 12 | 13 | -------------------------------------------------------------------------------- /rti_dvl/scripts/pynmea2/__init__.py: -------------------------------------------------------------------------------- 1 | # pylint: disable=missing-docstring 2 | # pylint: disable=wildcard-import 3 | # pylint: disable=invalid-name 4 | 5 | from ._version import __version__ 6 | version = __version__ 7 | 8 | from .nmea import NMEASentence, ProprietarySentence, QuerySentence 9 | from .nmea import ChecksumError, ParseError, SentenceTypeError 10 | 11 | parse = NMEASentence.parse 12 | 13 | from .types import * 14 | 15 | from .stream import NMEAStreamReader 16 | from .nmea_file import NMEAFile 17 | 18 | -------------------------------------------------------------------------------- /bluerov_control/launch/params/move_base/costmap_common_params.yaml: -------------------------------------------------------------------------------- 1 | map_type: costmap 2 | origin_z: 0.0 3 | z_resolution: 1 4 | z_voxels: 2 5 | 6 | obstacle_range: 2.5 7 | raytrace_range: 3.0 8 | 9 | publish_voxel_map: false 10 | transform_tolerance: 3.0 11 | meter_scoring: true 12 | 13 | footprint: [[-0.21, -0.165], [-0.21, 0.165], [0.21, 0.165], [0.21, -0.165]] 14 | footprint_padding: 0.1 15 | 16 | plugins: 17 | - {name: inflater_layer, type: "costmap_2d::InflationLayer"} 18 | 19 | inflater_layer: 20 | inflation_radius: 0.3 21 | -------------------------------------------------------------------------------- /rti_dvl/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rti_dvl 4 | 0.0.1 5 | The RTI DVL ROS driver 6 | Jinkun Wang 7 | BSD 3-Clause License 8 | https://github.com/RobustFieldAutonomyLab/bluerov 9 | 10 | catkin 11 | 12 | rospy 13 | python-serial 14 | -------------------------------------------------------------------------------- /bar30_depth/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | bar30_depth 4 | 0.0.1 5 | The Bar30 depth sensor ROS driver 6 | Jinkun Wang 7 | BSD 3-Clause License 8 | https://github.com/RobustFieldAutonomyLab/bluerov 9 | 10 | catkin 11 | 12 | rospy 13 | bluerov_bridge 14 | -------------------------------------------------------------------------------- /bluerov_control/launch/params/move_base/global_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | global_frame: map 3 | robot_base_frame: base_link 4 | update_frequency: 2.0 5 | publish_frequency: 2.0 6 | width: 40.0 7 | height: 40.0 8 | resolution: 0.5 9 | origin_x: -10.0 10 | origin_y: -10.0 11 | static_map: true 12 | rolling_window: false 13 | static_layer: {map_topic: /map} 14 | 15 | plugins: 16 | - {name: static_layer, type: "costmap_2d::StaticLayer"} 17 | - {name: inflater_layer, type: "costmap_2d::InflationLayer"} 18 | -------------------------------------------------------------------------------- /bluerov_launch/scripts/mavproxy.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | # 14551 - qgc 4 | # 14552 - depth 5 | # 14553 - control 6 | # 14554 - dashboard 7 | 8 | if [ ! -x "$(command -v mavproxy.py)" ]; then 9 | echo "MAVProxy not found..." 10 | echo "Install MAVProxy..." 11 | sudo pip install MAVProxy 12 | fi 13 | 14 | # Otherwise mavlink can't receive any message! 15 | timeout 1s bash -c "echo -ne '\n' | netcat -ul 14550" 16 | mavproxy.py --master=udp:192.168.2.1:14550 --out=udp:192.168.2.1:14551 --out=udp:192.168.2.1:14552 --out=udp:192.168.2.1:14553 --out=udp:192.168.2.1:14554 --speech --logfile /tmp/mav.tlog -------------------------------------------------------------------------------- /bluerov_launch/scripts/qgc.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | if [ ! -f "/usr/local/bin/QGroundControl.AppImage" ]; then 4 | echo "QGroundControl not found..." 5 | echo "Install QGroundControl..." 6 | 7 | sudo usermod -a -G dialout $USER 8 | sudo apt-get remove modemmanager -y 9 | sudo apt install gstreamer1.0-plugins-bad gstreamer1.0-libav -y 10 | 11 | sudo wget https://s3-us-west-2.amazonaws.com/qgroundcontrol/latest/QGroundControl.AppImage -P /usr/local/bin/ 12 | sudo chmod +x /usr/local/bin/QGroundControl.AppImage 13 | fi 14 | 15 | /usr/local/bin/QGroundControl.AppImage &> /dev/null -------------------------------------------------------------------------------- /bluerov/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | bluerov 4 | 0.0.1 5 | 6 | The BlueROV ROS package supports VectorNav VN-100 IMU, Oculus Sonar and RTI Seapilot DVL. 7 | 8 | Jinkun Wang 9 | Jinkun Wang 10 | BSD 3-Clause License 11 | 12 | https://github.com/RobustFieldAutonomyLab/bluerov 13 | 14 | catkin 15 | python-catkin-tools 16 | -------------------------------------------------------------------------------- /bluerov_control/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | bluerov_control 4 | 0.0.0 5 | Send twist message to BlueROV 6 | Jinkun Wang 7 | BSD 3-Clause License 8 | Jinkun Wang 9 | 10 | catkin 11 | rospy 12 | move_base 13 | teb_local_planner 14 | map_server 15 | bluerov_bridge 16 | sensor_msgs 17 | geometry_msgs 18 | joy 19 | 20 | -------------------------------------------------------------------------------- /imu_vn_100/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | imu_vn_100 4 | 0.0.0 5 | The imu_vn_100 package 6 | 7 | Ke Sun 8 | Apache 2.0 9 | 10 | Ke Sun 11 | 12 | catkin 13 | 14 | roscpp 15 | sensor_msgs 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /bluerov_launch/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | bluerov_launch 4 | 0.0.1 5 | The bluerov_launch package 6 | 7 | Jinkun Wang 8 | BSD 3-Clause License 9 | 10 | catkin 11 | bar30_depth 12 | bluerov_control 13 | sonar_oculus 14 | rti_dvl 15 | imu_vn_100 16 | 17 | socat 18 | 19 | python-pymavlink 20 | cv_bridge 21 | -------------------------------------------------------------------------------- /rti_dvl/scripts/pynmea2/types/proprietary/rdi.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Support for proprietary message(s) from RD Instruments 3 | ''' 4 | 5 | from ... import nmea 6 | 7 | 8 | class RDI(nmea.ProprietarySentence): 9 | ''' 10 | RD Instruments message. Only one sentence known? 11 | ''' 12 | sentence_types = {} 13 | def __new__(_cls, manufacturer, data): 14 | name = manufacturer + data[0] 15 | cls = _cls.sentence_types.get(name, _cls) 16 | return super(RDI, cls).__new__(cls) 17 | 18 | 19 | class RDID(RDI): 20 | ''' 21 | RD Instruments heading, pitch and roll data 22 | ''' 23 | fields = ( 24 | ('Subtype', 'subtype'), 25 | ("Pitch", "pitch", float), 26 | ("Roll", "roll", float), 27 | ("Heading", "heading", float) 28 | ) 29 | -------------------------------------------------------------------------------- /sonar_oculus/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sonar_oculus 4 | 1.0.0 5 | The sonar_oculus package 6 | Vincent Sieben 7 | Pedro Vaz Teixeira 8 | Jinkun Wang 9 | Schlumberger retains all rights and privledges 10 | 11 | catkin 12 | 13 | roscpp 14 | sensor_msgs 15 | dynamic_reconfigure 16 | message_generation 17 | message_runtime 18 | cv_bridge 19 | python-scipy 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /bluerov_control/launch/joy_control.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /sonar_oculus/launch/sonar_oculus.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /bluerov_control/launch/params/move_base/dwa_local_planner_params.yaml: -------------------------------------------------------------------------------- 1 | DWAPlannerROS: 2 | 3 | acc_lim_th: 9.0 4 | acc_lim_x: 8.0 5 | acc_lim_y: 0.0 6 | 7 | max_vel_x: 1.0 8 | min_vel_x: 0.5 9 | 10 | max_vel_y: 0 11 | min_vel_y: 0 12 | 13 | max_trans_vel: 1.0 14 | min_trans_vel: 0.5 15 | max_rot_vel: 0.6 16 | min_rot_vel: 0.3 17 | 18 | xy_goal_tolerance: 0.5 19 | yaw_goal_tolerance: 0.314 20 | latch_xy_goal_tolerance: false 21 | 22 | sim_time: 2.0 23 | sim_granularity: 0.025 24 | goal_distance_bias: 100.0 25 | path_distance_bias: 0.0 26 | occdist_scale: 0.1 27 | stop_time_buffer: 0.2 28 | oscillation_reset_dist: 0.05 29 | forward_point_distance: 0.325 30 | scaling_speed: 0.25 31 | max_scaling_factor: 0.2 32 | vx_samples: 10 33 | vy_samples: 10 34 | vtheta_samples: 20 35 | sim_period: 0.1 36 | xy_goal_tolerance: 0.2 37 | yaw_goal_tolerance: 0.17 38 | rot_stopped_vel: 0.01 39 | trans_stopped_vel: 0.01 -------------------------------------------------------------------------------- /sonar_oculus/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(sonar_oculus) 3 | 4 | add_compile_options(-std=c++11) 5 | 6 | find_package(catkin REQUIRED COMPONENTS 7 | roscpp 8 | sensor_msgs 9 | message_generation 10 | dynamic_reconfigure 11 | ) 12 | 13 | add_message_files( 14 | FILES 15 | OculusFire.msg 16 | OculusPing.msg 17 | ) 18 | 19 | generate_messages( 20 | DEPENDENCIES 21 | sensor_msgs 22 | ) 23 | 24 | generate_dynamic_reconfigure_options( 25 | cfg/OculusParams.cfg 26 | ) 27 | 28 | catkin_package( 29 | CATKIN_DEPENDS roscpp sensor_msgs message_runtime 30 | ) 31 | 32 | include_directories( 33 | ${catkin_INCLUDE_DIRS} 34 | ) 35 | 36 | add_executable(${PROJECT_NAME}_node src/sonar.cpp src/OculusClient.cpp) 37 | 38 | add_dependencies(${PROJECT_NAME}_node ${PROJECT_NAME}_gencfg) 39 | 40 | add_dependencies(${PROJECT_NAME}_node ${PROJECT_NAME}_generate_messages_cpp) 41 | 42 | target_link_libraries(${PROJECT_NAME}_node 43 | ${catkin_LIBRARIES} 44 | ${roslib_LIBRARIES} 45 | ) 46 | -------------------------------------------------------------------------------- /rti_dvl/scripts/pynmea2/seatalk_utils.py: -------------------------------------------------------------------------------- 1 | # pylint: disable=invalid-name 2 | class SeaTalk(object): 3 | '''Mixin to add Seatalk functionality. Based on Thomas knauf's work 4 | http://www.thomasknauf.de/seatalk.htm''' 5 | byte_to_command = { 6 | '00': 'Depth below transducer', 7 | '01': 'Equipment ID', 8 | '05': 'Engine RPM and PITCH', 9 | '10': 'Apparent Wind Angle', 10 | '11': 'Apparent Wind Speed', 11 | '20': 'Speed through water', 12 | '50': 'LAT position', 13 | '51': 'LON position', 14 | '52': 'Speed over Ground', 15 | '53': 'Course over Ground', 16 | '82': 'Target waypoint name', 17 | '84': 'Compass heading Autopilot course and Rudder position', 18 | '9C': 'Compass heading and Rudder position' 19 | } 20 | 21 | # pylint: disable=no-member 22 | @property 23 | def command_name(self): 24 | '''Get seatalk command's meaning''' 25 | return self.byte_to_command.get(self.cmd, 'Unknown Command') 26 | 27 | 28 | -------------------------------------------------------------------------------- /rti_dvl/scripts/pynmea2/LICENSE: -------------------------------------------------------------------------------- 1 | Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 2 | 3 | The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 4 | 5 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 6 | -------------------------------------------------------------------------------- /imu_vn_100/src/imu_vn_100_cont.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright [2015] [Ke Sun] 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include 18 | #include 19 | 20 | using namespace imu_vn_100; 21 | 22 | int main(int argc, char** argv) { 23 | ros::init(argc, argv, "imu_vn_100"); 24 | ros::NodeHandle pnh("~"); 25 | 26 | try { 27 | ImuVn100 imu(pnh); 28 | imu.Stream(true); 29 | ros::spin(); 30 | } catch (const std::exception& e) { 31 | ROS_INFO("%s: %s", pnh.getNamespace().c_str(), e.what()); 32 | } 33 | } 34 | -------------------------------------------------------------------------------- /imu_vn_100/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(imu_vn_100) 3 | 4 | set(CMAKE_CXX_FLAGS "-std=gnu++11") 5 | 6 | find_package(catkin REQUIRED COMPONENTS 7 | roscpp 8 | sensor_msgs 9 | ) 10 | find_package(Boost REQUIRED) 11 | 12 | catkin_package( 13 | INCLUDE_DIRS include vncpplib/include 14 | LIBRARIES imu_vn_100 15 | CATKIN_DEPENDS roscpp sensor_msgs 16 | DEPENDS Boost 17 | ) 18 | 19 | include_directories( 20 | include 21 | vncpplib/include 22 | ${catkin_INCLUDE_DIRS} 23 | ) 24 | 25 | 26 | add_library(imu_vn_100 27 | vncpplib/src/arch/linux/vncp_services.c 28 | vncpplib/src/vndevice.c 29 | vncpplib/src/vn100.c 30 | src/imu_vn_100.cpp 31 | ) 32 | target_link_libraries(imu_vn_100 33 | ${catkin_LIBRARIES} 34 | ) 35 | add_dependencies(imu_vn_100 36 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 37 | ${catkin_EXPORTED_TARGETS} 38 | ) 39 | 40 | add_executable(imu_vn_100_cont_node 41 | src/imu_vn_100_cont.cpp) 42 | target_link_libraries(imu_vn_100_cont_node 43 | ${catkin_LIBRARIES} 44 | imu_vn_100 45 | ) 46 | add_dependencies(imu_vn_100_cont_node 47 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 48 | ${catkin_EXPORTED_TARGETS} 49 | ) 50 | -------------------------------------------------------------------------------- /imu_vn_100/vncpplib/LICENSE.txt: -------------------------------------------------------------------------------- 1 | The MIT License (MIT) 2 | 3 | Copyright (c) 2014 VectorNav Technologies, LLC 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /sonar_oculus/src/DataWrapper.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * (c) Copyright 2017 Blueprint Subsea. 3 | * This file is part of Oculus Viewer 4 | * 5 | * Oculus Viewer is free software: you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation, either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * Oculus Viewer is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License 16 | * along with this program. If not, see . 17 | * 18 | *****************************************************************************/ 19 | 20 | #pragma once 21 | 22 | // ---------------------------------------------------------------------------- 23 | // User data configuration structure 24 | #pragma pack(push, 1) 25 | struct UserConfig 26 | { 27 | uint32_t m_ipAddr; 28 | uint32_t m_ipMask; 29 | uint32_t m_bDhcpEnable; 30 | }; 31 | #pragma pack(pop) 32 | 33 | -------------------------------------------------------------------------------- /INSTALL.md: -------------------------------------------------------------------------------- 1 | # Installation 2 | 3 | ``` 4 | rosdep install --from-paths src --ignore-src -r -y 5 | 6 | # MAVProxy and QGroundControl will automatically be installed when calling 7 | # rosrun bluerov_launch mavproxy.sh 8 | # rosrun bluerov_launch qgc.sh 9 | ``` 10 | 11 | # Build workspace 12 | 13 | ``` 14 | $ mkdir -p WORKSPACE/src 15 | $ cd WORKSPACE 16 | $ catkin init 17 | $ catkin config --merge-devel 18 | $ catkin config --cmake-args -DCMAKE_BUILD_TYPE=Releas 19 | ``` 20 | 21 | # Manual installation 22 | 23 | ## Dependencies 24 | 25 | ``` 26 | #sudo apt-get install python-pip 27 | sudo pip install catkin_tools mavproxy pymavlink scipy 28 | 29 | # install ROS dependencies 30 | sudo apt-get install ros-DISTRO-joy ros-DISTRO-cv-bridge 31 | ``` 32 | 33 | ## QGroundControl 34 | 35 | Before installing QGroundControl for the first time: 36 | 37 | ``` 38 | sudo usermod -a -G dialout $USER 39 | sudo apt-get remove modemmanager -y 40 | sudo apt install gstreamer1.0-plugins-bad gstreamer1.0-libav -y 41 | ``` 42 | 43 | Logout and login again to enable the change to user permissions. 44 | 45 | To install QGroundControl for Ubuntu Linux 16.04 LTS or later, Download [QGroundControl.AppImage](https://s3-us-west-2.amazonaws.com/qgroundcontrol/latest/QGroundControl.AppImage) 46 | 47 | ``` 48 | chmod +x ./QGroundControl.AppImage 49 | sudo mv ./QGroundControl.AppImage /usr/local/bin/QGroundControl.AppImage 50 | ``` 51 | -------------------------------------------------------------------------------- /rti_dvl/scripts/pynmea2/types/proprietary/srf.py: -------------------------------------------------------------------------------- 1 | # SiRF 2 | 3 | from ... import nmea 4 | 5 | 6 | class SRF(nmea.ProprietarySentence): 7 | sentence_types = {} 8 | 9 | def __new__(_cls, manufacturer, data): 10 | name = manufacturer + data[0] 11 | cls = _cls.sentence_types.get(name, _cls) 12 | return super(SRF, cls).__new__(cls) 13 | 14 | def __init__(self, manufacturer, data): 15 | self.sentence_type = manufacturer + data[0] 16 | super(SRF, self).__init__(manufacturer, data) 17 | 18 | 19 | class SRF103(SRF): 20 | fields = ( 21 | ("Subtype", "subtype"), 22 | ('Sentence type', 'sentence'), 23 | # 00=GGA 24 | # 01=GLL 25 | # 02=GSA 26 | # 03=GSV 27 | # 04=RMC 28 | # 05=VTG 29 | ('Command', 'command'), 30 | # 0=Set 31 | # 1=Query 32 | ('Rate', 'rate'), 33 | ('Checksum', 'checksum'), 34 | # 0=No, 1=Yes 35 | ) 36 | 37 | 38 | class SRF100(SRF): 39 | fields = ( 40 | ("Subtype", "subtype"), 41 | ('Protocol', 'protocol'), 42 | # 0 = SiRF Binary 43 | # 1 = NMEA 44 | ('Baud Rate', 'baud'), 45 | # 1200, 2400, 4800, 9600, 19200, 38400, 57600, 115200 46 | ('Data bits', 'databits'), 47 | # 8 (, 7 in NMEA) 48 | ('Stop bits', 'stopbits'), 49 | # 0, 1 50 | ('Parity', 'parity'), 51 | # 0, 1=Odd, 2=Even 52 | ) 53 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2018, Robust Field Autonomy Lab 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | * Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | * Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | * Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /imu_vn_100/.travis.yml: -------------------------------------------------------------------------------- 1 | sudo: required 2 | dist: trusty 3 | language: minimal # Force travis to use its minimal image with default Python settings 4 | compiler: 5 | - gcc 6 | notifications: 7 | email: 8 | recipients: 9 | - sunke.polyu@gmail.com 10 | before_install: # Use this to prepare the system to install prerequisites or dependencies 11 | # Define some config vars 12 | - export ROS_DISTRO=indigo 13 | - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu trusty main" > /etc/apt/sources.list.d/ros-latest.list' 14 | - wget http://packages.ros.org/ros.key -O - | sudo apt-key add - 15 | - sudo apt-get update -qq 16 | install: # Use this to install any prerequisites or dependencies necessary to run your build 17 | - sudo apt-get install -q -y python-rosdep 18 | # Setup rosdep 19 | - sudo rosdep init 20 | - rosdep update 21 | before_script: # Use this to prepare your build for testing e.g. copy database configurations, environment variables, etc. 22 | - export CI_SOURCE_PATH=$(pwd) 23 | - export REPOSITORY_NAME=${PWD##*/} 24 | - echo "Testing branch $TRAVIS_BRANCH of $REPOSITORY_NAME" 25 | # Create workspace 26 | - mkdir -p ~/ros/catkin_ws/src 27 | - cd ~/ros/catkin_ws/src 28 | - ln -s $CI_SOURCE_PATH . # Link the repo we are testing to the new workspace 29 | - rosdep install --from-paths . --ignore-src --rosdistro $ROS_DISTRO -y 30 | - source /opt/ros/$ROS_DISTRO/setup.bash 31 | - catkin_init_workspace 32 | - cd ../ 33 | - export ROS_PARALLEL_JOBS='-j2 -l2' # Limit parallel jobs 34 | script: # All commands must exit with code 0 on success. Anything else is considered failure. 35 | - catkin_make -DCMAKE_BUILD_TYPE=Release 36 | -------------------------------------------------------------------------------- /imu_vn_100/vncpplib/include/vectornav.h: -------------------------------------------------------------------------------- 1 | /** 2 | * \file 3 | * 4 | * \section LICENSE 5 | * The MIT License (MIT) 6 | * 7 | * Copyright (c) 2014 VectorNav Technologies, LLC 8 | * 9 | * Permission is hereby granted, free of charge, to any person obtaining a copy 10 | * of this software and associated documentation files (the "Software"), to deal 11 | * in the Software without restriction, including without limitation the rights 12 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 13 | * copies of the Software, and to permit persons to whom the Software is 14 | * furnished to do so, subject to the following conditions: 15 | * 16 | * The above copyright notice and this permission notice shall be included in all 17 | * copies or substantial portions of the Software. 18 | * 19 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 20 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 21 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 22 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 23 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 24 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 25 | * SOFTWARE. 26 | * 27 | * \section DESCRIPTION 28 | * This header file includes all of the header file inclusions needed to use 29 | * all of the features of the VectorNav C/C++ Library. 30 | */ 31 | #ifndef _VECTORNAV_H_ 32 | #define _VECTORNAV_H_ 33 | 34 | #include "vn100.h" 35 | #include "vn200.h" 36 | #include "vn_math.h" 37 | 38 | #endif /* VECTORNAV_H */ 39 | -------------------------------------------------------------------------------- /rti_dvl/scripts/pynmea2/types/proprietary/grm.py: -------------------------------------------------------------------------------- 1 | # Garmin 2 | 3 | from decimal import Decimal 4 | 5 | from ... import nmea 6 | 7 | class GRM(nmea.ProprietarySentence): 8 | sentence_types = {} 9 | 10 | def __new__(_cls, manufacturer, data): 11 | name = manufacturer + data[0] 12 | cls = _cls.sentence_types.get(name, _cls) 13 | return super(GRM, cls).__new__(cls) 14 | 15 | def __init__(self, manufacturer, data): 16 | self.sentence_type = manufacturer + data[0] 17 | super(GRM, self).__init__(manufacturer, data) 18 | 19 | 20 | class GRME(GRM): 21 | """ GARMIN Estimated position error 22 | """ 23 | fields = ( 24 | ("Subtype", "subtype"), 25 | ("Estimated Horiz. Position Error", "hpe", Decimal), 26 | ("Estimated Horiz. Position Error Unit (M)", "hpe_unit"), 27 | ("Estimated Vert. Position Error", "vpe", Decimal), 28 | ("Estimated Vert. Position Error Unit (M)", "vpe_unit"), 29 | ("Estimated Horiz. Position Error", "osepe", Decimal), 30 | ("Overall Spherical Equiv. Position Error", "osepe_unit") 31 | ) 32 | 33 | 34 | class GRMM(GRM): 35 | """ GARMIN Map Datum 36 | """ 37 | fields = ( 38 | ("Subtype", "subtype"), 39 | ('Currently Active Datum', 'datum'), 40 | ) 41 | 42 | 43 | class GRMZ(GRM): 44 | """ GARMIN Altitude Information 45 | """ 46 | fields = ( 47 | ("Subtype", "subtype"), 48 | ("Altitude", "altitude", Decimal), 49 | ("Altitude Units (Feet)", "altitude_unit"), 50 | ("Positional Fix Dimension (2=user, 3=GPS)", "pos_fix_dim") 51 | ) 52 | 53 | 54 | 55 | -------------------------------------------------------------------------------- /bluerov_control/launch/move_base.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /imu_vn_100/vncpplib/include/vn_math.h: -------------------------------------------------------------------------------- 1 | /** 2 | * \file 3 | * 4 | * \section LICENSE 5 | * The MIT License (MIT) 6 | * 7 | * Copyright (c) 2014 VectorNav Technologies, LLC 8 | * 9 | * Permission is hereby granted, free of charge, to any person obtaining a copy 10 | * of this software and associated documentation files (the "Software"), to deal 11 | * in the Software without restriction, including without limitation the rights 12 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 13 | * copies of the Software, and to permit persons to whom the Software is 14 | * furnished to do so, subject to the following conditions: 15 | * 16 | * The above copyright notice and this permission notice shall be included in all 17 | * copies or substantial portions of the Software. 18 | * 19 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 20 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 21 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 22 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 23 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 24 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 25 | * SOFTWARE. 26 | * 27 | * \section DESCRIPTION 28 | * This header file provides access the math module of the VectorNav C/C++ 29 | * Library. 30 | * 31 | * Notes 32 | * - Indexes used within the math library use 0 based indexing. For example, 33 | * the first component of a 3 dimensional vector is referenced in code 34 | * as vector3->c0. 35 | */ 36 | #ifndef _VN_MATH_H_ 37 | #define _VN_MATH_H_ 38 | 39 | #include "vn_linearAlgebra.h" 40 | #include "vn_kinematics.h" 41 | 42 | #endif /* _VN_MATH_H_ */ -------------------------------------------------------------------------------- /imu_vn_100/vncpplib/include/vn_common.h: -------------------------------------------------------------------------------- 1 | /** 2 | * \file 3 | * 4 | * \section LICENSE 5 | * The MIT License (MIT) 6 | * 7 | * Copyright (c) 2014 VectorNav Technologies, LLC 8 | * 9 | * Permission is hereby granted, free of charge, to any person obtaining a copy 10 | * of this software and associated documentation files (the "Software"), to deal 11 | * in the Software without restriction, including without limitation the rights 12 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 13 | * copies of the Software, and to permit persons to whom the Software is 14 | * furnished to do so, subject to the following conditions: 15 | * 16 | * The above copyright notice and this permission notice shall be included in all 17 | * copies or substantial portions of the Software. 18 | * 19 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 20 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 21 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 22 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 23 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 24 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 25 | * SOFTWARE. 26 | * 27 | * \section DESCRIPTION 28 | * This header file provides access to features commonly used throughout the 29 | * entire VectorNav C/C++ Library. 30 | */ 31 | #ifndef _VN_COMMON_H_ 32 | #define _VN_COMMON_H_ 33 | 34 | #include "vn_errorCodes.h" 35 | 36 | /** 37 | * \brief Retrieves the last error code encountered. Note that the stored error 38 | * code is cleared on the first call to this function. 39 | * 40 | * \return The VectorNav error code. 41 | */ 42 | VN_ERROR_CODE vn_getErrorCode(); 43 | 44 | #endif /* _VN_COMMON_H_ */ -------------------------------------------------------------------------------- /imu_vn_100/vncpplib/include/vnint.h: -------------------------------------------------------------------------------- 1 | /** 2 | * \file 3 | * 4 | * \section LICENSE 5 | * The MIT License (MIT) 6 | * 7 | * Copyright (c) 2014 VectorNav Technologies, LLC 8 | * 9 | * Permission is hereby granted, free of charge, to any person obtaining a copy 10 | * of this software and associated documentation files (the "Software"), to deal 11 | * in the Software without restriction, including without limitation the rights 12 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 13 | * copies of the Software, and to permit persons to whom the Software is 14 | * furnished to do so, subject to the following conditions: 15 | * 16 | * The above copyright notice and this permission notice shall be included in all 17 | * copies or substantial portions of the Software. 18 | * 19 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 20 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 21 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 22 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 23 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 24 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 25 | * SOFTWARE. 26 | * 27 | * \section DESCRIPTION 28 | * This header file provides definitions for standard integer types since 29 | * Visual Studio 2008 and earlier do not include the stdint.h header file. 30 | */ 31 | #ifndef _VNINT_H_ 32 | #define _VNINT_H_ 33 | 34 | typedef signed __int8 int8_t; 35 | typedef signed __int16 int16_t; 36 | typedef signed __int32 int32_t; 37 | typedef signed __int64 int64_t; 38 | typedef unsigned __int8 uint8_t; 39 | typedef unsigned __int16 uint16_t; 40 | typedef unsigned __int32 uint32_t; 41 | typedef unsigned __int64 uint64_t; 42 | 43 | #endif /* _VNINT_H_ */ 44 | -------------------------------------------------------------------------------- /bar30_depth/scripts/bar30_depth_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import sys 3 | import socket 4 | import rospy 5 | from bar30_depth.msg import Depth 6 | 7 | from bluerov_bridge import Bridge 8 | 9 | FLUID_DENSITY = {'fresh': 9.97, 'salt': 10.29} 10 | 11 | if __name__ == '__main__': 12 | rospy.init_node('bar30_depth_node') 13 | device = rospy.get_param('~device', 'udp:192.168.2.1:14552') 14 | 15 | if not rospy.has_param('/water'): 16 | rospy.set_param('/water', 'salt') 17 | water = rospy.get_param('/water', 'fresh') 18 | 19 | while not rospy.is_shutdown(): 20 | try: 21 | bridge = Bridge(device, write=False) 22 | except socket.error: 23 | rospy.logerr( 24 | 'Failed to make mavlink connection to device {}'.format( 25 | device)) 26 | rospy.sleep(1.0) 27 | else: 28 | break 29 | if rospy.is_shutdown(): 30 | sys.exit(-1) 31 | 32 | bridge.update() 33 | 34 | depth_pub = rospy.Publisher('/bar30/depth/raw', Depth, queue_size=10) 35 | rate = rospy.Rate(100) 36 | while not rospy.is_shutdown(): 37 | msg = bridge.get_msg(type='SCALED_PRESSURE2') 38 | if msg is not None: 39 | d = Depth() 40 | d.header.stamp = rospy.Time.now() 41 | d.time = msg.time_boot_ms / 1000.0 42 | d.pressure_abs = msg.press_abs 43 | d.pressure_diff = msg.press_diff 44 | d.temperature = msg.temperature / 100.0 45 | # Assume pressure_diff is temperature compensated 46 | # https://github.com/bluerobotics/ardusub/blob/978cd64a1e3b0cb5ba1f3bcc995fcc39bea7e9ff/libraries/AP_Baro/AP_Baro_MS5611.cpp#L481 47 | # https://github.com/bluerobotics/ms5837-python/blob/c83bdc969ea1654a2e2759783546245709bd9914/ms5837.py#L146 48 | d.depth = d.pressure_diff / (FLUID_DENSITY[water] * 9.80665) 49 | depth_pub.publish(d) 50 | rate.sleep() 51 | -------------------------------------------------------------------------------- /rti_dvl/scripts/pynmea2/types/proprietary/chr.py: -------------------------------------------------------------------------------- 1 | from ... import nmea 2 | 3 | 4 | class CHR(nmea.ProprietarySentence): 5 | sentence_types = {} 6 | 7 | def __new__(_cls, manufacturer, data): 8 | name = manufacturer + data[0] 9 | cls = _cls.sentence_types.get(name, _cls) 10 | return super(CHR, cls).__new__(cls) 11 | 12 | 13 | class CHRS(CHR): 14 | fields = ( 15 | ('Identity', 'identity', str), 16 | ('The actual measured angular rate in degrees/s after calibration has been applied', 'rx', float), 17 | ('The actual measured angular rate in degrees/s after calibration has been applied', 'ry', float), 18 | ('The actual measured angular rate in degrees/s after calibration has been applied', 'rz', float), 19 | ('The time at which the last rate gyro data was measured', 'rt', float), 20 | ('The actual measured acceleration in m/s/s after calibration has been applied', 'ax', float), 21 | ('The actual measured acceleration in m/s/s after calibration has been applied', 'ay', float), 22 | ('The actual measured acceleration in m/s/s after calibration has been applied', 'az', float), 23 | ('The time at which the last acceleration data was measured', 'at', float), 24 | ('The actual measured magnetic field after calibration has been applied', 'mx', float), 25 | ('The actual measured magnetic field after calibration has been applied', 'my', float), 26 | ('The actual measured magnetic field after calibration has been applied', 'mz', float), 27 | ('The time at which the last magnetometer data was measured', 'mt', float) 28 | ) 29 | 30 | 31 | class CHRA(CHR): 32 | fields = ( 33 | ('Identity', 'identity', str), 34 | ('The estimated roll angle', 'r', float), 35 | ('The estimated pitch angle', 'p', float), 36 | ('The estimated yaw angle', 'y', float), 37 | ('The time that the Euler Angles were measured', 't', float) 38 | ) 39 | -------------------------------------------------------------------------------- /imu_vn_100/vncpplib/include/vn_kinematics.h: -------------------------------------------------------------------------------- 1 | /** 2 | * \file 3 | * 4 | * \section LICENSE 5 | * The MIT License (MIT) 6 | * 7 | * Copyright (c) 2014 VectorNav Technologies, LLC 8 | * 9 | * Permission is hereby granted, free of charge, to any person obtaining a copy 10 | * of this software and associated documentation files (the "Software"), to deal 11 | * in the Software without restriction, including without limitation the rights 12 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 13 | * copies of the Software, and to permit persons to whom the Software is 14 | * furnished to do so, subject to the following conditions: 15 | * 16 | * The above copyright notice and this permission notice shall be included in all 17 | * copies or substantial portions of the Software. 18 | * 19 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 20 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 21 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 22 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 23 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 24 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 25 | * SOFTWARE. 26 | * 27 | * \section DESCRIPTION 28 | * This header file provides access to the VectorNav Kinematics library. 29 | */ 30 | #ifndef _VN_KINEMATICS_H 31 | #define _VN_KINEMATICS_H 32 | 33 | /** 34 | * \brief Holds attitude data expressed in yaw, pitch, roll format. 35 | */ 36 | typedef struct { 37 | double yaw; /**< Yaw */ 38 | double pitch; /**< Pitch */ 39 | double roll; /**< Roll */ 40 | } VnYpr; 41 | 42 | /** 43 | * \brief Holds attitude data expressed in quaternion format. 44 | */ 45 | typedef struct { 46 | double x; /**< X */ 47 | double y; /**< Y */ 48 | double z; /**< Z */ 49 | double w; /**< W */ 50 | } VnQuaternion; 51 | 52 | #endif /* _VN_KINEMATICS_H */ 53 | -------------------------------------------------------------------------------- /sonar_oculus/cfg/OculusParams.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | PACKAGE = "sonar_oculus" 4 | 5 | import math 6 | 7 | from dynamic_reconfigure.parameter_generator_catkin import * 8 | 9 | 10 | gen = ParameterGenerator() 11 | 12 | mode_enum = gen.enum([ 13 | gen.const('LowFrequency', int_t, 1, 'M750d: 750kHz, M1200d: 1.2Mhz'), 14 | gen.const('HighFrequency', int_t, 2, 'M750d: 1.2Mhz, M1200d: 2.1Mhz') 15 | ], 'Mode') 16 | gen.add("Mode", int_t, 0, 'Frequency mode', 1, 1, 2, edit_method=mode_enum) 17 | ping_rate_enum = gen.enum([ 18 | gen.const('Normal', int_t, 0, '10Hz max ping rate'), 19 | gen.const('High', int_t, 1, '15Hz max ping rate'), 20 | gen.const('Highest', int_t, 2, '40Hz max ping rate'), 21 | gen.const('Low', int_t, 3, '5Hz max ping rate'), 22 | gen.const('Lowest', int_t, 4, '2Hz max ping rate') 23 | ], 'Ping Rate') 24 | gen.add("PingRate", int_t, 0, 'Ping rate', 3, 0, 4, edit_method=ping_rate_enum) 25 | gen.add("Gain", int_t, 0, "Gain in percentage", 20, 0, 100) 26 | # gen.add("Speed", int_t, 0, "Speed of sound in m/s", 0, 1400, 1600) 27 | gen.add("Range", int_t, 0, "Range in meters", 10, 1, 120) 28 | gen.add("Salinity", int_t, 0, "Salinity in ppm", 0, 0, 50) 29 | 30 | # Colormap for oculus_viewer 31 | cm_enum = gen.enum([gen.const("Autumn", int_t, 0, "Autumn"), 32 | gen.const("Bone", int_t, 1, "Bone"), 33 | gen.const("Cool", int_t, 2, "Cool"), 34 | gen.const("Hot", int_t, 3, "Hot"), 35 | gen.const("HSV", int_t, 4, "HSV"), 36 | gen.const("Jet", int_t, 5, "Jet"), 37 | gen.const("Ocean", int_t, 6, "Ocean"), 38 | gen.const("Pink", int_t, 7, "Pink"), 39 | gen.const("Rainbow", int_t, 8, "Rainbow"), 40 | gen.const("Spring", int_t, 9, "Spring"), 41 | gen.const("Summer", int_t, 10, "Summer"), 42 | gen.const("Winter", int_t, 11, "Winter")], 43 | "Colormap") 44 | gen.add("Colormap", int_t, 0, "Colormap for oculus_viewer", 2, 0, 11, edit_method=cm_enum) 45 | gen.add("Raw", bool_t, 0, "Raw image", False) 46 | 47 | exit(gen.generate(PACKAGE, "sonar_oculus", "OculusParams")) 48 | -------------------------------------------------------------------------------- /rti_dvl/scripts/pynmea2/nmea_file.py: -------------------------------------------------------------------------------- 1 | try: 2 | # pylint: disable=used-before-assignment 3 | basestring = basestring 4 | except NameError: # py3 5 | basestring = str 6 | 7 | from .nmea import NMEASentence 8 | 9 | 10 | class NMEAFile(object): 11 | """ 12 | Reads NMEA sentences from a file similar to a standard python file object. 13 | """ 14 | 15 | def __init__(self, f, *args, **kwargs): 16 | super(NMEAFile, self).__init__() 17 | if isinstance(f, basestring) or args or kwargs: 18 | self._file = self.open(f, *args, **kwargs) 19 | else: 20 | self._file = f 21 | self._context = None 22 | 23 | def open(self, fp, mode='r'): 24 | """ 25 | Open the NMEAFile. 26 | """ 27 | self._file = open(fp, mode=mode) 28 | return self._file 29 | 30 | def close(self): 31 | """ 32 | Close the NMEAFile. 33 | """ 34 | self._file.close() 35 | 36 | def __iter__(self): 37 | """ 38 | Iterate through the file yielding NMEASentences 39 | :return: 40 | """ 41 | for line in self._file: 42 | yield self.parse(line) 43 | 44 | def __enter__(self): 45 | if hasattr(self._file, '__enter__'): 46 | self._context = self._file.__enter__() 47 | return self 48 | 49 | def __exit__(self, exc_type, exc_val, exc_tb): 50 | if self._context: 51 | ctx = self._context 52 | self._context = None 53 | ctx.__exit__(exc_type, exc_val, exc_tb) 54 | 55 | def next(self): 56 | """ 57 | Iterate through the file object returning NMEASentence objects 58 | :return: NMEASentence 59 | """ 60 | data = self._file.readline() 61 | return self.parse(data) 62 | 63 | def parse(self, s): 64 | return NMEASentence.parse(s) 65 | 66 | def readline(self): 67 | """ 68 | Return the next NMEASentence in the file object 69 | :return: NMEASentence 70 | """ 71 | data = self._file.readline() 72 | s = self.parse(data) 73 | return s 74 | 75 | def read(self): 76 | """ 77 | Return a list of NMEASentence objects for each line in the file 78 | :return: list of NMEASentence objects 79 | """ 80 | return [s for s in self] 81 | -------------------------------------------------------------------------------- /rti_dvl/scripts/pynmea2/stream.py: -------------------------------------------------------------------------------- 1 | from __future__ import unicode_literals 2 | from . import nmea 3 | 4 | __all__ = ['NMEAStreamReader'] 5 | 6 | ERRORS = ('raise', 'yield', 'ignore') 7 | 8 | class NMEAStreamReader(object): 9 | ''' 10 | Reads NMEA sentences from a stream. 11 | ''' 12 | def __init__(self, stream=None, errors='raise'): 13 | ''' 14 | Create NMEAStreamReader object. 15 | 16 | `stream`: file-like object to read from, can be omitted to 17 | pass data to `next` manually. 18 | must support `.readline()` which returns a string 19 | 20 | `errors`: behaviour when a parse error is encountered. can be one of: 21 | `'raise'` (default) raise an exception immediately 22 | `'yield'` yield the ParseError as an element in the 23 | stream, and continue reading at the next line 24 | `'ignore'` completely ignore and suppress the error, and 25 | continue reading at the next line 26 | ''' 27 | 28 | if errors not in ERRORS: 29 | raise ValueError('errors must be one of {!r} (was: {!r})' 30 | .format(ERRORS, errors)) 31 | 32 | self.errors = errors 33 | self.stream = stream 34 | self.buffer = '' 35 | 36 | def next(self, data=None): 37 | ''' 38 | consume `data` (if given, or calls `stream.read()` if `stream` was given 39 | in the constructor) and yield a list of `NMEASentence` objects parsed 40 | from the stream (may be empty) 41 | ''' 42 | if data is None: 43 | if self.stream: 44 | data = self.stream.readline() 45 | else: 46 | return 47 | 48 | lines = (self.buffer + data).split('\n') 49 | self.buffer = lines.pop() 50 | 51 | for line in lines: 52 | try: 53 | msg = nmea.NMEASentence.parse(line) 54 | yield msg 55 | except nmea.ParseError as e: 56 | if self.errors == 'raise': 57 | raise e 58 | if self.errors == 'yield': 59 | yield e 60 | if self.errors == 'ignore': 61 | pass 62 | -------------------------------------------------------------------------------- /rti_dvl/scripts/pynmea2/types/proprietary/ubx.py: -------------------------------------------------------------------------------- 1 | # pylint: disable=wildcard-import,unused-wildcard-import 2 | from decimal import Decimal 3 | 4 | from ... import nmea 5 | from ...nmea_utils import * 6 | 7 | 8 | # u-blox 9 | class UBX(nmea.ProprietarySentence): 10 | sentence_types = {} 11 | 12 | def __new__(_cls, manufacturer, data): 13 | name = manufacturer + data[1] 14 | cls = _cls.sentence_types.get(name, _cls) 15 | return super(UBX, cls).__new__(cls) 16 | 17 | def __init__(self, manufacturer, data): 18 | self.sentence_type = manufacturer + data[1] 19 | super(UBX, self).__init__(manufacturer, data[2:]) 20 | 21 | 22 | class UBX00(UBX, LatLonFix): 23 | """ Lat/Long Position Data 24 | """ 25 | fields = ( 26 | ("Timestamp (UTC)", "timestamp", timestamp), 27 | ("Latitude", "lat"), 28 | ("Latitude Direction", "lat_dir"), 29 | ("Longitude", "lon"), 30 | ("Longitude Direction", "lon_dir"), 31 | ("Altitude above user datum ellipsoid", "alt_ref"), 32 | ("Navigation Status", "nav_stat"), 33 | ("Horizontal Accuracy Estimate", "h_acc"), 34 | ("Vertical Accuracy Estimate", "v_acc"), 35 | ("Speed over Ground", "sog"), 36 | ("Course over Ground", "cog"), 37 | ("Vertical Velocity", "v_vel"), 38 | ("Age of Differential Corrections", "diff_age"), 39 | ("Horizontal Dilution of Precision", "hdop"), 40 | ("Vertical Dilution of Precision", "vdop"), 41 | ("Time Dilution of Precision", "tdop"), 42 | ("Number of Satellites Used", "num_svs"), 43 | ("Reserved", "reserved") 44 | 45 | ) 46 | 47 | 48 | class UBX03(UBX): 49 | """ Satellite Status 50 | """ 51 | fields = ( 52 | ("Number of GNSS Satellites Tracked", "num_sv", int), 53 | ) 54 | 55 | @property 56 | def satellite_list(self): 57 | return self.data[1:] 58 | 59 | 60 | class UBX04(UBX): 61 | """ Time and Day Clock Information 62 | """ 63 | fields = ( 64 | ("UTC Time", "time", timestamp), 65 | ("UTC Date", "date", datestamp), 66 | ("UTC Time of Week", "utc_tow"), 67 | ("UTC Week Number", "utc_wk"), 68 | ("Leap Seconds", "leap_sec"), 69 | ("Receiver Clock Bias", "clk_bias", int), 70 | ("Receiver Clock Drift", "clk_drift", Decimal), 71 | ("Time Pulse Granularity", "tp_gran", int), 72 | ) 73 | -------------------------------------------------------------------------------- /imu_vn_100/vncpplib/include/vn_linearAlgebra.h: -------------------------------------------------------------------------------- 1 | /** 2 | * \file 3 | * 4 | * \section LICENSE 5 | * The MIT License (MIT) 6 | * 7 | * Copyright (c) 2014 VectorNav Technologies, LLC 8 | * 9 | * Permission is hereby granted, free of charge, to any person obtaining a copy 10 | * of this software and associated documentation files (the "Software"), to deal 11 | * in the Software without restriction, including without limitation the rights 12 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 13 | * copies of the Software, and to permit persons to whom the Software is 14 | * furnished to do so, subject to the following conditions: 15 | * 16 | * The above copyright notice and this permission notice shall be included in all 17 | * copies or substantial portions of the Software. 18 | * 19 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 20 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 21 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 22 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 23 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 24 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 25 | * SOFTWARE. 26 | * 27 | * \section DESCRIPTION 28 | * This header file provides access to the linear algebra features. 29 | * 30 | * Notes 31 | * - Indexes used within the math library use 0 based indexing. For example, 32 | * the first component of a 3 dimensional vector is referenced in code 33 | * as vector3->c0. 34 | */ 35 | #ifndef _VN_LINEAR_ALGEBRA_H_ 36 | #define _VN_LINEAR_ALGEBRA_H_ 37 | 38 | /** 39 | * \brief A vector of length 3. 40 | */ 41 | typedef struct { 42 | double c0; /**< Component 0 */ 43 | double c1; /**< Component 1 */ 44 | double c2; /**< Component 2 */ 45 | } VnVector3; 46 | 47 | /** 48 | * \brief A 3x3 matrix. 49 | */ 50 | typedef struct { 51 | double c00; /**< Component 0,0 */ 52 | double c01; /**< Component 0,1 */ 53 | double c02; /**< Component 0,2 */ 54 | double c10; /**< Component 1,0 */ 55 | double c11; /**< Component 1,1 */ 56 | double c12; /**< Component 1,2 */ 57 | double c20; /**< Component 2,0 */ 58 | double c21; /**< Component 2,1 */ 59 | double c22; /**< Component 2,2 */ 60 | } VnMatrix3x3; 61 | 62 | #endif /* _VN_LINEAR_ALGEBRA_H_ */ -------------------------------------------------------------------------------- /bluerov_launch/launch/bluerov_launch.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 | -------------------------------------------------------------------------------- /imu_vn_100/launch/vn_100_cont.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 | -------------------------------------------------------------------------------- /CONFIG.md: -------------------------------------------------------------------------------- 1 | # Configuration 2 | 3 | ## BlueROV2 4 | 5 | ### USB devices 6 | 7 | Create serial rules for devices in `/etc/udev/rules.d/99-usb-serial.rules`, which is intended to set up a consistent device name. The attributes can be found by `udevadm info`, e.g., `udevadm info -a /dev/ttyUSB0 | grep 'idVendor' | head -n1`. 8 | 9 | ``` 10 | SUBSYSTEM=="tty", ATTRS{idVendor}=="0856", ATTRS{idProduct}=="ac11", ATTRS{serial}=="BB9O7S8F", SYMLINK+="rowe_dvl", MODE="0777" 11 | SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6001", ATTRS{serial}=="A703G09Z", SYMLINK+="rowe_dvl", MODE="0777" 12 | ``` 13 | 14 | ### Remote serial ports 15 | 16 | ssh to `pi@192.169.2.2` and add the following lines to `companion/.companion.rc` to enable virtual serial ports over TCP. 17 | 18 | - DVL: 14660 19 | - IMU: 14661 20 | 21 | ``` 22 | socat tcp-listen:14661,reuseaddr,fork,ignoreeof file:/dev/ttyAMA0,nonblock,waitlock=/var/run/ttyAMA0.lock,b115200,raw,echo=0 & 23 | socat tcp-listen:14660,reuseaddr,fork file:/dev/rowe_dvl,nonblock,waitlock=/var/run/rowe_dvl.lock,b115200,raw,echo=0 & 24 | ``` 25 | 26 | ### Disable camera 27 | 28 | ssh to `pi@192.168.2.2` and comment the following line in `companion/.companion.rc`. 29 | 30 | ``` 31 | sudo -H -u pi screen -dm -S video $COMPANION_DIR/tools/streamer.py 32 | ``` 33 | 34 | ## Topside 35 | 36 | ### IP addresses 37 | 38 | - Computer: `192.168.2.1` 39 | - BlueROV2: `192.168.2.2` user:password: pi:companion 40 | - Oculus M750d: `192.168.2.3` 41 | - Oculus M120d: `192.168.2.4` 42 | 43 | 44 | ### QGroundControl 45 | 46 | - In General, disable auto connect to UDP and add two UDP Comm Links on ports 14550 and 14551. 47 | - Use 14551 with ROS as the messages are directed to 14551 (see [mavproxy.sh](./bluerov_launch/scripts/mavproxy.sh)). 48 | - Use 14550 without ROS. 49 | 50 | - Calibrate joystick. 51 | - [b0]/[b1]: arm/disarm 52 | - [b8]/[b3]: gain inc/dec 53 | - [a1]/[a0]/[a5]/[a2]: x/y/z/yaw 54 | - [a3]/[a4]: x/yaw cruise control 55 | 56 | ``` 57 | [b6] [b7] [b10] | 58 | | 59 | [a1 + ] [b8] [a4+|-] | [b11] 60 | [a0 -][a2-|+][a0 +] [b2] [b9] | [a5 u+] 61 | [a1 - ] [b3] [a3+|-] | [b5] 62 | | 63 | [b0] [b1] [b4] | 64 | ``` 65 | 66 | ### Set salinity 67 | 68 | For pressure sensor, the salinity can be configured in the launch file. But currently for DVL, configure the salinity by running 69 | 70 | ``` 71 | rosrun rti_dvl minicom.sh 72 | 73 | # Type 74 | STOP 75 | CWS 0 => fresh 35 => salt 76 | START 77 | ``` 78 | -------------------------------------------------------------------------------- /bluerov_control/launch/params/move_base/teb_local_planner_params.yaml: -------------------------------------------------------------------------------- 1 | TebLocalPlannerROS: 2 | 3 | # Trajectory 4 | 5 | teb_autosize: True 6 | dt_ref: 0.3 7 | dt_hysteresis: 0.1 8 | max_samples: 500 9 | global_plan_overwrite_orientation: True 10 | allow_init_with_backwards_motion: False 11 | max_global_plan_lookahead_dist: 3.0 12 | global_plan_viapoint_sep: -1 13 | global_plan_prune_distance: 1 14 | exact_arc_length: False 15 | feasibility_check_no_poses: 5 16 | publish_feedback: False 17 | 18 | # Robot 19 | 20 | max_vel_x: 0.4 21 | max_vel_x_backwards: 0.2 22 | max_vel_y: 0.1 23 | max_vel_theta: 0.3 24 | acc_lim_x: 0.2 25 | acc_lim_y: 0.1 26 | acc_lim_theta: 0.1 27 | min_turning_radius: 0.0 # omni-drive robot (can turn on place!) 28 | 29 | footprint_model: 30 | type: "point" 31 | 32 | # GoalTolerance 33 | 34 | xy_goal_tolerance: 0.2 35 | yaw_goal_tolerance: 0.1 36 | free_goal_vel: False 37 | complete_global_plan: True 38 | 39 | # Obstacles 40 | 41 | min_obstacle_dist: 0.25 # This value must also include our robot radius, since footprint_model is set to "point". 42 | inflation_dist: 0.6 43 | include_costmap_obstacles: True 44 | costmap_obstacles_behind_robot_dist: 1.0 45 | obstacle_poses_affected: 15 46 | costmap_converter_plugin: "" 47 | costmap_converter_spin_thread: True 48 | costmap_converter_rate: 5 49 | 50 | # Optimization 51 | 52 | no_inner_iterations: 5 53 | no_outer_iterations: 4 54 | optimization_activate: True 55 | optimization_verbose: False 56 | penalty_epsilon: 0.1 57 | obstacle_cost_exponent: 4 58 | weight_max_vel_x: 2 59 | weight_max_vel_y: 2 60 | weight_max_vel_theta: 1 61 | weight_acc_lim_x: 1 62 | weight_acc_lim_y: 1 63 | weight_acc_lim_theta: 1 64 | weight_kinematics_nh: 1 # WE HAVE A HOLONOMIC ROBOT, JUST ADD A SMALL PENALTY 65 | weight_kinematics_forward_drive: 1 66 | weight_kinematics_turning_radius: 1 67 | weight_optimaltime: 1 # must be > 0 68 | weight_shortest_path: 0 69 | weight_obstacle: 100 70 | weight_inflation: 0.2 71 | weight_dynamic_obstacle: 10 72 | weight_dynamic_obstacle_inflation: 0.2 73 | weight_viapoint: 1 74 | weight_adapt_factor: 2 75 | 76 | # Homotopy Class Planner 77 | 78 | enable_homotopy_class_planning: True 79 | enable_multithreading: True 80 | max_number_classes: 4 81 | selection_cost_hysteresis: 1.0 82 | selection_prefer_initial_plan: 0.9 83 | selection_obst_cost_scale: 1.0 84 | selection_alternative_time_cost: False 85 | 86 | roadmap_graph_no_samples: 15 87 | roadmap_graph_area_width: 5 88 | roadmap_graph_area_length_scale: 1.0 89 | h_signature_prescaler: 0.5 90 | h_signature_threshold: 0.1 91 | obstacle_heading_threshold: 0.45 92 | switching_blocking_period: 0.0 93 | viapoints_all_candidates: True 94 | delete_detours_backwards: True 95 | max_ratio_detours_duration_best_duration: 3.0 96 | visualize_hc_graph: False 97 | visualize_with_time_as_z_axis_scale: False 98 | 99 | # Recovery 100 | 101 | shrink_horizon_backup: True 102 | shrink_horizon_min_duration: 10 103 | oscillation_recovery: True 104 | oscillation_v_eps: 0.1 105 | oscillation_omega_eps: 0.1 106 | oscillation_recovery_min_duration: 10 107 | oscillation_filter_duration: 10 -------------------------------------------------------------------------------- /sonar_oculus/scripts/oculus_viewer.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import numpy as np 3 | import cv2 4 | from scipy.interpolate import interp1d 5 | 6 | import rospy 7 | import cv_bridge 8 | from sonar_oculus.msg import OculusPing 9 | from sensor_msgs.msg import Image 10 | from dynamic_reconfigure.server import Server 11 | 12 | REVERSE_Z = 1 13 | global res, height, rows, width, cols, map_x, map_y, f_bearings 14 | res, height, rows, width, cols = None, None, None, None, None 15 | map_x, map_y = None, None 16 | f_bearings = None 17 | 18 | bridge = cv_bridge.CvBridge() 19 | 20 | to_rad = lambda bearing: bearing * np.pi / 18000 21 | 22 | 23 | def generate_map_xy(ping): 24 | _res = ping.range_resolution 25 | _height = ping.num_ranges * _res 26 | _rows = ping.num_ranges 27 | _width = np.sin( 28 | to_rad(ping.bearings[-1] - ping.bearings[0]) / 2) * _height * 2 29 | _cols = int(np.ceil(_width / _res)) 30 | 31 | global res, height, rows, width, cols, map_x, map_y, f_bearings 32 | if res == _res and height == _height and rows == _rows and width == _width and cols == _cols: 33 | return 34 | res, height, rows, width, cols = _res, _height, _rows, _width, _cols 35 | 36 | bearings = to_rad(np.asarray(ping.bearings, dtype=np.float32)) 37 | f_bearings = interp1d( 38 | bearings, 39 | range(len(bearings)), 40 | kind='linear', 41 | bounds_error=False, 42 | fill_value=-1, 43 | assume_sorted=True) 44 | 45 | XX, YY = np.meshgrid(range(cols), range(rows)) 46 | x = res * (rows - YY) 47 | y = res * (-cols / 2.0 + XX + 0.5) 48 | b = np.arctan2(y, x) * REVERSE_Z 49 | r = np.sqrt(np.square(x) + np.square(y)) 50 | map_y = np.asarray(r / res, dtype=np.float32) 51 | map_x = np.asarray(f_bearings(b), dtype=np.float32) 52 | 53 | 54 | def ping_callback(msg): 55 | raw = rospy.get_param('/sonar_oculus_node/Raw', False) 56 | cm = rospy.get_param('/sonar_oculus_node/Colormap', 1) 57 | if raw: 58 | img = bridge.imgmsg_to_cv2(msg.ping, desired_encoding='passthrough') 59 | img = cv2.normalize( 60 | img, None, alpha=0, beta=255, norm_type=cv2.NORM_MINMAX) 61 | img = cv2.applyColorMap(img, cm) 62 | img_msg = bridge.cv2_to_imgmsg(img, encoding="bgr8") 63 | img_msg.header.stamp = rospy.Time.now() 64 | img_pub.publish(img_msg) 65 | else: 66 | generate_map_xy(msg) 67 | 68 | img = bridge.imgmsg_to_cv2(msg.ping, desired_encoding='passthrough') 69 | img = np.array(img, dtype=img.dtype, order='F') 70 | 71 | if cols > img.shape[1]: 72 | img.resize(rows, cols) 73 | img = cv2.remap(img, map_x, map_y, cv2.INTER_LINEAR) 74 | 75 | # img_msg = bridge.cv2_to_imgmsg(img, encoding="mono8") 76 | 77 | # img = cv2.normalize( 78 | # img, None, alpha=0, beta=255, norm_type=cv2.NORM_MINMAX) 79 | img = cv2.applyColorMap(img, cm) 80 | img_msg = bridge.cv2_to_imgmsg(img, encoding="bgr8") 81 | img_msg.header.stamp = rospy.Time.now() 82 | img_pub.publish(img_msg) 83 | 84 | 85 | if __name__ == '__main__': 86 | rospy.init_node('oculus_viewer') 87 | img_pub = rospy.Publisher('/sonar_oculus_node/image', Image, queue_size=10) 88 | ping_sub = rospy.Subscriber('/sonar_oculus_node/ping', OculusPing, 89 | ping_callback, None, 10) 90 | 91 | rospy.spin() 92 | -------------------------------------------------------------------------------- /imu_vn_100/vncpplib/include/vn_errorCodes.h: -------------------------------------------------------------------------------- 1 | /** 2 | * \file 3 | * 4 | * \section LICENSE 5 | * The MIT License (MIT) 6 | * 7 | * Copyright (c) 2014 VectorNav Technologies, LLC 8 | * 9 | * Permission is hereby granted, free of charge, to any person obtaining a copy 10 | * of this software and associated documentation files (the "Software"), to deal 11 | * in the Software without restriction, including without limitation the rights 12 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 13 | * copies of the Software, and to permit persons to whom the Software is 14 | * furnished to do so, subject to the following conditions: 15 | * 16 | * The above copyright notice and this permission notice shall be included in all 17 | * copies or substantial portions of the Software. 18 | * 19 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 20 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 21 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 22 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 23 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 24 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 25 | * SOFTWARE. 26 | * 27 | * \section DESCRIPTION 28 | * This header file defines the error codes used within the VectorNav C/C++ 29 | * Library. 30 | */ 31 | #ifndef _VN_ERRORCODES_H_ 32 | #define _VN_ERRORCODES_H_ 33 | 34 | #if defined(_MSC_VER) && _MSC_VER <= 1500 35 | /* Visual Studio 2008 and earlier do not include the stdint.h header file. */ 36 | #include "vnint.h" 37 | #else 38 | #include 39 | #endif 40 | 41 | /** Type define for VectorNav error codes. */ 42 | typedef uint32_t VN_ERROR_CODE; 43 | 44 | /** 45 | * @defgroup VN_ERROR_CODE VectorNav Error Code Definitions 46 | * 47 | * @{ 48 | */ 49 | #define VNERR_NO_ERROR 0 /**< No error occurred. */ 50 | #define VNERR_UNKNOWN_ERROR 1 /**< An unknown error occurred. */ 51 | #define VNERR_NOT_IMPLEMENTED 2 /**< The operation is not implemented. */ 52 | #define VNERR_TIMEOUT 3 /**< Operation timed out. */ 53 | #define VNERR_INVALID_VALUE 4 /**< Invalid value was provided. */ 54 | #define VNERR_FILE_NOT_FOUND 5 /**< The file was not found. */ 55 | #define VNERR_NOT_CONNECTED 6 /**< Not connected to the sensor. */ 56 | #define VNERR_PERMISSION_DENIED 7 /**< Permission is denied. */ 57 | #define VNERR_SENSOR_HARD_FAULT 8 /**< Sensor experienced a hard fault error. */ 58 | #define VNERR_SENSOR_SERIAL_BUFFER_OVERFLOW 9 /**< Sensor experienced a serial buffer overflow error. */ 59 | #define VNERR_SENSOR_INVALID_CHECKSUM 10 /**< Sensor reported an invalid checksum error. */ 60 | #define VNERR_SENSOR_INVALID_COMMAND 11 /**< Sensor reported an invalid command error. */ 61 | #define VNERR_SENSOR_NOT_ENOUGH_PARAMETERS 12 /**< Sensor reported a not enough parameters error. */ 62 | #define VNERR_SENSOR_TOO_MANY_PARAMETERS 13 /**< Sensor reported a too many parameters error. */ 63 | #define VNERR_SENSOR_INVALID_PARAMETER 14 /**< Sensor reported an invalid parameter error. */ 64 | #define VNERR_SENSOR_UNAUTHORIZED_ACCESS 15 /**< Sensor reported an unauthorized access error. */ 65 | #define VNERR_SENSOR_WATCHDOG_RESET 16 /**< Sensor reported a watchdog reset error. */ 66 | #define VNERR_SENSOR_OUTPUT_BUFFER_OVERFLOW 17 /**< Sensor reported an output buffer overflow error. */ 67 | #define VNERR_SENSOR_INSUFFICIENT_BAUD_RATE 18 /**< Sensor reported an insufficient baudrate error. */ 68 | #define VNERR_SENSOR_ERROR_BUFFER_OVERFLOW 19 /**< Sensor reported an error buffer overflow error. */ 69 | /** @} */ 70 | 71 | #endif /* _VN_ERRORCODES_H_ */ 72 | -------------------------------------------------------------------------------- /waterlinked_gps/scripts/waterlinked_gps_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | # -*- coding: utf-8 -*- 3 | 4 | from __future__ import print_function 5 | import requests 6 | import argparse 7 | import json 8 | import rospy 9 | import time 10 | from sensor_msgs.msg import NavSatFix 11 | from geometry_msgs.msg import PointStamped 12 | 13 | 14 | def get_data(url): 15 | try: 16 | r = requests.get(url) 17 | except requests.exceptions.RequestException, exc: 18 | print('Exception occured {}'.format(exc)) 19 | return None 20 | 21 | if r.status_code != requests.codes.ok: 22 | print('Got error {}: {}'.format(r.status_code, r.text)) 23 | return None 24 | 25 | return r.json() 26 | 27 | 28 | def get_acoustic_position_filtered(base_url): 29 | return get_data('{}/api/v1/position/acoustic/filtered'.format(base_url)) 30 | 31 | 32 | def get_acoustic_position_raw(base_url): 33 | return get_data('{}/api/v1/position/acoustic/raw'.format(base_url)) 34 | 35 | 36 | def get_global_position(base_url): 37 | return get_data('{}/api/v1/position/global'.format(base_url)) 38 | 39 | 40 | def publish_raw(data_raw): 41 | raw_point = PointStamped() 42 | raw_point.header.frame_id = "/map" 43 | raw_point.header.stamp = rospy.Time.now() 44 | raw_point.point.x = data_raw['x'] 45 | raw_point.point.y = data_raw['y'] 46 | raw_point.point.z = data_raw['z'] 47 | raw_pub.publish(raw_point) 48 | 49 | 50 | def publish_filtered(data_filtered): 51 | filtered_point = PointStamped() 52 | filtered_point.header.frame_id = "/map" 53 | filtered_point.header.stamp = rospy.Time.now() 54 | filtered_point.point.x = data_filtered['x'] 55 | filtered_point.point.y = data_filtered['y'] 56 | filtered_point.point.z = data_filtered['z'] 57 | filtered_pub.publish(filtered_point) 58 | 59 | 60 | if __name__ == '__main__': 61 | rospy.init_node('waterlinked_gps_node') 62 | url = rospy.get_param('url', 'http://192.168.2.94') 63 | 64 | raw_pub = rospy.Publisher( 65 | '/waterlinked_gps/acoustic_position/raw', PointStamped, queue_size=120) 66 | filtered_pub = rospy.Publisher( 67 | '/waterlinked_gps/acoustic_position/filtered', 68 | PointStamped, 69 | queue_size=120) 70 | 71 | rate = rospy.Rate(50) 72 | last_raw, last_filtered = None, None 73 | while not rospy.is_shutdown(): 74 | data_raw = get_acoustic_position_raw(url) 75 | if data_raw is None: 76 | rospy.logerr('Fail to retrieve raw position from waterlinked gps') 77 | rospy.sleep(1.0) 78 | continue 79 | 80 | if last_raw is None: 81 | publish_raw(data_raw) 82 | last_raw = data_raw 83 | else: 84 | dist = (data_raw['x'] - last_raw['x']) ** 2 +\ 85 | (data_raw['y'] - last_raw['y']) ** 2 +\ 86 | (data_raw['z'] - last_raw['z']) ** 2 87 | if dist > 1e-10: 88 | publish_raw(data_raw) 89 | last_raw = data_raw 90 | 91 | data_filtered = get_acoustic_position_filtered(url) 92 | if data_filtered is None: 93 | rospy.logerr( 94 | 'Fail to retrieve filtered position from waterlinked gps') 95 | rospy.sleep(1.0) 96 | continue 97 | 98 | if last_filtered is None: 99 | publish_filtered(data_filtered) 100 | last_filtered = data_filtered 101 | else: 102 | dist = (data_filtered['x'] - last_filtered['x']) ** 2 +\ 103 | (data_filtered['y'] - last_filtered['y']) ** 2 +\ 104 | (data_filtered['z'] - last_filtered['z']) ** 2 105 | if dist > 1e-10: 106 | publish_filtered(data_filtered) 107 | last_filtered = data_filtered 108 | 109 | rate.sleep() 110 | -------------------------------------------------------------------------------- /imu_vn_100/include/imu_vn_100/imu_vn_100.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright [2015] [Ke Sun] 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef IMU_VN_100_ROS_H_ 18 | #define IMU_VN_100_ROS_H_ 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | #include 27 | 28 | namespace imu_vn_100 { 29 | 30 | /** 31 | * @brief ImuVn100 The class is a ros wrapper for the Imu class 32 | * @author Ke Sun 33 | */ 34 | class ImuVn100 { 35 | public: 36 | static constexpr int kBaseImuRate = 800; 37 | static constexpr int kDefaultImuRate = 100; 38 | static constexpr int kDefaultSyncOutRate = 20; 39 | 40 | explicit ImuVn100(const ros::NodeHandle& pnh); 41 | ImuVn100(const ImuVn100&) = delete; 42 | ImuVn100& operator=(const ImuVn100&) = delete; 43 | ~ImuVn100(); 44 | 45 | void Initialize(); 46 | 47 | void Stream(bool async = true); 48 | 49 | void PublishData(const VnDeviceCompositeData& data); 50 | 51 | void RequestOnce(); 52 | 53 | void Idle(bool need_reply = true); 54 | 55 | void Resume(bool need_reply = true); 56 | 57 | void Disconnect(); 58 | 59 | void Configure(); 60 | 61 | struct SyncInfo { 62 | unsigned count = 0; 63 | ros::Time time; 64 | 65 | int rate = -1; 66 | double rate_double = -1; 67 | int pulse_width_us = 1000; 68 | int skip_count = 0; 69 | 70 | void Update(const unsigned sync_count, const ros::Time& sync_time); 71 | void FixSyncRate(); 72 | bool SyncEnabled() const; 73 | }; 74 | 75 | const SyncInfo sync_info() const { return sync_info_; } 76 | 77 | private: 78 | ros::NodeHandle pnh_; 79 | Vn100 imu_; 80 | 81 | // Settings 82 | std::string port_; 83 | int baudrate_ = 921600; 84 | int imu_rate_ = kDefaultImuRate; 85 | double imu_rate_double_ = kDefaultImuRate; 86 | std::string frame_id_; 87 | 88 | bool enable_mag_ = true; 89 | bool enable_pres_ = true; 90 | bool enable_temp_ = true; 91 | bool enable_rpy_ = false; 92 | 93 | bool binary_output_ = true; 94 | int binary_async_mode_ = BINARY_ASYNC_MODE_SERIAL_2; 95 | 96 | bool imu_compensated_ = false; 97 | 98 | bool tf_ned_to_enu_ = false; 99 | 100 | bool vpe_enable_ = true; 101 | int vpe_heading_mode_ = 1; 102 | int vpe_filtering_mode_ = 1; 103 | int vpe_tuning_mode_ = 1; 104 | VnVector3 vpe_mag_base_tuning_; 105 | VnVector3 vpe_mag_adaptive_tuning_; 106 | VnVector3 vpe_mag_adaptive_filtering_; 107 | VnVector3 vpe_accel_base_tuning_; 108 | VnVector3 vpe_accel_adaptive_tuning_; 109 | VnVector3 vpe_accel_adaptive_filtering_; 110 | 111 | SyncInfo sync_info_; 112 | 113 | ros::Publisher imu_pub_; 114 | 115 | void FixImuRate(); 116 | void LoadParameters(); 117 | 118 | // used for clock synchronziation 119 | bool synchronize_time_; 120 | double hardware_clock_; 121 | uint64_t last_hardware_time_stamp_; 122 | double hardware_clock_adj_; 123 | const double adj_alpha_ = .01; 124 | uint64_t adj_count_; 125 | 126 | ros::Time getSynchronizedTime(uint64_t time_stamp, const ros::Time &system_time); 127 | }; 128 | 129 | // Just don't like type that is ALL CAP 130 | using VnErrorCode = VN_ERROR_CODE; 131 | void VnEnsure(const VnErrorCode& error_code); 132 | 133 | } // namespace imu_vn_100 134 | 135 | #endif // IMU_VN_100_ROS_H_ 136 | -------------------------------------------------------------------------------- /rti_dvl/scripts/pynmea2/types/proprietary/tnl.py: -------------------------------------------------------------------------------- 1 | # -- TRIMBLE -- # 2 | 3 | # pylint: disable=wildcard-import,unused-wildcard-import 4 | from ... import nmea 5 | from ...nmea_utils import * 6 | """ Support for proprietary messages from BD9xx recievers. 7 | Documentation: www.trimble.com/OEM_ReceiverHelp/v4.85/en/ 8 | """ 9 | 10 | class TNL(nmea.ProprietarySentence): 11 | sentence_types = {} 12 | """ 13 | Generic Trimble Message 14 | """ 15 | def __new__(_cls, manufacturer, data): 16 | ''' 17 | Return the correct sentence type based on the first field 18 | ''' 19 | sentence_type = data[0] or data[1] 20 | name = manufacturer + sentence_type 21 | cls = _cls.sentence_types.get(name, _cls) 22 | return super(TNL, cls).__new__(cls) 23 | 24 | def __init__(self, manufacturer, data): 25 | self.sentence_type = data[0] or data[1] 26 | super(TNL, self).__init__(manufacturer, data) 27 | 28 | 29 | class TNLAVR(TNL): 30 | """ 31 | Trimble AVR Message 32 | """ 33 | fields = ( 34 | ('Empty', '_'), 35 | ('Sentence Type', 'type'), 36 | ('Timestamp', 'timestamp', timestamp), 37 | ('Yaw Angle', 'yaw_angle'), 38 | ('Yaw', 'yaw'), 39 | ('Tilt Angle', 'tilt_angle'), 40 | ('Tilt', 'tilt'), 41 | ('Roll Angle', 'roll_angle'), 42 | ('Roll', 'roll'), 43 | ('Baseline Range', 'baseline'), 44 | ('GPS Quality', 'gps_quality'), 45 | ('PDOP', 'pdop'), 46 | ('Total number of satelites in use', 'num_sats'), 47 | ) 48 | 49 | 50 | class TNLBPQ(TNL, LatLonFix, DatetimeFix): 51 | """ 52 | Trimble BPQ Message 53 | """ 54 | fields = ( 55 | ('Empty', '_'), 56 | ('Sentence Type', 'type'), 57 | ('Timestamp', 'timestamp', timestamp), 58 | ("Datestamp", "datestamp", datestamp), 59 | ("Latitude", "lat"), 60 | ("Latitude Direction", "lat_dir"), 61 | ("Longitude", "lon"), 62 | ("Longitude Direction", "lon_dir"), 63 | ('Height Ellipsoid', 'height'), 64 | ('Meters', 'meters'), 65 | ('Mode fix type', 'mode_fix_type'), 66 | ('Total number of satelites in use', 'num_sats'), 67 | ) 68 | 69 | 70 | class TNLGGK(TNL, LatLonFix, DatetimeFix): 71 | """ 72 | Trimble GGK Message 73 | """ 74 | fields = ( 75 | ('Empty', '_'), 76 | ('Sentence Type', 'type'), 77 | ('Timestamp', 'timestamp', timestamp), 78 | ("Datestamp", "datestamp", datestamp), 79 | ("Latitude", "lat"), 80 | ("Latitude Direction", "lat_dir"), 81 | ("Longitude", "lon"), 82 | ("Longitude Direction", "lon_dir"), 83 | ('GPS Quality', 'quality'), 84 | ('Total number of satelites in use', 'num_sats'), 85 | ('DOP', 'dop'), 86 | ('Height Ellipsoid', 'height'), 87 | ('Meters', 'meters'), 88 | ('Mode fix type', 'mode_fix_type'), 89 | ) 90 | 91 | 92 | class TNLVHD(TNL, DatetimeFix): 93 | """ 94 | Trimble VHD Message 95 | """ 96 | fields = ( 97 | ('Empty', '_'), 98 | ('Sentence Type', 'type'), 99 | ('Timestamp', 'timestamp', timestamp), 100 | ("Datestamp", "datestamp", datestamp), 101 | ('Azimuth Angle', 'azimuth'), 102 | ('AzimuthTime', 'azdt'), 103 | ('Vertical Angle', 'vertical'), 104 | ('VerticalTime', 'vertdt'), 105 | ('Range', 'range'), 106 | ('RangeTime', 'rdt'), 107 | ('GPS Quality', 'gps_quality'), 108 | ('Total number of satelites in use', 'num_sats'), 109 | ('PDOP', 'pdop'), 110 | ) 111 | 112 | 113 | class TNLPJT(TNL): 114 | """ 115 | Trimble PJT Message 116 | """ 117 | fields = ( 118 | ('Empty', '_'), 119 | ('Sentence Type', 'type'), 120 | ('Coordinate System', 'coord_name'), 121 | ('Project Name', 'project_name'), 122 | ) 123 | -------------------------------------------------------------------------------- /rti_dvl/scripts/pynmea2/types/proprietary/sxn.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Seapath 3 | 4 | Message types: 5 | 6 | $PSXN,20,horiz-qual,hgt-qual,head-qual,rp-qual*csum term 7 | $PSXN,22,gyro-calib,gyro-offs*csum term 8 | $PSXN,23,roll,pitch,head,heave*csum term 9 | $PSXN,24,roll-rate,pitch-rate,yaw-rate,vertical-vel*csum term 10 | $PSXN,21,event*csum term 11 | 12 | Where: 13 | 14 | horiz-qual: Horizontal position and velocity quality: 0 = normal, 1 = reduced performance, 2= invalid data. 15 | hgt-qual: Height and vertical velocity quality: 0 = normal, 1 = reduced performance, 2 =invalid data. 16 | head-qual: Heading quality: 0 = normal, 1 = reduced performance, 2 = invalid data. 17 | rp-qual: Roll and pitch quality: 0 = normal, 1 = reduced performance, 2 = invalid data. 18 | gyro-calib: Gyro calibration value since system start-up in degrees on format d.dd. 19 | gyro-offs: Short-term gyro offset in degrees on format d.dd. 20 | roll: Roll in degrees on format d.dd. Positive with port side up. 21 | pitch: Pitch in degrees on format d.dd. Positive with bow up. 22 | heave: Heave in metres on format d.dd. Positive down. 23 | roll-rate: Roll rate in degrees per second on format d.dd. Positive when port side is moving upwards. 24 | pitch-rate: Pitch rate in degrees per second on format d.dd. Positive when bow is moving upwards. 25 | yaw-rate: Yaw rate in degrees per second on format d.dd. Positive when bow is moving towards starboard. 26 | vertical-vel: Vertical velocity in metres per second on format d.dd. Positive when moving downwards. 27 | event: Event code: 1 = system restart. 28 | csum: Checksum (exclusive or) of all characters between, but not including, the preceding $ and * , hexadecimal (00 - FF). 29 | term: CR-LF (2 bytes, values 13 and 10). 30 | 31 | Samples: 32 | 33 | $PSXN,20,0,0,0,0*3B 34 | $PSXN,23,0.30,-0.97,298.57,0.13*1B 35 | $PSXN,26,0,44.5000,0.7800,-0.9000,NRP*6D 36 | 37 | ''' 38 | from ... import nmea 39 | 40 | 41 | class SXN(nmea.ProprietarySentence): 42 | sentence_types = {} 43 | 44 | def __new__(_cls, manufacturer, data): 45 | name = manufacturer + data[1] 46 | cls = _cls.sentence_types.get(name, _cls) 47 | return super(SXN, cls).__new__(cls) 48 | 49 | 50 | class SXN20(SXN): 51 | fields = ( 52 | ('Blank', '_blank'), 53 | ('Message Type', 'message_type', int), 54 | ('Horizontal position and velocity quality', 'horiz_qual', int), 55 | ('Height and vertical velocity quality', 'hgt_qual', int), 56 | ('Heading quality', 'head_qual', int), 57 | ('Roll and pitch quality', 'rp_qual', int), 58 | ) 59 | 60 | 61 | class SXN21(SXN): 62 | fields = ( 63 | ('Blank', '_blank'), 64 | ('Message Type', 'message_type', int), 65 | ('Event code: 1 = system restart.', 'event', int), 66 | ) 67 | 68 | 69 | class SXN22(SXN): 70 | fields = ( 71 | ('Blank', '_blank'), 72 | ('Message Type', 'message_type', int), 73 | ('Gyro calibration value since system start-up in degrees', 'gyro_calib', float), 74 | ('Short-term gyro offset in degrees', 'gyro_ffs', float), 75 | ) 76 | 77 | 78 | class SXN23(SXN): 79 | fields = ( 80 | ('Blank', '_blank'), 81 | ('Message Type', 'message_type', int), 82 | ('Roll in degrees. Positive with port side up.', 'roll', float), 83 | ('Pitch in degrees. Positive with bow up.', 'pitch', float), 84 | ('Heading, degrees true (0.00 - 359.99).', 'head', float), 85 | ('Heave in metres. Positive down.', 'heave', float) 86 | ) 87 | 88 | 89 | class SXN24(SXN): 90 | fields = ( 91 | ('Blank', '_blank'), 92 | ('Message Type', 'message_type', int), 93 | ('Roll rate in degrees/second. Positive when port side is moving upwards.', 'roll_rate', float), 94 | ('Pitch rate in degrees/second. Positive when bow is moving upwards.', 'pitch_rate', float), 95 | ('Yaw rate in degrees/second. Positive when bow is moving towards starboard.', 'yaw_rate', float), 96 | ('Vertical velocity in metres/second. Positive when moving downwards.', 'vertical_vel', float) 97 | ) 98 | -------------------------------------------------------------------------------- /rti_dvl/scripts/pynmea2/nmea_utils.py: -------------------------------------------------------------------------------- 1 | #pylint: disable=invalid-name 2 | import datetime 3 | 4 | def timestamp(s): 5 | ''' 6 | Converts a timestamp given in "hhmmss[.ss]" ASCII format to a 7 | datetime.time object 8 | ''' 9 | ms_s = s[6:] 10 | ms = ms_s and int(float(ms_s) * 1000000) or 0 11 | 12 | t = datetime.time( 13 | hour=int(s[0:2]), 14 | minute=int(s[2:4]), 15 | second=int(s[4:6]), 16 | microsecond=ms) 17 | return t 18 | 19 | 20 | def datestamp(s): 21 | ''' 22 | Converts a datestamp given in "DDMMYY" ASCII format to a 23 | datetime.datetime object 24 | ''' 25 | return datetime.datetime.strptime(s, '%d%m%y').date() 26 | 27 | 28 | import re 29 | def dm_to_sd(dm): 30 | ''' 31 | Converts a geographic coordiante given in "degres/minutes" dddmm.mmmm 32 | format (ie, "12319.943281" = 123 degrees, 19.953281 minutes) to a signed 33 | decimal (python float) format 34 | ''' 35 | # '12319.943281' 36 | if not dm or dm == '0': 37 | return 0. 38 | d, m = re.match(r'^(\d+)(\d\d\.\d+)$', dm).groups() 39 | return float(d) + float(m) / 60 40 | 41 | 42 | class LatLonFix(object): 43 | '''Mixin to add `lattitude` and `longitude` properties as signed decimals 44 | to NMEA sentences which have coordiantes given as degrees/minutes (lat, lon) 45 | and cardinal directions (lat_dir, lon_dir)''' 46 | #pylint: disable=no-member 47 | @property 48 | def latitude(self): 49 | '''Lattitude in signed degrees (python float)''' 50 | sd = dm_to_sd(self.lat) 51 | if self.lat_dir == 'N': 52 | return +sd 53 | elif self.lat_dir == 'S': 54 | return -sd 55 | else: 56 | return 0. 57 | 58 | @property 59 | def longitude(self): 60 | '''Longitude in signed degrees (python float)''' 61 | sd = dm_to_sd(self.lon) 62 | if self.lon_dir == 'E': 63 | return +sd 64 | elif self.lon_dir == 'W': 65 | return -sd 66 | else: 67 | return 0. 68 | 69 | @staticmethod 70 | def _minutes(x): 71 | return abs(x * 60.) % 60. 72 | 73 | @staticmethod 74 | def _seconds(x): 75 | return abs(x * 3600.) % 60. 76 | 77 | @property 78 | def latitude_minutes(self): 79 | return self._minutes(self.latitude) 80 | 81 | @property 82 | def longitude_minutes(self): 83 | return self._minutes(self.longitude) 84 | 85 | @property 86 | def latitude_seconds(self): 87 | return self._seconds(self.latitude) 88 | 89 | @property 90 | def longitude_seconds(self): 91 | return self._seconds(self.longitude) 92 | 93 | 94 | class DatetimeFix(object): 95 | #pylint: disable=no-member 96 | @property 97 | def datetime(self): 98 | return datetime.datetime.combine(self.datestamp, self.timestamp) 99 | 100 | 101 | class ValidStatusFix(object): 102 | #pylint: disable=no-member 103 | @property 104 | def is_valid(self): 105 | return self.status == 'A' 106 | 107 | 108 | class ValidGSAFix(object): 109 | #pylint: disable=no-member 110 | @property 111 | def is_valid(self): 112 | return int(self.mode_fix_type) in [2, 3] 113 | 114 | 115 | class ValidGGAFix(object): 116 | #pylint: disable=no-member 117 | @property 118 | def is_valid(self): 119 | return self.gps_qual in range(1,6) 120 | 121 | 122 | class ValidVBWFix(object): 123 | #pylint: disable=no-member 124 | @property 125 | def is_valid(self): 126 | return self.data_validity_water_spd == self.data_validity_grnd_spd == 'A' 127 | 128 | 129 | class TZInfo(datetime.tzinfo): 130 | def __init__(self, hh, mm): 131 | self.hh = hh 132 | self.mm = mm 133 | super(TZInfo, self).__init__() 134 | 135 | def tzname(self, dt): 136 | return '' 137 | 138 | def dst(self, dt): 139 | return datetime.timedelta(0) 140 | 141 | def utcoffset(self, dt): 142 | return datetime.timedelta(hours=self.hh, minutes=self.mm) 143 | -------------------------------------------------------------------------------- /sonar_oculus/src/OculusClient.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * (c) Copyright 2017 Blueprint Subsea. 3 | * This file is part of Oculus Viewer 4 | * 5 | * Oculus Viewer is free software: you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation, either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * Oculus Viewer is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License 16 | * along with this program. If not, see . 17 | * 18 | *****************************************************************************/ 19 | 20 | #include 21 | #include 22 | #include "Oculus.h" 23 | #include "DataWrapper.h" 24 | 25 | // ---------------------------------------------------------------------------- 26 | // OsBufferEntry - contains a return message and an embedded image 27 | class OsBufferEntry 28 | { 29 | public: 30 | OsBufferEntry(); 31 | ~OsBufferEntry(); 32 | 33 | // Methods 34 | void AddRawToEntry(char* pData, uint64_t nData); 35 | void ProcessRaw(char* pData); 36 | 37 | // Data 38 | OculusSimplePingResult m_rfm; // The fixed length return fire message 39 | unsigned char* m_pImage; // The image data 40 | short* m_pBrgs; // The bearing table 41 | std::mutex m_mutex; // Lock for buffer accesss 42 | 43 | uint8_t* m_pRaw; // The raw data 44 | uint32_t m_rawSize; // Size of the raw data record 45 | }; 46 | 47 | 48 | class OsClientCtrl; 49 | #define OS_BUFFER_SIZE 1 50 | 51 | // ---------------------------------------------------------------------------- 52 | // OsReadThread - a worker thread used to read data from the network for the client 53 | class OsReadThread : public std::thread 54 | { 55 | public: 56 | OsReadThread(); 57 | ~OsReadThread(); 58 | 59 | void run(); 60 | 61 | void Startup(); 62 | void Shutdown(); 63 | bool IsActive(); 64 | void SetActive(bool active); 65 | void ProcessRxBuffer(); 66 | void ProcessPayload(char* pData, uint64_t nData); 67 | 68 | // Data 69 | OsClientCtrl* m_pClient; // back pointer to the parent client 70 | bool m_active; // Is the run exec active 71 | std::mutex m_mutex; // Mutex protection for m_active 72 | std::mutex m_sending; // Mutex protection for m_active 73 | 74 | std::string m_hostname; // The hostname/address of the sonar 75 | uint16_t m_port; // The port for sonar comms (currently fixed) 76 | int32_t m_nFlushes; // Number of times the rx buffer has had to be flushed 77 | 78 | // The raw receive buffer 79 | char* m_pRxBuffer; // The rx buffer for incomming data 80 | int32_t m_nRxMax; // The maximum size of the rx Buffer 81 | int32_t m_nRxIn; // The current amount of unprocessed data in the buffer 82 | 83 | // The recieve buffer for messages 84 | OsBufferEntry m_osBuffer[OS_BUFFER_SIZE]; 85 | unsigned m_osInject; // The position for the next inject 86 | 87 | // The raw send buffer 88 | int* m_pSocket; //point to socket fd 89 | char* m_pToSend; 90 | int64_t m_nToSend; 91 | }; 92 | 93 | 94 | // ---------------------------------------------------------------------------- 95 | // ClientCtrl - used to communicate with the oculus sonar 96 | class OsClientCtrl 97 | { 98 | public: 99 | OsClientCtrl(); 100 | ~OsClientCtrl(); 101 | 102 | bool Connect(); 103 | bool Disconnect(); 104 | bool IsOpen(); 105 | void WriteData(char* pData, uint16_t length); 106 | void Fire(int mode, int pingRate, double range, double gain, double speedOfSound, double salinity); 107 | void DummyMessage(); 108 | 109 | std::string m_hostname; // The hostname/address of the sonar 110 | std::string m_mask; 111 | UserConfig m_config; // Oculus user configuration 112 | OsReadThread m_readData; // The worker thread for reading data 113 | }; 114 | 115 | 116 | -------------------------------------------------------------------------------- /bluerov_control/scripts/joy_control.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import rospy 3 | from sensor_msgs.msg import Joy 4 | from geometry_msgs.msg import Twist 5 | 6 | from bluerov_bridge import Bridge 7 | 8 | 9 | cmd_vel_enabled = False 10 | armed = False 11 | depth_hold = False 12 | x, y, z, yaw = 1500, 1500, 1500, 1500 13 | 14 | 15 | def cmd_vel_sub(msg): 16 | if not cmd_vel_enabled: 17 | return 18 | 19 | global x, y, z, yaw 20 | vel_x, vel_y = msg.linear.x, msg.linear.y 21 | omega_z = msg.angular.z 22 | 23 | vel_x = max(-max_vel, min(max_vel, vel_x)) 24 | vel_y = max(-max_vel, min(max_vel, vel_y)) 25 | omega_z = max(-max_omega, min(max_omega, omega_z)) 26 | 27 | x = 1500 + int(vel_to_cmd * vel_x) 28 | y = 1500 + int(vel_to_cmd * vel_y) 29 | z = 65535 30 | yaw = 1500 + int(omega_to_cmd * omega_z) 31 | 32 | 33 | def joy_callback(msg): 34 | global x, y, z, yaw, armed, cmd_vel_enabled, depth_hold 35 | 36 | # Arm / disarm 37 | if msg.buttons[0]: 38 | armed = True 39 | rospy.loginfo('Arm the vehicle...') 40 | if msg.buttons[1]: 41 | armed = False 42 | rospy.loginfo('Disarm the vehicle...') 43 | 44 | if msg.buttons[8]: 45 | cmd_vel_enabled = True 46 | rospy.loginfo('Enable move_base control...') 47 | if msg.buttons[3]: 48 | cmd_vel_enabled = False 49 | rospy.loginfo('Disable move_base control...') 50 | 51 | # Depth hold / manual 52 | if msg.buttons[2]: 53 | depth_hold = True 54 | rospy.loginfo('Turn on depth hold mode...') 55 | if msg.buttons[9]: 56 | depth_hold = False 57 | rospy.loginfo('Turn on manual mode...') 58 | 59 | if cmd_vel_enabled: 60 | return 61 | 62 | # Movement 63 | x1 = 1500 + int(msg.axes[1] * translation_limit) 64 | y1 = 1500 + int(msg.axes[0] * translation_limit) 65 | z = 1500 + int(msg.axes[5] * translation_limit) 66 | yaw1 = 1500 - int(msg.axes[2] * rotation_limit) 67 | 68 | # Cruise control 69 | x2 = 1500 + int(msg.axes[3] * translation_limit) 70 | y2 = 1500 71 | yaw2 = 1500 - int(msg.axes[4] * rotation_limit) 72 | 73 | # Normal movement has higher priority 74 | x = x1 if x1 != 1500 else x2 75 | y = y1 if y1 != 1500 else y2 76 | yaw = yaw1 if yaw1 != 1500 else yaw2 77 | 78 | 79 | if __name__ == '__main__': 80 | rospy.init_node('bluerov_cruise_control_node') 81 | 82 | translation_limit = rospy.get_param('~translation_limit', 100) 83 | rotation_limit = rospy.get_param('~rotation_limit', 80) 84 | max_vel = rospy.get_param('~max_vel', 0.2) 85 | max_omega = rospy.get_param('~max_omega', 0.15) 86 | vel_to_cmd = translation_limit / max_vel 87 | omega_to_cmd = rotation_limit / max_omega 88 | 89 | device = 'udp:192.168.2.1:14553' 90 | while not rospy.is_shutdown(): 91 | try: 92 | bridge = Bridge(device) 93 | except socket.error: 94 | rospy.logerr( 95 | 'Failed to make mavlink connection to device {}'.format(device) 96 | ) 97 | rospy.sleep(1.0) 98 | else: 99 | break 100 | if rospy.is_shutdown(): 101 | sys.exit(-1) 102 | bridge.wait_conn() 103 | 104 | joy_sub = rospy.Subscriber('/joy', Joy, joy_callback, queue_size=10) 105 | cmd_vel_sub = rospy.Subscriber('/cmd_vel', Twist, cmd_vel_sub) 106 | 107 | while not rospy.is_shutdown(): 108 | bridge.set_mode('manual') 109 | bridge.arm_throttle(False) 110 | mode, arm = bridge.get_mode() 111 | if mode == 'MANUAL' and not arm: 112 | break 113 | rospy.sleep(0.5) 114 | 115 | rate = rospy.Rate(20) 116 | while not rospy.is_shutdown(): 117 | bridge.update() 118 | 119 | if armed: 120 | bridge.arm_throttle(True) 121 | else: 122 | bridge.arm_throttle(False) 123 | 124 | if depth_hold: 125 | bridge.set_mode('alt_hold') 126 | else: 127 | bridge.set_mode('manual') 128 | 129 | bridge.set_cmd_vel(x, y, z, yaw) 130 | rate.sleep() 131 | 132 | while not rospy.is_shutdown(): 133 | bridge.set_mode('manual') 134 | mode, arm = bridge.arm_throttle(False) 135 | if mode == 'MANUAL' and not arm: 136 | break 137 | rospy.sleep(0.5) 138 | -------------------------------------------------------------------------------- /rti_dvl/scripts/pynmea2/types/proprietary/ash.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Support for proprietary messages from Ashtech receivers. 3 | ''' 4 | # pylint: disable=wildcard-import,unused-wildcard-import 5 | from decimal import Decimal 6 | import re 7 | 8 | from ... import nmea 9 | from ...nmea_utils import * 10 | 11 | 12 | class ASH(nmea.ProprietarySentence): 13 | ''' 14 | Generic Ashtech Response Message 15 | ''' 16 | sentence_types = {} 17 | def __new__(_cls, manufacturer, data): 18 | ''' 19 | Return the correct sentence type based on the first field 20 | ''' 21 | sentence_type = data[1] 22 | name = manufacturer + 'R' + sentence_type 23 | if name not in _cls.sentence_types: 24 | # ASHRATT does not have a sentence type 25 | if ASHRATT.match(data): 26 | return super(ASH, ASHRATT).__new__(ASHRATT) 27 | cls = _cls.sentence_types.get(name, ASH) 28 | return super(ASH, cls).__new__(cls) 29 | 30 | 31 | class ASHRATT(ASH): 32 | ''' 33 | RT300 proprietary attitude sentence 34 | ''' 35 | @staticmethod 36 | def match(data): 37 | return re.match(r'^\d{6}\.\d{3}$', data[1]) 38 | 39 | def __init__(self, *args, **kwargs): 40 | self.subtype = 'ATT' 41 | super(ASHRATT, self).__init__(*args, **kwargs) 42 | 43 | fields = ( 44 | ('R', '_r'), 45 | ('Timestamp', 'timestamp', timestamp), 46 | ('Heading Angle', 'true_heading', float), 47 | ('Is True Heading', 'is_true_heading'), 48 | ('Roll Angle', 'roll', float), 49 | ('Pitch Angle', 'pitch', float), 50 | ('Heave', 'heading', float), 51 | ('Roll Accuracy Estimate', 'roll_accuracy', float), 52 | ('Pitch Accuracy Estimate', 'pitch_accuracy', float), 53 | ('Heading Accuracy Estimate', 'heading_accuracy', float), 54 | ('Aiding Status', 'aiding_status', Decimal), 55 | ('IMU Status', 'imu_status', Decimal), 56 | ) 57 | 58 | 59 | class ASHRHPR(ASH): 60 | ''' 61 | Ashtech HPR Message 62 | ''' 63 | fields = ( 64 | ('R', '_r'), 65 | ('Subtype', 'subtype'), 66 | ('Timestamp', 'timestamp', timestamp), 67 | ('Heading Angle', 'heading', Decimal), 68 | ('Pitch Angle', 'pitch', Decimal), 69 | ('Roll Angle', 'roll', Decimal), 70 | ('Carrier measurement RMS', 'carrier_rms', Decimal), 71 | ('Baseline measurement RMS', 'baseline_rms', Decimal), 72 | ('Integer Ambiguity', 'integer_ambiguity'), 73 | ('Mode', 'mode'), 74 | ('Status', 'status'), 75 | ('PDOP', 'pdop', float), 76 | ) 77 | 78 | 79 | class ASHRLTN(ASH): 80 | ''' 81 | Ashtech LTN Message 82 | ''' 83 | fields = ( 84 | ('R', '_r'), 85 | ('Subtype', 'subtype'), 86 | ('Latency (ms)', 'latency', int), 87 | ) 88 | 89 | 90 | class ASHRPOS(ASH, LatLonFix): 91 | ''' 92 | Ashtech POS Message 93 | ''' 94 | fields = ( 95 | ('R', '_r'), 96 | ('Subtype', 'subtype'), 97 | ('Solution Type', 'mode', int), 98 | ('Satellites used in Solution', 'sat_count', int), 99 | ('Timestamp', 'timestamp', timestamp), 100 | ('Latitude', 'lat'), 101 | ('Latitude Direction', 'lat_dir'), 102 | ('Longitude', 'lon'), 103 | ('Longitude Direction', 'lon_dir'), 104 | ('Altitude above WGS84 ellipsoid, meters', 'altitude'), 105 | ('Empty', '__'), 106 | ("True Track/Course Over Ground", "course", float), 107 | ("Speed Over Ground", "spd_over_grnd", float), 108 | ('Vertical Velocity', 'vertical_velocity', Decimal), 109 | ('PDOP', 'pdop', float), 110 | ('HDOP', 'hdop', float), 111 | ('VDOP', 'vdop', float), 112 | ('TDOP', 'tdop', float), 113 | ('Base station ID', 'station_id', int) 114 | ) 115 | 116 | 117 | class ASHRVEL(ASH): 118 | ''' 119 | Ashtech VEL Message 120 | ''' 121 | fields = ( 122 | ('R', '_r'), 123 | ('Subtype', 'subtype'), 124 | ('ENU', 'enu', int), 125 | ('Timestamp', 'timestamp', timestamp), 126 | ('Easting', 'easting', Decimal), 127 | ('Northing', 'northing', Decimal), 128 | ('Vertical Velocity', 'vertical', Decimal), 129 | ('Easting RMS', 'easting_rms', Decimal), 130 | ('Northing RMS', 'northing_rms', Decimal), 131 | ('Vertical RMS', 'vertical_rms', Decimal), 132 | ('Applied effective velocity smoothing interval (ms)', 'smoothing', Decimal), 133 | ) 134 | -------------------------------------------------------------------------------- /bluerov_launch/scripts/inspect_bag.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import numpy as np 3 | import matplotlib.pyplot as plt 4 | import rospy 5 | import rosbag 6 | from rti_dvl.msg import DVL 7 | from sensor_msgs.msg import Imu 8 | from bar30_depth.msg import Depth 9 | from tf import transformations 10 | 11 | plt.style.use('seaborn-talk') 12 | DVL_TOPIC = '/rti/body_velocity/raw' 13 | IMU_TOPIC = '/vn100/imu/raw' 14 | DEPTH_TOPIC = '/bar30/depth/raw' 15 | 16 | 17 | def read_bag(file, start=None, duration=None): 18 | bag = rosbag.Bag(file) 19 | start = start if start is not None else 0 20 | start_time = bag.get_start_time() + start 21 | end_time = bag.get_end_time() 22 | if duration is None or duration < 0 or duration == float("inf"): 23 | duration = end_time - start_time 24 | else: 25 | end_time = start_time + duration 26 | 27 | for topic, msg, t in bag.read_messages( 28 | topics=[IMU_TOPIC, DVL_TOPIC, DEPTH_TOPIC], 29 | start_time=rospy.Time.from_sec(start_time), 30 | end_time=rospy.Time.from_sec(end_time), 31 | ): 32 | yield topic, msg 33 | 34 | bag.close() 35 | 36 | 37 | def load_nav_data(file, start=0, duration=None): 38 | dvl, depth, imu = [], [], [] 39 | for topic, msg in read_bag(file, start, duration): 40 | time = msg.header.stamp.to_sec() 41 | if topic == DVL_TOPIC: 42 | dvl.append( 43 | (time, msg.velocity.x, msg.velocity.y, msg.velocity.z, msg.altitude) 44 | ) 45 | elif topic == DEPTH_TOPIC: 46 | depth.append((time, msg.depth)) 47 | elif topic == IMU_TOPIC: 48 | ax = msg.linear_acceleration.x 49 | ay = msg.linear_acceleration.y 50 | az = msg.linear_acceleration.z 51 | wx = msg.angular_velocity.x 52 | wy = msg.angular_velocity.y 53 | wz = msg.angular_velocity.z 54 | qx = msg.orientation.x 55 | qy = msg.orientation.y 56 | qz = msg.orientation.z 57 | qw = msg.orientation.w 58 | # IMU is -roll90 59 | q = transformations.quaternion_matrix((qx, qy, qz, qw)) 60 | r = q.dot(transformations.euler_matrix(0, 0, np.pi / 2, 'szyx')) 61 | y, p, r = transformations.euler_from_matrix(r, 'szyx') 62 | # Hardware time 63 | t = msg.linear_acceleration_covariance[0] 64 | imu.append((time, ax, ay, az, wx, wy, wz, r, p, y, t)) 65 | 66 | dvl = np.array(dvl) 67 | depth = np.array(depth) 68 | imu = np.array(imu) 69 | t0 = [a[0, 0] for a in (dvl, depth, imu) if len(a)] 70 | if not t0: 71 | return None, None, None 72 | else: 73 | t0 = min(t0) 74 | 75 | if len(dvl): 76 | dvl[:, 0] -= t0 77 | if len(imu): 78 | imu[:, 0] -= t0 79 | imu[:, -1] -= imu[0, -1] 80 | if len(depth): 81 | depth[:, 0] -= t0 82 | return dvl, depth, imu 83 | 84 | 85 | def plot_nav_data(dvl, depth, imu, max_vel=1.0): 86 | fig, (ax1, ax2, ax3) = plt.subplots(3, 1, sharex=True, figsize=(15, 8), dpi=200) 87 | ax1.plot(dvl[:, 0], dvl[:, 1], 'r', label='vx', alpha=0.7) 88 | ax1.plot(dvl[:, 0], dvl[:, 2], 'g', label='vy', alpha=0.7) 89 | ax1.plot(dvl[:, 0], dvl[:, 3], 'b', label='vz', alpha=0.7) 90 | 91 | vel = dvl[:, 1:4].ravel() 92 | vmin = np.min(vel[(vel < 0) & (vel > -max_vel)]) - 0.1 93 | vmax = np.max(vel[(vel > 0) & (vel < max_vel)]) + 0.1 94 | ax1.set_ylim(-vmax, vmax) 95 | ax1.legend(fontsize='x-large', loc='upper right') 96 | ax1.grid(axis='y') 97 | ax1.set_ylabel('m/s') 98 | 99 | ax2.plot(dvl[:, 0], dvl[:, 4], 'k', label='altitude', alpha=0.7) 100 | ax2.plot(depth[:, 0], depth[:, 1], 'c', label='depth', alpha=0.7) 101 | 102 | dmax = max(np.max(dvl[dvl[:, 4] > 0, 4]), np.max(depth[:, 1])) + 0.1 103 | dmin = min(np.min(dvl[dvl[:, 4] > 0, 4]), np.max(depth[:, 1])) - 0.1 104 | ax2.set_ylim(dmin, dmax) 105 | ax2.legend(fontsize='x-large', loc='upper right') 106 | ax2.grid(axis='y') 107 | ax2.set_ylabel('m') 108 | 109 | ax3.plot(imu[:, 0], np.rad2deg(imu[:, 7]), 'k', alpha=0.7, label='roll') 110 | ax3.plot(imu[:, 0], np.rad2deg(imu[:, 8]), 'c', alpha=0.7, label='pitch') 111 | ax3.legend(fontsize='x-large', loc='upper right') 112 | ax3.grid(axis='y') 113 | ax3.set_ylabel('deg') 114 | plt.tight_layout() 115 | 116 | 117 | if __name__ == "__main__": 118 | import sys 119 | bagname = sys.argv[1] 120 | dvl, depth, imu = load_nav_data(bagname) 121 | plot_nav_data(dvl, depth, imu) 122 | plt.show() 123 | 124 | -------------------------------------------------------------------------------- /rti_dvl/scripts/pynmea2/types/proprietary/rti.py: -------------------------------------------------------------------------------- 1 | from ... import nmea 2 | 3 | 4 | class RTI(nmea.ProprietarySentence): 5 | sentence_types = {} 6 | 7 | def __new__(_cls, manufacturer, data): 8 | name = manufacturer + data[0] 9 | cls = _cls.sentence_types.get(name, _cls) 10 | return super(RTI, cls).__new__(cls) 11 | 12 | 13 | class RTI01(RTI): 14 | fields = ( 15 | ('Identity', 'identity', str), 16 | ('Start time of this sample in hundreds of seconds since power up or user reset', 'time', int), 17 | ('Sample number', 'number', int), 18 | ('Temperature in hundreds of degrees Celsius', 'temperature', int), 19 | ('Bottom track X velocity component mm/s', 'x', int), 20 | ('Bottom track Y velocity component mm/s', 'y', int), 21 | ('Bottom track Z velocity component mm/s', 'z', int), 22 | ('Depth below transducer in mm', 'depth', int), 23 | ('Water mass X velocity component mm/s', '_blank'), 24 | ('Water mass Y velocity component mm/s', '_blank'), 25 | ('Water mass Z velocity component mm/s', '_blank'), 26 | ('Depth of water mass measurement in mm', '_blank'), 27 | ('Built in test and status bits in hexadecimal0000', '_blank'), 28 | ('Sub System', '_blank'), 29 | ('Sub System index', '_blank') 30 | ) 31 | 32 | 33 | class RTI02(RTI): 34 | fields = ( 35 | ('Identity', 'identity', str), 36 | ('Start time of this sample in hundreds of seconds since power up or user reset', 'time', int), 37 | ('Sample number', 'number', int), 38 | ('Temperature in hundreds of degrees Celsius', 'temperature', int), 39 | ('Bottom track East velocity component mm/s', 'east', int), 40 | ('Bottom track North velocity component mm/s', 'north', int), 41 | ('Bottom track Up velocity component mm/s', 'up', int), 42 | ('Depth below transducer in mm', 'depth', int), 43 | ('Water mass East velocity component mm/s', '_blank'), 44 | ('Water mass North velocity component mm/s', '_blank'), 45 | ('Water mass Up velocity component mm/s', '_blank'), 46 | ('Depth of water mass measurement in mm', '_blank'), 47 | ('Built in test and status bits in hexadecimal0000', '_blank'), 48 | ('Sub System', '_blank'), 49 | ('Sub System index', '_blank') 50 | ) 51 | 52 | 53 | class RTI03(RTI): 54 | fields = ( 55 | ('Identity', 'identity', str), 56 | ('Start time of this sample in hundreds of seconds since power up or user reset', 'time', int), 57 | ('Sample number', 'number', int), 58 | ('Temperature in hundreds of degrees Celsius', 'temperature', int), 59 | ('Bottom track X velocity component mm/s', 'x', int), 60 | ('Bottom track Y velocity component mm/s', 'y', int), 61 | ('Bottom track Z velocity component mm/s', 'z', int), 62 | ('Bottom track Q velocity component mm/s', 'q', int), 63 | ('Depth below transducer in mm', 'depth', int), 64 | ('Water mass X velocity component mm/s', '_blank'), 65 | ('Water mass Y velocity component mm/s', '_blank'), 66 | ('Water mass Z velocity component mm/s', '_blank'), 67 | ('Water mass Q velocity component mm/s', '_blank'), 68 | ('Depth of water mass measurement in mm', '_blank'), 69 | ('Built in test and status bits in hexadecimal0000', '_blank'), 70 | ('Sub System', '_blank'), 71 | ('Sub System index', '_blank') 72 | ) 73 | 74 | 75 | class RTI30(RTI): 76 | fields = ( 77 | ('Identity', 'identity', str), 78 | ('Heading used during the bottom track ping', 'heading', float), 79 | ('Pitch used during the bottom track ping', 'pitch', float), 80 | ('Roll used during the bottom track ping', 'roll', float), 81 | ('Sub System', '_blank'), 82 | ('Sub System index', '_blank') 83 | ) 84 | 85 | 86 | class RTI31(RTI): 87 | fields = ( 88 | ('Identity', 'identity', str), 89 | ('Heading used during the water mass ping', 'heading', float), 90 | ('Pitch used during the water mass ping', 'pitch', float), 91 | ('Roll used during the water mass ping', 'roll', float), 92 | ('Sub System', '_blank'), 93 | ('Sub System index', '_blank') 94 | ) 95 | 96 | 97 | class RTI32(RTI): 98 | fields = ( 99 | ('Identity', 'identity', str), 100 | ('Heading used during the bottom track ping', 'heading', float), 101 | ('Pitch used during the bottom track ping', 'pitch', float), 102 | ('Roll used during the bottom track ping', 'roll', float), 103 | ('Pressure in BAR', 'pressure', float), 104 | ('Water Temperature in degrees C', 'temperature', float), 105 | ('Sub System', '_blank'), 106 | ('Sub System index', '_blank') 107 | ) 108 | 109 | 110 | class RTI33(RTI): 111 | fields = ( 112 | ('Identity', 'identity', str), 113 | ('Heading used during the water mass ping', 'heading', float), 114 | ('Pitch used during the water mass ping', 'pitch', float), 115 | ('Roll used during the water mass ping', 'roll', float), 116 | ('Pressure in BAR', 'pressure', float), 117 | ('Water Temperature in degrees C', 'temperature', float), 118 | ('Sub System', '_blank'), 119 | ('Sub System index', '_blank') 120 | ) 121 | 122 | 123 | class RTI34(RTI): 124 | fields = ( 125 | ('Identity', 'identity', str), 126 | ('Heading', 'heading', float), 127 | ('Pitch', 'pitch', float), 128 | ('Roll', 'roll', float) 129 | ) 130 | -------------------------------------------------------------------------------- /bluerov_launch/scripts/dashboard.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import time 3 | import curses 4 | 5 | import rospy 6 | from rostopic import ROSTopicHz 7 | from rti_dvl.msg import BottomTrack 8 | from sensor_msgs.msg import Imu 9 | from bar30_depth.msg import Depth 10 | from sonar_oculus.msg import OculusPing 11 | 12 | from bluerov_bridge import Bridge 13 | 14 | 15 | OCULUS_PARTNUMBER = {0: 'NAN', 1032: 'M750d', 1042: 'M1200d'} 16 | 17 | dvl, depth, imu, ping = BottomTrack(), Depth(), Imu(), OculusPing() 18 | hz = ROSTopicHz(-1) 19 | dvl_hz, depth_hz, imu_hz, sonar_hz = 0, 0, 0, 0 20 | 21 | 22 | def dvl_callback(msg): 23 | global dvl 24 | dvl = msg 25 | hz.callback_hz(msg, 'dvl') 26 | 27 | 28 | def depth_callback(msg): 29 | global depth 30 | depth = msg 31 | hz.callback_hz(msg, 'depth') 32 | 33 | 34 | def imu_callback(msg): 35 | global imu 36 | imu = msg 37 | hz.callback_hz(msg, 'imu') 38 | 39 | 40 | def sonar_callback(msg): 41 | global ping 42 | ping = msg 43 | hz.callback_hz(msg, 'sonar') 44 | 45 | 46 | if __name__ == "__main__": 47 | rospy.init_node('bluerov_dashboard') 48 | 49 | device = 'udp:192.168.2.1:14553' 50 | while not rospy.is_shutdown(): 51 | try: 52 | rov = Bridge(device) 53 | except socket.error: 54 | rospy.logerr( 55 | 'Failed to make mavlink connection to device {}'.format( 56 | device)) 57 | rospy.sleep(1.0) 58 | else: 59 | break 60 | if rospy.is_shutdown(): 61 | sys.exit(-1) 62 | rov.update() 63 | 64 | dvl_sub = rospy.Subscriber('/rti/bottom_tracking/raw', BottomTrack, dvl_callback, queue_size=100) 65 | depth_sub = rospy.Subscriber('/bar30/depth/raw', Depth, depth_callback, queue_size=100) 66 | imu_sub = rospy.Subscriber('/vn100/imu/raw', Imu, imu_callback, queue_size=1000) 67 | sonar_sub = rospy.Subscriber('/sonar_oculus_node/ping', OculusPing, sonar_callback, queue_size=10) 68 | 69 | stdscr = curses.initscr() 70 | curses.noecho() 71 | curses.cbreak() 72 | curses.curs_set(0) 73 | 74 | rospy.on_shutdown(curses.endwin) 75 | 76 | stdscr.addstr(2, 5, 'BlueROV2 Dashboard') 77 | 78 | n, rate = 0, rospy.Rate(10) 79 | while not rospy.is_shutdown(): 80 | rov.update() 81 | 82 | voltage, current = rov.get_battery() 83 | mode, arm = rov.get_mode() 84 | arm = 'ARM' if arm else 'DISARM' 85 | 86 | cx, cy, cz, cr = rov.get_cmd_vel() 87 | 88 | water = rospy.get_param('/water', 'NAN') 89 | 90 | # 1 Hz 91 | if n % 10 == 0: 92 | ret = hz.get_hz('dvl') 93 | dvl_hz = ret[0] if ret else 0.0 94 | ret = hz.get_hz('depth') 95 | depth_hz = ret[0] if ret else 0.0 96 | ret = hz.get_hz('imu') 97 | imu_hz = ret[0] if ret else 0.0 98 | ret = hz.get_hz('sonar') 99 | sonar_hz = ret[0] if ret else 0.0 100 | n += 1 101 | 102 | stdscr.addstr( 4, 5, '======== Vehicle ============') 103 | stdscr.addstr( 5, 5, 'Time | {:>15s}'.format(time.strftime('%H:%M:%S', time.gmtime()))) 104 | stdscr.addstr( 6, 5, 'Battery | {:04.1f}V {:03.1f}A'.format(voltage, current)) 105 | stdscr.addstr( 7, 5, 'Mode | {:>8s} {:>6s}'.format(mode, arm)) 106 | stdscr.addstr( 8, 5, '=============================') 107 | 108 | stdscr.addstr(10, 5, '======== RC_CHANNELS ========') 109 | stdscr.addstr(11, 5, 'Forward | {:>15d}'.format(cx)) 110 | stdscr.addstr(12, 5, 'Lateral | {:>15d}'.format(cy)) 111 | stdscr.addstr(13, 5, 'Throttle | {:>15d}'.format(cz)) 112 | stdscr.addstr(14, 5, 'Yaw | {:>15d}'.format(cr)) 113 | stdscr.addstr(15, 5, '=============================') 114 | 115 | stdscr.addstr(17, 5, '======== Sensor Rate ========') 116 | stdscr.addstr(18, 5, 'DVL | {:>13.1f} Hz'.format(dvl_hz)) 117 | stdscr.addstr(19, 5, 'Depth | {:>13.1f} Hz'.format(depth_hz)) 118 | stdscr.addstr(20, 5, 'IMU | {:>13.1f} Hz'.format(imu_hz)) 119 | stdscr.addstr(21, 5, 'Sonar | {:>13.1f} Hz'.format(sonar_hz)) 120 | stdscr.addstr(22, 5, '=============================') 121 | 122 | stdscr.addstr(24, 5, '======== Environment ========') 123 | stdscr.addstr(25, 5, 'Depth | {:>14.1f} m'.format(depth.depth)) 124 | stdscr.addstr(26, 5, 'Altitude | {:>14.1f} m'.format(dvl.altitude)) 125 | stdscr.addstr(27, 5, 'Water | {:>16s}'.format(water)) 126 | stdscr.addstr(28, 5, '=============================') 127 | 128 | stdscr.addstr(30, 5, '======== Sonar ==============') 129 | stdscr.addstr(31, 5, 'Mode | {:>16s}'.format(OCULUS_PARTNUMBER[ping.part_number])) 130 | stdscr.addstr(32, 5, 'Salinity | {:>16.1f}'.format(ping.fire_msg.salinity)) 131 | stdscr.addstr(33, 5, 'Gain | {:>16.1f}'.format(ping.fire_msg.gain)) 132 | stdscr.addstr(34, 5, 'Range | {:>16.1f}'.format(ping.fire_msg.range)) 133 | stdscr.addstr(35, 5, '=============================') 134 | 135 | stdscr.addstr(36, 5, '======== Velocity ===========') 136 | stdscr.addstr(37, 5, 'X | {:>12.2f} m/s'.format(dvl.velocity.x)) 137 | stdscr.addstr(38, 5, 'Y | {:>12.2f} m/s'.format(dvl.velocity.y)) 138 | stdscr.addstr(39, 5, 'Z | {:>12.2f} m/s'.format(dvl.velocity.z)) 139 | stdscr.addstr(40, 5, '=============================') 140 | 141 | stdscr.refresh() 142 | rate.sleep() 143 | 144 | curses.nocbreak(); stdscr.keypad(0); curses.echo() 145 | curses.endwin() 146 | -------------------------------------------------------------------------------- /bluerov_launch/scripts/video.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | """ 3 | BlueRov video capture class 4 | """ 5 | 6 | import cv2 7 | import gi 8 | import numpy as np 9 | 10 | import rospy 11 | from sensor_msgs.msg import Image, CameraInfo 12 | import cv_bridge 13 | 14 | pub_info = True 15 | try: 16 | import camera_info_manager 17 | except ImportError as e: 18 | rospy.logerr('camera_info_manager_py not installed') 19 | pub_info = False 20 | 21 | 22 | gi.require_version('Gst', '1.0') 23 | from gi.repository import Gst 24 | 25 | 26 | class Video(): 27 | """BlueRov video capture class constructor 28 | 29 | Attributes: 30 | port (int): Video UDP port 31 | video_codec (string): Source h264 parser 32 | video_decode (string): Transform YUV (12bits) to BGR (24bits) 33 | video_pipe (object): GStreamer top-level pipeline 34 | video_sink (object): Gstreamer sink element 35 | video_sink_conf (string): Sink configuration 36 | video_source (string): Udp source ip and port 37 | """ 38 | 39 | def __init__(self, port=5600): 40 | """Summary 41 | 42 | Args: 43 | port (int, optional): UDP port 44 | """ 45 | 46 | Gst.init(None) 47 | 48 | self.port = port 49 | self._frame = None 50 | 51 | # [Software component diagram](https://www.ardusub.com/software/components.html) 52 | # UDP video stream (:5600) 53 | self.video_source = 'udpsrc port={}'.format(self.port) 54 | # [Rasp raw image](http://picamera.readthedocs.io/en/release-0.7/recipes2.html#raw-image-capture-yuv-format) 55 | # Cam -> CSI-2 -> H264 Raw (YUV 4-4-4 (12bits) I420) 56 | self.video_codec = '! application/x-rtp, payload=96 ! rtph264depay ! h264parse ! avdec_h264' 57 | # Python don't have nibble, convert YUV nibbles (4-4-4) to OpenCV standard BGR bytes (8-8-8) 58 | self.video_decode = \ 59 | '! decodebin ! videoconvert ! video/x-raw,format=(string)BGR ! videoconvert' 60 | # Create a sink to get data 61 | self.video_sink_conf = \ 62 | '! appsink emit-signals=true sync=false max-buffers=2 drop=true' 63 | 64 | self.video_pipe = None 65 | self.video_sink = None 66 | 67 | self.run() 68 | 69 | def start_gst(self, config=None): 70 | """ Start gstreamer pipeline and sink 71 | Pipeline description list e.g: 72 | [ 73 | 'videotestsrc ! decodebin', \ 74 | '! videoconvert ! video/x-raw,format=(string)BGR ! videoconvert', 75 | '! appsink' 76 | ] 77 | 78 | Args: 79 | config (list, optional): Gstreamer pileline description list 80 | """ 81 | 82 | if not config: 83 | config = \ 84 | [ 85 | 'videotestsrc ! decodebin', 86 | '! videoconvert ! video/x-raw,format=(string)BGR ! videoconvert', 87 | '! appsink' 88 | ] 89 | 90 | command = ' '.join(config) 91 | self.video_pipe = Gst.parse_launch(command) 92 | self.video_pipe.set_state(Gst.State.PLAYING) 93 | self.video_sink = self.video_pipe.get_by_name('appsink0') 94 | 95 | @staticmethod 96 | def gst_to_opencv(sample): 97 | """Transform byte array into np array 98 | 99 | Args: 100 | sample (TYPE): Description 101 | 102 | Returns: 103 | TYPE: Description 104 | """ 105 | buf = sample.get_buffer() 106 | caps = sample.get_caps() 107 | array = np.ndarray( 108 | (caps.get_structure(0).get_value('height'), 109 | caps.get_structure(0).get_value('width'), 3), 110 | buffer=buf.extract_dup(0, buf.get_size()), 111 | dtype=np.uint8) 112 | return array 113 | 114 | def frame(self): 115 | """ Get Frame 116 | 117 | Returns: 118 | iterable: bool and image frame, cap.read() output 119 | """ 120 | return self._frame 121 | 122 | def frame_available(self): 123 | """Check if frame is available 124 | 125 | Returns: 126 | bool: true if frame is available 127 | """ 128 | return type(self._frame) != type(None) 129 | 130 | def run(self): 131 | """ Get frame to update _frame 132 | """ 133 | 134 | self.start_gst([ 135 | self.video_source, self.video_codec, self.video_decode, 136 | self.video_sink_conf 137 | ]) 138 | 139 | self.video_sink.connect('new-sample', self.callback) 140 | 141 | def callback(self, sink): 142 | sample = sink.emit('pull-sample') 143 | new_frame = self.gst_to_opencv(sample) 144 | self._frame = new_frame 145 | 146 | return Gst.FlowReturn.OK 147 | 148 | 149 | if __name__ == '__main__': 150 | rospy.init_node("bluerov_video_node") 151 | # Create the video object 152 | # Add port= if is necessary to use a different one 153 | video = Video(4777) 154 | bridge = cv_bridge.CvBridge() 155 | img_pub = rospy.Publisher('/camera/image', Image, queue_size=10) 156 | 157 | if pub_info: 158 | info_pub = rospy.Publisher('/camera/camera_info', CameraInfo, queue_size=10) 159 | info_manger = camera_info_manager.CameraInfoManager(cname='camera', namespace='camera') 160 | info_manger.loadCameraInfo() 161 | 162 | rate = rospy.Rate(10) 163 | while not rospy.is_shutdown(): 164 | # Wait for the next frame 165 | if not video.frame_available(): 166 | continue 167 | 168 | frame = video.frame() 169 | 170 | img_msg = bridge.cv2_to_imgmsg(frame, encoding="bgr8") 171 | img_msg.header.frame_id = 'camera' 172 | img_msg.header.stamp = rospy.Time.now() 173 | img_pub.publish(img_msg) 174 | 175 | if pub_info: 176 | info_msg = info_manger.getCameraInfo() 177 | info_msg.header.frame_id = img_msg.header.frame_id 178 | info_msg.header.stamp = img_msg.header.stamp 179 | info_msg.width = frame.shape[1] 180 | info_msg.height = frame.shape[0] 181 | info_pub.publish(info_msg) 182 | 183 | rate.sleep() 184 | -------------------------------------------------------------------------------- /rti_dvl/scripts/rti_dvl_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import sys 3 | import rospy 4 | from std_srvs.srv import Trigger, TriggerResponse, TriggerRequest 5 | from rti_dvl.msg import DVL, Command, BottomTrack 6 | 7 | import pynmea2 8 | import serial 9 | 10 | SALINITY = {'fresh': 0, 'salt': 35} 11 | 12 | 13 | class SyncTime(object): 14 | """Time translator 15 | - See https://github.com/ethz-asl/cuckoo_time_translator for more details. 16 | - Implement https://github.com/ros-drivers/urg_node/blob/d2722c60f1b4713bbe1d39f32849090dece0104d/src/urg_c_wrapper.cpp#L1052 17 | """ 18 | 19 | def __init__(self): 20 | self.hardware_clock = 0.0 21 | self.last_hardware_time_stamp = 0.0 22 | self.hardware_clock_adj = 0.0 23 | self.adj_count = 0 24 | self.adj_alpha = 0.01 25 | 26 | def sync(self, time_stamp, system_time_stamp): 27 | delta = time_stamp - self.last_hardware_time_stamp 28 | self.hardware_clock += delta 29 | cur_adj = system_time_stamp.to_sec() - self.hardware_clock 30 | if self.adj_count > 0: 31 | self.hardware_clock_adj = ( 32 | self.adj_alpha * cur_adj + (1.0 - self.adj_alpha) * self.hardware_clock_adj) 33 | else: 34 | self.hardware_clock_adj = cur_adj 35 | 36 | self.adj_count += 1 37 | self.last_hardware_time_stamp = time_stamp 38 | 39 | stamp = system_time_stamp 40 | if self.adj_count > 100: 41 | stamp = stamp.from_sec(self.hardware_clock + self.hardware_clock_adj) 42 | 43 | if abs((stamp - system_time_stamp).to_sec()) > 0.1: 44 | self.adj_count = 0 45 | self.hardware_clock = 0.0 46 | self.last_hardware_time_stamp = 0.0 47 | self.stamp = system_time_stamp 48 | return stamp 49 | 50 | 51 | def write_to_dvl(s, duration=0.5): 52 | dvl.write((s.strip() + '\r').encode()) 53 | rospy.sleep(duration) 54 | 55 | 56 | if __name__ == '__main__': 57 | rospy.init_node('rti_dvl_node') 58 | 59 | dev = rospy.get_param('~dev', '/tmp/rti_dvl') 60 | # Get configurations split by ; 61 | config = rospy.get_param('~commands', '').split(';') 62 | 63 | if not rospy.has_param('/water'): 64 | rospy.set_param('/water', 'fresh') 65 | water = rospy.get_param('/water', 'fresh') 66 | salinity = SALINITY[water] 67 | 68 | dvl_pub = rospy.Publisher('/rti/body_velocity/raw', DVL, queue_size=100) 69 | bt_pub = rospy.Publisher('/rti/bottom_tracking/raw', BottomTrack, queue_size=100) 70 | 71 | while not rospy.is_shutdown(): 72 | try: 73 | rospy.loginfo('Open device {}'.format(dev)) 74 | dvl = serial.Serial(dev, dsrdtr=True, rtscts=True, timeout=1.0) 75 | # dvl = serial.Serial(dev) 76 | rospy.sleep(1.0) 77 | except serial.SerialException: 78 | rospy.logerr('Fail to open device {}'.format(dev)) 79 | rospy.sleep(1.0) 80 | else: 81 | break 82 | if rospy.is_shutdown(): 83 | sys.exit(-1) 84 | 85 | # STOP 86 | while dvl.in_waiting: 87 | line = dvl.readline() 88 | if line.strip().startswith('STOP'): 89 | break 90 | write_to_dvl('STOP') 91 | rospy.loginfo_throttle(1.0, 'Trying to stop DVL pinging...') 92 | 93 | # Zero pressure sensor 94 | write_to_dvl('CPZ') 95 | # Change water salinity 96 | write_to_dvl('CWS {}'.format(salinity)) 97 | # Write extra commands in config file 98 | for c in config: 99 | if c: 100 | write_to_dvl(c) 101 | rospy.loginfo('Write to DVL: ' + c) 102 | 103 | cmd = Command() 104 | while True: 105 | write_to_dvl('CSHOW') 106 | rospy.loginfo_throttle(1.0, 'Trying to read DVL commands...') 107 | 108 | received = False 109 | while dvl.in_waiting: 110 | line = dvl.readline() 111 | cp = line.strip().split(' ', 1) 112 | if len(cp) == 2: 113 | c, p = cp 114 | if c == 'CEPO': 115 | received = True 116 | c = c.split('[', 1)[0] 117 | if c in cmd.__slots__: 118 | setattr(cmd, c, p) 119 | 120 | if received: 121 | break 122 | else: 123 | rospy.sleep(1.0) 124 | 125 | # Start 126 | while not dvl.in_waiting: 127 | write_to_dvl('START') 128 | rospy.loginfo_throttle(1.0, 'Trying to start DVL pinging...') 129 | 130 | sync = SyncTime() 131 | reader = pynmea2.NMEAStreamReader(errors='ignore') 132 | 133 | bt = None 134 | while not rospy.is_shutdown(): 135 | try: 136 | char = dvl.read() 137 | except serial.SerialException: 138 | break 139 | for msg in reader.next(char): 140 | if isinstance(msg, pynmea2.types.rti.RTI01) or isinstance(msg, pynmea2.types.rti.RTI03): 141 | stamp = sync.sync(msg.time / 100.0, rospy.Time.now()) 142 | 143 | # For back compability 144 | d = DVL() 145 | d.header.stamp = stamp 146 | d.velocity.x = msg.x / 1000.0 147 | d.velocity.y = msg.y / 1000.0 148 | d.velocity.z = msg.z / 1000.0 149 | d.temperature = msg.temperature / 100.0 150 | d.altitude = msg.depth / 1000.0 151 | d.time = msg.time / 100.0 152 | dvl_pub.publish(d) 153 | 154 | bt = BottomTrack() 155 | bt.header.stamp = stamp 156 | bt.command = cmd 157 | bt.velocity.x = msg.x / 1000.0 158 | bt.velocity.y = msg.y / 1000.0 159 | bt.velocity.z = msg.z / 1000.0 160 | bt.temperature = msg.temperature / 100.0 161 | bt.altitude = msg.depth / 1000.0 162 | bt.time = msg.time / 100.0 163 | bt.sample = msg.number 164 | 165 | if isinstance(msg, pynmea2.types.rti.RTI30) or isinstance(msg, pynmea2.types.rti.RTI32): 166 | if bt is None: 167 | continue 168 | bt.orientation.x = msg.roll 169 | bt.orientation.y = msg.pitch 170 | bt.orientation.z = msg.heading 171 | bt_pub.publish(bt) 172 | 173 | dvl.close() 174 | -------------------------------------------------------------------------------- /imu_vn_100/README.md: -------------------------------------------------------------------------------- 1 | [![Build Status](https://travis-ci.org/KumarRobotics/imu_vn_100.svg?branch=master)](https://travis-ci.org/KumarRobotics/imu\_vn\_100) 2 | 3 | # imu\_vn\_100 4 | 5 | ![Picture of IMU](https://www.vectornav.com/images/default-source/products/vn-100-red-chip_right.png?sfvrsn=2302aad6_8) 6 | 7 | The `imu_vn_100` package is a linux ROS driver for VN-100 Rugged IMU of [VECTORNAV](http://www.vectornav.com/). The package is developed based on the official [SDK v0.3.2](http://www.vectornav.com/support/downloads) for Linux. The user manual for the device can be found [here](http://www.vectornav.com/docs/default-source/documentation/vn-100-documentation/UM001.pdf?sfvrsn=10). 8 | 9 | The package is tested on Ubuntu 14.04 with ROS indigo. 10 | 11 | Note that the official SDK is modified within this package due to some bugs or unsupported features. Please carefully update the SDK, since the new SDK may not work with the provided software. 12 | 13 | ## License 14 | 15 | * The license for the official SDK is the MIT license which is included in the `include/imu_vn_100/vncpplib` 16 | * The license for the other codes is Apache 2.0 whenever not specified. 17 | 18 | ## Compiling 19 | 20 | This is a Catkin package. Make sure the package is on `ROS_PACKAGE_PATH` after cloning the package to your workspace. And the normal procedure for compiling a catkin package will work. 21 | 22 | ``` 23 | cd your_work_space 24 | catkin_make --pkg imu_vn_100 --cmake-args -DCMAKE_BUILD_TYPE=Release 25 | ``` 26 | 27 | ## Example Usage 28 | 29 | **Parameters** 30 | 31 | `port` (`string`, `default: \tty\USB0`) 32 | 33 | Port which the device connected to. This can be checked by command `dmesg`. 34 | 35 | `frame_id` (`string`, `default: imu`) 36 | 37 | The frame ID entry for the sent messages. 38 | 39 | `baudrate` (`int`, `921600`) 40 | 41 | The baud rate of the serial port. The available baud rates can be checked on the user manual. It is suggested that the baud rate is kept at `921600` to ensure the high frequency transmission. The device will send `permission denied` error code if the baud rate cannot support the desired data package at the desired frequency. 42 | 43 | `imu_rate` (`int`, `200`) 44 | 45 | The rate of the IMU data. 46 | 47 | ``` 48 | enable_magn (bool, default: true) 49 | enable_press (bool, default: true) 50 | enable_temp (bool, default: true) 51 | enable_rpy (bool, default: false) 52 | ``` 53 | 54 | Enable other possible messages that the driver is available to send. Note that the frequency of the data for each of these messages will be the same with the IMU data, if the topic is enabled. 55 | 56 | `sync_rate` (`int`, `20`) 57 | 58 | The rate of the sync out trigger signal. Note that the actual rate may not exactly match the provided rate, unless the provided rate is a divisor of 800. When `sync_rate` <= 0, it is disabled. 59 | 60 | `binary_output` (`boolean`, `default: true`) 61 | 62 | Use binary protocol for receiving messages instead of ASCII. 63 | 64 | `binary_async_mode` (`int`, `default: 2`) 65 | 66 | Set serial port for binary messages to one of: 67 | 68 | * `1` - Serial port 1 69 | * `2` - Serial port 2 70 | 71 | `imu_compensated` (`boolean`, `default: true`) 72 | 73 | Use *compensated* IMU measurements (i.e. angular velocity, linear acceleration, magnetic field). 74 | 75 | `vpe_enable` (`boolean`, `default: true`) 76 | 77 | Use Vector Processing Engine. 78 | 79 | `vpe_heading_mode` (`int`, `default: 1`) 80 | 81 | Set VPE heading mode to one of: 82 | 83 | * `0` - Absolute 84 | * `1` - Relative 85 | * `2` - Indoor 86 | 87 | `vpe_filtering_mode` (`int`, `default: 1`) 88 | 89 | Set VPE filtering mode to one of: 90 | 91 | * `0` - Off 92 | * `1` - *MODE 1* 93 | 94 | `vpe_tuning_mode` (`int`, `default: 1`) 95 | 96 | Set VPE tuning mode to one of: 97 | 98 | * `0` - Off 99 | * `1` - *MODE 1* 100 | 101 | **Published Topics** 102 | 103 | `imu/imu` (`sensor_msgs/Imu`) 104 | 105 | If `imu_compensated` is `false`, the default, then the message contains the *uncompensated* (for the definition of UNCOMPENSATED, please refer to the [user manual](http://www.vectornav.com/docs/default-source/documentation/vn-100-documentation/UM001.pdf?sfvrsn=10)) angular velocity and linear acceleration. Otherwise both are *compensated*. 106 | 107 | `imu/magnetic_field` (`sensor_msgs/MagneticField`) 108 | 109 | Magnetic field. If `imu_compensated` is `false` then it is *uncompensated* otherwise it is *compensated*. 110 | 111 | `imu/pressure` (`sensor_msgs/FluidPressure`) 112 | 113 | Pressure. 114 | 115 | `imu/temperature` (`sensor_msgs/Temperature`) 116 | 117 | Temperature in degree Celsius 118 | 119 | `imu/rpy` (`geometry_msgs/Vector3Stamped`) 120 | 121 | Estimated *attitute* roll (`x`), pitch (`y`) and yaw (`z`) angles measured in radians in body frame. These are with respect to NED or ENU coordinate frame depending on`tf_ned_to_enu`. 122 | 123 | **Node** 124 | 125 | With the provided launch file, do 126 | 127 | ``` 128 | roslaunch imu_vn_100 vn_100_cont.launch 129 | ``` 130 | 131 | ## FAQ 132 | 133 | 1. The driver can't open my device?\ 134 | Make sure you have ownership of the device in `/dev`. 135 | 136 | 2. Why I have permission error during the initialization process of the driver?\ 137 | Most often, this is because the baud rate you set does not match the package size to be received. Try increase the baud rate. 138 | 139 | 3. Why is the IMU data output rate much lower than what is set?\ 140 | This may be due to a recent change in the FTDI USB-Serial driver in the Linux kernel, the following shell script might help: 141 | ```bash 142 | # Reduce latency in the FTDI serial-USB kernel driver to 1ms 143 | # This is required due to https://github.com/torvalds/linux/commit/c6dce262 144 | for file in $(ls /sys/bus/usb-serial/devices/); do 145 | value=`cat /sys/bus/usb-serial/devices/$file/latency_timer` 146 | if [ $value -gt 1 ]; then 147 | echo "Setting low_latency mode for $file" 148 | sudo sh -c "echo 1 > /sys/bus/usb-serial/devices/$file/latency_timer" 149 | fi 150 | done 151 | ``` 152 | 153 | ## Bug Report 154 | 155 | Prefer to open an issue. You can also send an E-mail to sunke.polyu@gmail.com. 156 | 157 | ## Bugs or Features Required of the Official SDK 158 | 159 | * [Solved] Define macros for options to set synchronization register and communication control register. (vndevice.h) 160 | 161 | * [Solved] Pressure entry is not properly parsed and overwrites temperature in `vndevice_processAsyncData` when the async data output type is set to `VNASYNC_VNIMU`. (vndevice.c) 162 | 163 | * [Solved] Synchronization time or count is not parsed in `vndevice_processAsyncData`. (vndevice.c) 164 | 165 | * Angular velocity and linear acceleration are flipped when the device is set to binary output using `BG1_IMU` 166 | 167 | * Orientation and uncompensated IMU measurements cannot be acquired within a single setting using async output. 168 | -------------------------------------------------------------------------------- /rti_dvl/msg/Command.msg: -------------------------------------------------------------------------------- 1 | # Subsystem Configuration 2 | # 3: 600 kHz 4 beam 20 degree piston 3 | string CEPO 4 | 5 | # CEAUTOSTART On/OFF 6 | # 0 OFF 7 | # 1 DO NOT USE 8 | # 2 RS232 9 | # 3 RS485 10 | # 4 RS422 11 | # 5 EMAC 12 | # 6 UDP 13 | string CEAUTOSTART 14 | 15 | # Ensemble output period in HH:MM:SS.hh 16 | string CEI 17 | 18 | # Output format 19 | # [1] - RoweTech Binary (RTB) 20 | # [1] - RoweTech DVL (RTD) 21 | # [2] - ASCII Format 22 | # [2] - DVL Format 23 | # [3] - NMEA Status String 24 | # [4] - Ocean Server NMEA DVL Format 25 | # [5] - DVL Format 26 | # [100] - PD0 27 | # [103] - PD3 Binary Format 28 | # [104] - PD4 Binary Format 29 | # [105] - PD5 Binary Format 30 | # [106] - PD6 ASCII Format 31 | # [113] - PD13 ASCII Format 32 | string C485OUT 33 | string C232OUT 34 | 35 | # 1 - Turn ON Bottom Tracking 36 | # 0 - Turn OFF Bottom Tracking 37 | string CBTON 38 | 39 | # Bottom Track Broadband. Sets Bottom Track coded pulse transmission and lag. 40 | # n, m, d, x 41 | # [n] - Transmit Pulse Type and Processing 42 | # 0 = Narrowband. 43 | # Provides long range profiles at the expense of variance. 44 | # Not recommended for use with bin size less than the default bin size. 45 | # 1 = Broadband. 46 | # Typically 15% less range than narrow band but has greatly reduced variance (depending on lag length). 47 | # Used in conjunction with CWPBP for small bins. 48 | # 2 = Un-coded Broadband (no ambiguity resolver). 49 | # Non-coded has slightly higher variance than the coded transmit without the annoying autocorrelation side peaks. Better for small bins. 50 | # 4 = Non Coded Broadband pulse-to-pulse (no ambiguity resolver). 51 | # Provides ultra low variance for small bin sizes. 52 | # 7 = Auto Switch (n=0, n=2, n=4) 53 | # [m] - Lag length in vertical meters (m) 54 | # The lag length will set the maximum velocity the ADCP can read. A rule of thumb is to set the lag length, where 1.5 is a 'safety factor'. 55 | # [d] - Long Range Narrowband Auto Switch Depth (m) 56 | # The depth to switch from Broadband to Narrowband in meters. This is used in the Auto Switch mode. 57 | # [x] - Beam Multiplex 58 | # 1 = ping and process each beam one at a time. 59 | # 2 = ping and process beam pairs. 60 | # 4 = ping and process all four beams together. 61 | string CBTBB 62 | 63 | # Set the maximum depth to search for the bottom. This will reduce the amount of time in the search algorithm. 64 | string CBTMX 65 | 66 | # Blanking distance for the Bottom Track ping. This is the area that will be ignored just after the ADCP. 67 | # n.nn, m.mm 68 | # [n.nn] - 0 to 10 meters. Sets the vertical distance from the face of the transducer at which the bottom detection algorithm begins searching for the bottom when range to the bottom is LESS than CBTT parameter b. 69 | # [m.mm] - 0 to 300 meters. Sets the vertical distance from the face of the transducer at which the bottom detection algorithm begins searching for the bottom when range to the bottom is GREATER than CBTT parameter b. 70 | string CBTBL 71 | 72 | # Bottom Track thresholds. 73 | # a,b,c,d 74 | # [a] - SNR (dB) Shallow Detection Threshold. SNR(dB) shallow detection threshold. Lowering the SNR counts "a" and/or "c" will allow to the DVL to detect smaller bottom echo at greater range. The consequence is that DVL may false detect the bottom at the wrong range when the bottom signal is weak. 75 | # [b] - Depth(m) Shallow Switch. Depth(m) at which the bottom track switches from using the shallow to the deep SNR. Conditions in shallow water (high backscatter) can be different than deep water so "b" allows for two different SNR settings one for shallow ("a") and one for deep ("c"). 76 | # [c] - SNR(dB) Deep Detection Threshold. SNR(dB) deep detection threshold. Lowering the SNR counts "a" and/or "c" will allow to the DVL to detect smaller bottom echo at greater range. The consequence is that DVL may false detect the bottom at the wrong range when the bottom signal is weak. 77 | # [d] - Depth(m) Deep Switch. Depth(m) at which the bottom track switches from low to high gain receive. The ADCP/DVL has a high power transmitter. In shallow water the bottom echo may saturate the receiver input. While this does not harm the system saturation limits the measurable signal level of the bottom echo which can make it difficult to detect the bottom in a high water backscatter environment. The ADCP/DVL places the receiver in low gain when the depth is below the "d" parameter setting. The change in gain is about 40 dB. If you observe the ADCP/DVL having difficulty detecting the bottom near the "d" setting you may need set "d" to a deeper or more shallow depth depending on the depth where the detection is poor. A good rule to follow is a strong bottom echo requires a larger value in d and a weak bottom echo a smaller value. 78 | string CBTT 79 | 80 | # Not used for bottom tracking only 81 | # This is used to create a pause between the Water Profile and Bottom Track ping. This will allow the Bottom Track ping to die down if it is reverberating and causing interference in the Water Profile ping. 82 | string CBTTBP 83 | 84 | # Set thresholds to mark the data bad. This will give 3 options of forcing data to be bad. This can also be used to make all the data good by settings all the thresholds to 0. 85 | # c.cc,q.qq,v.vv 86 | # [c.cc] - Correlation threshold. 87 | # [q.qq] - Error Velocity threshold. Range: m/s 88 | # [v.vv] - Velocity threshold. Range: m/s 89 | string CBTST 90 | 91 | # Low pass filtering helps reduce velocity measurement noise 92 | # a,b,c,d,e 93 | # We do not recommend that the filter be enabled. 94 | string CBTFILT 95 | 96 | # Water Speed of Sound Control. 97 | # 1, 2, 3, 4 98 | # [1] - Water Temperature Source 99 | # [2] - Transducer Depth Source 100 | # [3] - Salinity Source 101 | # [4] - Speed of Sound Source 102 | # Choices 103 | # 0 = Command 104 | # 1 = Sensor 105 | # 2 = Internal Calculation. 106 | string CWSSC 107 | 108 | # Water Salinity (ppt). Used in the water speed of sound calculation 109 | string CWS 110 | 111 | # Water Temperature (degrees celsius). Used in the water speed of sound calculation if the temperature sensor is not available. 112 | string CWT 113 | 114 | # Transducer Depth (meters). Used in the water speed of sound calculation. 115 | string CTD 116 | 117 | # Water Speed of Sound (meters per second). 118 | string CWSS 119 | 120 | # This will set the heading offset for the ADCP. The heading offset will be applied to the heading value before it is to be used within the system for calculations. Typically only the first value is given. 121 | string CHO 122 | # Heading source. Used to state whether to use no heading, internal compass heading or GPS heading. This value should be set to GPS HDT if you cannot calibrate the compass in its environment. 123 | string CHS 124 | # Tilt Source. Set whether to use the tilt from internal compass or no tilt. 125 | string CTS 126 | 127 | # Velocity Scale Factor 128 | # n.nn, m.mm 129 | # [n.nn] - Water Velocity Scale Factor 130 | # [m.mm] - Bottom Track Scale Factor 131 | string CVSF 132 | -------------------------------------------------------------------------------- /rti_dvl/scripts/pynmea2/nmea.py: -------------------------------------------------------------------------------- 1 | import re 2 | import operator 3 | from functools import reduce 4 | 5 | 6 | class ParseError(ValueError): 7 | def __init__(self, message, data): 8 | super(ParseError, self).__init__((message, data)) 9 | 10 | class SentenceTypeError(ParseError): 11 | pass 12 | 13 | class ChecksumError(ParseError): 14 | pass 15 | 16 | 17 | class NMEASentenceType(type): 18 | sentence_types = {} 19 | def __init__(cls, name, bases, dct): 20 | type.__init__(cls, name, bases, dct) 21 | base = bases[0] 22 | if base is object: 23 | return 24 | base.sentence_types[name] = cls 25 | cls.name_to_idx = dict((f[1], i) for i, f in enumerate(cls.fields)) 26 | 27 | 28 | # http://mikewatkins.ca/2008/11/29/python-2-and-3-metaclasses/ 29 | NMEASentenceBase = NMEASentenceType('NMEASentenceBase', (object,), {}) 30 | 31 | 32 | class NMEASentence(NMEASentenceBase): 33 | ''' 34 | Base NMEA Sentence 35 | 36 | Parses and generates NMEA strings 37 | 38 | Examples: 39 | 40 | >>> s = NMEASentence.parse("$GPGGA,184353.07,1929.045,S,02410.506,E,1,04,2.6,100.00,M,-33.9,M,,0000*6D") 41 | >>> print(s) 42 | ''' 43 | 44 | sentence_re = re.compile(r''' 45 | # start of string, optional whitespace, optional '$' 46 | ^\s*\$? 47 | 48 | # message (from '$' or start to checksum or end, non-inclusve) 49 | (?P 50 | # sentence type identifier 51 | (?P 52 | 53 | # proprietary sentence 54 | (P\w{3})| 55 | 56 | # query sentence, ie: 'CCGPQ,GGA' 57 | # NOTE: this should have no data 58 | (\w{2}\w{2}Q,\w{3})| 59 | 60 | # taker sentence, ie: 'GPGGA' 61 | (\w{2}\w{3},) 62 | ) 63 | 64 | # rest of message 65 | (?P[^*]*) 66 | 67 | ) 68 | # checksum: *HH 69 | (?:[*](?P[A-F0-9]{2}))? 70 | 71 | # optional trailing whitespace 72 | \s*[\r\n]*$ 73 | ''', re.X | re.IGNORECASE) 74 | 75 | talker_re = \ 76 | re.compile(r'^(?P\w{2})(?P\w{3}),$') 77 | query_re = \ 78 | re.compile(r'^(?P\w{2})(?P\w{2})Q,(?P\w{3})$') 79 | proprietary_re = \ 80 | re.compile(r'^P(?P\w{3})$') 81 | 82 | name_to_idx = {} 83 | fields = () 84 | 85 | @staticmethod 86 | def checksum(nmea_str): 87 | return reduce(operator.xor, map(ord, nmea_str), 0) 88 | 89 | @staticmethod 90 | def parse(line, check=False): 91 | ''' 92 | parse(line) 93 | 94 | Parses a string representing a NMEA 0183 sentence, and returns a 95 | NMEASentence object 96 | 97 | Raises ValueError if the string could not be parsed, or if the checksum 98 | did not match. 99 | ''' 100 | match = NMEASentence.sentence_re.match(line) 101 | if not match: 102 | raise ParseError('could not parse data', line) 103 | 104 | # pylint: disable=bad-whitespace 105 | nmea_str = match.group('nmea_str') 106 | data_str = match.group('data') 107 | checksum = match.group('checksum') 108 | sentence_type = match.group('sentence_type').upper() 109 | data = data_str.split(',') 110 | 111 | if checksum: 112 | cs1 = int(checksum, 16) 113 | cs2 = NMEASentence.checksum(nmea_str) 114 | if cs1 != cs2: 115 | raise ChecksumError( 116 | 'checksum does not match: %02X != %02X' % (cs1, cs2), data) 117 | elif check: 118 | raise ChecksumError( 119 | 'strict checking requested but checksum missing', data) 120 | 121 | talker_match = NMEASentence.talker_re.match(sentence_type) 122 | if talker_match: 123 | talker = talker_match.group('talker') 124 | sentence = talker_match.group('sentence') 125 | cls = TalkerSentence.sentence_types.get(sentence) 126 | 127 | if not cls: 128 | # TODO instantiate base type instead of fail 129 | raise SentenceTypeError( 130 | 'Unknown sentence type %s' % sentence_type, line) 131 | return cls(talker, sentence, data) 132 | 133 | query_match = NMEASentence.query_re.match(sentence_type) 134 | if query_match and not data_str: 135 | talker = query_match.group('talker') 136 | listener = query_match.group('listener') 137 | sentence = query_match.group('sentence') 138 | return QuerySentence(talker, listener, sentence) 139 | 140 | proprietary_match = NMEASentence.proprietary_re.match(sentence_type) 141 | if proprietary_match: 142 | manufacturer = proprietary_match.group('manufacturer') 143 | cls = ProprietarySentence.sentence_types.get(manufacturer, ProprietarySentence) 144 | return cls(manufacturer, data) 145 | 146 | raise ParseError( 147 | 'could not parse sentence type: %r' % sentence_type, line) 148 | 149 | def __getattr__(self, name): 150 | #pylint: disable=invalid-name 151 | t = type(self) 152 | try: 153 | i = t.name_to_idx[name] 154 | except KeyError: 155 | raise AttributeError(name) 156 | f = t.fields[i] 157 | if i < len(self.data): 158 | v = self.data[i] 159 | else: 160 | v = '' 161 | if len(f) >= 3: 162 | if v == '': 163 | return None 164 | try: 165 | return f[2](v) 166 | except: 167 | return v 168 | else: 169 | return v 170 | 171 | def __setattr__(self, name, value): 172 | #pylint: disable=invalid-name 173 | t = type(self) 174 | if name not in t.name_to_idx: 175 | return object.__setattr__(self, name, value) 176 | 177 | i = t.name_to_idx[name] 178 | self.data[i] = str(value) 179 | 180 | def __repr__(self): 181 | #pylint: disable=invalid-name 182 | r = [] 183 | d = [] 184 | t = type(self) 185 | for i, v in enumerate(self.data): 186 | if i >= len(t.fields): 187 | d.append(v) 188 | continue 189 | name = t.fields[i][1] 190 | r.append('%s=%r' % (name, getattr(self, name))) 191 | 192 | return '<%s(%s)%s>' % ( 193 | type(self).__name__, 194 | ', '.join(r), 195 | d and ' data=%r' % d or '' 196 | ) 197 | 198 | def identifier(self): 199 | raise NotImplementedError 200 | 201 | def render(self, checksum=True, dollar=True, newline=False): 202 | res = self.identifier() + ','.join(self.data) 203 | if checksum: 204 | res += '*%02X' % NMEASentence.checksum(res) 205 | if dollar: 206 | res = '$' + res 207 | if newline: 208 | res += (newline is True) and '\r\n' or newline 209 | return res 210 | 211 | def __str__(self): 212 | return self.render() 213 | 214 | 215 | class TalkerSentence(NMEASentence): 216 | sentence_types = {} 217 | def __init__(self, talker, sentence_type, data): 218 | self.talker = talker 219 | self.sentence_type = sentence_type 220 | self.data = list(data) 221 | 222 | def identifier(self): 223 | return '%s%s,' % (self.talker, self.sentence_type) 224 | 225 | 226 | class QuerySentence(NMEASentence): 227 | sentence_types = {} 228 | def __init__(self, talker, listener, sentence_type): 229 | self.talker = talker 230 | self.listener = listener 231 | self.sentence_type = sentence_type 232 | self.data = [] 233 | 234 | def identifier(self): 235 | return '%s%sQ,%s,' % (self.talker, self.listener, self.sentence_type) 236 | 237 | 238 | class ProprietarySentence(NMEASentence): 239 | sentence_types = {} 240 | def __init__(self, manufacturer, data): 241 | self.manufacturer = manufacturer 242 | self.data = list(data) 243 | 244 | def identifier(self): 245 | return 'P%s' % (self.manufacturer) 246 | -------------------------------------------------------------------------------- /sonar_oculus/src/Oculus.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * (c) Copyright 2017 Blueprint Subsea. 3 | * This file is part of Oculus Viewer 4 | * 5 | * Oculus Viewer is free software: you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation, either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * Oculus Viewer is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License 16 | * along with this program. If not, see . 17 | * 18 | *****************************************************************************/ 19 | 20 | #pragma once 21 | 22 | #include 23 | 24 | // ----------------------------------------------------------------------------- 25 | // Collection of data classes provided by JGS 26 | // updated 10/11/15 for ping is etc 27 | // updated 07/12/15 for additional fields 28 | 29 | // All structures are single byte packed 30 | #pragma pack(push, 1) 31 | 32 | // The test id contained in the oculus header file 33 | #define OCULUS_CHECK_ID 0x4f53 34 | 35 | // ----------------------------------------------------------------------------- 36 | enum OculusMasterStatusType : uint8_t 37 | { 38 | oculusMasterStatusSsblBoot, 39 | oculusMasterStatusSsblRun, 40 | oculusMasterStatusMainBoot, 41 | oculusMasterStatusMainRun, 42 | }; 43 | 44 | // ----------------------------------------------------------------------------- 45 | enum OculusPauseReasonType : uint8_t 46 | { 47 | oculusPauseMagSwitch, 48 | oculusPauseBootFromMain, 49 | oculusPauseFlashError, 50 | oculusPauseJtagLoad, 51 | }; 52 | 53 | // ----------------------------------------------------------------------------- 54 | enum OculusTemperatureStatusType : uint8_t 55 | { 56 | oculusTempGood, 57 | oculusTempOverheat, 58 | oculusTempReserved, 59 | oculusTempOvermax, 60 | }; 61 | 62 | // ----------------------------------------------------------------------------- 63 | class OculusConstants 64 | { 65 | public: 66 | static const uint32_t OSS_MAX_BEAMS = 512; 67 | }; 68 | 69 | // ----------------------------------------------------------------------------- 70 | enum OculusDeviceType : uint16_t 71 | { 72 | deviceTypeUndefined = 0, 73 | deviceTypeImagingSonar = 1, 74 | }; 75 | 76 | // ----------------------------------------------------------------------------- 77 | enum OculusPartNumberType : uint16_t 78 | { 79 | partNumberUndefined = 0, 80 | partNumberM750d = 1032, 81 | partNumberM750d_Fusion = 1134, 82 | partNumberM750d_Artemis = 1135, 83 | partNumberM370 = 1041, 84 | partNumberM1200d = 1042, 85 | }; 86 | 87 | // ----------------------------------------------------------------------------- 88 | enum OculusMessageType : uint16_t 89 | { 90 | messageSimpleFire = 0x15, 91 | messagePingResult = 0x22, 92 | messageSimplePingResult = 0x23, 93 | messageDummy = 0xff, 94 | }; 95 | 96 | // ----------------------------------------------------------------------------- 97 | enum PingRateType : uint8_t 98 | { 99 | pingRateNormal = 0x00, // 10Hz max ping rate 100 | pingRateHigh = 0x01, // 15Hz max ping rate 101 | pingRateHighest = 0x02, // 40Hz max ping rate 102 | pingRateLow = 0x03, // 5Hz max ping rate 103 | pingRateLowest = 0x04, // 2Hz max ping rate 104 | pingRateStandby = 0x05, // Disable ping 105 | }; 106 | 107 | // ----------------------------------------------------------------------------- 108 | enum DataSizeType : uint8_t 109 | { 110 | dataSize8Bit, 111 | dataSize16Bit, 112 | dataSize24Bit, 113 | dataSize32Bit, 114 | }; 115 | 116 | // ----------------------------------------------------------------------------- 117 | struct OculusMessageHeader 118 | { 119 | public: 120 | uint16_t oculusId; // Fixed ID 0x4f53 121 | uint16_t srcDeviceId; // The device id of the source 122 | uint16_t dstDeviceId; // The device id of the destination 123 | uint16_t msgId; // Message identifier 124 | uint16_t msgVersion; 125 | uint32_t payloadSize; // The size of the message payload (header not included) 126 | uint16_t spare2; 127 | }; 128 | 129 | // ----------------------------------------------------------------------------- 130 | struct OculusSimpleFireMessage 131 | { 132 | public: 133 | OculusMessageHeader head; // The standard message header 134 | 135 | uint8_t masterMode; // mode 0 is flexi mode, needs full fire message (not available for third party developers) 136 | // mode 1 - Low Frequency Mode (wide aperture, navigation) 137 | // mode 2 - High Frequency Mode (narrow aperture, target identification) 138 | PingRateType pingRate; // Sets the maximum ping rate. 139 | uint8_t networkSpeed; // Used to reduce the network comms speed (useful for high latency shared links) 140 | uint8_t gammaCorrection; // 0 and 0xff = gamma correction = 1.0 141 | // Set to 127 for gamma correction = 0.5 142 | uint8_t flags; // bit 0: 0 = interpret range as percent, 1 = interpret range as meters 143 | // bit 1: 0 = 8 bit data, 1 = 16 bit data 144 | // bit 2: 0 = wont send gain, 1 = send gain 145 | // bit 3: 0 = send full return message, 1 = send simple return message 146 | // bit 4: 0 = gain assistance off, 1 = gain assistance on 147 | // bit 5: 0 = low power mode off, 1 = low power mode on 148 | double range; // The range demand in percent or meters depending on flags 149 | double gainPercent; // The gain demand if gain assistance is off or intensity demand if gain assistance is on 150 | double speedOfSound; // meters/second, if set to zero then internal calc will apply using salinity 151 | double salinity; // ppt, set to zero if we are in fresh water and 35.0 if we are in salt water 152 | }; 153 | 154 | // ----------------------------------------------------------------------------- 155 | struct OculusSimplePingResult 156 | { 157 | public: 158 | OculusSimpleFireMessage fireMessage; 159 | uint32_t pingId; /* An incrementing number */ 160 | uint32_t status; 161 | double frequency; /* The acoustic frequency (Hz) */ 162 | double temperature; /* The external temperature (deg C) */ 163 | double pressure; /* The external pressure (bar) */ 164 | double speedOfSoundUsed; /* The actual used speed of sound (m/s). May be different to the speed of sound set in the fire message */ 165 | uint32_t pingStartTime; 166 | DataSizeType dataSize; /* The size of the individual data entries */ 167 | double rangeResolution; /* The range in metres corresponding to a single range line */ 168 | uint16_t nRanges; /* The number of range lines in the image*/ 169 | uint16_t nBeams; /* The number of bearings in the image */ 170 | uint32_t imageOffset; /* The offset in bytes of the image data from the start of the network message */ 171 | uint32_t imageSize; /* The size in bytes of the image data */ 172 | uint32_t messageSize; /* The total size in bytes of the network message */ 173 | // *** NOT ADDITIONAL VARIABLES BEYOND THIS POINT *** 174 | // There will be an array of bearings (shorts) found at the end of the message structure 175 | // Allocated at run time 176 | // short bearings[]; 177 | // The bearings to each of the beams in 0.01 degree resolution 178 | }; 179 | 180 | // ----------------------------------------------------------------------------- 181 | struct OculusVersionInfo 182 | { 183 | public: 184 | uint32_t firmwareVersion0; /* The arm0 firmware version major(8 bits), minor(8 bits), build (16 bits) */ 185 | uint32_t firmwareDate0; /* The arm0 firmware date */ 186 | uint32_t firmwareVersion1; /* The arm1 firmware version major(8 bits), minor(8 bits), build (16 bits) */ 187 | uint32_t firmwareDate1; /* The arm1 firmware date */ 188 | uint32_t firmwareVersion2; /* The bitfile version */ 189 | uint32_t firmwareDate2; /* The bitfile date */ 190 | }; 191 | 192 | // ----------------------------------------------------------------------------- 193 | struct OculusStatusMsg 194 | { 195 | public: 196 | OculusMessageHeader hdr; 197 | 198 | uint32_t deviceId; 199 | OculusDeviceType deviceType; 200 | OculusPartNumberType partNumber; 201 | uint32_t status; 202 | OculusVersionInfo versinInfo; 203 | uint32_t ipAddr; 204 | uint32_t ipMask; 205 | uint32_t connectedIpAddr; 206 | uint8_t macAddr0; 207 | uint8_t macAddr1; 208 | uint8_t macAddr2; 209 | uint8_t macAddr3; 210 | uint8_t macAddr4; 211 | uint8_t macAddr5; 212 | double temperature0; 213 | double temperature1; 214 | double temperature2; 215 | double temperature3; 216 | double temperature4; 217 | double temperature5; 218 | double temperature6; 219 | double temperature7; 220 | double pressure; 221 | }; 222 | 223 | #pragma pack(pop) 224 | -------------------------------------------------------------------------------- /imu_vn_100/vncpplib/include/vncp_services.h: -------------------------------------------------------------------------------- 1 | /** 2 | * \cond INCLUDE_PRIVATE 3 | * \file 4 | * 5 | * \section LICENSE 6 | * The MIT License (MIT) 7 | * 8 | * Copyright (c) 2014 VectorNav Technologies, LLC 9 | * 10 | * Permission is hereby granted, free of charge, to any person obtaining a copy 11 | * of this software and associated documentation files (the "Software"), to deal 12 | * in the Software without restriction, including without limitation the rights 13 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 14 | * copies of the Software, and to permit persons to whom the Software is 15 | * furnished to do so, subject to the following conditions: 16 | * 17 | * The above copyright notice and this permission notice shall be included in all 18 | * copies or substantial portions of the Software. 19 | * 20 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 21 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 22 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 23 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 24 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 25 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 26 | * SOFTWARE. 27 | * 28 | * \section DESCRIPTION 29 | * This header file provides access to the cross-platform services for the 30 | * VectorNav C/C++ Library. 31 | */ 32 | #ifndef _VNCP_SERVICES_H_ 33 | #define _VNCP_SERVICES_H_ 34 | 35 | #if defined(_MSC_VER) && _MSC_VER <= 1500 36 | /* Visual Studio 2008 and earlier do not include the stdint.h header file. */ 37 | #include "vnint.h" 38 | #else 39 | #include 40 | #endif 41 | 42 | #ifdef WIN32 43 | 44 | /* Disable some warnings for Visual Studio with -Wall. */ 45 | #if defined(_MSC_VER) 46 | #pragma warning(push) 47 | #pragma warning(disable:4668) 48 | #pragma warning(disable:4820) 49 | #pragma warning(disable:4255) 50 | #endif 51 | 52 | #include 53 | 54 | #if defined(_MSC_VER) 55 | #pragma warning(pop) 56 | #endif 57 | 58 | #endif 59 | 60 | #if defined(__linux__) || defined(__QNXNTO__) || defined(__APPLE__) 61 | #include 62 | #endif 63 | 64 | /* Determine the level of C support. */ 65 | #if __STDC__ 66 | #if defined (__STDC_VERSION__) 67 | #if (__STDC_VERSION__ >= 1999901L) 68 | #define C99 69 | #endif 70 | #endif 71 | #endif 72 | 73 | /* Define boolean type. */ 74 | #if defined (C99) 75 | #include 76 | #else 77 | #if !defined (__cplusplus) 78 | #if !defined (__GNUC__) 79 | /* _Bool builtin type is included in GCC. */ 80 | /* ISO C Standard: 5.2.5 An object declared as type _Bool is large 81 | * enough to store the values 0 and 1. */ 82 | typedef int8_t _Bool; 83 | #endif 84 | 85 | /* ISO C Standard: 7.16 Boolean type */ 86 | #define bool _Bool 87 | #define true 1 88 | #define false 0 89 | #define __bool_true_false_are_defined 1 90 | #endif 91 | #endif 92 | 93 | #include "vn_errorCodes.h" 94 | 95 | #ifdef __cplusplus 96 | extern "C" { 97 | #endif 98 | 99 | #define VN_NULL ((void *) 0) 100 | 101 | #ifdef WIN32 102 | typedef HANDLE VN_HANDLE; 103 | typedef CRITICAL_SECTION VN_CRITICAL_SECTION; 104 | #elif defined(__linux__) || defined(__QNXNTO__) || defined(__APPLE__) 105 | typedef union { 106 | pthread_t pThreadHandle; 107 | int comPortHandle; 108 | pthread_mutex_t mutexHandle; 109 | void* conditionAndMutexStruct; 110 | } VN_HANDLE; 111 | typedef pthread_mutex_t VN_CRITICAL_SECTION; 112 | #endif 113 | 114 | typedef void *(*VN_THREAD_START_ROUTINE)(void*); 115 | 116 | /** 117 | * \brief Creates a new thread. 118 | * 119 | * \param[out] newThreadHandle Handle of the newly created thread. 120 | * \param[in] startRoutine Pointer to the routine the new thread should execute. 121 | * \param[in] routineData Pointer to data that will be passed to the new thread's execution routine. 122 | * 123 | * \return VectorNav error code. 124 | */ 125 | VN_ERROR_CODE vncp_thread_startNew( 126 | VN_HANDLE* newThreadHandle, 127 | VN_THREAD_START_ROUTINE startRoutine, 128 | void* routineData); 129 | 130 | /** 131 | * \brief Opens a COM port. 132 | * 133 | * \param[out] newComPortHandle Handle to the newly opened COM port. 134 | * \param[in] portName The name of the COM port to open. 135 | * \param[in] baudrate The baudrate to communicate at. 136 | * 137 | * \return VectorNav error code. 138 | */ 139 | VN_ERROR_CODE vncp_comPort_open( 140 | VN_HANDLE* newComPortHandle, 141 | char const* portName, 142 | unsigned int baudrate); 143 | 144 | /** 145 | * \brief Write data out of a COM port. 146 | * 147 | * \param[in] comPortHandle Handle to an open COM port. 148 | * \param[in] dataToWrite Pointer to array of bytes to write out the COM port. 149 | * \param[in] numOfBytesToWrite The number of bytes to write from the dataToWrite pointer. 150 | * 151 | * \return VectorNav error code. 152 | */ 153 | VN_ERROR_CODE vncp_comPort_writeData( 154 | VN_HANDLE comPortHandle, 155 | char const* dataToWrite, 156 | unsigned int numOfBytesToWrite); 157 | 158 | /** 159 | * \brief Reads data from a COM port. Will block temporarily for a short amount 160 | * of time and then return if no data has been received. 161 | * 162 | * \param[in] comPortHandle Handle to an open COM port. 163 | * \param[out] readBuffer Pointer to a buffer where data read from the COM port will be placed into. 164 | * \param[in] numOfBytesToRead The number of bytes to attempt reading from the COM port. 165 | * \param[out] numOfBytesActuallyRead The number of bytes actually read from the COM port during the read attempt. 166 | * 167 | * \return VectorNav error code. 168 | */ 169 | VN_ERROR_CODE vncp_comPort_readData( 170 | VN_HANDLE comPortHandle, 171 | char* readBuffer, 172 | unsigned int numOfBytesToRead, 173 | unsigned int* numOfBytesActuallyRead); 174 | 175 | /** 176 | * \brief Closes the COM port. 177 | * 178 | * \param[in] comPortHandle Handle to an open COM port. 179 | * 180 | * \return VectorNav error code. 181 | */ 182 | VN_ERROR_CODE vncp_comPort_close( 183 | VN_HANDLE comPortHandle); 184 | 185 | /** 186 | * \brief Determines if the COM port needs to be optimized. Currently only 187 | * USB virtual COM ports on Windows need to be optimized for their 188 | * Latency Timer value. 189 | * 190 | * \param[in] portName The COM port name. 191 | * \param[out] isOptimized true if the COM port does not need optimization; 192 | * false if the COM port needs to be optimized. 193 | * 194 | * \return VectorNav error code. 195 | */ 196 | VN_ERROR_CODE vncp_comPort_isOptimized( 197 | char const* portName, 198 | bool* isOptimized); 199 | 200 | /** 201 | * \brief Attempts to optimize the COM port. Currently only USB virtual COM 202 | * ports on Windows need to be optimized for their Latency Timer value. 203 | * 204 | * \param[in] portName The COM port name. 205 | * 206 | * \return VectorNav error code. 207 | */ 208 | VN_ERROR_CODE vncp_comPort_optimize( 209 | char const* portName); 210 | 211 | /** 212 | * \brief Creates a new event. 213 | * 214 | * \param[out] newEventHandle Returns the handle of the newly created event. 215 | * 216 | * \return VectorNav error code. 217 | */ 218 | VN_ERROR_CODE vncp_event_create( 219 | VN_HANDLE* newEventHandle); 220 | 221 | /** 222 | * \brief Causes the calling thread to wait on an event until the event is signalled. 223 | * 224 | * \param[in] eventHandle Handle to the event. 225 | * \param[in] timeout The number of milliseconds to wait before the 226 | * thread stops listening. -1 indicates that the wait time is inifinite. If a 227 | * timeout does occur, the value VNERR_TIMEOUT will be retured. 228 | * 229 | * \return VectorNav error code. 230 | */ 231 | VN_ERROR_CODE vncp_event_waitFor( 232 | VN_HANDLE eventHandle, 233 | int timeout); 234 | 235 | /** 236 | * \brief Puts the provided event into a signalled state. 237 | * 238 | * \param[in] eventHandle Handle to the event. 239 | * 240 | * \return VectorNav error code. 241 | */ 242 | VN_ERROR_CODE vncp_event_signal( 243 | VN_HANDLE eventHandle); 244 | 245 | /** 246 | * \brief Initializes a new critical section object. 247 | * 248 | * \param[out] criticalSection Returns the newly initialized created critical control object. 249 | * 250 | * \return VectorNav error code. 251 | */ 252 | VN_ERROR_CODE vncp_criticalSection_initialize( 253 | VN_CRITICAL_SECTION* criticalSection); 254 | 255 | /** 256 | * \brief Attempt to enter a critical section. 257 | * 258 | * \param[in] criticalSection Pointer to the critical section control object. 259 | * 260 | * \return VectorNav error code. 261 | */ 262 | VN_ERROR_CODE vncp_criticalSection_enter( 263 | VN_CRITICAL_SECTION* criticalSection); 264 | 265 | /** 266 | * \brief Signals that the current executing thread is leaving the critical section. 267 | * 268 | * \param[in] criticalSection Pointer to the critical section control object. 269 | * 270 | * \return VectorNav error code. 271 | */ 272 | VN_ERROR_CODE vncp_criticalSection_leave( 273 | VN_CRITICAL_SECTION* criticalSection); 274 | 275 | /** 276 | * \brief Disposes and frees the resources associated with a critical section control object. 277 | * 278 | * \param[in] criticalSection Pointer to the critical section control object. 279 | * 280 | * \return VectorNav error code. 281 | */ 282 | VN_ERROR_CODE vncp_criticalSection_dispose( 283 | VN_CRITICAL_SECTION* criticalSection); 284 | 285 | /** 286 | * \brief Sleeps the current thread the specified number of milliseconds. 287 | * 288 | * \param[in] numOfMillisecondsToSleep The number of milliseconds to pause the current thread. 289 | * 290 | * \return VectorNav error code. 291 | */ 292 | VN_ERROR_CODE vncp_sleepInMs( 293 | unsigned int numOfMillisecondsToSleep); 294 | 295 | /** 296 | * \brief Starts timer for counting a time difference in milliseconds. 297 | */ 298 | void vncp_startMsTimer(); 299 | 300 | /** 301 | * \brief Stops the millisecond timer and returns the difference in milliseconds. 302 | * 303 | * \return The time difference in milliseconds. If the value is -1.0, this 304 | * indicates the vncp_startMsTimer was not called first. 305 | */ 306 | double vncp_stopMsTimer(); 307 | 308 | #ifdef __cplusplus 309 | } 310 | #endif 311 | 312 | #endif /* _VNCP_SERVICES_H_ */ 313 | 314 | /** \endcond */ 315 | -------------------------------------------------------------------------------- /sonar_oculus/src/sonar.cpp: -------------------------------------------------------------------------------- 1 | // VSieben@slb.com (original author, 2017) 2 | // pvt@mit.edu (extensions, 2018) 3 | // jwang92@stevens.edu 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include "Oculus.h" 21 | #include "OculusClient.h" 22 | 23 | // ROS includes 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | // Dynamic server 30 | #include 31 | #include 32 | 33 | #define BUFLEN 200 34 | #define DATALEN 200000 35 | #define PORT_UDP 52102 36 | #define PORT_TCP 52100 37 | 38 | // Global sonar configuration 39 | int mode = 1; // 0 => dev/not used, 1 => ~750khz, 2 => ~1.2Mhz. 40 | int ping_rate = 0; // 0 => normal 41 | double range = 10; // m, limited to 120m in mode 1, and 40m in mode 2 42 | double gain = 20; //% 43 | double soundspeed = 0; // m/s 44 | double salinity = 0; // ppm, 0 = freshwater, 35=saltwater 45 | 46 | // Error handling function 47 | void error(const char *msg) { 48 | perror(msg); 49 | exit(0); 50 | } 51 | 52 | // Create sonar oculus control class 53 | OsClientCtrl sonar; 54 | uint16_t partNumber; 55 | 56 | // Callback for dynamic reconfigure server 57 | void callback(sonar_oculus::OculusParamsConfig &config, uint32_t level) { 58 | mode = config.Mode; 59 | ping_rate = config.PingRate; 60 | gain = config.Gain; 61 | range = config.Range; 62 | salinity = config.Salinity; 63 | 64 | if (partNumber == OculusPartNumberType::partNumberM750d) { 65 | if (mode == 1) 66 | range = std::max(0.1, std::min(range, 120.0)); 67 | else if (mode == 2) 68 | range = std::max(0.1, std::min(range, 40.0)); 69 | } 70 | if (partNumber == OculusPartNumberType::partNumberM1200d) { 71 | range = std::max(0.1, std::min(range, 30.0)); 72 | } 73 | 74 | sonar.Fire(mode, ping_rate, range, gain, soundspeed, (double)salinity); 75 | } 76 | 77 | // Main program for listening to sonar 78 | int main(int argc, char **argv) { 79 | // Initialize ROS 80 | ROS_INFO("Initializing..."); 81 | ros::init(argc, argv, "sonar_oculus"); 82 | ros::NodeHandle nh("~"); 83 | 84 | // Read water from /water then set parameter for dynamic_reconfigure 85 | std::string water; 86 | nh.param("/water", water, "fresh"); 87 | nh.setParam("/sonar_oculus_node/Salinity", water == "fresh" ? 0 : 35); 88 | 89 | ros::Publisher ping_pub = nh.advertise("ping", 1); 90 | 91 | // Setup dynamic server 92 | dynamic_reconfigure::Server serverParam; 93 | dynamic_reconfigure::Server::CallbackType f; 94 | 95 | f = boost::bind(&callback, _1, _2); 96 | serverParam.setCallback(f); 97 | 98 | // Variable declarations 99 | // Communications 100 | struct sockaddr_in serverUDP, clientUDP; 101 | struct sockaddr_in serverTCP, clientTCP; 102 | int sockUDP, sockTCP, sockTCPfd, datagramSize, n; 103 | int buf_size = DATALEN; 104 | int keepalive = 1; 105 | socklen_t lengthServerUDP, lengthClientUDP; 106 | socklen_t lengthServerTCP, lengthClientTCP; 107 | char datagramMessage[BUFLEN], buffer[BUFLEN], sonardata[DATALEN]; 108 | 109 | // Clear and intialize values of server and client network info 110 | lengthServerUDP = sizeof(serverUDP); 111 | bzero((char *)&serverUDP, lengthServerUDP); 112 | serverUDP.sin_family = AF_INET; 113 | serverUDP.sin_addr.s_addr = htonl(INADDR_ANY); 114 | // serverUDP.sin_addr.s_addr = inet_addr(SONAR_ADDR); 115 | serverUDP.sin_port = htons(PORT_UDP); 116 | 117 | lengthClientUDP = sizeof(clientUDP); 118 | lengthServerTCP = sizeof(serverTCP); 119 | 120 | ROS_INFO("Connecting..."); 121 | // Create the UDP listening socket or exit 122 | sockUDP = socket(AF_INET, SOCK_DGRAM, 0); 123 | if (sockUDP < 0) 124 | error("Error opening UDP listening socket"); 125 | 126 | // Bind the UDP socket to address and port, or exit with error 127 | if (bind(sockUDP, (struct sockaddr *)&serverUDP, lengthServerUDP) < 0) 128 | error("Error binding UDP listening socket"); 129 | listen(sockUDP, 5); 130 | 131 | while (true) { 132 | int64_t bytesAvailable = 0; 133 | if (ioctl(sockUDP, FIONREAD, &bytesAvailable) < 0) { 134 | printf("ioctl failed and returned errno %s \n",strerror(errno)); 135 | ros::Duration(1.0).sleep(); 136 | continue; 137 | } 138 | 139 | OculusStatusMsg osm; 140 | if (bytesAvailable > 0) { 141 | unsigned bytesRead = read(sockUDP, (char*)&osm, bytesAvailable); 142 | 143 | uint32_t ts = (osm.status >> 14) & 0x0003; 144 | if (ts == 0) { 145 | ROS_INFO("Temperature OK; the sonar will ping normally"); 146 | } else if (ts == 1) { 147 | ROS_WARN("Temperature high; the sonar will ping at reduced rate"); 148 | } else if (ts == 3) { 149 | ROS_ERROR("Temperature shutdown; the sonar will not longer ping"); 150 | ros::requestShutdown(); 151 | return 0; 152 | } 153 | 154 | struct in_addr ip_addr; 155 | ip_addr.s_addr = osm.ipAddr; 156 | partNumber = osm.partNumber; 157 | ROS_INFO_STREAM("The IP address is " << inet_ntoa(ip_addr)); 158 | ROS_INFO_STREAM("Oculus part number is " << partNumber); 159 | 160 | bzero((char *)&serverTCP, lengthServerTCP); 161 | serverTCP.sin_family = AF_INET; 162 | serverTCP.sin_addr.s_addr = osm.ipAddr; 163 | serverTCP.sin_port = htons(PORT_TCP); 164 | 165 | // Create the TCP socket for main communication or exit 166 | sockTCP = socket(AF_INET, SOCK_STREAM, 0); 167 | if (sockTCP < 0) 168 | error("Error opening TCP main socket"); 169 | // Connect to the sonar Server via TCP socket or exit with error 170 | if (connect(sockTCP, (struct sockaddr *)&serverTCP, lengthServerTCP) < 0) 171 | error("Error connecting TCP socket"); 172 | if (setsockopt(sockTCP, SOL_SOCKET, SO_RCVBUF, &buf_size, sizeof(buf_size)) < 173 | 0) 174 | error("Error increasing RCVBUF for TCP socket"); 175 | if (setsockopt(sockTCP, SOL_SOCKET, SO_KEEPALIVE, &keepalive, 176 | sizeof(keepalive)) < 0) 177 | error("Error keeping alive option set for TCP socket"); 178 | listen(sockTCP, 5); 179 | break; 180 | } 181 | ros::Duration(1.0).sleep(); 182 | } 183 | 184 | std::string frame_str; 185 | 186 | // Setup Sonar and messages 187 | // Pass the socket to the control 188 | sonar.m_readData.m_pSocket = &sockTCP; 189 | // Connect and instance a thread 190 | sonar.Connect(); 191 | 192 | ROS_INFO("Connected!"); 193 | 194 | // Send Ping and initiate data collection 195 | sonar.Fire(mode, ping_rate, range, gain, soundspeed, salinity); 196 | // Get frame ID 197 | if (nh.getParam("frame", frame_str)) { 198 | ROS_INFO("Got param: %s", frame_str.c_str()); 199 | } else { 200 | ROS_ERROR("Failed to get param 'frame'"); 201 | frame_str = "/sonar"; 202 | } 203 | 204 | // Run continously 205 | ros::Rate r(50); // pvt: sonar should be under 40Hz (reduced 100 to 50) 206 | unsigned int latest_id = 0; // keep track of latest ping to avoid republishing 207 | while (ros::ok()) { 208 | 209 | // Run the readthread sonar 210 | sonar.m_readData.run(); 211 | 212 | // Get bins and beams #. 213 | unsigned int nbins = sonar.m_readData.m_osBuffer[0].m_rfm.nRanges; 214 | unsigned int nbeams = sonar.m_readData.m_osBuffer[0].m_rfm.nBeams; 215 | unsigned int id = sonar.m_readData.m_osBuffer[0].m_rfm.pingId; 216 | 217 | // Create pointcloud message from sonar data 218 | if (nbeams > 0 && nbins > 0 && id > latest_id) { 219 | latest_id = id; 220 | 221 | // sonar image 222 | if ( sonar.m_readData.m_osBuffer[0].m_rawSize){ 223 | sensor_msgs::Image sonar_image; 224 | sonar_image.header.stamp = ros::Time::now(); 225 | sonar_image.height = nbins; 226 | sonar_image.width = nbeams; 227 | sonar_image.encoding = "8UC1"; 228 | // sonar_image.is_bigendian = 0; // default works 229 | sonar_image.step = nbeams; 230 | sonar_image.data.resize(nbeams * nbins); 231 | std::copy(sonar.m_readData.m_osBuffer[0].m_pImage, 232 | sonar.m_readData.m_osBuffer[0].m_pImage + 233 | nbins*nbeams, 234 | sonar_image.data.begin()); 235 | 236 | // fire msg 237 | sonar_oculus::OculusFire fire_msg; 238 | fire_msg.header.stamp = ros::Time::now(); 239 | 240 | fire_msg.mode = sonar.m_readData.m_osBuffer[0].m_rfm.fireMessage.masterMode; 241 | fire_msg.gamma = 242 | sonar.m_readData.m_osBuffer[0].m_rfm.fireMessage.gammaCorrection; 243 | fire_msg.flags = sonar.m_readData.m_osBuffer[0].m_rfm.fireMessage.flags; 244 | fire_msg.range = sonar.m_readData.m_osBuffer[0].m_rfm.fireMessage.range; 245 | fire_msg.gain = 246 | sonar.m_readData.m_osBuffer[0].m_rfm.fireMessage.gainPercent; 247 | fire_msg.speed_of_sound = 248 | sonar.m_readData.m_osBuffer[0].m_rfm.fireMessage.speedOfSound; 249 | fire_msg.salinity = 250 | sonar.m_readData.m_osBuffer[0].m_rfm.fireMessage.salinity; 251 | 252 | // sonar ping 253 | sonar_oculus::OculusPing ping_msg; 254 | ping_msg.header.frame_id = frame_str; 255 | ping_msg.header.stamp = fire_msg.header.stamp; 256 | ping_msg.ping = sonar_image; 257 | ping_msg.fire_msg = fire_msg; 258 | ping_msg.ping_id = id; 259 | ping_msg.part_number = partNumber; 260 | 261 | ping_msg.start_time = sonar.m_readData.m_osBuffer[0].m_rfm.pingStartTime; 262 | ping_msg.bearings.resize(nbeams); 263 | for (int i = 0; i < nbeams; ++i) 264 | ping_msg.bearings[i] = sonar.m_readData.m_osBuffer[0].m_pBrgs[i]; 265 | ping_msg.range_resolution = 266 | sonar.m_readData.m_osBuffer[0].m_rfm.rangeResolution; 267 | ping_msg.num_ranges = nbins; 268 | ping_msg.num_beams = nbeams; 269 | 270 | ping_pub.publish(ping_msg); 271 | } 272 | } // if (nbins>0 && nbeams>0 && id>latest_id) 273 | 274 | // Fire sonar (so we sleep while the ping travels) 275 | r.sleep(); 276 | ros::spinOnce(); 277 | } 278 | 279 | // Disconnect and close 280 | sonar.Disconnect(); 281 | 282 | // Exit 283 | close(sockUDP); 284 | close(sockTCP); 285 | return 0; 286 | 287 | } // main 288 | -------------------------------------------------------------------------------- /imu_vn_100/LICENSE.txt: -------------------------------------------------------------------------------- 1 | 2 | Apache License 3 | Version 2.0, January 2004 4 | http://www.apache.org/licenses/ 5 | 6 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 7 | 8 | 1. Definitions. 9 | 10 | "License" shall mean the terms and conditions for use, reproduction, 11 | and distribution as defined by Sections 1 through 9 of this document. 12 | 13 | "Licensor" shall mean the copyright owner or entity authorized by 14 | the copyright owner that is granting the License. 15 | 16 | "Legal Entity" shall mean the union of the acting entity and all 17 | other entities that control, are controlled by, or are under common 18 | control with that entity. For the purposes of this definition, 19 | "control" means (i) the power, direct or indirect, to cause the 20 | direction or management of such entity, whether by contract or 21 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 22 | outstanding shares, or (iii) beneficial ownership of such entity. 23 | 24 | "You" (or "Your") shall mean an individual or Legal Entity 25 | exercising permissions granted by this License. 26 | 27 | "Source" form shall mean the preferred form for making modifications, 28 | including but not limited to software source code, documentation 29 | source, and configuration files. 30 | 31 | "Object" form shall mean any form resulting from mechanical 32 | transformation or translation of a Source form, including but 33 | not limited to compiled object code, generated documentation, 34 | and conversions to other media types. 35 | 36 | "Work" shall mean the work of authorship, whether in Source or 37 | Object form, made available under the License, as indicated by a 38 | copyright notice that is included in or attached to the work 39 | (an example is provided in the Appendix below). 40 | 41 | "Derivative Works" shall mean any work, whether in Source or Object 42 | form, that is based on (or derived from) the Work and for which the 43 | editorial revisions, annotations, elaborations, or other modifications 44 | represent, as a whole, an original work of authorship. For the purposes 45 | of this License, Derivative Works shall not include works that remain 46 | separable from, or merely link (or bind by name) to the interfaces of, 47 | the Work and Derivative Works thereof. 48 | 49 | "Contribution" shall mean any work of authorship, including 50 | the original version of the Work and any modifications or additions 51 | to that Work or Derivative Works thereof, that is intentionally 52 | submitted to Licensor for inclusion in the Work by the copyright owner 53 | or by an individual or Legal Entity authorized to submit on behalf of 54 | the copyright owner. For the purposes of this definition, "submitted" 55 | means any form of electronic, verbal, or written communication sent 56 | to the Licensor or its representatives, including but not limited to 57 | communication on electronic mailing lists, source code control systems, 58 | and issue tracking systems that are managed by, or on behalf of, the 59 | Licensor for the purpose of discussing and improving the Work, but 60 | excluding communication that is conspicuously marked or otherwise 61 | designated in writing by the copyright owner as "Not a Contribution." 62 | 63 | "Contributor" shall mean Licensor and any individual or Legal Entity 64 | on behalf of whom a Contribution has been received by Licensor and 65 | subsequently incorporated within the Work. 66 | 67 | 2. Grant of Copyright License. Subject to the terms and conditions of 68 | this License, each Contributor hereby grants to You a perpetual, 69 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 70 | copyright license to reproduce, prepare Derivative Works of, 71 | publicly display, publicly perform, sublicense, and distribute the 72 | Work and such Derivative Works in Source or Object form. 73 | 74 | 3. Grant of Patent License. Subject to the terms and conditions of 75 | this License, each Contributor hereby grants to You a perpetual, 76 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 77 | (except as stated in this section) patent license to make, have made, 78 | use, offer to sell, sell, import, and otherwise transfer the Work, 79 | where such license applies only to those patent claims licensable 80 | by such Contributor that are necessarily infringed by their 81 | Contribution(s) alone or by combination of their Contribution(s) 82 | with the Work to which such Contribution(s) was submitted. If You 83 | institute patent litigation against any entity (including a 84 | cross-claim or counterclaim in a lawsuit) alleging that the Work 85 | or a Contribution incorporated within the Work constitutes direct 86 | or contributory patent infringement, then any patent licenses 87 | granted to You under this License for that Work shall terminate 88 | as of the date such litigation is filed. 89 | 90 | 4. Redistribution. You may reproduce and distribute copies of the 91 | Work or Derivative Works thereof in any medium, with or without 92 | modifications, and in Source or Object form, provided that You 93 | meet the following conditions: 94 | 95 | (a) You must give any other recipients of the Work or 96 | Derivative Works a copy of this License; and 97 | 98 | (b) You must cause any modified files to carry prominent notices 99 | stating that You changed the files; and 100 | 101 | (c) You must retain, in the Source form of any Derivative Works 102 | that You distribute, all copyright, patent, trademark, and 103 | attribution notices from the Source form of the Work, 104 | excluding those notices that do not pertain to any part of 105 | the Derivative Works; and 106 | 107 | (d) If the Work includes a "NOTICE" text file as part of its 108 | distribution, then any Derivative Works that You distribute must 109 | include a readable copy of the attribution notices contained 110 | within such NOTICE file, excluding those notices that do not 111 | pertain to any part of the Derivative Works, in at least one 112 | of the following places: within a NOTICE text file distributed 113 | as part of the Derivative Works; within the Source form or 114 | documentation, if provided along with the Derivative Works; or, 115 | within a display generated by the Derivative Works, if and 116 | wherever such third-party notices normally appear. The contents 117 | of the NOTICE file are for informational purposes only and 118 | do not modify the License. You may add Your own attribution 119 | notices within Derivative Works that You distribute, alongside 120 | or as an addendum to the NOTICE text from the Work, provided 121 | that such additional attribution notices cannot be construed 122 | as modifying the License. 123 | 124 | You may add Your own copyright statement to Your modifications and 125 | may provide additional or different license terms and conditions 126 | for use, reproduction, or distribution of Your modifications, or 127 | for any such Derivative Works as a whole, provided Your use, 128 | reproduction, and distribution of the Work otherwise complies with 129 | the conditions stated in this License. 130 | 131 | 5. Submission of Contributions. Unless You explicitly state otherwise, 132 | any Contribution intentionally submitted for inclusion in the Work 133 | by You to the Licensor shall be under the terms and conditions of 134 | this License, without any additional terms or conditions. 135 | Notwithstanding the above, nothing herein shall supersede or modify 136 | the terms of any separate license agreement you may have executed 137 | with Licensor regarding such Contributions. 138 | 139 | 6. Trademarks. This License does not grant permission to use the trade 140 | names, trademarks, service marks, or product names of the Licensor, 141 | except as required for reasonable and customary use in describing the 142 | origin of the Work and reproducing the content of the NOTICE file. 143 | 144 | 7. Disclaimer of Warranty. Unless required by applicable law or 145 | agreed to in writing, Licensor provides the Work (and each 146 | Contributor provides its Contributions) on an "AS IS" BASIS, 147 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 148 | implied, including, without limitation, any warranties or conditions 149 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 150 | PARTICULAR PURPOSE. You are solely responsible for determining the 151 | appropriateness of using or redistributing the Work and assume any 152 | risks associated with Your exercise of permissions under this License. 153 | 154 | 8. Limitation of Liability. In no event and under no legal theory, 155 | whether in tort (including negligence), contract, or otherwise, 156 | unless required by applicable law (such as deliberate and grossly 157 | negligent acts) or agreed to in writing, shall any Contributor be 158 | liable to You for damages, including any direct, indirect, special, 159 | incidental, or consequential damages of any character arising as a 160 | result of this License or out of the use or inability to use the 161 | Work (including but not limited to damages for loss of goodwill, 162 | work stoppage, computer failure or malfunction, or any and all 163 | other commercial damages or losses), even if such Contributor 164 | has been advised of the possibility of such damages. 165 | 166 | 9. Accepting Warranty or Additional Liability. While redistributing 167 | the Work or Derivative Works thereof, You may choose to offer, 168 | and charge a fee for, acceptance of support, warranty, indemnity, 169 | or other liability obligations and/or rights consistent with this 170 | License. However, in accepting such obligations, You may act only 171 | on Your own behalf and on Your sole responsibility, not on behalf 172 | of any other Contributor, and only if You agree to indemnify, 173 | defend, and hold each Contributor harmless for any liability 174 | incurred by, or claims asserted against, such Contributor by reason 175 | of your accepting any such warranty or additional liability. 176 | 177 | END OF TERMS AND CONDITIONS 178 | 179 | APPENDIX: How to apply the Apache License to your work. 180 | 181 | To apply the Apache License to your work, attach the following 182 | boilerplate notice, with the fields enclosed by brackets "[]" 183 | replaced with your own identifying information. (Don't include 184 | the brackets!) The text should be enclosed in the appropriate 185 | comment syntax for the file format. We also recommend that a 186 | file or class name and description of purpose be included on the 187 | same "printed page" as the copyright notice for easier 188 | identification within third-party archives. 189 | 190 | Copyright [yyyy] [name of copyright owner] 191 | 192 | Licensed under the Apache License, Version 2.0 (the "License"); 193 | you may not use this file except in compliance with the License. 194 | You may obtain a copy of the License at 195 | 196 | http://www.apache.org/licenses/LICENSE-2.0 197 | 198 | Unless required by applicable law or agreed to in writing, software 199 | distributed under the License is distributed on an "AS IS" BASIS, 200 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 201 | See the License for the specific language governing permissions and 202 | limitations under the License. 203 | -------------------------------------------------------------------------------- /bluerov_bridge/src/bluerov_bridge/bridge.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import time 3 | 4 | from pymavlink import mavutil 5 | 6 | class Bridge(object): 7 | """ MAVLink bridge 8 | 9 | Attributes: 10 | conn (TYPE): MAVLink connection 11 | data (dict): Deal with all data 12 | """ 13 | def __init__(self, device='udp:192.168.2.1:14550', baudrate=115200, **kwargs): 14 | """ 15 | Args: 16 | device (str, optional): Input device 17 | https://ardupilot.github.io/MAVProxy/html/getting_started/starting.html#master 18 | baudrate (int, optional): Baudrate for serial communication 19 | """ 20 | self.conn = mavutil.mavlink_connection(device, baud=baudrate, **kwargs) 21 | self.data = {} 22 | 23 | def wait_conn(self): 24 | msg = None 25 | while not msg: 26 | self.conn.mav.ping_send( 27 | time.time(), # Unix time 28 | 0, # Ping number 29 | 0, # Request ping of all systems 30 | 0 # Request ping of all components 31 | ) 32 | msg = self.conn.recv_match() 33 | time.sleep(0.5) 34 | 35 | def get_data(self): 36 | """ Return data 37 | 38 | Returns: 39 | TYPE: Dict 40 | """ 41 | return self.data 42 | 43 | def get_msg(self, type): 44 | """ Return msg 45 | 46 | Returns: 47 | TYPE: Dict 48 | """ 49 | return self.conn.recv_match(type=type) 50 | 51 | def get_all_msgs(self): 52 | """ Return all mavlink messages 53 | 54 | Returns: 55 | TYPE: dict 56 | """ 57 | msgs = [] 58 | while True: 59 | msg = self.conn.recv_match() 60 | if msg != None: 61 | msgs.append(msg) 62 | else: 63 | break 64 | return msgs 65 | 66 | def update(self): 67 | """ Update data dict 68 | """ 69 | # Get all messages 70 | msgs = self.get_all_msgs() 71 | # Update dict 72 | for msg in msgs: 73 | self.data[msg.get_type()] = msg.to_dict() 74 | 75 | def print_data(self): 76 | """ Debug function, print data dict 77 | """ 78 | print(self.data) 79 | 80 | def set_mode(self, mode): 81 | """ Set ROV mode 82 | http://ardupilot.org/copter/docs/flight-modes.html 83 | 84 | Args: 85 | mode (str): MMAVLink mode 86 | 87 | Returns: 88 | TYPE: Description 89 | """ 90 | mode = mode.upper() 91 | if mode not in self.conn.mode_mapping(): 92 | print('Unknown mode : {}'.format(mode)) 93 | print('Try:', list(self.conn.mode_mapping().keys())) 94 | return 95 | mode_id = self.conn.mode_mapping()[mode] 96 | self.conn.set_mode(mode_id) 97 | 98 | def decode_mode(self, base_mode, custom_mode): 99 | """ Decode mode from heartbeat 100 | http://mavlink.org/messages/common#heartbeat 101 | 102 | Args: 103 | base_mode (TYPE): System mode bitfield, see MAV_MODE_FLAG ENUM in mavlink/include/mavlink_types.h 104 | custom_mode (TYPE): A bitfield for use for autopilot-specific flags. 105 | 106 | Returns: 107 | [str, bool]: Type mode string, arm state 108 | """ 109 | flight_mode = "" 110 | 111 | mode_list = [ 112 | [mavutil.mavlink.MAV_MODE_FLAG_MANUAL_INPUT_ENABLED, 'MANUAL'], 113 | [mavutil.mavlink.MAV_MODE_FLAG_STABILIZE_ENABLED, 'STABILIZE'], 114 | [mavutil.mavlink.MAV_MODE_FLAG_GUIDED_ENABLED, 'GUIDED'], 115 | [mavutil.mavlink.MAV_MODE_FLAG_AUTO_ENABLED, 'AUTO'], 116 | [mavutil.mavlink.MAV_MODE_FLAG_TEST_ENABLED, 'TEST'] 117 | ] 118 | 119 | if base_mode == 0: 120 | flight_mode = "PreFlight" 121 | elif base_mode & mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED: 122 | flight_mode = mavutil.mode_mapping_sub[custom_mode] 123 | else: 124 | for mode_value, mode_name in mode_list: 125 | if base_mode & mode_value: 126 | flight_mode = mode_name 127 | 128 | arm = bool(base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED) 129 | 130 | return flight_mode, arm 131 | 132 | def set_guided_mode(self): 133 | """ Set guided mode 134 | """ 135 | #https://github.com/ArduPilot/pymavlink/pull/128 136 | params = [mavutil.mavlink.MAV_MODE_GUIDED, 0, 0, 0, 0, 0, 0] 137 | self.send_command_long(mavutil.mavlink.MAV_CMD_DO_SET_MODE, params) 138 | 139 | def send_command_long(self, command, params=[0, 0, 0, 0, 0, 0, 0], confirmation=0): 140 | """ Function to abstract long commands 141 | 142 | Args: 143 | command (mavlink command): Command 144 | params (list, optional): param1, param2, ..., param7 145 | confirmation (int, optional): Confirmation value 146 | """ 147 | self.conn.mav.command_long_send( 148 | self.conn.target_system, # target system 149 | self.conn.target_component, # target component 150 | command, # mavlink command 151 | confirmation, # confirmation 152 | params[0], # params 153 | params[1], 154 | params[2], 155 | params[3], 156 | params[4], 157 | params[5], 158 | params[6] 159 | ) 160 | 161 | def set_position_target_local_ned(self, param=[]): 162 | """ Create a SET_POSITION_TARGET_LOCAL_NED message 163 | http://mavlink.org/messages/common#SET_POSITION_TARGET_LOCAL_NED 164 | 165 | Args: 166 | param (list, optional): param1, param2, ..., param11 167 | """ 168 | if len(param) != 11: 169 | print('SET_POISITION_TARGET_GLOBAL_INT need 11 params') 170 | 171 | # Set mask 172 | mask = 0b0000000111111111 173 | for i, value in enumerate(param): 174 | if value is not None: 175 | mask -= 1<