Fix gazebo8 warnings part 7: ifdef's for Joint::GetAngle and some cle…

…anup (ros-simulation#642)

* fix major version check >= 8, instead of > 8

* gazebo_ros_bumper: use new API in commented code

* gazebo_ros_api_plugin: world pose in local vars

* worldLinearVel as local var in hand of god plugin

* gazebo8+: Joint::GetAngle -> Joint::Position

gazebo_plugins/src/gazebo_ros_bumper.cpp

@@ -183,9 +183,9 @@ void GazeboRosBumper::OnContact()
   /*
   if (myFrame)
   {
-    frame_pose = myFrame->GetWorldPose();  //-this->myBody->GetCoMPose();
-    frame_pos = frame_pose.pos;
-    frame_rot = frame_pose.rot;
+    frame_pose = myFrame->WorldPose();  //-this->myBody->GetCoMPose();
+    frame_pos = frame_pose.Pos();
+    frame_rot = frame_pose.Rot();
   }
   else
   */

gazebo_plugins/src/gazebo_ros_diff_drive.cpp

@@ -203,9 +203,13 @@ void GazeboRosDiffDrive::publishWheelJointState()
 
     for ( int i = 0; i < 2; i++ ) {
         physics::JointPtr joint = joints_[i];
-        ignition::math::Angle angle = joint->GetAngle ( 0 ).Ign();
+#if GAZEBO_MAJOR_VERSION >= 8
+        double position = joint->Position ( 0 );
+#else
+        double position = joint->GetAngle ( 0 ).Radian();
+#endif
         joint_state_.name[i] = joint->GetName();
-        joint_state_.position[i] = angle.Radian () ;
+        joint_state_.position[i] = position;
     }
     joint_state_publisher_.publish ( joint_state_ );
 }
@@ -218,7 +222,7 @@ 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 ());
 
-#if GAZEBO_MAJOR_VERSION > 8
+#if GAZEBO_MAJOR_VERSION >= 8
         ignition::math::Pose3d poseWheel = joints_[i]->GetChild()->RelativePose();
 #else
         ignition::math::Pose3d poseWheel = joints_[i]->GetChild()->GetRelativePose().Ign();

gazebo_plugins/src/gazebo_ros_hand_of_god.cpp

@@ -161,9 +161,11 @@ namespace gazebo
     // Relative transform from actual to desired pose
 #if GAZEBO_MAJOR_VERSION >= 8
     ignition::math::Pose3d world_pose = floating_link_->DirtyPose();
+    ignition::math::Vector3d worldLinearVel = floating_link_->WorldLinearVel();
     ignition::math::Vector3d relativeAngularVel = floating_link_->RelativeAngularVel();
 #else
     ignition::math::Pose3d world_pose = floating_link_->GetDirtyPose().Ign();
+    ignition::math::Vector3d worldLinearVel = floating_link_->GetWorldLinearVel().Ign();
     ignition::math::Vector3d relativeAngularVel = floating_link_->GetRelativeAngularVel().Ign();
 #endif
     ignition::math::Vector3d err_pos = hog_desired.Pos() - world_pose.Pos();
@@ -173,7 +175,7 @@ namespace gazebo
     ignition::math::Quaterniond not_a_quaternion = err_rot.Log();
 
     floating_link_->AddForce(
-        kl_ * err_pos - cl_ * floating_link_->GetWorldLinearVel().Ign());
+        kl_ * err_pos - cl_ * worldLinearVel);
 
     floating_link_->AddRelativeTorque(
         ka_ * ignition::math::Vector3d(not_a_quaternion.X(), not_a_quaternion.Y(), not_a_quaternion.Z())

gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp

@@ -120,9 +120,13 @@ void GazeboRosJointStatePublisher::publishJointStates() {
 
     for ( int i = 0; i < joints_.size(); i++ ) {
         physics::JointPtr joint = joints_[i];
-        ignition::math::Angle angle = joint->GetAngle ( 0 ).Ign();
+#if GAZEBO_MAJOR_VERSION >= 8
+        double position = joint->Position ( 0 );
+#else
+        double position = joint->GetAngle ( 0 ).Radian();
+#endif
         joint_state_.name[i] = joint->GetName();
-        joint_state_.position[i] = angle.Radian () ;
+        joint_state_.position[i] = position;
     }
     joint_state_publisher_.publish ( joint_state_ );
 }

gazebo_plugins/src/gazebo_ros_tricycle_drive.cpp

@@ -166,7 +166,11 @@ void GazeboRosTricycleDrive::publishWheelJointState()
     joint_state_.effort.resize ( joints.size() );
     for ( std::size_t i = 0; i < joints.size(); i++ ) {
         joint_state_.name[i] = joints[i]->GetName();
+#if GAZEBO_MAJOR_VERSION >= 8
+        joint_state_.position[i] = joints[i]->Position ( 0 );
+#else
         joint_state_.position[i] = joints[i]->GetAngle ( 0 ).Radian();
+#endif
         joint_state_.velocity[i] = joints[i]->GetVelocity ( 0 );
         joint_state_.effort[i] = joints[i]->GetForce ( 0 );
     }
@@ -250,7 +254,11 @@ void GazeboRosTricycleDrive::motorController ( double target_speed, double targe
     }
     joint_wheel_actuated_->SetParam ( "vel", 0, applied_speed );
 
+#if GAZEBO_MAJOR_VERSION >= 8
+    double current_angle = joint_steering_->Position ( 0 );
+#else
     double current_angle = joint_steering_->GetAngle ( 0 ).Radian();
+#endif
 
     // truncate target angle
     if (target_angle > +M_PI / 2.0)

gazebo_ros/src/gazebo_ros_api_plugin.cpp

@@ -1008,8 +1008,12 @@ bool GazeboRosApiPlugin::getJointProperties(gazebo_msgs::GetJointProperties::Req
     res.damping.clear(); // to be added to gazebo
     //res.damping.push_back(joint->GetDamping(0));
 
-    res.position.clear(); // use GetAngle(i)
+    res.position.clear();
+#if GAZEBO_MAJOR_VERSION >= 8
+    res.position.push_back(joint->Position(0));
+#else
     res.position.push_back(joint->GetAngle(0).Radian());
+#endif
 
     res.rate.clear(); // use GetVelocity(i)
     res.rate.push_back(joint->GetVelocity(0));
@@ -1721,20 +1725,22 @@ bool GazeboRosApiPlugin::applyBodyWrench(gazebo_msgs::ApplyBodyWrench::Request &
     //        transform wrench from reference_point in reference_frame
     //        into the reference frame of the body
     //        first, translate by reference point to the body frame
-    ignition::math::Pose3d target_to_reference = frame->GetWorldPose().Ign() - body->GetWorldPose().Ign();
+    ignition::math::Pose3d framePose = frame->GetWorldPose().Ign();
+    ignition::math::Pose3d bodyPose = body->GetWorldPose().Ign();
+    ignition::math::Pose3d target_to_reference = framePose - bodyPose;
     ROS_DEBUG_NAMED("api_plugin", "reference frame for applied wrench: [%f %f %f, %f %f %f]-[%f %f %f, %f %f %f]=[%f %f %f, %f %f %f]",
-              body->GetWorldPose().Ign().Pos().X(),
-              body->GetWorldPose().Ign().Pos().Y(),
-              body->GetWorldPose().Ign().Pos().Z(),
-              body->GetWorldPose().Ign().Rot().Euler().X(),
-              body->GetWorldPose().Ign().Rot().Euler().Y(),
-              body->GetWorldPose().Ign().Rot().Euler().Z(),
-              frame->GetWorldPose().Ign().Pos().X(),
-              frame->GetWorldPose().Ign().Pos().Y(),
-              frame->GetWorldPose().Ign().Pos().Z(),
-              frame->GetWorldPose().Ign().Rot().Euler().X(),
-              frame->GetWorldPose().Ign().Rot().Euler().Y(),
-              frame->GetWorldPose().Ign().Rot().Euler().Z(),
+              bodyPose.Pos().X(),
+              bodyPose.Pos().Y(),
+              bodyPose.Pos().Z(),
+              bodyPose.Rot().Euler().X(),
+              bodyPose.Rot().Euler().Y(),
+              bodyPose.Rot().Euler().Z(),
+              framePose.Pos().X(),
+              framePose.Pos().Y(),
+              framePose.Pos().Z(),
+              framePose.Rot().Euler().X(),
+              framePose.Rot().Euler().Y(),
+              framePose.Rot().Euler().Z(),
               target_to_reference.Pos().X(),
               target_to_reference.Pos().Y(),
               target_to_reference.Pos().Z(),

gazebo_ros_control/src/default_robot_hw_sim.cpp

@@ -251,14 +251,19 @@ void DefaultRobotHWSim::readSim(ros::Time time, ros::Duration period)
   for(unsigned int j=0; j < n_dof_; j++)
   {
     // Gazebo has an interesting API...
+#if GAZEBO_MAJOR_VERSION >= 8
+    double position = sim_joints_[j]->Position(0);
+#else
+    double position = sim_joints_[j]->GetAngle(0).Radian();
+#endif
     if (joint_types_[j] == urdf::Joint::PRISMATIC)
     {
-      joint_position_[j] = sim_joints_[j]->GetAngle(0).Radian();
+      joint_position_[j] = position;
     }
     else
     {
       joint_position_[j] += angles::shortest_angular_distance(joint_position_[j],
-                            sim_joints_[j]->GetAngle(0).Radian());
+                            position);
     }
     joint_velocity_[j] = sim_joints_[j]->GetVelocity(0);
     joint_effort_[j] = sim_joints_[j]->GetForce((unsigned int)(0));