Fixes for compilation and warnings in Lunar-devel (ros-simulation#573)

Multiple fixes for compilation and warnings coming from Gazebo8 and ignition-math3

gazebo_plugins/include/gazebo_plugins/gazebo_ros_imu.h

@@ -75,24 +75,24 @@ namespace gazebo
     private: std::string topic_name_;
 
     /// \brief allow specifying constant xyz and rpy offsets
-    private: math::Pose offset_;
+    private: ignition::math::Pose3d offset_;
 
     /// \brief A mutex to lock access to fields
     /// that are used in message callbacks
     private: boost::mutex lock_;
 
     /// \brief save last_time
     private: common::Time last_time_;
-    private: math::Vector3 last_vpos_;
-    private: math::Vector3 last_veul_;
-    private: math::Vector3 apos_;
-    private: math::Vector3 aeul_;
+    private: ignition::math::Vector3d last_vpos_;
+    private: ignition::math::Vector3d last_veul_;
+    private: ignition::math::Vector3d apos_;
+    private: ignition::math::Vector3d aeul_;
 
     // rate control
     private: double update_rate_;
 
     /// \brief: keep initial pose to offset orientation in imu message
-    private: math::Pose initial_pose_;
+    private: ignition::math::Pose3d initial_pose_;
 
     /// \brief Gaussian noise
     private: double gaussian_noise_;

gazebo_plugins/include/gazebo_plugins/gazebo_ros_imu_sensor.h

@@ -19,8 +19,8 @@
 
 #include <gazebo/common/Plugin.hh>
 #include <gazebo/common/UpdateInfo.hh>
-#include <gazebo/math/Vector3.hh>
-#include <gazebo/math/Pose.hh>
+#include <ignition/math/Vector3.hh>
+#include <ignition/math/Pose3.hh>
 #include <ros/ros.h>
 #include <sensor_msgs/Imu.h>
 #include <string>
@@ -79,11 +79,11 @@ namespace gazebo
     /// \brief Pointer to the sdf config file.
     sdf::ElementPtr sdf;
     /// \brief Orientation data from the sensor.
-    gazebo::math::Quaternion orientation;
+    ignition::math::Quaterniond orientation;
     /// \brief Linear acceleration data from the sensor.
-    math::Vector3 accelerometer_data;
+    ignition::math::Vector3d accelerometer_data;
     /// \brief Angular velocity data from the sensor.
-    math::Vector3 gyroscope_data;
+    ignition::math::Vector3d gyroscope_data;
 
     /// \brief Seed for the Gaussian noise generator.
     unsigned int seed;
@@ -100,8 +100,8 @@ namespace gazebo
     /// \brief Gaussian noise.
     double gaussian_noise;
     /// \brief Offset parameter, position part is unused.
-    math::Pose offset;
+    ignition::math::Pose3d offset;
   };
 }
 
-#endif //GAZEBO_ROS_IMU_SENSOR_H
+#endif //GAZEBO_ROS_IMU_SENSOR_H

gazebo_plugins/include/gazebo_plugins/gazebo_ros_p3d.h

@@ -89,21 +89,21 @@ namespace gazebo
     private: std::string tf_frame_name_;
 
     /// \brief allow specifying constant xyz and rpy offsets
-    private: math::Pose offset_;
+    private: ignition::math::Pose3d offset_;
 
     /// \brief mutex to lock access to fields used in message callbacks
     private: boost::mutex lock;
 
     /// \brief save last_time
     private: common::Time last_time_;
-    private: math::Vector3 last_vpos_;
-    private: math::Vector3 last_veul_;
-    private: math::Vector3 apos_;
-    private: math::Vector3 aeul_;
-    private: math::Vector3 last_frame_vpos_;
-    private: math::Vector3 last_frame_veul_;
-    private: math::Vector3 frame_apos_;
-    private: math::Vector3 frame_aeul_;
+    private: ignition::math::Vector3d last_vpos_;
+    private: ignition::math::Vector3d last_veul_;
+    private: ignition::math::Vector3d apos_;
+    private: ignition::math::Vector3d aeul_;
+    private: ignition::math::Vector3d last_frame_vpos_;
+    private: ignition::math::Vector3d last_frame_veul_;
+    private: ignition::math::Vector3d frame_apos_;
+    private: ignition::math::Vector3d frame_aeul_;
 
     // rate control
     private: double update_rate_;

gazebo_plugins/include/gazebo_plugins/gazebo_ros_planar_move.h

@@ -90,7 +90,7 @@ namespace gazebo {
       double rot_;
       bool alive_;
       common::Time last_odom_publish_time_;
-      math::Pose last_odom_pose_;
+      ignition::math::Pose3d last_odom_pose_;
 
   };
 

gazebo_plugins/src/gazebo_ros_block_laser.cpp

@@ -82,7 +82,7 @@ void GazeboRosBlockLaser::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
   std::string worldName = _parent->WorldName();
   this->world_ = physics::get_world(worldName);
 
-  last_update_time_ = this->world_->GetSimTime();
+  last_update_time_ = this->world_->SimTime();
 
   this->node_ = transport::NodePtr(new transport::Node());
   this->node_->Init(worldName);
@@ -405,9 +405,9 @@ void GazeboRosBlockLaser::OnStats( const boost::shared_ptr<msgs::WorldStatistics
 {
   this->sim_time_  = msgs::Convert( _msg->sim_time() );
 
-  math::Pose pose;
-  pose.pos.x = 0.5*sin(0.01*this->sim_time_.Double());
-  gzdbg << "plugin simTime [" << this->sim_time_.Double() << "] update pose [" << pose.pos.x << "]\n";
+  ignition::math::Pose3d pose;
+  pose.Pos().X() = 0.5*sin(0.01*this->sim_time_.Double());
+  gzdbg << "plugin simTime [" << this->sim_time_.Double() << "] update pose [" << pose.Pos().X() << "]\n";
 }
 
 

gazebo_plugins/src/gazebo_ros_bumper.cpp

@@ -32,9 +32,9 @@
 #include <sdf/Param.hh>
 #include <gazebo/common/Exception.hh>
 #include <gazebo/sensors/SensorTypes.hh>
-#include <gazebo/math/Pose.hh>
-#include <gazebo/math/Quaternion.hh>
-#include <gazebo/math/Vector3.hh>
+#include <ignition/math/Pose3.hh>
+#include <ignition/math/Quaternion.hh>
+#include <ignition/math/Vector3.hh>
 
 #include <tf/tf.h>
 
@@ -157,7 +157,7 @@ void GazeboRosBumper::OnContact()
       *Simulator::Instance()->GetMRMutex());
     // look through all models in the world, search for body
     // name that matches frameName
-    phyaics::Model_V all_models = World::Instance()->GetModels();
+    physics::Model_V all_models = World::Instance()->GetModels();
     for (physics::Model_V::iterator iter = all_models.begin();
       iter != all_models.end(); iter++)
     {
@@ -177,13 +177,13 @@ void GazeboRosBumper::OnContact()
 */
   // get reference frame (body(link)) pose and subtract from it to get
   // relative force, torque, position and normal vectors
-  math::Pose pose, frame_pose;
-  math::Quaternion rot, frame_rot;
-  math::Vector3 pos, frame_pos;
+  ignition::math::Pose3d pose, frame_pose;
+  ignition::math::Quaterniond rot, frame_rot;
+  ignition::math::Vector3d pos, frame_pos;
   /*
   if (myFrame)
   {
-    frame_pose = myFrame->GetWorldPose();  //-this->myBody->GetCoMPose();
+    frame_pose = myFrame->WorldPose();  //-this->myBody->GetCoMPose();
     frame_pos = frame_pose.pos;
     frame_rot = frame_pose.rot;
   }
@@ -192,9 +192,9 @@ void GazeboRosBumper::OnContact()
   {
     // no specific frames specified, use identity pose, keeping
     // relative frame at inertial origin
-    frame_pos = math::Vector3(0, 0, 0);
-    frame_rot = math::Quaternion(1, 0, 0, 0);  // gazebo u,x,y,z == identity
-    frame_pose = math::Pose(frame_pos, frame_rot);
+    frame_pos = ignition::math::Vector3d(0, 0, 0);
+    frame_rot = ignition::math::Quaterniond(1, 0, 0, 0);  // gazebo u,x,y,z == identity
+    frame_pose = ignition::math::Pose3d(frame_pos, frame_rot);
   }
 
 
@@ -242,34 +242,34 @@ void GazeboRosBumper::OnContact()
     {
       // loop through individual contacts between collision1 and collision2
       // gzerr << j << "  Position:"
-      //       << contact.position(j).x() << " "
-      //       << contact.position(j).y() << " "
-      //       << contact.position(j).z() << "\n";
+      //       << contact.position(j).X()() << " "
+      //       << contact.position(j).Y()() << " "
+      //       << contact.position(j).Z()() << "\n";
       // gzerr << "   Normal:"
-      //       << contact.normal(j).x() << " "
-      //       << contact.normal(j).y() << " "
-      //       << contact.normal(j).z() << "\n";
+      //       << contact.normal(j).X()() << " "
+      //       << contact.normal(j).Y()() << " "
+      //       << contact.normal(j).Z()() << "\n";
       // gzerr << "   Depth:" << contact.depth(j) << "\n";
 
       // Get force, torque and rotate into user specified frame.
       // frame_rot is identity if world is used (default for now)
-      math::Vector3 force = frame_rot.RotateVectorReverse(math::Vector3(
+      ignition::math::Vector3d force = frame_rot.RotateVectorReverse(ignition::math::Vector3d(
                               contact.wrench(j).body_1_wrench().force().x(),
                             contact.wrench(j).body_1_wrench().force().y(),
                             contact.wrench(j).body_1_wrench().force().z()));
-      math::Vector3 torque = frame_rot.RotateVectorReverse(math::Vector3(
+      ignition::math::Vector3d torque = frame_rot.RotateVectorReverse(ignition::math::Vector3d(
                             contact.wrench(j).body_1_wrench().torque().x(),
                             contact.wrench(j).body_1_wrench().torque().y(),
                             contact.wrench(j).body_1_wrench().torque().z()));
 
       // set wrenches
       geometry_msgs::Wrench wrench;
-      wrench.force.x  = force.x;
-      wrench.force.y  = force.y;
-      wrench.force.z  = force.z;
-      wrench.torque.x = torque.x;
-      wrench.torque.y = torque.y;
-      wrench.torque.z = torque.z;
+      wrench.force.x  = force.X();
+      wrench.force.y  = force.Y();
+      wrench.force.z  = force.Z();
+      wrench.torque.x = torque.X();
+      wrench.torque.y = torque.Y();
+      wrench.torque.z = torque.Z();
       state.wrenches.push_back(wrench);
 
       total_wrench.force.x  += wrench.force.x;
@@ -281,27 +281,27 @@ void GazeboRosBumper::OnContact()
 
       // transform contact positions into relative frame
       // set contact positions
-      gazebo::math::Vector3 position = frame_rot.RotateVectorReverse(
-          math::Vector3(contact.position(j).x(),
-                        contact.position(j).y(),
-                        contact.position(j).z()) - frame_pos);
+      ignition::math::Vector3d position = frame_rot.RotateVectorReverse(
+         ignition::math::Vector3d(contact.position(j).x(),
+                                  contact.position(j).y(),
+                                  contact.position(j).z()) - frame_pos);
       geometry_msgs::Vector3 contact_position;
-      contact_position.x = position.x;
-      contact_position.y = position.y;
-      contact_position.z = position.z;
+      contact_position.x = position.X();
+      contact_position.y = position.Y();
+      contact_position.z = position.Z();
       state.contact_positions.push_back(contact_position);
 
       // rotate normal into user specified frame.
       // frame_rot is identity if world is used.
-      math::Vector3 normal = frame_rot.RotateVectorReverse(
-          math::Vector3(contact.normal(j).x(),
-                        contact.normal(j).y(),
-                        contact.normal(j).z()));
+     ignition::math::Vector3d normal = frame_rot.RotateVectorReverse(
+         ignition::math::Vector3d(contact.normal(j).x(),
+                                  contact.normal(j).y(),
+                                  contact.normal(j).z()));
       // set contact normals
       geometry_msgs::Vector3 contact_normal;
-      contact_normal.x = normal.x;
-      contact_normal.y = normal.y;
-      contact_normal.z = normal.z;
+      contact_normal.x = normal.X();
+      contact_normal.y = normal.Y();
+      contact_normal.z = normal.Z();
       state.contact_normals.push_back(contact_normal);
 
       // set contact depth, interpenetration

gazebo_plugins/src/gazebo_ros_camera_utils.cpp

@@ -476,7 +476,7 @@ void GazeboRosCameraUtils::Init()
   else
   {
     // check against float precision
-    if (!gazebo::math::equal(this->focal_length_, computed_focal_length))
+    if (!ignition::math::equal(this->focal_length_, computed_focal_length))
     {
       ROS_WARN_NAMED("camera_utils", "The <focal_length>[%f] you have provided for camera_ [%s]"
                " is inconsistent with specified image_width [%d] and"

gazebo_plugins/src/gazebo_ros_diff_drive.cpp

@@ -52,7 +52,6 @@
 
 #include <gazebo_plugins/gazebo_ros_diff_drive.h>
 
-#include <gazebo/math/gzmath.hh>
 #include <sdf/sdf.hh>
 
 #include <ros/ros.h>
@@ -133,7 +132,7 @@ void GazeboRosDiffDrive::Load ( physics::ModelPtr _parent, sdf::ElementPtr _sdf
     // Initialize update rate stuff
     if ( this->update_rate_ > 0.0 ) this->update_period_ = 1.0 / this->update_rate_;
     else this->update_period_ = 0.0;
-    last_update_time_ = parent->GetWorld()->GetSimTime();
+    last_update_time_ = parent->GetWorld()->SimTime();
 
     // Initialize velocity stuff
     wheel_speed_[RIGHT] = 0;
@@ -185,7 +184,7 @@ void GazeboRosDiffDrive::Load ( physics::ModelPtr _parent, sdf::ElementPtr _sdf
 
 void GazeboRosDiffDrive::Reset()
 {
-  last_update_time_ = parent->GetWorld()->GetSimTime();
+  last_update_time_ = parent->GetWorld()->SimTime();
   pose_encoder_.x = 0;
   pose_encoder_.y = 0;
   pose_encoder_.theta = 0;
@@ -210,11 +209,11 @@ void GazeboRosDiffDrive::publishWheelJointState()
 
     for ( int i = 0; i < 2; i++ ) {
         physics::JointPtr joint = joints_[i];
-        math::Angle angle = joint->GetAngle ( 0 );
+        ignition::math::Angle angle = joint->Position(0);
         joint_state_.name[i] = joint->GetName();
-        joint_state_.position[i] = angle.Radian () ;
+        joint_state_.position[i] = angle.Radian();
     }
-    joint_state_publisher_.publish ( joint_state_ );
+    joint_state_publisher_.publish(joint_state_);
 }
 
 void GazeboRosDiffDrive::publishWheelTF()
@@ -225,10 +224,10 @@ void GazeboRosDiffDrive::publishWheelTF()
         std::string wheel_frame = gazebo_ros_->resolveTF(joints_[i]->GetChild()->GetName ());
         std::string wheel_parent_frame = gazebo_ros_->resolveTF(joints_[i]->GetParent()->GetName ());
 
-        math::Pose poseWheel = joints_[i]->GetChild()->GetRelativePose();
+        ignition::math::Pose3d poseWheel = joints_[i]->GetChild()->RelativePose();
 
-        tf::Quaternion qt ( poseWheel.rot.x, poseWheel.rot.y, poseWheel.rot.z, poseWheel.rot.w );
-        tf::Vector3 vt ( poseWheel.pos.x, poseWheel.pos.y, poseWheel.pos.z );
+        tf::Quaternion qt ( poseWheel.Rot().X(), poseWheel.Rot().Y(), poseWheel.Rot().Z(), poseWheel.Rot().W() );
+        tf::Vector3 vt ( poseWheel.Pos().X(), poseWheel.Pos().Y(), poseWheel.Pos().Z() );
 
         tf::Transform tfWheel ( qt, vt );
         transform_broadcaster_->sendTransform (
@@ -259,7 +258,7 @@ void GazeboRosDiffDrive::UpdateChild()
 
 
     if ( odom_source_ == ENCODER ) UpdateOdometryEncoder();
-    common::Time current_time = parent->GetWorld()->GetSimTime();
+    common::Time current_time = parent->GetWorld()->SimTime();
     double seconds_since_last_update = ( current_time - last_update_time_ ).Double();
 
     if ( seconds_since_last_update > update_period_ ) {
@@ -361,7 +360,7 @@ void GazeboRosDiffDrive::UpdateOdometryEncoder()
 {
     double vl = joints_[LEFT]->GetVelocity ( 0 );
     double vr = joints_[RIGHT]->GetVelocity ( 0 );
-    common::Time current_time = parent->GetWorld()->GetSimTime();
+    common::Time current_time = parent->GetWorld()->SimTime();
     double seconds_since_last_update = ( current_time - last_odom_update_ ).Double();
     last_odom_update_ = current_time;
 
@@ -431,9 +430,9 @@ void GazeboRosDiffDrive::publishOdometry ( double step_time )
     }
     if ( odom_source_ == WORLD ) {
         // getting data form gazebo world
-        math::Pose pose = parent->GetWorldPose();
-        qt = tf::Quaternion ( pose.rot.x, pose.rot.y, pose.rot.z, pose.rot.w );
-        vt = tf::Vector3 ( pose.pos.x, pose.pos.y, pose.pos.z );
+        ignition::math::Pose3d pose = parent->WorldPose();
+        qt = tf::Quaternion ( pose.Rot().X(), pose.Rot().Y(), pose.Rot().Z(), pose.Rot().W() );
+        vt = tf::Vector3 ( pose.Pos().X(), pose.Pos().Y(), pose.Pos().Z() );
 
         odom_.pose.pose.position.x = vt.x();
         odom_.pose.pose.position.y = vt.y();
@@ -445,14 +444,14 @@ void GazeboRosDiffDrive::publishOdometry ( double step_time )
         odom_.pose.pose.orientation.w = qt.w();
 
         // get velocity in /odom frame
-        math::Vector3 linear;
-        linear = parent->GetWorldLinearVel();
-        odom_.twist.twist.angular.z = parent->GetWorldAngularVel().z;
+        ignition::math::Vector3d linear;
+        linear = parent->WorldLinearVel();
+        odom_.twist.twist.angular.z = parent->WorldAngularVel().Z();
 
         // convert velocity to child_frame_id (aka base_footprint)
-        float yaw = pose.rot.GetYaw();
-        odom_.twist.twist.linear.x = cosf ( yaw ) * linear.x + sinf ( yaw ) * linear.y;
-        odom_.twist.twist.linear.y = cosf ( yaw ) * linear.y - sinf ( yaw ) * linear.x;
+        float yaw = pose.Rot().Yaw();
+        odom_.twist.twist.linear.x = cosf ( yaw ) * linear.X() + sinf ( yaw ) * linear.Y();
+        odom_.twist.twist.linear.y = cosf ( yaw ) * linear.Y() - sinf ( yaw ) * linear.X();
     }
 
     tf::Transform base_footprint_to_odom ( qt, vt );