Afterworks and docker environment updates

[woensug-choi]

After modifying the plugin to the new interface structure (#34) and debugging the tidal oscillation plugin, docker environments are updated.

I believe this PR could be our first working set-up to test the glider missions.

Note that the ROS topic names have changed.

• Command/status ROS topic name
  • Before : /glider_hybrid_whoi/direct_kinematics/UwGliderCommand, /glider_hybrid_whoi/direct_kinematics/UwGliderStatus
  • After : /glider_hybrid_whoi/kinematics/UwGliderCommand, /glider_hybrid_whoi/kinematics/UwGliderStatus

[daewok]

Sorry for the delay, moving and other things sucked up all my time.

I think I have all the changes needed implemented on my end, but I can't easily test because the glider's velocities stay ~0 no matter what commands I give. I'm pretty sure the issue is in this repo, based on 5636017 and c1c5ba4, but just applying those commits didn't work.

[woensug-choi]

Sorry for the delay, moving and other things sucked up all my time.

I think I have all the changes needed implemented on my end, but I can't easily test because the glider's velocities stay ~0 no matter what commands I give. I'm pretty sure the issue is in this repo, based on 5636017 and c1c5ba4, but just applying those commits didn't work.

Quick check. Did you change the topic name accordingly? Any possible way that I can reproduce your problem on my side?

[daewok]

Quick check. Did you change the topic name accordingly?

Yes.

Any possible way that I can reproduce your problem on my side?

Sorry, I figured you were aware of it based on those commits I referenced.

It turns out the problem is a bit more nuanced than I first realized. In one terminal, run:

docker/build.bash
docker/run.bash -- roslaunch glider_hybrid_whoi_gazebo BuzzBay_stratified_current_docker.launch

In another terminal run:

docker/join.bash
/ros_entrypoint.sh rostopic echo /glider_hybrid_whoi/kinematics/UwGliderStatus

In yet another terminal run:

docker/join.bash
/ros_entrypoint.sh rostopic pub /glider_hybrid_whoi/kinematics/UwGliderCommand frl_vehicle_msgs/UwGliderCommand "pitch_cmd_type: 2
target_pitch_value: 0.4538
motor_cmd_type: 0
target_motor_cmd: 0.0
rudder_control_mode: 1
target_heading: 0.0
rudder_angle: 0
target_rudder_angle: 0.0
target_pumped_volume: 226.0"

I see the glider's pitch and heading change, but not its depth (it should rise to the surface).

This command makes the glider dive and move laterally as expected:

/ros_entrypoint.sh rostopic pub /glider_hybrid_whoi/kinematics/UwGliderCommand frl_vehicle_msgs/UwGliderCommand "pitch_cmd_type: 2
target_pitch_value: 0.4538
motor_cmd_type: 0
target_motor_cmd: 0.0
rudder_control_mode: 1
target_heading: 0.0
rudder_angle: 0
target_rudder_angle: 0.0
target_pumped_volume: -226.0"

However, if I let it dive for a while and then send this command, the glider warps back to its starting location nearly instantly (definitely not expected):

/ros_entrypoint.sh rostopic pub /glider_hybrid_whoi/kinematics/UwGliderCommand frl_vehicle_msgs/UwGliderCommand "pitch_cmd_type: 2
target_pitch_value: 0.4538
motor_cmd_type: 0
target_motor_cmd: 0.0
rudder_control_mode: 1
target_heading: 0.0
rudder_angle: 0
target_rudder_angle: 0.0
target_pumped_volume: 226.0"

[woensug-choi]

@daewook I've found that there is an error when target_pumped_volume is negative. Working on it.

[woensug-choi]

@daewok Strange... the same command msg works fine on start_demo_kinematics_stratified_current.launch. Investigating...

[daewok]

Also, at some point between b75c1e6 and 837955a, latitude and longitude got reversed in the glider status msg.

h/t @greg-burgess

[daewok]

If you add a start_demo_kinematics_stratified_current_docker.launch file, I'd be happy to help poke around and figure out the differences.

[daewok]

Anything more I can do to help debug?

[woensug-choi]

I think I fixed the first problem. The cause seems to be with the strange bounding box estimation from the gazebo function.
Previous : link->BoundingBox()
New : visual->BoundingBox()

For the record,
I am not sure why it's a problem now and it was not before. I wrote a submergence check code before fixing the vehicle on the surface when it's not submerged. The wrong bounding box would not have the submergence check to work. It did before.
Anyway, on the latest version, the output of each method of getting bounding box reports following,
(XLength, YLength, ZLength) of BoundingBox from link->BoundingBox() = (69.8115, 61.1424, 83.5323) -- Don't know where these numbers came from
(XLength, YLength, ZLength) of BoundingBox from visual->BoundingBox() = (0.937536, 0.286699,0.245178) -- Expected value.

[daewok]

OK, the first problem does seem to be fixed, but now I can see there's another
one. This line is incorrect:

status_msg.altitude = this->modelXYZ.Z();

The altitude should be the distance from the sea floor. In its current state,
the altitude is always negative and everything on my end flips out because it
thinks it's crashed into the seafloor.

It used to be:

status_msg.altitude = this->sensorAltitude;

But it looks like the kinematics plugin doesn't have a sensorAltitude field
any more.

We could probably get this from the /dvl/dvl topic, but that's not how the
real hardware behaves. There is a separate altimeter on the glider that is a
single beam. I believe it is mounted 26 degrees off vertical (so that when
the glider is yo'ing down, the altimeter is facing straight down). This
altimeter is what the glider control system uses when deciding if it's gotten
too close to the seafloor; to the best of my knowledge it does not use the DVL
info at all.

I think I fixed the first problem. The cause seems to be with the strange
bounding box estimation from the gazebo function. Previous :
link->BoundingBox() New : visual->BoundingBox()

For the record, I am not sure why it's a problem now and it was not before.

I wonder, does it have to do with the DVL beams being added in? I don't think b75c1e6 had them.

[woensug-choi]

@daewok

• Lat/lon error 79cce4f
  • I had a typo error defining north as latitude and east as longitude. It should be fixed.
• Altitude sensor
  • I thought, altitude information is something like a byproduct of the DVL sensor since I could not find a separate altimeter sensor plugin. I've removed that altimeter before since we have a better DVL sensor. I put it back with a topic name altimeter. And yes, it has DVL beams published as well.
  • One thing that needs to be discussed is that since we have a separate DeadReckoning module that reads sensor values to publish, I thought UwGliderStatus is supposedly more like vehicle state values obtained from the simulator. If I am to obtain the altitude values in that sense, I should calculate the distance between straight down bathymetry ground and the vehicle. This could take some time to figure out.
  • TODO : add altimeter sensor at interface graphml
• Randomness fix and modify to keep up with dave repo 5c8b8b8
  • I've found strange randomness in the ocean current when prescribed values are small. It had to do with not giving a default value. It's fixed at the dave repo side. New commit numbers are applied for the docker along with new plugin names (dave repo is going through a reorganization)

Again, the PR is collecting multiple subjects. Shall we merge this after your confirmation of the lat/lon fix?

[daewok]

From my perspective I'm happy removing altitude from the UwGliderStatus message and obtaining it from the new altitude topic. But @greg-burgess may have other thoughts in case he wants the "true" altitude (but my personal opinion is that that should be figured out during post processing).

The dave update brought in a new dependency that is breaking the Docker build:

dave_robot_launch: Cannot locate rosdep definition for [teledyne_nortek_dvl1000_4000_description]
dave_sensor_launch: Cannot locate rosdep definition for [teledyne_nortek_dvl1000_4000_description_uuvsim]

Which repo provides those packages?

I'm happy closing this as soon as I can test the altitude topic. I'll probably just open an issue to record the fact that the altitmeter model probably isn't quite what we want long term.

[woensug-choi]

@daewok Hmm... I don't know what was wrong. But it's working now. I hope we don't see that again.

What I checked.

• BuzzBay_stratified_current_docker.launch: altimeter working, sudden jumps not happening.
• Note
  • it seems a surface is somewhere near 18 m (altimeter reading)
  • so now, don't use altimeter in the UwGliderStatus (not altimeter value. just negative of depth)

[daewok]

Seems to work from my end. I'm happy merging this