path: root/gazebo7.patch

From 03c3b38073f2461eca1f132ae98c69d028fe8c83 Mon Sep 17 00:00:00 2001
From: Boris Gromov <0xff.root@gmail.com>
Date: Sat, 24 Oct 2015 23:26:00 +0200
Subject: Fix for Gazebo 7

Fix invalid signal name on OS X

scripts/gazebo: line 30: kill: SIGINT: invalid signal specification

Restart package resolving from last position, do not start all over.

Update readme with the correct doc link

Current installing guide 404s

Fix gazebo6 deprecation warnings

Several RaySensor functions are deprecated in gazebo6
and are removed in gazebo7.
The return type is changed to use ignition math
and the function name is changed.
This adds ifdef's to handle the changes.

Fix gazebo7 build errors

The SensorPtr types have changed from boost:: pointers
to std:: pointers,
which requires boost::dynamic_pointer_cast to change to
std::dynamic_pointer_cast.
A helper macro is added that adds a `using` statement
corresponding to the correct type of dynamic_pointer_cast.
This macro should be narrowly scoped to protect
other code.

Add missing boost header

Some boost headers were remove from gazebo7 header files
and gazebo_ros_joint_state_publisher.cpp was using it
implicitly.

Fix compiler error with SetHFOV

In gazebo7, the rendering::Camera::SetHFOV function
is overloaded with a potential for ambiguity,
as reported in the following issue:
https://bitbucket.org/osrf/gazebo/issues/1830
This fixes the build by explicitly defining the
Angle type.

gazebo_ros_utils.h: include gazebo_config.h

Make sure to include gazebo_config.h,
which defines the GAZEBO_MAJOR_VERSION macro

Added a missing initialization inside Differential Drive

diff --git a/include/gazebo_plugins/MultiCameraPlugin.h b/include/gazebo_plugins/MultiCameraPlugin.h
index ff38ef6..b3092d0 100644
--- a/include/gazebo_plugins/MultiCameraPlugin.h
+++ b/include/gazebo_plugins/MultiCameraPlugin.h
@@ -43,7 +43,7 @@ namespace gazebo
                               unsigned int _width, unsigned int _height,
                               unsigned int _depth, const std::string &_format);
 
-    protected: boost::shared_ptr<sensors::MultiCameraSensor> parentSensor;
+    protected: sensors::MultiCameraSensorPtr parentSensor;
 
     protected: std::vector<unsigned int> width, height, depth;
     protected: std::vector<std::string> format;
diff --git a/include/gazebo_plugins/gazebo_ros_utils.h b/include/gazebo_plugins/gazebo_ros_utils.h
index 3db4c45..6b016b8 100644
--- a/include/gazebo_plugins/gazebo_ros_utils.h
+++ b/include/gazebo_plugins/gazebo_ros_utils.h
@@ -39,8 +39,17 @@
 #include <gazebo/common/common.hh>
 #include <gazebo/physics/physics.hh>
 #include <gazebo/sensors/Sensor.hh>
+#include <gazebo/gazebo_config.h>
 #include <ros/ros.h>
 
+#ifndef GAZEBO_SENSORS_USING_DYNAMIC_POINTER_CAST
+# if GAZEBO_MAJOR_VERSION >= 7
+#define GAZEBO_SENSORS_USING_DYNAMIC_POINTER_CAST using std::dynamic_pointer_cast
+# else
+#define GAZEBO_SENSORS_USING_DYNAMIC_POINTER_CAST using boost::dynamic_pointer_cast
+# endif
+#endif
+
 namespace gazebo
 {
 
diff --git a/src/MultiCameraPlugin.cpp b/src/MultiCameraPlugin.cpp
index 8001a22..11f663c 100644
--- a/src/MultiCameraPlugin.cpp
+++ b/src/MultiCameraPlugin.cpp
@@ -17,6 +17,7 @@
 #include <gazebo/sensors/DepthCameraSensor.hh>
 #include <gazebo/sensors/CameraSensor.hh>
 #include <gazebo_plugins/MultiCameraPlugin.h>
+#include <gazebo_plugins/gazebo_ros_utils.h>
 
 using namespace gazebo;
 GZ_REGISTER_SENSOR_PLUGIN(MultiCameraPlugin)
@@ -40,15 +41,16 @@ void MultiCameraPlugin::Load(sensors::SensorPtr _sensor,
   if (!_sensor)
     gzerr << "Invalid sensor pointer.\n";
 
+  GAZEBO_SENSORS_USING_DYNAMIC_POINTER_CAST;
   this->parentSensor =
-    boost::dynamic_pointer_cast<sensors::MultiCameraSensor>(_sensor);
+    dynamic_pointer_cast<sensors::MultiCameraSensor>(_sensor);
 
   if (!this->parentSensor)
   {
     gzerr << "MultiCameraPlugin requires a CameraSensor.\n";
-    if (boost::dynamic_pointer_cast<sensors::DepthCameraSensor>(_sensor))
+    if (dynamic_pointer_cast<sensors::DepthCameraSensor>(_sensor))
       gzmsg << "It is a depth camera sensor\n";
-    if (boost::dynamic_pointer_cast<sensors::CameraSensor>(_sensor))
+    if (dynamic_pointer_cast<sensors::CameraSensor>(_sensor))
       gzmsg << "It is a camera sensor\n";
   }
 
diff --git a/src/gazebo_ros_block_laser.cpp b/src/gazebo_ros_block_laser.cpp
index d8f40bc..d03b9f1 100644
--- a/src/gazebo_ros_block_laser.cpp
+++ b/src/gazebo_ros_block_laser.cpp
@@ -24,6 +24,7 @@
 #include <assert.h>
 
 #include <gazebo_plugins/gazebo_ros_block_laser.h>
+#include <gazebo_plugins/gazebo_ros_utils.h>
 
 #include <gazebo/physics/World.hh>
 #include <gazebo/physics/HingeJoint.hh>
@@ -86,7 +87,8 @@ void GazeboRosBlockLaser::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
   this->node_ = transport::NodePtr(new transport::Node());
   this->node_->Init(worldName);
 
-  this->parent_ray_sensor_ = boost::dynamic_pointer_cast<sensors::RaySensor>(this->parent_sensor_);
+  GAZEBO_SENSORS_USING_DYNAMIC_POINTER_CAST;
+  this->parent_ray_sensor_ = dynamic_pointer_cast<sensors::RaySensor>(this->parent_sensor_);
 
   if (!this->parent_ray_sensor_)
     gzthrow("GazeboRosBlockLaser controller requires a Ray Sensor as its parent");
@@ -230,8 +232,13 @@ void GazeboRosBlockLaser::PutLaserData(common::Time &_updateTime)
 
   this->parent_ray_sensor_->SetActive(false);
 
+#if GAZEBO_MAJOR_VERSION >= 6
+  auto maxAngle = this->parent_ray_sensor_->AngleMax();
+  auto minAngle = this->parent_ray_sensor_->AngleMin();
+#else
   math::Angle maxAngle = this->parent_ray_sensor_->GetAngleMax();
   math::Angle minAngle = this->parent_ray_sensor_->GetAngleMin();
+#endif
 
   double maxRange = this->parent_ray_sensor_->GetRangeMax();
   double minRange = this->parent_ray_sensor_->GetRangeMin();
@@ -240,8 +247,13 @@ void GazeboRosBlockLaser::PutLaserData(common::Time &_updateTime)
 
   int verticalRayCount = this->parent_ray_sensor_->GetVerticalRayCount();
   int verticalRangeCount = this->parent_ray_sensor_->GetVerticalRangeCount();
+#if GAZEBO_MAJOR_VERSION >= 6
+  auto verticalMaxAngle = this->parent_ray_sensor_->VerticalAngleMax();
+  auto verticalMinAngle = this->parent_ray_sensor_->VerticalAngleMin();
+#else
   math::Angle verticalMaxAngle = this->parent_ray_sensor_->GetVerticalAngleMax();
   math::Angle verticalMinAngle = this->parent_ray_sensor_->GetVerticalAngleMin();
+#endif
 
   double yDiff = maxAngle.Radian() - minAngle.Radian();
   double pDiff = verticalMaxAngle.Radian() - verticalMinAngle.Radian();
diff --git a/src/gazebo_ros_bumper.cpp b/src/gazebo_ros_bumper.cpp
index 059f1d9..f8dbdd0 100644
--- a/src/gazebo_ros_bumper.cpp
+++ b/src/gazebo_ros_bumper.cpp
@@ -39,6 +39,7 @@
 #include <tf/tf.h>
 
 #include <gazebo_plugins/gazebo_ros_bumper.h>
+#include <gazebo_plugins/gazebo_ros_utils.h>
 
 namespace gazebo
 {
@@ -65,7 +66,8 @@ GazeboRosBumper::~GazeboRosBumper()
 // Load the controller
 void GazeboRosBumper::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
 {
-  this->parentSensor = boost::dynamic_pointer_cast<sensors::ContactSensor>(_parent);
+  GAZEBO_SENSORS_USING_DYNAMIC_POINTER_CAST;
+  this->parentSensor = dynamic_pointer_cast<sensors::ContactSensor>(_parent);
   if (!this->parentSensor)
   {
     ROS_ERROR("Contact sensor parent is not of type ContactSensor");
diff --git a/src/gazebo_ros_camera_utils.cpp b/src/gazebo_ros_camera_utils.cpp
index 2129b65..4574e8d 100644
--- a/src/gazebo_ros_camera_utils.cpp
+++ b/src/gazebo_ros_camera_utils.cpp
@@ -360,7 +360,11 @@ void GazeboRosCameraUtils::LoadThread()
 // Set Horizontal Field of View
 void GazeboRosCameraUtils::SetHFOV(const std_msgs::Float64::ConstPtr& hfov)
 {
-  this->camera_->SetHFOV(hfov->data);
+#if GAZEBO_MAJOR_VERSION >= 7
+  this->camera_->SetHFOV(ignition::math::Angle(hfov->data));
+#else
+  this->camera_->SetHFOV(gazebo::math::Angle(hfov->data));
+#endif
 }
 
 ////////////////////////////////////////////////////////////////////////////////
diff --git a/src/gazebo_ros_diff_drive.cpp b/src/gazebo_ros_diff_drive.cpp
index 74dd734..e9e497a 100644
--- a/src/gazebo_ros_diff_drive.cpp
+++ b/src/gazebo_ros_diff_drive.cpp
@@ -127,6 +127,10 @@ void GazeboRosDiffDrive::Load ( physics::ModelPtr _parent, sdf::ElementPtr _sdf
     wheel_speed_[RIGHT] = 0;
     wheel_speed_[LEFT] = 0;
 
+    // Initialize velocity support stuff
+    wheel_speed_instr_[RIGHT] = 0;
+    wheel_speed_instr_[LEFT] = 0;
+
     x_ = 0;
     rot_ = 0;
     alive_ = true;
diff --git a/src/gazebo_ros_gpu_laser.cpp b/src/gazebo_ros_gpu_laser.cpp
index 811fc81..6b36c48 100644
--- a/src/gazebo_ros_gpu_laser.cpp
+++ b/src/gazebo_ros_gpu_laser.cpp
@@ -75,8 +75,9 @@ void GazeboRosLaser::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
   // save pointers
   this->sdf = _sdf;
 
+  GAZEBO_SENSORS_USING_DYNAMIC_POINTER_CAST;
   this->parent_ray_sensor_ =
-    boost::dynamic_pointer_cast<sensors::GpuRaySensor>(_parent);
+    dynamic_pointer_cast<sensors::GpuRaySensor>(_parent);
 
   if (!this->parent_ray_sensor_)
     gzthrow("GazeboRosLaser controller requires a Ray Sensor as its parent");
diff --git a/src/gazebo_ros_joint_state_publisher.cpp b/src/gazebo_ros_joint_state_publisher.cpp
index 6c1ede1..d78b3d8 100644
--- a/src/gazebo_ros_joint_state_publisher.cpp
+++ b/src/gazebo_ros_joint_state_publisher.cpp
@@ -25,6 +25,7 @@
  *  SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *
  **/
+#include <boost/algorithm/string.hpp>
 #include <gazebo_plugins/gazebo_ros_joint_state_publisher.h>
 #include <tf/transform_broadcaster.h>
 #include <tf/transform_listener.h>
diff --git a/src/gazebo_ros_laser.cpp b/src/gazebo_ros_laser.cpp
index 815c456..80e60a2 100644
--- a/src/gazebo_ros_laser.cpp
+++ b/src/gazebo_ros_laser.cpp
@@ -72,8 +72,9 @@ void GazeboRosLaser::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
   // save pointers
   this->sdf = _sdf;
 
+  GAZEBO_SENSORS_USING_DYNAMIC_POINTER_CAST;
   this->parent_ray_sensor_ =
-    boost::dynamic_pointer_cast<sensors::RaySensor>(_parent);
+    dynamic_pointer_cast<sensors::RaySensor>(_parent);
 
   if (!this->parent_ray_sensor_)
     gzthrow("GazeboRosLaser controller requires a Ray Sensor as its parent");
diff --git a/src/gazebo_ros_range.cpp b/src/gazebo_ros_range.cpp
index 9387dde..cb229fe 100644
--- a/src/gazebo_ros_range.cpp
+++ b/src/gazebo_ros_range.cpp
@@ -35,6 +35,7 @@
 /** \author Jose Capriles, Bence Magyar. */
 
 #include "gazebo_plugins/gazebo_ros_range.h"
+#include "gazebo_plugins/gazebo_ros_utils.h"
 
 #include <algorithm>
 #include <string>
@@ -92,8 +93,9 @@ void GazeboRosRange::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
 
   this->last_update_time_ = common::Time(0);
 
+  GAZEBO_SENSORS_USING_DYNAMIC_POINTER_CAST;
   this->parent_ray_sensor_ =
-    boost::dynamic_pointer_cast<sensors::RaySensor>(this->parent_sensor_);
+    dynamic_pointer_cast<sensors::RaySensor>(this->parent_sensor_);
 
   if (!this->parent_ray_sensor_)
     gzthrow("GazeboRosRange controller requires a Ray Sensor as its parent");