search

Field-Robotics-Lab / glider_hybrid_whoi

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

add UW GPS Model plugin #11

Closed BruceMty closed 4 years ago

BruceMty commented 4 years ago

Two GPS approaches were evaluated: uuv_simulator and Hector. uuv_simulator provides a sensor plugin and Hector provides a model plugin. uuv_simulator publishes NavSatFix. Hector publishes location and velocity. I chose to work with Hector because it publishes more. I copied the Hector GPS code into our workspace and modified it to only publish when elevation >= -0.1 meters. Here are some details about the UUV and Hector implementations:

UUV: uuv_sensor_plugins/uuv_sensor_ros_plugins/src/GPSROSPlugin.cc Sensor plugin

Hector: hector_gazebo/hector_gazebo_plugins/src/gazebo_ros_gps.cpp Model plugin

File urdf/glider_hybrid_whoi_base.xacro was modified to include file uw_gps_plugin.xacro which loads the UW GPS Model plugin.

To test: type these commands in each of three command windows:

Then Raise the glider to an elevation >= -0.1 meters to see topic data.

Two topics are published when elevation >= -0.1: glider_hybrid_whoi/fix and glider_hybrid_whoi/fix_velocity

Elevation is hardcoded in file glider_hybrid_whoi/hybrid_glider_gazebo_plugins/hybrid_glider_gazebo_ros_plugins/src/UnderwaterGPSROSPlugin.cc. If desired, we can change this to 0.0 or make it selectable.

Note: Usually, when attempting to start the glider, Gazebo fails with this message, even without this pull request:

gzserver: /home/bruce/uuv_ws/src/glider_hybrid_whoi/hybrid_glider_gazebo_plugins/hybrid_glider_gazebo_plugins/src/UnderwaterObjectPlugin.cc:213: virtual void hybridglider::UnderwaterObjectPlugin::Update(const gazebo::common::UpdateInfo&): Assertion `(!std::isnan(linearAccel) && !std::isnan(angularAccel))&&("Linear or angular accelerations are invalid.")' failed.
Aborted (core dumped)
[gazebo-2] process has died [pid 24817, exit code 134, cmd /opt/ros/melodic/lib/gazebo_ros/gzserver --verbose -e ode worlds/ocean_waves.world __name:=gazebo __log:=/home/bruce/.ros/log/03efc85e-ef05-11ea-beb1-50eb71101139/gazebo-2.log].
log file: /home/bruce/.ros/log/03efc85e-ef05-11ea-beb1-50eb71101139/gazebo-2*.log
woensug-choi commented 4 years ago

First thought the problem is caused by the time scheme. But found strange bug at Eigen Library and fixed it. Will be added on next commit.

Details

Code (Before)

  Eigen::Matrix6d Ma_cg
  Ma_cg(0,0) = -X_udot;  Ma_cg(1,1) = -Y_vdot;  Ma_cg(2,2) = -Z_wdot;  
  Ma_cg(3,3) = -K_pdot;  Ma_cg(4,4) = -M_qdot;  Ma_cg(5,5) = -N_rdot;

Results (Before)

first iteration

[Msg] Ma_cg 
     1.96198 6.91911e-310 6.91908e-310 6.91912e-310  1.4822e-322            0
6.91911e-310      48.1908 6.91908e-310 6.91898e-310 1.22524e-314 6.91931e-310
6.91911e-310 6.91912e-310      48.1908 4.64366e-310 6.91911e-310 1.86757e-321
           0 6.91908e-310            0           -0 6.91912e-310            0
6.91931e-310 1.91443e-316 6.91912e-310 1.93674e-321      8.38116 2.47033e-323
6.91931e-310 6.91908e-310  1.4822e-323 6.91919e-310 6.91911e-310      8.38116

next iteration

[Msg] Ma_cg 
     1.96198 -1.47819e-23 -2.85918e-24 3.16202e-322 8.48798e-314 3.16202e-322
           0      48.1908            0 6.91911e-310 3.45846e-323 6.91911e-310
 1.90867e-23     0.407361      48.1908 4.74303e-322 3.16202e-322 4.74303e-322
           0            0 2.71736e-322           -0 2.76677e-322         -nan
    -0.04075     -0.04075 1.01124e-313 4.94066e-324      8.38116 4.94066e-324
           0            0 3.45846e-323 2.76677e-322 1.58101e-322      8.38116

Modification

Eigen::Matrix6d Ma_cg -> Eigen::Matrix6d Ma_cg = Eigen::Matrix6d::Identity();

woensug-choi commented 4 years ago

Sorry for the early incomplete merge. Merged 'revert' PR. How do I make this PR open again? P.S. seen compile error not having hector_gazebo_plugin resources