Merge pull request ros-simulation#381 from ros-simulation/gazebo7_fixes

Gazebo7 fixes
cohen39 · Jan 16, 2016 · 95338f8 · 95338f8
2 parents b1cb887 + 64aa69a
commit 95338f8
Show file tree

Hide file tree

Showing 10 changed files with 34 additions and 10 deletions.
diff --git a/gazebo_plugins/include/gazebo_plugins/MultiCameraPlugin.h b/gazebo_plugins/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/gazebo_plugins/include/gazebo_plugins/gazebo_ros_utils.h b/gazebo_plugins/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/gazebo_plugins/src/MultiCameraPlugin.cpp b/gazebo_plugins/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/gazebo_plugins/src/gazebo_ros_block_laser.cpp b/gazebo_plugins/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");

diff --git a/gazebo_plugins/src/gazebo_ros_bumper.cpp b/gazebo_plugins/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/gazebo_plugins/src/gazebo_ros_camera_utils.cpp b/gazebo_plugins/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/gazebo_plugins/src/gazebo_ros_gpu_laser.cpp b/gazebo_plugins/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/gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp b/gazebo_plugins/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/gazebo_plugins/src/gazebo_ros_laser.cpp b/gazebo_plugins/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/gazebo_plugins/src/gazebo_ros_range.cpp b/gazebo_plugins/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");