Kinematics only

[woensug-choi]

New kinematics only PR

[woensug-choi]

Kinematics plugin for WHOI hybrid gliders

Kinematics control plugin for WHOI hybrid gliders

How-to

Installation

• First check to make sure you meet the System Requirements.
• Then choose from one of the following two installation options:
  1. Directly on Host
     1. Install environment and dependent repositories : Instructions to install ROS Melodic, Gazebo 9, UUV Simulator and DAVE directly on your host machine.
     2. Clone this repository in ~/uuv/src folder and compile with catkin_make at ~/uuv directory.
  2. Using Docker
     1. Make sure you have Docker v19.03 or higher (installation instructions) and nvidia-container-toolkit (installation instructions)
     2. Run the build.bash script located in the docker folder of this repository

         ./build.bash .

     3. Run the container with run.bash script

         ./run.bash glider_kinematics

        • Opening additional terminals

          ./join.bash glider_kinematics

          Quickstart

• Running the simulator (Run each commands in separate terminal window)
  1. Spawn underwater world with gazebo

     roslaunch glider_hybrid_whoi_gazebo seafloor_underwater_world.launch

  2. Spawn glider into the gazebo world

     roslaunch glider_hybrid_whoi_gazebo start_demo_teleop_kinematics.launch joy_id:=0

  3. Control glider with ROS

     # Using test python script
     rosrun direct_kinematics_ros_plugins test_directKinematics.py
     # Directly to the ros node
     # Toward surface
     rostopic pub /glider_hybrid_whoi/direct_kinematics/UwGlidCommand frl_vehicle_msgs/UwGliderCommand '{motor_cmd_type: 1, target_motor_cmd: 2, pitch_cmd_type: 2, target_pitch_value: -0.4, rudder_control_mode: 1, target_heading: 0.8}'
     # Back to underwater
     rostopic pub /glider_hybrid_whoi/direct_kinematics/UwGlidCommand frl_vehicle_msgs/UwGliderCommand '{motor_cmd_type: 1, target_motor_cmd: 2, pitch_cmd_type: 2, target_pitch_value: 0.4, rudder_control_mode: 1, target_heading: -0.8}'

  4. Glider status

     rostopic echo /glider_hybrid_whoi/direct_kinematics/UwGliderStatus

Features

• Organized based on support for frl_vehicle_msgs

  Glider dynamic parameters

• Parametes for pitch control, buoyancy induced velocity with the flight model, and thruster power is defined at glider_hybrid_whoi_base_kinematics.xacro

  Pitch control

  Battery position	Target once	Target servo
  :heavy_check_mark:	:heavy_check_mark:	:heavy_check_mark:

• Change of pitch is defined with first-order function with two coefficients (mx + b) for battery position command values
• coefficient name : f_pitch_battpos_cal_m, f_pitch_battpos_cal_b

Thruster control

Voltage	Power
:heavy_check_mark:	:heavy_check_mark:

• Thruster power is defined with 2nd-order fucntion with three coefficients (ax^2 + bx + c) for both voltage/power command values
• coefficient name : f_thruster_voltage_v1, f_thruster_voltage_v2, f_thruster_voltage_v3, f_thruster_power_w1, f_thruster_power_w2, f_thruster_power_w3
• Final thrust to the glider is calculated with sqrt(motorpower/(fluidDensity*Area*C_D)
• Propeller rotation is visualized using the command value as rotation speed.

Rudder control

Heading	Angle (Center, Port, Stbd)	Angle (Direct)
:heavy_check_mark:	:heavy_check_mark:	:heavy_check_mark:

• The size of the port and starboard angle is set to PI/6.

Buoyancy engine control

Flight model
:heavy_check_mark:

• Simple flight model is adopted to change the angle of attach with pumped volume to produce buoyancy induced velocity
• +/- 3 degrees depending on the sign of the pitch
• Calculated with Equations 24 and 28 from Eichhorn et al. paper.

Glider status

Lat/Lon	Roll-Pitch-Yaw	Heading	Depth	Altitude	Power	Rudder angle	Battery position	Pumped Volume	nav_sat_fix
:heavy_check_mark:*	:heavy_check_mark:	:heavy_check_mark:	:heavy_check_mark:	:heavy_check_mark:*	:heavy_multiplication_x:	:heavy_check_mark:	:heavy_check_mark:	:heavy_check_mark:	:heavy_check_mark:

• Lat/Lon is acquired from the GPS sensor and Altitude from the DVL sensor. Which is also sent through nav_sat_fix

Code Structure Diagram

[woensug-choi]

To-dos for First half of December

• [x] Strip-off dynamics part and make new Kinematics PR (including Docker)
• [x] Add nav_sat_fix to glider status (https://github.com/Field-Robotics-Lab/frl_msgs/pull/8)
• [x] Standalone case-set PR with Buzzard Bay bathymetry & Stratified ocean current (with Kolumbo data)

[woensug-choi]

@daewok Dockerfile is at docker folder :+1: It takes quite some time to build. Check installation with Docker section how to use build.bash, run.bash, join.bash

[woensug-choi]

@daewok I've referenced as much as I can from your GitLab Dockefile. I've made it through catkin_make compile. But was not able to play the plugin.

1. Gazebo doesn't launch with complaints of GUI driver fail. I tried to add nvidia/opengl:base-ubuntu18.04 at runtime image but without success. (Maybe run.bash issue?) https://github.com/Field-Robotics-Lab/glider_hybrid_whoi/blob/c1b3aff1b45be5935b931e32933af7dec21f5b8d/docker/Dockerfile#L95-L96
2. I thought the method with install-script would install dependent packages (e.g. ros-melodic-xacro) but it did not. Had to add apt-get installs later again. (https://github.com/Field-Robotics-Lab/glider_hybrid_whoi/blob/c1b3aff1b45be5935b931e32933af7dec21f5b8d/docker/Dockerfile#L160-L182)
3. Can you provide more details about sourcing environment scripts other than writing it at .bashrc

Could you give it a look? It's at a new branch Kinematics_only_newdockerfile https://github.com/Field-Robotics-Lab/glider_hybrid_whoi/blob/c1b3aff1b45be5935b931e32933af7dec21f5b8d/docker/Dockerfile#L1-L201

[daewok]

I just pushed some commits to the new branch. With them, I'm able to do ./build.bash and ./run.bash -w glider_kinematics and get the simulator to run. (note that I'm not using my GPU and I haven't tried with a GPU yet).

The reason so many deps were missed is that the rosdep ... install command was actually failing, but it was masked since it was in a pipe and -o pipefail was not set. I changed that so it should now complain if rosdep fails for some reason.

[woensug-choi]

I just pushed some commits to the new branch. With them, I'm able to do ./build.bash and ./run.bash -w glider_kinematics and get the simulator to run. (note that I'm not using my GPU and I haven't tried with a GPU yet).

The reason so many deps were missed is that the rosdep ... install command was actually failing, but it was masked since it was in a pipe and -o pipefail was not set. I changed that so it should now complain if rosdep fails for some reason.

Can't launch anything from my side. I am getting X error for gazebo at ./run.bash -w glider_kinematics. Any suggestions on how to solve this issue?

[woensug-choi]

@mabelzhang Isn't this a magic line to set model paths? I am trying to reference the models newly included in this PR.

  <export>
    <gazebo_ros gazebo_model_path="${prefix}/models"/>
    <gazebo_ros gazebo_media_path="${prefix}"/>
  </export>

I am trying to upload a Buzzard Bay heightmap. But I can't locate it using model://. It shouldn't be difficult. What else do I have to do to add a path for models in this PR?

[mabelzhang]

That looks correct. Adding the path usually takes me a few rounds of back and forth testing... Are you installing the packages when you compile? Try rospack find to see where ROS thinks the package is, whether in src or in devel or install. It could be that the models are only in src but rospack find is locating a compiled directory. In that case, you would want to install the models directory in CMakeLists.txt. You could run Gazebo with --verbose to see if it prints anything about the path.

[woensug-choi]

Added buzzards bay partially (Quicks Hole area) Near with automatic bathymetry respawning plugin.

• Launch command

  roslaunch glider_hybrid_whoi_gazebo BuzzBay.launch

• Click the play button on the Gazebo window and wait for the first bathymetry to be spawned.
• Next bathymetry tile will be spawned and the previous tile will be removed automatically.

---

Problmes to be solved

• [x] : Spherical coordinate system in the simulator
  • Not 100% confident that the bathymetry tiles are correctly oriented.
    • Intended glider initial location (Coordinate translated from EPSG:4326 to EPSG:26987)

      Lat / Lon : -70.853° / 41.45°
      X / Y : 470503.30 / 50036.53

    • Tiles are correctly spawned according to the intended initial location

      [Dbg] [bathy_plugin.cc:396] Inserted model: buzzbay.R_-70.857_-70.852_41.447_41.452

  • GPS sensor data is generating unexpected Lat/Lon values.
    • GPS sensor definition doesn't take any much arguments for the coordinate system. I was guessing it's taking parent links (base_link) frame which has ned transform. (On rqt window, base_link has transformation OK)
    • Other coordinate system settings is at Buzzbay.world inherited from Dave project's plugin which also inherited from Mike Jakuba's plugin from Wind_energy. Regretfully, not sure what it does exactly.

      <!-- Setting this to origin of EPSG:26987 -->
      <spherical_coordinates>
        <longitude_deg>-70.5</longitude_deg>
        <latitude_deg>41.0</latitude_deg>
      </spherical_coordinates>

    • Current result of the GPS sensor data is as follows (Looks like ENU/NED coordinate frame error)

      longitude: 76.0371042964
      latitude: -40.4165916257

  • Possible solutions
    • [x] Fix launch/xacro files defining coordinate frames.
    • [ ] Insert a code to calculate correct Lat/Lon for final output of GliderStatus.msg
  • Related
    • https://github.com/Field-Robotics-Lab/dave/issues/77
    • https://github.com/uuvsimulator/uuv_simulator/issues/165

[woensug-choi]

Added FULL Buzzards bay bathymetry

• Roughly 1500x1500 m tiles with 50 m overlap regions are included (almost 780 MB)
• Launch command

  roslaunch glider_hybrid_whoi_gazebo BuzzBay.launch

• Click the play button on the Gazebo window and wait for the first bathymetry to be spawned.
• Testing script for auto-spawning and auto-removing of tiles

  rosrun direct_kinematics_ros_plugins test_directKinematics.py

  • The script contains moving the glider toward east (rudder_control_mode=RUDDER_CONTROL_HEADING=1 with target_heading = pi/2)
  • Next bathymetry tile will be spawned and the previous tile will be removed automatically.

Issue on GPS accuracy

The vehicle's position and GPS value do not match exactly.

• Vehicle's initial position
  • x: 480381.43, y: 56666.85 which is Lon: -70.735, Lat: 41.51 calculated using epsg.io
• GPS value obtained from UwGliderStatus.msg
  • Lon: -70.712083444, Lat: 41.2136645438

This is also the case of Dave repo's rexrov tutorial case https://github.com/Field-Robotics-Lab/dave/issues/77#issuecomment-748878031. It seems it's a problem with the GPS sensor since the bathymetry is spawning correctly.

[woensug-choi]

I think the PR is ready to review for merge. @daewok could you review this from the kinematics functionality point of view?

Future Todo

• Include stratified ocean current (with Kolumbo data)
• Separate GPS sensor location and the actual location
  • the actual location (x,y (EPSG:26987 NAD83) from converted into lat/lon (EPSG:4326 WGS 84))

Future issues

• Docker deployments: Dockerfile satisfying both Eric's usage and X window for Gazebo
• GPS sensor accuracy

[daewok]

I'm just getting back online after moving and xmas. I also have my desktop with GPU finally set up. So I think I'm going to tackle the Docker stuff first, since having it Dockerized will make my workflow easier.

[woensug-choi]

Happy new year! Were you able to get the dockerfile working? Did you review this PR? I only see the Merge button.

[daewok]

Just pushed my work. I tried to update the docker/README.MD file well enough to document how it works now.

I also added a docker-compose.yml file that launches both the Gazebo simulator and the simulated glider control system. You should just need to run docker-compose build and docker-compose up to have the simulator start up. In the absence of any other input, the glider control system just tries to make the glider safe by surfacing.

The docker-compose file currently assumes that if you want to use a GPU the nvidia runtime is set as your default Docker runtime. If that's not the case, you'll need to create a local docker-compose.override.yml file setting the runtime to use.

To get a glider console, you can run:

docker-compose exec glider_control_sim /ros_entrypoint.sh rosrun slocum_glider_sim_driver slocum_glider_sim_console

You can then run any of the subset of Glider commands that the simulated control system supports. You can run the simple one dive mission with run ini0.mi. You can run a simple box mission with run inibox01.mi. After the missions complete, you'll need to hit Ctrl-C to tell it to finish. If you want to quit the console hit Ctrl-D.

I did notice two oddities:

1. Sometimes Gazebo just hangs on me. Time stops progressing in the GUI and no more messages are sent from the plugins. I noticed this occasionally outside of Docker as well. Have you seen this?
2. The messages sent by the plugins have a time stamp of over 1600000000 seconds. That makes me wonder if the /use_sim_time parameter is being set early enough. I don't remember if this happened to me earlier or not.

Take a look at the commit message for 92fac83. It'd be nice if we could get those PRs mentioned in it merged before this is merged.

[woensug-choi]

Lots of commits! I am starting to understand why we need this. I've dealt with all PRs on this and dave repository.

• Dev/Prod image: works as expected!
• Docker compose: I get X Error.

  gazebo_1              | libGL error: No matching fbConfigs or visuals found
  gazebo_1              | libGL error: failed to load driver: swrast
  gazebo_1              | X Error of failed request:  GLXBadContext
  gazebo_1              |   Major opcode of failed request:  152 (GLX)
  gazebo_1              |   Minor opcode of failed request:  6 (X_GLXIsDirect)
  gazebo_1              |   Serial number of failed request:  44
  gazebo_1              |   Current serial number in output stream:  43

The last commit includes,

• Exclude pandas python library for test_directKinematics.py script
• Fix target dockerfile of build.bash
• Fix to set /use_sim_time parameter correctly at BuzzBay.launch file
• Set writeLog to false

For two oddities,

• No I have not experienced any hang-ups on Gazebo. Sometimes the gazebo complains about missing material. But I've never experienced freezing. Maybe not enough space at /tmp? (Changed writeLog to false)
• I've fixed the sim time problem! They were supposed to be in paused=false, use_sim_time=true, gui=true order. I've found an article that some codes should be added to make ROS node to wait for time synchronization article. I did not. But it works fine.

[daewok]

Weird. It worked on my desktop with an nvidia GPU and I just tested it on the computer in Caryn House mostly successfully (docker-compose build doesn't work on that computer... I'm guesing it's because the version of docker-compose on there is ancient and has a bug. I had to build with build.bash and then use docker-compose up).

Is the nvidia runtime set as the default on the computer you were using?

[woensug-choi]

@daewok How do I check if the Nvidia runtime is set as the default?

Brian has suggested that we merge this PR if the kinematics functionality is working before we diverge too much in one PR. I think it is very reasonable to do so. This PR already includes Kinematics, Bathymetry, and Docker. What do you think?

[daewok]

Run docker info and it should be in the output. If it's not the default, you can typically add "default-runtime": "nvidia" to /etc/docker/daemon.json (or however your OS configures Docker). You should also be able to use a docker-compose override file to tell docker-compose to use nvidia instead of the default, but I'm not sure what the contents of that should be. (And I don't think there's any issue with making nvidia the default over runc).

I wholeheartedly agree about merging this soon. It's already a beast.

[woensug-choi]

I messed up my system. Now back online. The workstation at WHOI has nvidia runtime as default. I'll double check on my local machine.

[woensug-choi]

Docker

• Runs ok with the following commands 👍

  # Terminal Window 1 at docker folder
  build.bash
  run.bash
  roslaunch glider_hybrid_whoi_gazebo BuzzBay.launch

• Rosparam use_sim_time is working correctly and could be checked by following commands 👍

  # Terminal Window 2 at docker folder
  join.bash
  source /opt/ros/melodic/setup.bash
  source ~/glider_hybrid_whoi/install/setup.bash
  rostopic echo /glider_hybrid_whoi/direct_kinematics/UwGliderStatus

  The log at the first window still shows time stamp of over 1600000000, however, the UwGliderStatus time stamp read on the rostopic correctly shows sim_time value (which is calculated using ros::time::now())

Docker-compose

• With Nvidia runtime, build.bash and then use docker-compose up launched ok 👍

  # Terminal Window 1 at docker folder
  build.bash
  docker-compose up

• Mission terminal console works as it's supposed to be 👍

  # Terminal Window 2 at docker folder
  docker-compose exec glider_control_sim /ros_entrypoint.sh rosrun slocum_glider_sim_driver slocum_glider_sim_console
  # Ctrl+C to see the typable terminal cursor
  run ini0.mi
  run inibox01.mi

  Is there join.bash functionality for docker-compose? I wanted to check whether use_sim_time works correctly.

During the tests, I have not seen any Gazebo hang-ups. Currently, building on another machine without Nvidia runtime. Will see what happens. (Update: with runc docker runtime, it works perfectly 👍 ).

I think this PR is ready to merge now.

[daewok]

Is there join.bash functionality for docker-compose? I wanted to check whether use_sim_time works correctly.

Something like this is probably what you want:

docker-compose exec gazebo bash

[woensug-choi]

I was able to call up a new window using docker-compose exec glider_control_sim /ros_entrypoint.sh bash The use_sim_time was correctly working in the docker-compose environment too.