for gazebo8+, call functions without Get (#640)

ros-simulation · Dec 15, 2017 · a86eba6 · a86eba6
1 parent 9dd92ea
commit a86eba6
Show file tree

Hide file tree

Showing 9 changed files with 87 additions and 7 deletions.
diff --git a/gazebo_plugins/src/gazebo_ros_bumper.cpp b/gazebo_plugins/src/gazebo_ros_bumper.cpp
@@ -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()->Models();
     for (physics::Model_V::iterator iter = all_models.begin();
       iter != all_models.end(); iter++)
     {

diff --git a/gazebo_plugins/src/gazebo_ros_f3d.cpp b/gazebo_plugins/src/gazebo_ros_f3d.cpp
@@ -152,11 +152,14 @@ void GazeboRosF3D::UpdateChild()
   ignition::math::Vector3d torque;
   ignition::math::Vector3d force;
 
-  // get force on body
+  // get force and torque on body
+#if GAZEBO_MAJOR_VERSION >= 8
+  force = this->link_->WorldForce();
+  torque = this->link_->WorldTorque();
+#else
   force = this->link_->GetWorldForce().Ign();
-
-  // get torque on body
   torque = this->link_->GetWorldTorque().Ign();
+#endif
 
   this->lock_.lock();
   // copy data into wrench message

diff --git a/gazebo_plugins/src/gazebo_ros_hand_of_god.cpp b/gazebo_plugins/src/gazebo_ros_hand_of_god.cpp
@@ -115,8 +115,13 @@ namespace gazebo
       return;
     }
 
+#if GAZEBO_MAJOR_VERSION >= 8
+    cl_ = 2.0 * sqrt(kl_*floating_link_->GetInertial()->Mass());
+    ca_ = 2.0 * sqrt(ka_*floating_link_->GetInertial()->IXX());
+#else
     cl_ = 2.0 * sqrt(kl_*floating_link_->GetInertial()->GetMass());
     ca_ = 2.0 * sqrt(ka_*floating_link_->GetInertial()->GetIXX());
+#endif
 
     // Create the TF listener for the desired position of the hog
     tf_buffer_.reset(new tf2_ros::Buffer());
@@ -154,7 +159,13 @@ namespace gazebo
         ignition::math::Quaterniond(q.w, q.x, q.y, q.z));
 
     // Relative transform from actual to desired pose
+#if GAZEBO_MAJOR_VERSION >= 8
+    ignition::math::Pose3d world_pose = floating_link_->DirtyPose();
+    ignition::math::Vector3d relativeAngularVel = floating_link_->RelativeAngularVel();
+#else
     ignition::math::Pose3d world_pose = floating_link_->GetDirtyPose().Ign();
+    ignition::math::Vector3d relativeAngularVel = floating_link_->GetRelativeAngularVel().Ign();
+#endif
     ignition::math::Vector3d err_pos = hog_desired.Pos() - world_pose.Pos();
     // Get exponential coordinates for rotation
     ignition::math::Quaterniond err_rot =  (ignition::math::Matrix4d(world_pose.Rot()).Inverse()
@@ -166,7 +177,7 @@ namespace gazebo
 
     floating_link_->AddRelativeTorque(
         ka_ * ignition::math::Vector3d(not_a_quaternion.X(), not_a_quaternion.Y(), not_a_quaternion.Z())
-      - ca_ * floating_link_->GetRelativeAngularVel().Ign());
+      - ca_ * relativeAngularVel);
 
     // Convert actual pose to TransformStamped message
     geometry_msgs::TransformStamped hog_actual_tform;

diff --git a/gazebo_plugins/src/gazebo_ros_joint_pose_trajectory.cpp b/gazebo_plugins/src/gazebo_ros_joint_pose_trajectory.cpp
@@ -222,8 +222,13 @@ void GazeboRosJointPoseTrajectory::SetTrajectory(
 
   if (this->disable_physics_updates_)
   {
+#if GAZEBO_MAJOR_VERSION >= 8
+    this->physics_engine_enabled_ = this->world_->PhysicsEnabled();
+    this->world_->SetPhysicsEnabled(false);
+#else
     this->physics_engine_enabled_ = this->world_->GetEnablePhysicsEngine();
     this->world_->EnablePhysicsEngine(false);
+#endif
   }
 }
 
@@ -295,8 +300,13 @@ bool GazeboRosJointPoseTrajectory::SetTrajectory(
   this->disable_physics_updates_ = req.disable_physics_updates;
   if (this->disable_physics_updates_)
   {
+#if GAZEBO_MAJOR_VERSION >= 8
+    this->physics_engine_enabled_ = this->world_->PhysicsEnabled();
+    this->world_->SetPhysicsEnabled(false);
+#else
     this->physics_engine_enabled_ = this->world_->GetEnablePhysicsEngine();
     this->world_->EnablePhysicsEngine(false);
+#endif
   }
 
   return true;
@@ -380,7 +390,13 @@ void GazeboRosJointPoseTrajectory::UpdateStates()
         this->reference_link_.reset();
         this->has_trajectory_ = false;
         if (this->disable_physics_updates_)
+        {
+#if GAZEBO_MAJOR_VERSION >= 8
+          this->world_->SetPhysicsEnabled(this->physics_engine_enabled_);
+#else
           this->world_->EnablePhysicsEngine(this->physics_engine_enabled_);
+#endif
+        }
       }
     }
   }

diff --git a/gazebo_plugins/src/gazebo_ros_joint_trajectory.cpp b/gazebo_plugins/src/gazebo_ros_joint_trajectory.cpp
@@ -224,8 +224,13 @@ void GazeboRosJointTrajectory::SetTrajectory(
 
   if (this->disable_physics_updates_)
   {
+#if GAZEBO_MAJOR_VERSION >= 8
+    this->physics_engine_enabled_ = this->world_->PhysicsEnabled();
+    this->world_->SetPhysicsEnabled(false);
+#else
     this->physics_engine_enabled_ = this->world_->GetEnablePhysicsEngine();
     this->world_->EnablePhysicsEngine(false);
+#endif
   }
 }
 
@@ -297,8 +302,13 @@ bool GazeboRosJointTrajectory::SetTrajectory(
   this->disable_physics_updates_ = req.disable_physics_updates;
   if (this->disable_physics_updates_)
   {
+#if GAZEBO_MAJOR_VERSION >= 8
+    this->physics_engine_enabled_ = this->world_->PhysicsEnabled();
+    this->world_->SetPhysicsEnabled(false);
+#else
     this->physics_engine_enabled_ = this->world_->GetEnablePhysicsEngine();
     this->world_->EnablePhysicsEngine(false);
+#endif
   }
 
   return true;
@@ -382,7 +392,13 @@ void GazeboRosJointTrajectory::UpdateStates()
         this->reference_link_.reset();
         this->has_trajectory_ = false;
         if (this->disable_physics_updates_)
+        {
+#if GAZEBO_MAJOR_VERSION >= 8
+          this->world_->SetPhysicsEnabled(this->physics_engine_enabled_);
+#else
           this->world_->EnablePhysicsEngine(this->physics_engine_enabled_);
+#endif
+        }
       }
     }
   }

diff --git a/gazebo_plugins/src/gazebo_ros_projector.cpp b/gazebo_plugins/src/gazebo_ros_projector.cpp
@@ -82,7 +82,11 @@ void GazeboRosProjector::Load( physics::ModelPtr _parent, sdf::ElementPtr _sdf )
   // Create a new transport node for talking to the projector
   this->node_.reset(new transport::Node());
   // Initialize the node with the world name
+#if GAZEBO_MAJOR_VERSION >= 8
+  this->node_->Init(this->world_->Name());
+#else
   this->node_->Init(this->world_->GetName());
+#endif
   // Setting projector topic
   std::string name = std::string("~/") + _parent->GetName() + "/" +
                       _sdf->Get<std::string>("projector");

diff --git a/gazebo_plugins/src/gazebo_ros_tricycle_drive.cpp b/gazebo_plugins/src/gazebo_ros_tricycle_drive.cpp
@@ -185,7 +185,11 @@ void GazeboRosTricycleDrive::publishWheelTF()
         std::string frame = gazebo_ros_->resolveTF ( joints[i]->GetName() );
         std::string parent_frame = gazebo_ros_->resolveTF ( joints[i]->GetParent()->GetName() );
 
+#if GAZEBO_MAJOR_VERSION >= 8
+        ignition::math::Pose3d pose = joints[i]->GetChild()->RelativePose();
+#else
         ignition::math::Pose3d pose = joints[i]->GetChild()->GetRelativePose().Ign();
+#endif
 
         tf::Quaternion qt ( pose.Rot().X(), pose.Rot().Y(), pose.Rot().Z(), pose.Rot().W() );
         tf::Vector3 vt ( pose.Pos().X(), pose.Pos().Y(), pose.Pos().Z() );

diff --git a/gazebo_plugins/src/gazebo_ros_vacuum_gripper.cpp b/gazebo_plugins/src/gazebo_ros_vacuum_gripper.cpp
@@ -179,8 +179,13 @@ void GazeboRosVacuumGripper::UpdateChild()
   }
   // apply force
   lock_.lock();
+#if GAZEBO_MAJOR_VERSION >= 8
+  ignition::math::Pose3d parent_pose = link_->WorldPose();
+  physics::Model_V models = world_->Models();
+#else
   ignition::math::Pose3d parent_pose = link_->GetWorldPose().Ign();
   physics::Model_V models = world_->GetModels();
+#endif
   for (size_t i = 0; i < models.size(); i++) {
     if (models[i]->GetName() == link_->GetName() ||
         models[i]->GetName() == parent_->GetName())
@@ -193,10 +198,17 @@ void GazeboRosVacuumGripper::UpdateChild()
       ignition::math::Pose3d diff = parent_pose - link_pose;
       double norm = diff.Pos().Length();
       if (norm < 0.05) {
+#if GAZEBO_MAJOR_VERSION >= 8
+        links[j]->SetLinearAccel(link_->WorldLinearAccel());
+        links[j]->SetAngularAccel(link_->WorldAngularAccel());
+        links[j]->SetLinearVel(link_->WorldLinearVel());
+        links[j]->SetAngularVel(link_->WorldAngularVel());
+#else
         links[j]->SetLinearAccel(link_->GetWorldLinearAccel());
         links[j]->SetAngularAccel(link_->GetWorldAngularAccel());
         links[j]->SetLinearVel(link_->GetWorldLinearVel());
         links[j]->SetAngularVel(link_->GetWorldAngularVel());
+#endif
         double norm_force = 1 / norm;
         if (norm < 0.01) {
           // apply friction like force

diff --git a/gazebo_ros/src/gazebo_ros_api_plugin.cpp b/gazebo_ros/src/gazebo_ros_api_plugin.cpp
@@ -173,7 +173,7 @@ void GazeboRosApiPlugin::loadGazeboRosApiPlugin(std::string world_name)
   world_ = gazebo::physics::get_world(world_name);
   if (!world_)
   {
-    //ROS_ERROR_NAMED("api_plugin", "world name: [%s]",world->GetName().c_str());
+    //ROS_ERROR_NAMED("api_plugin", "world name: [%s]",world->Name().c_str());
     // connect helper function to signal for scheduling torque/forces, etc
     ROS_FATAL_NAMED("api_plugin", "cannot load gazebo ros api server plugin, physics::get_world() fails to return world");
     return;
@@ -1034,9 +1034,22 @@ bool GazeboRosApiPlugin::getLinkProperties(gazebo_msgs::GetLinkProperties::Reque
     /// @todo: validate
     res.gravity_mode = body->GetGravityMode();
 
+    gazebo::physics::InertialPtr inertia = body->GetInertial();
+
+#if GAZEBO_MAJOR_VERSION >= 8
+    res.mass = body->GetInertial()->Mass();
+
+    res.ixx = inertia->IXX();
+    res.iyy = inertia->IYY();
+    res.izz = inertia->IZZ();
+    res.ixy = inertia->IXY();
+    res.ixz = inertia->IXZ();
+    res.iyz = inertia->IYZ();
+
+    ignition::math::Vector3d com = body->GetInertial()->CoG();
+#else
     res.mass = body->GetInertial()->GetMass();
 
-    gazebo::physics::InertialPtr inertia = body->GetInertial();
     res.ixx = inertia->GetIXX();
     res.iyy = inertia->GetIYY();
     res.izz = inertia->GetIZZ();
@@ -1045,6 +1058,7 @@ bool GazeboRosApiPlugin::getLinkProperties(gazebo_msgs::GetLinkProperties::Reque
     res.iyz = inertia->GetIYZ();
 
     ignition::math::Vector3d com = body->GetInertial()->GetCoG().Ign();
+#endif
     res.com.position.x = com.X();
     res.com.position.y = com.Y();
     res.com.position.z = com.Z();