Merge pull request #221 from ros-simulation/fix_build

Fix build for gazebo4

gazebo_plugins/src/gazebo_ros_gpu_laser.cpp

@@ -33,6 +33,7 @@
 #include <gazebo/common/Exception.hh>
 #include <gazebo/sensors/GpuRaySensor.hh>
 #include <gazebo/sensors/SensorTypes.hh>
+#include <gazebo/transport/transport.hh>
 
 #include <tf/tf.h>
 #include <tf/transform_listener.h>
@@ -124,17 +125,17 @@ void GazeboRosLaser::LoadThread()
   this->gazebo_node_->Init(this->world_name_);
 
   this->pmq.startServiceThread();
-  
+
   this->rosnode_ = new ros::NodeHandle(this->robot_namespace_);
-  
+
   this->tf_prefix_ = tf::getPrefixParam(*this->rosnode_);
   if(this->tf_prefix_.empty()) {
       this->tf_prefix_ = this->robot_namespace_;
       boost::trim_right_if(this->tf_prefix_,boost::is_any_of("/"));
   }
   ROS_INFO("GPU Laser Plugin (ns = %s) <tf_prefix_>, set to \"%s\"",
              this->robot_namespace_.c_str(), this->tf_prefix_.c_str());
-  
+
   // resolve tf prefix
   this->frame_name_ = tf::resolve(this->tf_prefix_, this->frame_name_);
 

gazebo_plugins/src/gazebo_ros_laser.cpp

@@ -33,6 +33,7 @@
 #include <gazebo/common/Exception.hh>
 #include <gazebo/sensors/RaySensor.hh>
 #include <gazebo/sensors/SensorTypes.hh>
+#include <gazebo/transport/transport.hh>
 
 #include <tf/tf.h>
 #include <tf/transform_listener.h>
@@ -78,16 +79,16 @@ void GazeboRosLaser::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
     gzthrow("GazeboRosLaser controller requires a Ray Sensor as its parent");
 
   this->robot_namespace_ =  GetRobotNamespace(_parent, _sdf, "Laser");
-  
+
   if (!this->sdf->HasElement("frameName"))
   {
     ROS_INFO("Laser plugin missing <frameName>, defaults to /world");
     this->frame_name_ = "/world";
   }
   else
     this->frame_name_ = this->sdf->Get<std::string>("frameName");
-  
-  
+
+
   if (!this->sdf->HasElement("topicName"))
   {
     ROS_INFO("Laser plugin missing <topicName>, defaults to /world");
@@ -104,8 +105,8 @@ void GazeboRosLaser::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
     ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. "
       << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
     return;
-  }   
-  
+  }
+
   ROS_INFO ( "Starting Laser Plugin (ns = %s)!", this->robot_namespace_.c_str() );
   // ros callback queue for processing subscription
   this->deferred_load_thread_ = boost::thread(
@@ -123,15 +124,15 @@ void GazeboRosLaser::LoadThread()
   this->pmq.startServiceThread();
 
   this->rosnode_ = new ros::NodeHandle(this->robot_namespace_);
-  
+
   this->tf_prefix_ = tf::getPrefixParam(*this->rosnode_);
   if(this->tf_prefix_.empty()) {
       this->tf_prefix_ = this->robot_namespace_;
       boost::trim_right_if(this->tf_prefix_,boost::is_any_of("/"));
   }
   ROS_INFO("Laser Plugin (ns = %s)  <tf_prefix_>, set to \"%s\"",
              this->robot_namespace_.c_str(), this->tf_prefix_.c_str());
-  
+
   // resolve tf prefix
   this->frame_name_ = tf::resolve(this->tf_prefix_, this->frame_name_);
 
@@ -159,8 +160,8 @@ void GazeboRosLaser::LaserConnect()
 {
   this->laser_connect_count_++;
   if (this->laser_connect_count_ == 1)
-    this->laser_scan_sub_ = 
-      this->gazebo_node_->Subscribe(this->parent_ray_sensor_->GetTopic(), 
+    this->laser_scan_sub_ =
+      this->gazebo_node_->Subscribe(this->parent_ray_sensor_->GetTopic(),
                                     &GazeboRosLaser::OnScan, this);
 }
 
@@ -190,12 +191,12 @@ void GazeboRosLaser::OnScan(ConstLaserScanStampedPtr &_msg)
   laser_msg.range_min = _msg->scan().range_min();
   laser_msg.range_max = _msg->scan().range_max();
   laser_msg.ranges.resize(_msg->scan().ranges_size());
-  std::copy(_msg->scan().ranges().begin(), 
-            _msg->scan().ranges().end(), 
+  std::copy(_msg->scan().ranges().begin(),
+            _msg->scan().ranges().end(),
             laser_msg.ranges.begin());
   laser_msg.intensities.resize(_msg->scan().intensities_size());
-  std::copy(_msg->scan().intensities().begin(), 
-            _msg->scan().intensities().end(), 
+  std::copy(_msg->scan().intensities().begin(),
+            _msg->scan().intensities().end(),
             laser_msg.intensities.begin());
   this->pub_queue_->push(laser_msg, this->pub_);
 }

gazebo_ros/src/gazebo_ros_api_plugin.cpp

@@ -21,6 +21,7 @@
  */
 
 #include <gazebo/common/Events.hh>
+#include <gazebo/gazebo_config.h>
 #include "gazebo_ros_api_plugin.h"
 
 namespace gazebo
@@ -1088,6 +1089,18 @@ bool GazeboRosApiPlugin::setPhysicsProperties(gazebo_msgs::SetPhysicsProperties:
 
   // stuff only works in ODE right now
   ode_pe->SetAutoDisableFlag(req.ode_config.auto_disable_bodies);
+#if GAZEBO_MAJOR_VERSION >= 3
+  ode_pe->SetParam("precon_iters", req.ode_config.sor_pgs_precon_iters);
+  ode_pe->SetParam("iters", req.ode_config.sor_pgs_iters);
+  ode_pe->SetParam("sor", req.ode_config.sor_pgs_w);
+  ode_pe->SetParam("cfm", req.ode_config.cfm);
+  ode_pe->SetParam("erp", req.ode_config.erp);
+  ode_pe->SetParam("contact_surface_layer",
+      req.ode_config.contact_surface_layer);
+  ode_pe->SetParam("contact_max_correcting_vel",
+      req.ode_config.contact_max_correcting_vel);
+  ode_pe->SetParam("max_contacts", req.ode_config.max_contacts);
+#else
   ode_pe->SetSORPGSPreconIters(req.ode_config.sor_pgs_precon_iters);
   ode_pe->SetSORPGSIters(req.ode_config.sor_pgs_iters);
   ode_pe->SetSORPGSW(req.ode_config.sor_pgs_w);
@@ -1096,6 +1109,7 @@ bool GazeboRosApiPlugin::setPhysicsProperties(gazebo_msgs::SetPhysicsProperties:
   ode_pe->SetContactSurfaceLayer(req.ode_config.contact_surface_layer);
   ode_pe->SetContactMaxCorrectingVel(req.ode_config.contact_max_correcting_vel);
   ode_pe->SetMaxContacts(req.ode_config.max_contacts);
+#endif
 
   world_->SetPaused(is_paused);
 
@@ -1117,7 +1131,26 @@ bool GazeboRosApiPlugin::getPhysicsProperties(gazebo_msgs::GetPhysicsProperties:
   res.gravity.z = gravity.z;
 
   // stuff only works in ODE right now
-  res.ode_config.auto_disable_bodies = world_->GetPhysicsEngine()->GetAutoDisableFlag();
+  res.ode_config.auto_disable_bodies =
+    world_->GetPhysicsEngine()->GetAutoDisableFlag();
+#if GAZEBO_MAJOR_VERSION >= 3
+  res.ode_config.sor_pgs_precon_iters = boost::any_cast<int>(
+    world_->GetPhysicsEngine()->GetParam("precon_iters"));
+  res.ode_config.sor_pgs_iters = boost::any_cast<int>(
+      world_->GetPhysicsEngine()->GetParam("iters"));
+  res.ode_config.sor_pgs_w = boost::any_cast<double>(
+      world_->GetPhysicsEngine()->GetParam("sor"));
+  res.ode_config.contact_surface_layer = boost::any_cast<double>(
+    world_->GetPhysicsEngine()->GetParam("contact_surface_layer"));
+  res.ode_config.contact_max_correcting_vel = boost::any_cast<double>(
+    world_->GetPhysicsEngine()->GetParam("contact_max_correcting_vel"));
+  res.ode_config.cfm = boost::any_cast<double>(
+      world_->GetPhysicsEngine()->GetParam("cfm"));
+  res.ode_config.erp = boost::any_cast<double>(
+      world_->GetPhysicsEngine()->GetParam("erp"));
+  res.ode_config.max_contacts = boost::any_cast<int>(
+    world_->GetPhysicsEngine()->GetParam("max_contacts"));
+#else
   res.ode_config.sor_pgs_precon_iters = world_->GetPhysicsEngine()->GetSORPGSPreconIters();
   res.ode_config.sor_pgs_iters = world_->GetPhysicsEngine()->GetSORPGSIters();
   res.ode_config.sor_pgs_w = world_->GetPhysicsEngine()->GetSORPGSW();
@@ -1126,6 +1159,7 @@ bool GazeboRosApiPlugin::getPhysicsProperties(gazebo_msgs::GetPhysicsProperties:
   res.ode_config.cfm = world_->GetPhysicsEngine()->GetWorldCFM();
   res.ode_config.erp = world_->GetPhysicsEngine()->GetWorldERP();
   res.ode_config.max_contacts = world_->GetPhysicsEngine()->GetMaxContacts();
+#endif
 
   res.success = true;
   res.status_message = "GetPhysicsProperties: got properties";
@@ -1153,6 +1187,26 @@ bool GazeboRosApiPlugin::setJointProperties(gazebo_msgs::SetJointProperties::Req
   {
     for(unsigned int i=0;i< req.ode_joint_config.damping.size();i++)
       joint->SetDamping(i,req.ode_joint_config.damping[i]);
+#if GAZEBO_MAJOR_VERSION >= 4
+    for(unsigned int i=0;i< req.ode_joint_config.hiStop.size();i++)
+      joint->SetParam("hi_stop",i,req.ode_joint_config.hiStop[i]);
+    for(unsigned int i=0;i< req.ode_joint_config.loStop.size();i++)
+      joint->SetParam("lo_stop",i,req.ode_joint_config.loStop[i]);
+    for(unsigned int i=0;i< req.ode_joint_config.erp.size();i++)
+      joint->SetParam("erp",i,req.ode_joint_config.erp[i]);
+    for(unsigned int i=0;i< req.ode_joint_config.cfm.size();i++)
+      joint->SetParam("cfm",i,req.ode_joint_config.cfm[i]);
+    for(unsigned int i=0;i< req.ode_joint_config.stop_erp.size();i++)
+      joint->SetParam("stop_erp",i,req.ode_joint_config.stop_erp[i]);
+    for(unsigned int i=0;i< req.ode_joint_config.stop_cfm.size();i++)
+      joint->SetParam("stop_cfm",i,req.ode_joint_config.stop_cfm[i]);
+    for(unsigned int i=0;i< req.ode_joint_config.fudge_factor.size();i++)
+      joint->SetParam("fudge_factor",i,req.ode_joint_config.fudge_factor[i]);
+    for(unsigned int i=0;i< req.ode_joint_config.fmax.size();i++)
+      joint->SetParam("fmax",i,req.ode_joint_config.fmax[i]);
+    for(unsigned int i=0;i< req.ode_joint_config.vel.size();i++)
+      joint->SetParam("vel",i,req.ode_joint_config.vel[i]);
+#else
     for(unsigned int i=0;i< req.ode_joint_config.hiStop.size();i++)
       joint->SetAttribute("hi_stop",i,req.ode_joint_config.hiStop[i]);
     for(unsigned int i=0;i< req.ode_joint_config.loStop.size();i++)
@@ -1171,6 +1225,7 @@ bool GazeboRosApiPlugin::setJointProperties(gazebo_msgs::SetJointProperties::Req
       joint->SetAttribute("fmax",i,req.ode_joint_config.fmax[i]);
     for(unsigned int i=0;i< req.ode_joint_config.vel.size();i++)
       joint->SetAttribute("vel",i,req.ode_joint_config.vel[i]);
+#endif
 
     res.success = true;
     res.status_message = "SetJointProperties: properties set";

gazebo_ros_control/src/default_robot_hw_sim.cpp

@@ -152,7 +152,13 @@ class DefaultRobotHWSim : public gazebo_ros_control::RobotHWSim
       joint_position_command_[j] = 0.0;
       joint_velocity_command_[j] = 0.0;
 
-      const std::string& hardware_interface = transmissions[j].actuators_[0].hardware_interface_;
+#if ROS_VERSION_MINOR > 10 || ROS_VERSION_MAJOR > 1
+      const std::string &hardware_interface =
+        transmissions[j].actuators_[0].hardware_interfaces_[0];
+#else
+      const std::string &hardware_interface =
+        transmissions[j].actuators_[0].hardware_interface_;
+#endif
 
       // Debug
       ROS_DEBUG_STREAM_NAMED("default_robot_hw_sim","Loading joint '" << joint_names_[j]
@@ -288,7 +294,11 @@ class DefaultRobotHWSim : public gazebo_ros_control::RobotHWSim
           break;
 
         case POSITION:
+#if GAZEBO_MAJOR_VERSION >= 4
+          sim_joints_[j]->SetPosition(0, joint_position_command_[j]);
+#else
           sim_joints_[j]->SetAngle(0, joint_position_command_[j]);
+#endif
           break;
 
         case POSITION_PID:

gazebo_ros_control/src/gazebo_ros_control_plugin.cpp

@@ -148,7 +148,7 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element
   const std::string urdf_string = getURDF(robot_description_);
   if (!parseTransmissionsFromURDF(urdf_string))
   {
-    ROS_ERROR("gazebo_ros_control", "Error parsing URDF in gazebo_ros_control plugin, plugin not active.\n");
+    ROS_ERROR_NAMED("gazebo_ros_control", "Error parsing URDF in gazebo_ros_control plugin, plugin not active.\n");
     return;
   }