Get aircraft_truth working with gazebo 9

josuehfa · Jul 20, 2018 · 067280c · 067280c
1 parent 4127327
commit 067280c
Show file tree

Hide file tree

Showing 2 changed files with 37 additions and 0 deletions.
diff --git a/rosplane_sim/include/rosplane_sim/aircraft_truth.h b/rosplane_sim/include/rosplane_sim/aircraft_truth.h
@@ -92,7 +92,11 @@ class AircraftTruth : public ModelPlugin
   void WindSpeedCallback(const geometry_msgs::Vector3 &wind);
 
   std::unique_ptr<FirstOrderFilter<double>>  rotor_velocity_filter_;
+#if GAZEBO_MAJOR_VERSION >=8
+  ignition::math::Vector3d wind_speed_W_;
+#else
   math::Vector3 wind_speed_W_;
+#endif
 };
 }
 

diff --git a/rosplane_sim/src/aircraft_truth.cpp b/rosplane_sim/src/aircraft_truth.cpp
@@ -28,7 +28,11 @@ AircraftTruth::AircraftTruth() :
 
 AircraftTruth::~AircraftTruth()
 {
+#if GAZEBO_MAJOR_VERSION >=8
+  updateConnection_.reset();
+#else
   event::Events::DisconnectWorldUpdateBegin(updateConnection_);
+#endif
   if (nh_)
   {
     nh_->shutdown();
@@ -100,6 +104,26 @@ void AircraftTruth::PublishTruth()
   msg.initial_lat = 0;
   msg.initial_lon = 0;
   msg.initial_alt = 0;
+
+#if GAZEBO_MAJOR_VERSION >= 8
+  ignition::math::Pose3d W_pose_W_C = link_->WorldCoGPose();
+  msg.position[0] = W_pose_W_C.Pos().X(); // We should check to make sure that this is right
+  msg.position[1] = -W_pose_W_C.Pos().Y();
+  msg.position[2] = -W_pose_W_C.Pos().Z();
+  ignition::math::Vector3d euler_angles = W_pose_W_C.Rot().Euler();
+  msg.phi = euler_angles.X();
+  msg.theta = -euler_angles.Y();
+  msg.psi = -euler_angles.Z();
+  ignition::math::Vector3d C_linear_velocity_W_C = link_->RelativeLinearVel();
+  double u = C_linear_velocity_W_C.X();
+  double v = -C_linear_velocity_W_C.Y();
+  double w = -C_linear_velocity_W_C.Z();
+  msg.Vg = sqrt(pow(u, 2.0) + pow(v, 2.0) + pow(w, 2.0));
+  ignition::math::Vector3d C_angular_velocity_W_C = link_->RelativeAngularVel();
+  msg.p = C_angular_velocity_W_C.X();
+  msg.q = -C_angular_velocity_W_C.Y();
+  msg.r = -C_angular_velocity_W_C.Z();
+#else
   math::Pose W_pose_W_C = link_->GetWorldCoGPose();
   msg.position[0] = W_pose_W_C.pos.x; // We should check to make sure that this is right
   msg.position[1] = -W_pose_W_C.pos.y;
@@ -117,6 +141,7 @@ void AircraftTruth::PublishTruth()
   msg.p = C_angular_velocity_W_C.x;
   msg.q = -C_angular_velocity_W_C.y;
   msg.r = -C_angular_velocity_W_C.z;
+#endif
 
   msg.wn = wind_.N;
   msg.we = wind_.E;
@@ -136,10 +161,18 @@ void AircraftTruth::PublishTruth()
   msg.quat[1] = v;
   msg.quat[2] = w;
 
+#if GAZEBO_MAJOR_VERSION >=8
+  msg.header.stamp.fromSec(world_->SimTime().Double());
+#else
   msg.header.stamp.fromSec(world_->GetSimTime().Double());
+#endif
   msg.header.frame_id = 1; // Denotes global frame
 
+#if GAZEBO_MAJOR_VERSION >=8
+  msg.psi_deg = fmod(euler_angles.X(), 2.0*M_PI)*180.0 / M_PI; //-360 to 360
+#else
   msg.psi_deg = fmod(euler_angles.x, 2.0*M_PI)*180.0 / M_PI; //-360 to 360
+#endif
   msg.psi_deg += (msg.psi_deg < -180.0 ? 360.0 : 0.0);
   msg.psi_deg -= (msg.psi_deg > 180.0 ? 360.0 : 0.0);
   msg.chi_deg = fmod(msg.chi, 2.0*M_PI)*180.0 / M_PI; //-360 to 360