From fbcc26eeb29c4fe661ac738583250ae51b296d47 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Thu, 31 Jul 2014 13:34:54 -0700 Subject: [PATCH 1/7] Fix build with gazebo4 and indigo --- gazebo_plugins/src/gazebo_ros_gpu_laser.cpp | 7 ++- gazebo_plugins/src/gazebo_ros_laser.cpp | 27 ++++---- gazebo_ros/src/gazebo_ros_api_plugin.cpp | 63 +++++++++++-------- .../src/default_robot_hw_sim.cpp | 5 +- .../src/gazebo_ros_control_plugin.cpp | 3 +- 5 files changed, 60 insertions(+), 45 deletions(-) diff --git a/gazebo_plugins/src/gazebo_ros_gpu_laser.cpp b/gazebo_plugins/src/gazebo_ros_gpu_laser.cpp index 49c6c0d3f..811fc81b2 100644 --- a/gazebo_plugins/src/gazebo_ros_gpu_laser.cpp +++ b/gazebo_plugins/src/gazebo_ros_gpu_laser.cpp @@ -33,6 +33,7 @@ #include #include #include +#include #include #include @@ -124,9 +125,9 @@ 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_; @@ -134,7 +135,7 @@ void GazeboRosLaser::LoadThread() } ROS_INFO("GPU Laser Plugin (ns = %s) , 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_); diff --git a/gazebo_plugins/src/gazebo_ros_laser.cpp b/gazebo_plugins/src/gazebo_ros_laser.cpp index 9f099258d..815c456e1 100644 --- a/gazebo_plugins/src/gazebo_ros_laser.cpp +++ b/gazebo_plugins/src/gazebo_ros_laser.cpp @@ -33,6 +33,7 @@ #include #include #include +#include #include #include @@ -78,7 +79,7 @@ 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 , defaults to /world"); @@ -86,8 +87,8 @@ void GazeboRosLaser::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf) } else this->frame_name_ = this->sdf->Get("frameName"); - - + + if (!this->sdf->HasElement("topicName")) { ROS_INFO("Laser plugin missing , 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,7 +124,7 @@ 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_; @@ -131,7 +132,7 @@ void GazeboRosLaser::LoadThread() } ROS_INFO("Laser Plugin (ns = %s) , 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_); } diff --git a/gazebo_ros/src/gazebo_ros_api_plugin.cpp b/gazebo_ros/src/gazebo_ros_api_plugin.cpp index 4fc33226d..716945f42 100644 --- a/gazebo_ros/src/gazebo_ros_api_plugin.cpp +++ b/gazebo_ros/src/gazebo_ros_api_plugin.cpp @@ -1088,14 +1088,16 @@ bool GazeboRosApiPlugin::setPhysicsProperties(gazebo_msgs::SetPhysicsProperties: // stuff only works in ODE right now ode_pe->SetAutoDisableFlag(req.ode_config.auto_disable_bodies); - 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); - ode_pe->SetWorldCFM(req.ode_config.cfm); - ode_pe->SetWorldERP(req.ode_config.erp); - 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); + 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); world_->SetPaused(is_paused); @@ -1117,15 +1119,24 @@ 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.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(); - res.ode_config.contact_surface_layer = world_->GetPhysicsEngine()->GetContactSurfaceLayer(); - res.ode_config.contact_max_correcting_vel = world_->GetPhysicsEngine()->GetContactMaxCorrectingVel(); - res.ode_config.cfm = world_->GetPhysicsEngine()->GetWorldCFM(); - res.ode_config.erp = world_->GetPhysicsEngine()->GetWorldERP(); - res.ode_config.max_contacts = world_->GetPhysicsEngine()->GetMaxContacts(); + res.ode_config.auto_disable_bodies = + world_->GetPhysicsEngine()->GetAutoDisableFlag(); + res.ode_config.sor_pgs_precon_iters = boost::any_cast( + world_->GetPhysicsEngine()->GetParam("precon_iters")); + res.ode_config.sor_pgs_iters = boost::any_cast( + world_->GetPhysicsEngine()->GetParam("iters")); + res.ode_config.sor_pgs_w = boost::any_cast( + world_->GetPhysicsEngine()->GetParam("sor")); + res.ode_config.contact_surface_layer = boost::any_cast( + world_->GetPhysicsEngine()->GetParam("contact_surface_layer")); + res.ode_config.contact_max_correcting_vel = boost::any_cast( + world_->GetPhysicsEngine()->GetParam("contact_max_correcting_vel")); + res.ode_config.cfm = boost::any_cast( + world_->GetPhysicsEngine()->GetParam("cfm")); + res.ode_config.erp = boost::any_cast( + world_->GetPhysicsEngine()->GetParam("erp")); + res.ode_config.max_contacts = boost::any_cast( + world_->GetPhysicsEngine()->GetParam("max_contacts")); res.success = true; res.status_message = "GetPhysicsProperties: got properties"; @@ -1154,23 +1165,23 @@ 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]); for(unsigned int i=0;i< req.ode_joint_config.hiStop.size();i++) - joint->SetAttribute("hi_stop",i,req.ode_joint_config.hiStop[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->SetAttribute("lo_stop",i,req.ode_joint_config.loStop[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->SetAttribute("erp",i,req.ode_joint_config.erp[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->SetAttribute("cfm",i,req.ode_joint_config.cfm[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->SetAttribute("stop_erp",i,req.ode_joint_config.stop_erp[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->SetAttribute("stop_cfm",i,req.ode_joint_config.stop_cfm[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->SetAttribute("fudge_factor",i,req.ode_joint_config.fudge_factor[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->SetAttribute("fmax",i,req.ode_joint_config.fmax[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->SetAttribute("vel",i,req.ode_joint_config.vel[i]); + joint->SetParam("vel",i,req.ode_joint_config.vel[i]); res.success = true; res.status_message = "SetJointProperties: properties set"; diff --git a/gazebo_ros_control/src/default_robot_hw_sim.cpp b/gazebo_ros_control/src/default_robot_hw_sim.cpp index 11882922a..864149c3b 100644 --- a/gazebo_ros_control/src/default_robot_hw_sim.cpp +++ b/gazebo_ros_control/src/default_robot_hw_sim.cpp @@ -152,7 +152,8 @@ 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_; + const std::string &hardware_interface = + transmissions[j].actuators_[0].hardware_interfaces_[0]; // Debug ROS_DEBUG_STREAM_NAMED("default_robot_hw_sim","Loading joint '" << joint_names_[j] @@ -288,7 +289,7 @@ class DefaultRobotHWSim : public gazebo_ros_control::RobotHWSim break; case POSITION: - sim_joints_[j]->SetAngle(0, joint_position_command_[j]); + sim_joints_[j]->SetPosition(0, joint_position_command_[j]); break; case POSITION_PID: diff --git a/gazebo_ros_control/src/gazebo_ros_control_plugin.cpp b/gazebo_ros_control/src/gazebo_ros_control_plugin.cpp index 88723320f..a70515ef0 100644 --- a/gazebo_ros_control/src/gazebo_ros_control_plugin.cpp +++ b/gazebo_ros_control/src/gazebo_ros_control_plugin.cpp @@ -148,7 +148,8 @@ 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; } From 885ee0b21bd1c2dae52135e3f2cc6566cd73a0e1 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Thu, 31 Jul 2014 13:57:16 -0700 Subject: [PATCH 2/7] Fix to work with gazebo3 --- gazebo_ros_control/src/default_robot_hw_sim.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/gazebo_ros_control/src/default_robot_hw_sim.cpp b/gazebo_ros_control/src/default_robot_hw_sim.cpp index 864149c3b..87c1fc274 100644 --- a/gazebo_ros_control/src/default_robot_hw_sim.cpp +++ b/gazebo_ros_control/src/default_robot_hw_sim.cpp @@ -289,7 +289,11 @@ class DefaultRobotHWSim : public gazebo_ros_control::RobotHWSim break; case POSITION: +#if GAZEBO_VERSION_MAJOR >= 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: From 78216fb4053b653cd144ac1953dde36c14c58fc7 Mon Sep 17 00:00:00 2001 From: osrf Date: Thu, 31 Jul 2014 14:26:19 -0700 Subject: [PATCH 3/7] Update for hydro + gazebo 1.9 --- gazebo_ros/src/gazebo_ros_api_plugin.cpp | 44 +++++++++++++++++++ .../src/default_robot_hw_sim.cpp | 5 +++ 2 files changed, 49 insertions(+) diff --git a/gazebo_ros/src/gazebo_ros_api_plugin.cpp b/gazebo_ros/src/gazebo_ros_api_plugin.cpp index 716945f42..ee22f4567 100644 --- a/gazebo_ros/src/gazebo_ros_api_plugin.cpp +++ b/gazebo_ros/src/gazebo_ros_api_plugin.cpp @@ -21,6 +21,7 @@ */ #include +#include #include "gazebo_ros_api_plugin.h" namespace gazebo @@ -1088,6 +1089,7 @@ 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 >= 4 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); @@ -1098,6 +1100,16 @@ bool GazeboRosApiPlugin::setPhysicsProperties(gazebo_msgs::SetPhysicsProperties: 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); + ode_pe->SetWorldCFM(req.ode_config.cfm); + ode_pe->SetWorldERP(req.ode_config.erp); + 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); @@ -1121,6 +1133,7 @@ bool GazeboRosApiPlugin::getPhysicsProperties(gazebo_msgs::GetPhysicsProperties: // stuff only works in ODE right now res.ode_config.auto_disable_bodies = world_->GetPhysicsEngine()->GetAutoDisableFlag(); +#if GAZEBO_MAJOR_VERSION >= 4 res.ode_config.sor_pgs_precon_iters = boost::any_cast( world_->GetPhysicsEngine()->GetParam("precon_iters")); res.ode_config.sor_pgs_iters = boost::any_cast( @@ -1137,6 +1150,16 @@ bool GazeboRosApiPlugin::getPhysicsProperties(gazebo_msgs::GetPhysicsProperties: world_->GetPhysicsEngine()->GetParam("erp")); res.ode_config.max_contacts = boost::any_cast( 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(); + res.ode_config.contact_surface_layer = world_->GetPhysicsEngine()->GetContactSurfaceLayer(); + res.ode_config.contact_max_correcting_vel = world_->GetPhysicsEngine()->GetContactMaxCorrectingVel(); + 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"; @@ -1164,6 +1187,7 @@ 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++) @@ -1182,6 +1206,26 @@ bool GazeboRosApiPlugin::setJointProperties(gazebo_msgs::SetJointProperties::Req 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++) + joint->SetAttribute("lo_stop",i,req.ode_joint_config.loStop[i]); + for(unsigned int i=0;i< req.ode_joint_config.erp.size();i++) + joint->SetAttribute("erp",i,req.ode_joint_config.erp[i]); + for(unsigned int i=0;i< req.ode_joint_config.cfm.size();i++) + joint->SetAttribute("cfm",i,req.ode_joint_config.cfm[i]); + for(unsigned int i=0;i< req.ode_joint_config.stop_erp.size();i++) + joint->SetAttribute("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->SetAttribute("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->SetAttribute("fudge_factor",i,req.ode_joint_config.fudge_factor[i]); + for(unsigned int i=0;i< req.ode_joint_config.fmax.size();i++) + 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"; diff --git a/gazebo_ros_control/src/default_robot_hw_sim.cpp b/gazebo_ros_control/src/default_robot_hw_sim.cpp index 87c1fc274..dadf304de 100644 --- a/gazebo_ros_control/src/default_robot_hw_sim.cpp +++ b/gazebo_ros_control/src/default_robot_hw_sim.cpp @@ -152,8 +152,13 @@ class DefaultRobotHWSim : public gazebo_ros_control::RobotHWSim joint_position_command_[j] = 0.0; joint_velocity_command_[j] = 0.0; +#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] From ca25590474cf9cd9db97239ffd44efd93d95dbec Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Thu, 31 Jul 2014 14:32:45 -0700 Subject: [PATCH 4/7] Removed a few warnings --- gazebo_ros/src/gazebo_ros_api_plugin.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gazebo_ros/src/gazebo_ros_api_plugin.cpp b/gazebo_ros/src/gazebo_ros_api_plugin.cpp index ee22f4567..1114f8768 100644 --- a/gazebo_ros/src/gazebo_ros_api_plugin.cpp +++ b/gazebo_ros/src/gazebo_ros_api_plugin.cpp @@ -1089,7 +1089,7 @@ 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 >= 4 +#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); @@ -1133,7 +1133,7 @@ bool GazeboRosApiPlugin::getPhysicsProperties(gazebo_msgs::GetPhysicsProperties: // stuff only works in ODE right now res.ode_config.auto_disable_bodies = world_->GetPhysicsEngine()->GetAutoDisableFlag(); -#if GAZEBO_MAJOR_VERSION >= 4 +#if GAZEBO_MAJOR_VERSION >= 3 res.ode_config.sor_pgs_precon_iters = boost::any_cast( world_->GetPhysicsEngine()->GetParam("precon_iters")); res.ode_config.sor_pgs_iters = boost::any_cast( From cafd3e217512f16d9dbac6a65a9226ff5958e2d6 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Thu, 31 Jul 2014 14:34:53 -0700 Subject: [PATCH 5/7] Reduced changes --- gazebo_ros_control/src/gazebo_ros_control_plugin.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/gazebo_ros_control/src/gazebo_ros_control_plugin.cpp b/gazebo_ros_control/src/gazebo_ros_control_plugin.cpp index a70515ef0..120432e38 100644 --- a/gazebo_ros_control/src/gazebo_ros_control_plugin.cpp +++ b/gazebo_ros_control/src/gazebo_ros_control_plugin.cpp @@ -148,8 +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_NAMED("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; } From ac88561c8fd189304753b6a72bcf720159275a56 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Mon, 4 Aug 2014 07:59:24 -0700 Subject: [PATCH 6/7] Fixed boost any cast --- gazebo_ros/src/gazebo_ros_api_plugin.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gazebo_ros/src/gazebo_ros_api_plugin.cpp b/gazebo_ros/src/gazebo_ros_api_plugin.cpp index 1114f8768..21da6c34a 100644 --- a/gazebo_ros/src/gazebo_ros_api_plugin.cpp +++ b/gazebo_ros/src/gazebo_ros_api_plugin.cpp @@ -1134,9 +1134,9 @@ bool GazeboRosApiPlugin::getPhysicsProperties(gazebo_msgs::GetPhysicsProperties: res.ode_config.auto_disable_bodies = world_->GetPhysicsEngine()->GetAutoDisableFlag(); #if GAZEBO_MAJOR_VERSION >= 3 - res.ode_config.sor_pgs_precon_iters = boost::any_cast( + res.ode_config.sor_pgs_precon_iters = boost::any_cast( world_->GetPhysicsEngine()->GetParam("precon_iters")); - res.ode_config.sor_pgs_iters = boost::any_cast( + res.ode_config.sor_pgs_iters = boost::any_cast( world_->GetPhysicsEngine()->GetParam("iters")); res.ode_config.sor_pgs_w = boost::any_cast( world_->GetPhysicsEngine()->GetParam("sor")); @@ -1148,7 +1148,7 @@ bool GazeboRosApiPlugin::getPhysicsProperties(gazebo_msgs::GetPhysicsProperties: world_->GetPhysicsEngine()->GetParam("cfm")); res.ode_config.erp = boost::any_cast( world_->GetPhysicsEngine()->GetParam("erp")); - res.ode_config.max_contacts = boost::any_cast( + res.ode_config.max_contacts = boost::any_cast( world_->GetPhysicsEngine()->GetParam("max_contacts")); #else res.ode_config.sor_pgs_precon_iters = world_->GetPhysicsEngine()->GetSORPGSPreconIters(); From fcc6ea3782502a2d16e5db47189e0aa59b076fb0 Mon Sep 17 00:00:00 2001 From: hsu Date: Fri, 15 Aug 2014 12:17:20 -0700 Subject: [PATCH 7/7] Update default_robot_hw_sim.cpp --- gazebo_ros_control/src/default_robot_hw_sim.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gazebo_ros_control/src/default_robot_hw_sim.cpp b/gazebo_ros_control/src/default_robot_hw_sim.cpp index dadf304de..1a45eb9c7 100644 --- a/gazebo_ros_control/src/default_robot_hw_sim.cpp +++ b/gazebo_ros_control/src/default_robot_hw_sim.cpp @@ -294,7 +294,7 @@ class DefaultRobotHWSim : public gazebo_ros_control::RobotHWSim break; case POSITION: -#if GAZEBO_VERSION_MAJOR >= 4 +#if GAZEBO_MAJOR_VERSION >= 4 sim_joints_[j]->SetPosition(0, joint_position_command_[j]); #else sim_joints_[j]->SetAngle(0, joint_position_command_[j]);