Fix gazebo7 deprecation warnings

gazebo_plugins/include/gazebo_plugins/gazebo_ros_utils.h

@@ -62,7 +62,11 @@ inline std::string GetModelName ( const sensors::SensorPtr &parent )
 {
     std::string modelName;
     std::vector<std::string> values;
+# if GAZEBO_MAJOR_VERSION >= 7
+    std::string scopedName = parent->ScopedName();
+# else
     std::string scopedName = parent->GetScopedName();
+#endif
     boost::replace_all ( scopedName, "::", "," );
     boost::split ( values, scopedName, boost::is_any_of ( "," ) );
     if ( values.size() < 2 ) {

gazebo_plugins/src/MultiCameraPlugin.cpp

@@ -18,6 +18,7 @@
 #include <gazebo/sensors/CameraSensor.hh>
 #include <gazebo_plugins/MultiCameraPlugin.h>
 #include <gazebo_plugins/gazebo_ros_utils.h>
+#include <gazebo/gazebo_config.h>
 
 using namespace gazebo;
 GZ_REGISTER_SENSOR_PLUGIN(MultiCameraPlugin)
@@ -60,17 +61,36 @@ void MultiCameraPlugin::Load(sensors::SensorPtr _sensor,
     return;
   }
 
+# if GAZEBO_MAJOR_VERSION >= 7
+  for (unsigned int i = 0; i < this->parentSensor->CameraCount(); ++i)
+# else
   for (unsigned int i = 0; i < this->parentSensor->GetCameraCount(); ++i)
+# endif
   {
+# if GAZEBO_MAJOR_VERSION >= 7
+    this->camera.push_back(this->parentSensor->Camera(i));
+# else
     this->camera.push_back(this->parentSensor->GetCamera(i));
+# endif
 
     // save camera attributes
+# if GAZEBO_MAJOR_VERSION >= 7
+    this->width.push_back(this->camera[i]->ImageWidth());
+    this->height.push_back(this->camera[i]->ImageHeight());
+    this->depth.push_back(this->camera[i]->ImageDepth());
+    this->format.push_back(this->camera[i]->ImageFormat());
+# else
     this->width.push_back(this->camera[i]->GetImageWidth());
     this->height.push_back(this->camera[i]->GetImageHeight());
     this->depth.push_back(this->camera[i]->GetImageDepth());
     this->format.push_back(this->camera[i]->GetImageFormat());
+# endif
 
+# if GAZEBO_MAJOR_VERSION >= 7
+    std::string cameraName = this->parentSensor->Camera(i)->Name();
+# else
     std::string cameraName = this->parentSensor->GetCamera(i)->GetName();
+# endif
     // gzdbg << "camera(" << i << ") name [" << cameraName << "]\n";
 
     // FIXME: hardcoded 2 camera support only

gazebo_plugins/src/gazebo_ros_block_laser.cpp

@@ -35,6 +35,7 @@
 #include <gazebo/sensors/RaySensor.hh>
 #include <gazebo/sensors/SensorTypes.hh>
 #include <gazebo/transport/Node.hh>
+#include <gazebo/gazebo_config.h>
 
 #include <geometry_msgs/Point32.h>
 #include <sensor_msgs/ChannelFloat32.h>
@@ -79,7 +80,12 @@ void GazeboRosBlockLaser::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
   this->parent_sensor_ = _parent;
 
   // Get the world name.
+# if GAZEBO_MAJOR_VERSION >= 7
+  std::string worldName = _parent->WorldName();
+# else
   std::string worldName = _parent->GetWorldName();
+# endif
+
   this->world_ = physics::get_world(worldName);
 
   last_update_time_ = this->world_->GetSimTime();
@@ -206,7 +212,11 @@ void GazeboRosBlockLaser::OnNewLaserScans()
 {
   if (this->topic_name_ != "")
   {
+# if GAZEBO_MAJOR_VERSION >= 7
+    common::Time sensor_update_time = this->parent_sensor_->LastUpdateTime();
+# else
     common::Time sensor_update_time = this->parent_sensor_->GetLastUpdateTime();
+# endif
     if (last_update_time_ < sensor_update_time)
     {
       this->PutLaserData(sensor_update_time);
@@ -240,13 +250,26 @@ void GazeboRosBlockLaser::PutLaserData(common::Time &_updateTime)
   math::Angle minAngle = this->parent_ray_sensor_->GetAngleMin();
 #endif
 
+# if GAZEBO_MAJOR_VERSION >= 7
+  double maxRange = this->parent_ray_sensor_->RangeMax();
+  double minRange = this->parent_ray_sensor_->RangeMin();
+  int rayCount = this->parent_ray_sensor_->RayCount();
+  int rangeCount = this->parent_ray_sensor_->RangeCount();
+# else
   double maxRange = this->parent_ray_sensor_->GetRangeMax();
   double minRange = this->parent_ray_sensor_->GetRangeMin();
   int rayCount = this->parent_ray_sensor_->GetRayCount();
   int rangeCount = this->parent_ray_sensor_->GetRangeCount();
+# endif
 
+#if GAZEBO_MAJOR_VERSION >= 7
+  int verticalRayCount = this->parent_ray_sensor_->VerticalRayCount();
+  int verticalRangeCount = this->parent_ray_sensor_->VerticalRangeCount();
+# else
   int verticalRayCount = this->parent_ray_sensor_->GetVerticalRayCount();
   int verticalRangeCount = this->parent_ray_sensor_->GetVerticalRangeCount();
+# endif
+
 #if GAZEBO_MAJOR_VERSION >= 6
   auto verticalMaxAngle = this->parent_ray_sensor_->VerticalAngleMax();
   auto verticalMinAngle = this->parent_ray_sensor_->VerticalAngleMin();
@@ -304,22 +327,37 @@ void GazeboRosBlockLaser::PutLaserData(common::Time &_updateTime)
       j3 = hja + vjb * rayCount;
       j4 = hjb + vjb * rayCount;
       // range readings of 4 corners
+# if GAZEBO_MAJOR_VERSION >= 7
+      r1 = std::min(this->parent_ray_sensor_->LaserShape()->GetRange(j1) , maxRange-minRange);
+      r2 = std::min(this->parent_ray_sensor_->LaserShape()->GetRange(j2) , maxRange-minRange);
+      r3 = std::min(this->parent_ray_sensor_->LaserShape()->GetRange(j3) , maxRange-minRange);
+      r4 = std::min(this->parent_ray_sensor_->LaserShape()->GetRange(j4) , maxRange-minRange);
+# else
       r1 = std::min(this->parent_ray_sensor_->GetLaserShape()->GetRange(j1) , maxRange-minRange);
       r2 = std::min(this->parent_ray_sensor_->GetLaserShape()->GetRange(j2) , maxRange-minRange);
       r3 = std::min(this->parent_ray_sensor_->GetLaserShape()->GetRange(j3) , maxRange-minRange);
       r4 = std::min(this->parent_ray_sensor_->GetLaserShape()->GetRange(j4) , maxRange-minRange);
+# endif
 
       // Range is linear interpolation if values are close,
       // and min if they are very different
       r = (1-vb)*((1 - hb) * r1 + hb * r2)
          +   vb *((1 - hb) * r3 + hb * r4);
 
       // Intensity is averaged
+#if GAZEBO_MAJOR_VERSION >= 7
+      intensity = 0.25*(this->parent_ray_sensor_->LaserShape()->GetRetro(j1) +
+                        this->parent_ray_sensor_->LaserShape()->GetRetro(j2) +
+                        this->parent_ray_sensor_->LaserShape()->GetRetro(j3) +
+                        this->parent_ray_sensor_->LaserShape()->GetRetro(j4));
+# else
       intensity = 0.25*(this->parent_ray_sensor_->GetLaserShape()->GetRetro(j1) +
                         this->parent_ray_sensor_->GetLaserShape()->GetRetro(j2) +
                         this->parent_ray_sensor_->GetLaserShape()->GetRetro(j3) +
                         this->parent_ray_sensor_->GetLaserShape()->GetRetro(j4));
 
+# endif
+
       // std::cout << " block debug "
       //           << "  ij("<<i<<","<<j<<")"
       //           << "  j1234("<<j1<<","<<j2<<","<<j3<<","<<j4<<")"
@@ -347,17 +385,17 @@ void GazeboRosBlockLaser::PutLaserData(common::Time &_updateTime)
         point.y = r * cos(pAngle) * sin(yAngle);
         point.z = -r * sin(pAngle);
 
-        this->cloud_msg_.points.push_back(point); 
-      } 
-      else 
-      { 
+        this->cloud_msg_.points.push_back(point);
+      }
+      else
+      {
         geometry_msgs::Point32 point;
         //pAngle is rotated by yAngle:
         point.x = r * cos(pAngle) * cos(yAngle) + this->GaussianKernel(0,this->gaussian_noise_);
         point.y = r * cos(pAngle) * sin(yAngle) + this->GaussianKernel(0,this->gaussian_noise_);
         point.z = -r * sin(pAngle) + this->GaussianKernel(0,this->gaussian_noise_);
-        this->cloud_msg_.points.push_back(point); 
-      } // only 1 channel 
+        this->cloud_msg_.points.push_back(point);
+      } // only 1 channel
 
       this->cloud_msg_.channels[0].values.push_back(intensity + this->GaussianKernel(0,this->gaussian_noise_)) ;
     }

gazebo_plugins/src/gazebo_ros_bumper.cpp

@@ -136,7 +136,11 @@ void GazeboRosBumper::OnContact()
     return;
 
   msgs::Contacts contacts;
+# if GAZEBO_MAJOR_VERSION >= 7
+  contacts = this->parentSensor->Contacts();
+# else
   contacts = this->parentSensor->GetContacts();
+# endif
   /// \TODO: need a time for each Contact in i-loop, they may differ
   this->contact_state_msg_.header.frame_id = this->frame_name_;
   this->contact_state_msg_.header.stamp = ros::Time(contacts.time().sec(),
@@ -210,7 +214,7 @@ void GazeboRosBumper::OnContact()
     // For each collision contact
     // Create a ContactState
     gazebo_msgs::ContactState state;
-    /// \TODO: 
+    /// \TODO:
     gazebo::msgs::Contact contact = contacts.contact(i);
 
     state.collision1_name = contact.collision1();

gazebo_plugins/src/gazebo_ros_camera.cpp

@@ -76,7 +76,11 @@ void GazeboRosCamera::OnNewFrame(const unsigned char *_image,
     unsigned int _width, unsigned int _height, unsigned int _depth,
     const std::string &_format)
 {
+# if GAZEBO_MAJOR_VERSION >= 7
+  this->sensor_update_time_ = this->parentSensor_->LastUpdateTime();
+# else
   this->sensor_update_time_ = this->parentSensor_->GetLastUpdateTime();
+# endif
 
   if (!this->parentSensor->IsActive())
   {

gazebo_plugins/src/gazebo_ros_camera_utils.cpp

@@ -105,7 +105,11 @@ void GazeboRosCameraUtils::Load(sensors::SensorPtr _parent,
   const std::string &_camera_name_suffix)
 {
   // Get the world name.
+# if GAZEBO_MAJOR_VERSION >= 7
+  std::string world_name = _parent->WorldName();
+# else
   std::string world_name = _parent->GetWorldName();
+# endif
 
   // Get the world_
   this->world_ = physics::get_world(world_name);
@@ -119,7 +123,7 @@ void GazeboRosCameraUtils::Load(sensors::SensorPtr _parent,
 
   std::stringstream ss;
   this->robot_namespace_ =  GetRobotNamespace(_parent, _sdf, "Camera");
-  
+
   this->image_topic_name_ = "image_raw";
   if (this->sdf->HasElement("imageTopicName"))
     this->image_topic_name_ = this->sdf->Get<std::string>("imageTopicName");
@@ -278,7 +282,7 @@ void GazeboRosCameraUtils::LoadThread()
           *this->rosnode_, this->camera_name_));
 
   this->itnode_ = new image_transport::ImageTransport(*this->rosnode_);
-  
+
   // resolve tf prefix
   this->tf_prefix_ = tf::getPrefixParam(*this->rosnode_);
   if(this->tf_prefix_.empty()) {
@@ -289,8 +293,8 @@ void GazeboRosCameraUtils::LoadThread()
 
   ROS_INFO("Camera Plugin (ns = %s)  <tf_prefix_>, set to \"%s\"",
              this->robot_namespace_.c_str(), this->tf_prefix_.c_str());
-  
-  
+
+
   if (!this->camera_name_.empty())
   {
     dyn_srv_ =
@@ -464,9 +468,15 @@ void GazeboRosCameraUtils::Init()
     this->cy_ = (static_cast<double>(this->height_) + 1.0) /2.0;
 
 
+# if GAZEBO_MAJOR_VERSION >= 7
+  double computed_focal_length =
+    (static_cast<double>(this->width_)) /
+    (2.0 * tan(this->camera_->HFOV().Radian() / 2.0));
+# else
   double computed_focal_length =
     (static_cast<double>(this->width_)) /
     (2.0 * tan(this->camera_->GetHFOV().Radian() / 2.0));
+# endif
 
   if (this->focal_length_ == 0)
   {
@@ -477,6 +487,17 @@ void GazeboRosCameraUtils::Init()
     // check against float precision
     if (!gazebo::math::equal(this->focal_length_, computed_focal_length))
     {
+# if GAZEBO_MAJOR_VERSION >= 7
+      ROS_WARN("The <focal_length>[%f] you have provided for camera_ [%s]"
+               " is inconsistent with specified image_width [%d] and"
+               " HFOV [%f].   Please double check to see that"
+               " focal_length = width_ / (2.0 * tan(HFOV/2.0)),"
+               " the explected focal_lengtth value is [%f],"
+               " please update your camera_ model description accordingly.",
+                this->focal_length_, this->parentSensor_->Name().c_str(),
+                this->width_, this->camera_->HFOV().Radian(),
+                computed_focal_length);
+# else
       ROS_WARN("The <focal_length>[%f] you have provided for camera_ [%s]"
                " is inconsistent with specified image_width [%d] and"
                " HFOV [%f].   Please double check to see that"
@@ -486,6 +507,8 @@ void GazeboRosCameraUtils::Init()
                 this->focal_length_, this->parentSensor_->GetName().c_str(),
                 this->width_, this->camera_->GetHFOV().Radian(),
                 computed_focal_length);
+
+# endif
     }
   }
 
@@ -602,7 +625,11 @@ void GazeboRosCameraUtils::PublishCameraInfo()
 
   if (this->camera_info_pub_.getNumSubscribers() > 0)
   {
+# if GAZEBO_MAJOR_VERSION >= 7
+    this->sensor_update_time_ = this->parentSensor_->LastUpdateTime();
+# else
     this->sensor_update_time_ = this->parentSensor_->GetLastUpdateTime();
+# endif
     common::Time cur_time = this->world_->GetSimTime();
     if (cur_time - this->last_info_update_time_ >= this->update_period_)
     {

gazebo_plugins/src/gazebo_ros_depth_camera.cpp

@@ -191,7 +191,11 @@ void GazeboRosDepthCamera::OnNewDepthFrame(const float *_image,
   if (!this->initialized_ || this->height_ <=0 || this->width_ <=0)
     return;
 
+# if GAZEBO_MAJOR_VERSION >= 7
+  this->depth_sensor_update_time_ = this->parentSensor->LastUpdateTime();
+# else
   this->depth_sensor_update_time_ = this->parentSensor->GetLastUpdateTime();
+# endif
   if (this->parentSensor->IsActive())
   {
     if (this->point_cloud_connect_count_ <= 0 &&
@@ -227,7 +231,11 @@ void GazeboRosDepthCamera::OnNewRGBPointCloud(const float *_pcd,
   if (!this->initialized_ || this->height_ <=0 || this->width_ <=0)
     return;
 
+# if GAZEBO_MAJOR_VERSION >= 7
+  this->depth_sensor_update_time_ = this->parentSensor->LastUpdateTime();
+# else
   this->depth_sensor_update_time_ = this->parentSensor->GetLastUpdateTime();
+# endif
   if (!this->parentSensor->IsActive())
   {
     if (this->point_cloud_connect_count_ > 0)
@@ -293,7 +301,11 @@ void GazeboRosDepthCamera::OnNewImageFrame(const unsigned char *_image,
     return;
 
   //ROS_ERROR("camera_ new frame %s %s",this->parentSensor_->GetName().c_str(),this->frame_name_.c_str());
+# if GAZEBO_MAJOR_VERSION >= 7
+  this->sensor_update_time_ = this->parentSensor->LastUpdateTime();
+# else
   this->sensor_update_time_ = this->parentSensor->GetLastUpdateTime();
+# endif
 
   if (!this->parentSensor->IsActive())
   {
@@ -376,7 +388,11 @@ bool GazeboRosDepthCamera::FillPointCloudHelper(
   float* toCopyFrom = (float*)data_arg;
   int index = 0;
 
+# if GAZEBO_MAJOR_VERSION >= 7
+  double hfov = this->parentSensor->DepthCamera()->HFOV().Radian();
+# else
   double hfov = this->parentSensor->GetDepthCamera()->GetHFOV().Radian();
+# endif
   double fl = ((double)this->width) / (2.0 *tan(hfov/2.0));
 
   // convert depth to point cloud
@@ -485,7 +501,11 @@ void GazeboRosDepthCamera::PublishCameraInfo()
 
   if (this->depth_info_connect_count_ > 0)
   {
+# if GAZEBO_MAJOR_VERSION >= 7
+    this->sensor_update_time_ = this->parentSensor_->LastUpdateTime();
+# else
     this->sensor_update_time_ = this->parentSensor_->GetLastUpdateTime();
+# endif
     common::Time cur_time = this->world_->GetSimTime();
     if (cur_time - this->last_depth_image_camera_info_update_time_ >= this->update_period_)
     {