search

Field-Robotics-Lab / glider_hybrid_whoi

(Hybrid-) AUG Simulator
8 stars 9 forks source link

Afterworks and docker environment updates #38

Closed woensug-choi closed 3 years ago

woensug-choi commented 3 years ago

After modifying the plugin to the new interface structure (https://github.com/Field-Robotics-Lab/glider_hybrid_whoi/issues/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.

daewok commented 3 years ago

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 commented 3 years ago

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 commented 3 years ago

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 commented 3 years ago

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

woensug-choi commented 3 years ago

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

daewok commented 3 years ago

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

h/t @greg-burgess

daewok commented 3 years ago

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 commented 3 years ago

Anything more I can do to help debug?

woensug-choi commented 3 years ago

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 commented 3 years ago

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 b75c1e6f8fc892850da786d3ec433b59275f8b4c had them.

woensug-choi commented 3 years ago

@daewok

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

daewok commented 3 years ago

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 commented 3 years ago

@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.

daewok commented 3 years ago

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