diff options
| -rw-r--r-- | .SRCINFO | 6 | ||||
| -rw-r--r-- | 2.7.4.patch | 1048 | ||||
| -rw-r--r-- | PKGBUILD | 16 |
3 files changed, 1063 insertions, 7 deletions
@@ -1,8 +1,8 @@ # Generated by mksrcinfo v8 -# Tue Dec 12 20:47:31 UTC 2017 +# Sun Feb 25 09:05:27 UTC 2018 pkgbase = ros-lunar-gazebo-ros pkgdesc = ROS - Provides ROS plugins that offer message and service publishers for interfacing with Gazebo through ROS. - pkgver = 2.7.3 + pkgver = 2.7.4 pkgrel = 1 url = http://gazebosim.org/tutorials?cat=connect_ros arch = any @@ -23,7 +23,9 @@ pkgbase = ros-lunar-gazebo-ros depends = ros-lunar-dynamic-reconfigure depends = tinyxml source = ros-lunar-gazebo-ros-2.7.3-0.tar.gz::https://github.com/ros-gbp/gazebo_ros_pkgs-release/archive/release/lunar/gazebo_ros/2.7.3-0.tar.gz + source = 2.7.4.patch sha256sums = 85b7b58bdc4575553204a25413a9835a881623a36bc77b0deeb0e7c17282f58f + sha256sums = 7d211947fae04f2a45a22991980a817e7fe6457e7ac2495748c3e1147105bb63 pkgname = ros-lunar-gazebo-ros diff --git a/2.7.4.patch b/2.7.4.patch new file mode 100644 index 000000000000..1118585afe3c --- /dev/null +++ b/2.7.4.patch @@ -0,0 +1,1048 @@ +diff -ura original/gazebo_ros_pkgs-release-release-lunar-gazebo_ros-2.7.3-0/CHANGELOG.rst patched/gazebo_ros_pkgs-release-release-lunar-gazebo_ros-2.7.3-0/CHANGELOG.rst +--- original/gazebo_ros_pkgs-release-release-lunar-gazebo_ros-2.7.3-0/CHANGELOG.rst 2017-12-11 13:53:51.000000000 +0100 ++++ patched/gazebo_ros_pkgs-release-release-lunar-gazebo_ros-2.7.3-0/CHANGELOG.rst 2018-02-25 10:00:35.138514249 +0100 +@@ -2,6 +2,19 @@ + Changelog for package gazebo_ros + ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + ++2.7.4 (2018-02-12) ++------------------ ++* Fix last gazebo8 warnings! (lunar-devel) (`#664 <https://github.com/ros-simulation/gazebo_ros_pkgs/issues/664>`_) ++* Fix for relative frame errors (lunar-devel) (`#663 <https://github.com/ros-simulation/gazebo_ros_pkgs/issues/663>`_) ++* Fix gazebo8 warnings part 7: retry `#642 <https://github.com/ros-simulation/gazebo_ros_pkgs/issues/642>`_ on lunar (`#660 <https://github.com/ros-simulation/gazebo_ros_pkgs/issues/660>`_) ++* Fix gazebo8 warnings part 10: ifdefs for GetModel, GetEntity, Light (lunar-devel) (`#657 <https://github.com/ros-simulation/gazebo_ros_pkgs/issues/657>`_) ++* gazebo8 warnings: ifdefs for Get.*Vel() (`#655 <https://github.com/ros-simulation/gazebo_ros_pkgs/issues/655>`_) ++* [gazebo_ros] don't overwrite parameter "use_sim_time" (lunar-devel) (`#607 <https://github.com/ros-simulation/gazebo_ros_pkgs/issues/607>`_) ++* Fix gazebo8 warnings part 8: ifdef's for GetWorldPose (lunar-devel) (`#652 <https://github.com/ros-simulation/gazebo_ros_pkgs/issues/652>`_) ++* Prevents GAZEBO_MODEL_DATABASE_URI from being overwritten (`#649 <https://github.com/ros-simulation/gazebo_ros_pkgs/issues/649>`_) ++* for gazebo8+, call functions without Get (`#640 <https://github.com/ros-simulation/gazebo_ros_pkgs/issues/640>`_) ++* Contributors: Jose Luis Rivero, Steven Peters ++ + 2.7.3 (2017-12-11) + ------------------ + * gazebo_ros_api_plugin: improve plugin xml parsing (`#627 <https://github.com/ros-simulation/gazebo_ros_pkgs/issues/627>`_) +diff -ura original/gazebo_ros_pkgs-release-release-lunar-gazebo_ros-2.7.3-0/launch/empty_world.launch patched/gazebo_ros_pkgs-release-release-lunar-gazebo_ros-2.7.3-0/launch/empty_world.launch +--- original/gazebo_ros_pkgs-release-release-lunar-gazebo_ros-2.7.3-0/launch/empty_world.launch 2017-12-11 13:53:51.000000000 +0100 ++++ patched/gazebo_ros_pkgs-release-release-lunar-gazebo_ros-2.7.3-0/launch/empty_world.launch 2018-02-25 10:00:35.139514235 +0100 +@@ -19,9 +19,7 @@ + <arg name="pub_clock_frequency" default="100"/> + + <!-- set use_sim_time flag --> +- <group if="$(arg use_sim_time)"> +- <param name="/use_sim_time" value="true" /> +- </group> ++ <param name="/use_sim_time" value="$(arg use_sim_time)"/> + + <!-- set command arguments --> + <arg unless="$(arg paused)" name="command_arg1" value=""/> +diff -ura original/gazebo_ros_pkgs-release-release-lunar-gazebo_ros-2.7.3-0/package.xml patched/gazebo_ros_pkgs-release-release-lunar-gazebo_ros-2.7.3-0/package.xml +--- original/gazebo_ros_pkgs-release-release-lunar-gazebo_ros-2.7.3-0/package.xml 2017-12-11 13:53:51.000000000 +0100 ++++ patched/gazebo_ros_pkgs-release-release-lunar-gazebo_ros-2.7.3-0/package.xml 2018-02-25 10:00:35.139514235 +0100 +@@ -1,7 +1,7 @@ + <?xml version="1.0"?> + <package format="2"> + <name>gazebo_ros</name> +- <version>2.7.3</version> ++ <version>2.7.4</version> + <description> + Provides ROS plugins that offer message and service publishers for interfacing with <a href="http://gazebosim.org">Gazebo</a> through ROS. + Formally simulator_gazebo/gazebo +diff -ura original/gazebo_ros_pkgs-release-release-lunar-gazebo_ros-2.7.3-0/scripts/gazebo patched/gazebo_ros_pkgs-release-release-lunar-gazebo_ros-2.7.3-0/scripts/gazebo +--- original/gazebo_ros_pkgs-release-release-lunar-gazebo_ros-2.7.3-0/scripts/gazebo 2017-12-11 13:53:51.000000000 +0100 ++++ patched/gazebo_ros_pkgs-release-release-lunar-gazebo_ros-2.7.3-0/scripts/gazebo 2018-02-25 10:00:35.139514235 +0100 +@@ -29,18 +29,22 @@ + + setup_path=$(pkg-config --variable=prefix gazebo)/share/gazebo/ + +-# source setup.sh, but keep local modifications to GAZEBO_MASTER_URI ++# source setup.sh, but keep local modifications to GAZEBO_MASTER_URI and GAZEBO_MODEL_DATABASE_URI + desired_master_uri="$GAZEBO_MASTER_URI" ++desired_model_database_uri="$GAZEBO_MODEL_DATABASE_URI" + . $setup_path/setup.sh + if [ "$desired_master_uri" = "" ]; then + desired_master_uri="$GAZEBO_MASTER_URI" + fi ++if [ "$desired_model_database_uri" = "" ]; then ++ desired_model_database_uri="$GAZEBO_MODEL_DATABASE_URI" ++fi + + final=$(relocate_remappings "${final}") + + # Combine the commands +-GAZEBO_MASTER_URI="$desired_master_uri" gzserver $final & +-GAZEBO_MASTER_URI="$desired_master_uri" gzclient $client_final ++GAZEBO_MASTER_URI="$desired_master_uri" GAZEBO_MODEL_DATABASE_URI="$desired_model_database_uri" gzserver $final & ++GAZEBO_MASTER_URI="$desired_master_uri" GAZEBO_MODEL_DATABASE_URI="$desired_model_database_uri" gzclient $client_final + + # Kill the server + kill -s $SIGNAL $! +diff -ura original/gazebo_ros_pkgs-release-release-lunar-gazebo_ros-2.7.3-0/scripts/gzclient patched/gazebo_ros_pkgs-release-release-lunar-gazebo_ros-2.7.3-0/scripts/gzclient +--- original/gazebo_ros_pkgs-release-release-lunar-gazebo_ros-2.7.3-0/scripts/gzclient 2017-12-11 13:53:51.000000000 +0100 ++++ patched/gazebo_ros_pkgs-release-release-lunar-gazebo_ros-2.7.3-0/scripts/gzclient 2018-02-25 10:00:35.139514235 +0100 +@@ -19,14 +19,18 @@ + + setup_path=$(pkg-config --variable=prefix gazebo)/share/gazebo/ + +-# source setup.sh, but keep local modifications to GAZEBO_MASTER_URI ++# source setup.sh, but keep local modifications to GAZEBO_MASTER_URI and GAZEBO_MODEL_DATABASE_URI + desired_master_uri="$GAZEBO_MASTER_URI" ++desired_model_database_uri="$GAZEBO_MODEL_DATABASE_URI" + . $setup_path/setup.sh + if [ "$desired_master_uri" = "" ]; then + desired_master_uri="$GAZEBO_MASTER_URI" + fi ++if [ "$desired_model_database_uri" = "" ]; then ++ desired_model_database_uri="$GAZEBO_MODEL_DATABASE_URI" ++fi + + final=$(relocate_remappings "${final}") + + # Combine the commands +-GAZEBO_MASTER_URI="$desired_master_uri" gzclient $final ++GAZEBO_MASTER_URI="$desired_master_uri" GAZEBO_MODEL_DATABASE_URI="$desired_model_database_uri" gzclient $final +diff -ura original/gazebo_ros_pkgs-release-release-lunar-gazebo_ros-2.7.3-0/scripts/gzserver patched/gazebo_ros_pkgs-release-release-lunar-gazebo_ros-2.7.3-0/scripts/gzserver +--- original/gazebo_ros_pkgs-release-release-lunar-gazebo_ros-2.7.3-0/scripts/gzserver 2017-12-11 13:53:51.000000000 +0100 ++++ patched/gazebo_ros_pkgs-release-release-lunar-gazebo_ros-2.7.3-0/scripts/gzserver 2018-02-25 10:00:35.139514235 +0100 +@@ -25,13 +25,17 @@ + + setup_path=$(pkg-config --variable=prefix gazebo)/share/gazebo/ + +-# source setup.sh, but keep local modifications to GAZEBO_MASTER_URI ++# source setup.sh, but keep local modifications to GAZEBO_MASTER_URI and GAZEBO_MODEL_DATABASE_URI + desired_master_uri="$GAZEBO_MASTER_URI" ++desired_model_database_uri="$GAZEBO_MODEL_DATABASE_URI" + . $setup_path/setup.sh + if [ "$desired_master_uri" = "" ]; then + desired_master_uri="$GAZEBO_MASTER_URI" + fi ++if [ "$desired_model_database_uri" = "" ]; then ++ desired_model_database_uri="$GAZEBO_MODEL_DATABASE_URI" ++fi + + final=$(relocate_remappings "${final}") + +-GAZEBO_MASTER_URI="$desired_master_uri" gzserver $final ++GAZEBO_MASTER_URI="$desired_master_uri" GAZEBO_MODEL_DATABASE_URI="$desired_model_database_uri" gzserver $final +diff -ura original/gazebo_ros_pkgs-release-release-lunar-gazebo_ros-2.7.3-0/src/gazebo_ros_api_plugin.cpp patched/gazebo_ros_pkgs-release-release-lunar-gazebo_ros-2.7.3-0/src/gazebo_ros_api_plugin.cpp +--- original/gazebo_ros_pkgs-release-release-lunar-gazebo_ros-2.7.3-0/src/gazebo_ros_api_plugin.cpp 2017-12-11 13:53:51.000000000 +0100 ++++ patched/gazebo_ros_pkgs-release-release-lunar-gazebo_ros-2.7.3-0/src/gazebo_ros_api_plugin.cpp 2018-02-25 10:00:35.141514208 +0100 +@@ -173,7 +173,7 @@ + world_ = gazebo::physics::get_world(world_name); + if (!world_) + { +- //ROS_ERROR_NAMED("api_plugin", "world name: [%s]",world->GetName().c_str()); ++ //ROS_ERROR_NAMED("api_plugin", "world name: [%s]",world->Name().c_str()); + // connect helper function to signal for scheduling torque/forces, etc + ROS_FATAL_NAMED("api_plugin", "cannot load gazebo ros api server plugin, physics::get_world() fails to return world"); + return; +@@ -500,11 +500,16 @@ + + + // set param for use_sim_time if not set by user already +- nh_->setParam("/use_sim_time", true); ++ if(!(nh_->hasParam("/use_sim_time"))) ++ nh_->setParam("/use_sim_time", true); + + // todo: contemplate setting environment variable ROBOT=sim here??? + nh_->getParam("pub_clock_frequency", pub_clock_frequency_); ++#if GAZEBO_MAJOR_VERSION >= 8 ++ last_pub_clock_time_ = world_->SimTime(); ++#else + last_pub_clock_time_ = world_->GetSimTime(); ++#endif + } + + void GazeboRosApiPlugin::onLinkStatesConnect() +@@ -625,14 +630,21 @@ + ignition::math::Quaterniond initial_q(req.initial_pose.orientation.w,req.initial_pose.orientation.x,req.initial_pose.orientation.y,req.initial_pose.orientation.z); + + // refernce frame for initial pose definition, modify initial pose if defined +- gazebo::physics::LinkPtr frame = boost::dynamic_pointer_cast<gazebo::physics::Link>(world_->GetEntity(req.reference_frame)); ++#if GAZEBO_MAJOR_VERSION >= 8 ++ gazebo::physics::EntityPtr frame = world_->EntityByName(req.reference_frame); ++#else ++ gazebo::physics::EntityPtr frame = world_->GetEntity(req.reference_frame); ++#endif + if (frame) + { + // convert to relative pose ++#if GAZEBO_MAJOR_VERSION >= 8 ++ ignition::math::Pose3d frame_pose = frame->WorldPose(); ++#else + ignition::math::Pose3d frame_pose = frame->GetWorldPose().Ign(); +- initial_xyz = frame_pose.Rot().RotateVector(initial_xyz); +- initial_xyz += frame_pose.Pos(); +- initial_q *= frame_pose.Rot(); ++#endif ++ initial_xyz = frame_pose.Pos() + frame_pose.Rot().RotateVector(initial_xyz); ++ initial_q = frame_pose.Rot() * initial_q; + } + + /// @todo: map is really wrong, need to use tf here somehow +@@ -720,7 +732,11 @@ + gazebo_msgs::DeleteModel::Response &res) + { + // clear forces, etc for the body in question ++#if GAZEBO_MAJOR_VERSION >= 8 ++ gazebo::physics::ModelPtr model = world_->ModelByName(req.model_name); ++#else + gazebo::physics::ModelPtr model = world_->GetModel(req.model_name); ++#endif + if (!model) + { + ROS_ERROR_NAMED("api_plugin", "DeleteModel: model [%s] does not exist",req.model_name.c_str()); +@@ -765,7 +781,11 @@ + } + { + //boost::recursive_mutex::scoped_lock lock(*world->GetMRMutex()); ++#if GAZEBO_MAJOR_VERSION >= 8 ++ if (!world_->ModelByName(req.model_name)) break; ++#else + if (!world_->GetModel(req.model_name)) break; ++#endif + } + ROS_DEBUG_NAMED("api_plugin", "Waiting for model deletion (%s)",req.model_name.c_str()); + usleep(1000); +@@ -780,7 +800,11 @@ + bool GazeboRosApiPlugin::deleteLight(gazebo_msgs::DeleteLight::Request &req, + gazebo_msgs::DeleteLight::Response &res) + { ++#if GAZEBO_MAJOR_VERSION >= 8 ++ gazebo::physics::LightPtr phy_light = world_->LightByName(req.light_name); ++#else + gazebo::physics::LightPtr phy_light = world_->Light(req.light_name); ++#endif + + if (phy_light == NULL) + { +@@ -796,7 +820,11 @@ + + for (int i = 0; i < 100; i++) + { ++#if GAZEBO_MAJOR_VERSION >= 8 ++ phy_light = world_->LightByName(req.light_name); ++#else + phy_light = world_->Light(req.light_name); ++#endif + if (phy_light == NULL) + { + res.success = true; +@@ -817,8 +845,13 @@ + bool GazeboRosApiPlugin::getModelState(gazebo_msgs::GetModelState::Request &req, + gazebo_msgs::GetModelState::Response &res) + { ++#if GAZEBO_MAJOR_VERSION >= 8 ++ gazebo::physics::ModelPtr model = world_->ModelByName(req.model_name); ++ gazebo::physics::EntityPtr frame = world_->EntityByName(req.relative_entity_name); ++#else + gazebo::physics::ModelPtr model = world_->GetModel(req.model_name); +- gazebo::physics::LinkPtr frame = boost::dynamic_pointer_cast<gazebo::physics::Link>(world_->GetEntity(req.relative_entity_name)); ++ gazebo::physics::EntityPtr frame = world_->GetEntity(req.relative_entity_name); ++#endif + if (!model) + { + ROS_ERROR_NAMED("api_plugin", "GetModelState: model [%s] does not exist",req.model_name.c_str()); +@@ -849,28 +882,39 @@ + res.header.frame_id = req.relative_entity_name; /// @brief this is a redundant information + } + // get model pose ++ // get model twist ++#if GAZEBO_MAJOR_VERSION >= 8 ++ ignition::math::Pose3d model_pose = model->WorldPose(); ++ ignition::math::Vector3d model_linear_vel = model->WorldLinearVel(); ++ ignition::math::Vector3d model_angular_vel = model->WorldAngularVel(); ++#else + ignition::math::Pose3d model_pose = model->GetWorldPose().Ign(); ++ ignition::math::Vector3d model_linear_vel = model->GetWorldLinearVel().Ign(); ++ ignition::math::Vector3d model_angular_vel = model->GetWorldAngularVel().Ign(); ++#endif + ignition::math::Vector3d model_pos = model_pose.Pos(); + ignition::math::Quaterniond model_rot = model_pose.Rot(); + +- // get model twist +- ignition::math::Vector3d model_linear_vel = model->GetWorldLinearVel().Ign(); +- ignition::math::Vector3d model_angular_vel = model->GetWorldAngularVel().Ign(); + + + if (frame) + { +- // convert to relative pose ++ // convert to relative pose, rates ++#if GAZEBO_MAJOR_VERSION >= 8 ++ ignition::math::Pose3d frame_pose = frame->WorldPose(); ++ ignition::math::Vector3d frame_vpos = frame->WorldLinearVel(); // get velocity in gazebo frame ++ ignition::math::Vector3d frame_veul = frame->WorldAngularVel(); // get velocity in gazebo frame ++#else + ignition::math::Pose3d frame_pose = frame->GetWorldPose().Ign(); +- model_pos = model_pos - frame_pose.Pos(); +- model_pos = frame_pose.Rot().RotateVectorReverse(model_pos); +- model_rot *= frame_pose.Rot().Inverse(); +- +- // convert to relative rates + ignition::math::Vector3d frame_vpos = frame->GetWorldLinearVel().Ign(); // get velocity in gazebo frame + ignition::math::Vector3d frame_veul = frame->GetWorldAngularVel().Ign(); // get velocity in gazebo frame +- model_linear_vel = frame_pose.Rot().RotateVector(model_linear_vel - frame_vpos); +- model_angular_vel = frame_pose.Rot().RotateVector(model_angular_vel - frame_veul); ++#endif ++ ignition::math::Pose3d model_rel_pose = model_pose - frame_pose; ++ model_pos = model_rel_pose.Pos(); ++ model_rot = model_rel_pose.Rot(); ++ ++ model_linear_vel = frame_pose.Rot().RotateVectorReverse(model_linear_vel - frame_vpos); ++ model_angular_vel = frame_pose.Rot().RotateVectorReverse(model_angular_vel - frame_veul); + } + /// @todo: FIXME map is really wrong, need to use tf here somehow + else if (req.relative_entity_name == "" || req.relative_entity_name == "world" || req.relative_entity_name == "map" || req.relative_entity_name == "/map") +@@ -910,7 +954,11 @@ + bool GazeboRosApiPlugin::getModelProperties(gazebo_msgs::GetModelProperties::Request &req, + gazebo_msgs::GetModelProperties::Response &res) + { ++#if GAZEBO_MAJOR_VERSION >= 8 ++ gazebo::physics::ModelPtr model = world_->ModelByName(req.model_name); ++#else + gazebo::physics::ModelPtr model = world_->GetModel(req.model_name); ++#endif + if (!model) + { + ROS_ERROR_NAMED("api_plugin", "GetModelProperties: model [%s] does not exist",req.model_name.c_str()); +@@ -972,10 +1020,16 @@ + bool GazeboRosApiPlugin::getWorldProperties(gazebo_msgs::GetWorldProperties::Request &req, + gazebo_msgs::GetWorldProperties::Response &res) + { +- res.sim_time = world_->GetSimTime().Double(); + res.model_names.clear(); ++#if GAZEBO_MAJOR_VERSION >= 8 ++ res.sim_time = world_->SimTime().Double(); ++ for (unsigned int i = 0; i < world_->ModelCount(); i ++) ++ res.model_names.push_back(world_->ModelByIndex(i)->GetName()); ++#else ++ res.sim_time = world_->GetSimTime().Double(); + for (unsigned int i = 0; i < world_->GetModelCount(); i ++) + res.model_names.push_back(world_->GetModel(i)->GetName()); ++#endif + gzerr << "disablign rendering has not been implemented, rendering is always enabled\n"; + res.rendering_enabled = true; //world->GetRenderEngineEnabled(); + res.success = true; +@@ -987,9 +1041,15 @@ + gazebo_msgs::GetJointProperties::Response &res) + { + gazebo::physics::JointPtr joint; ++#if GAZEBO_MAJOR_VERSION >= 8 ++ for (unsigned int i = 0; i < world_->ModelCount(); i ++) ++ { ++ joint = world_->ModelByIndex(i)->GetJoint(req.joint_name); ++#else + for (unsigned int i = 0; i < world_->GetModelCount(); i ++) + { + joint = world_->GetModel(i)->GetJoint(req.joint_name); ++#endif + if (joint) break; + } + +@@ -1007,8 +1067,12 @@ + res.damping.clear(); // to be added to gazebo + //res.damping.push_back(joint->GetDamping(0)); + +- res.position.clear(); // use GetAngle(i) ++ res.position.clear(); ++#if GAZEBO_MAJOR_VERSION >= 8 ++ res.position.push_back(joint->Position(0)); ++#else + res.position.push_back(joint->GetAngle(0).Radian()); ++#endif + + res.rate.clear(); // use GetVelocity(i) + res.rate.push_back(joint->GetVelocity(0)); +@@ -1022,7 +1086,11 @@ + bool GazeboRosApiPlugin::getLinkProperties(gazebo_msgs::GetLinkProperties::Request &req, + gazebo_msgs::GetLinkProperties::Response &res) + { ++#if GAZEBO_MAJOR_VERSION >= 8 ++ gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast<gazebo::physics::Link>(world_->EntityByName(req.link_name)); ++#else + gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast<gazebo::physics::Link>(world_->GetEntity(req.link_name)); ++#endif + if (!body) + { + res.success = false; +@@ -1034,9 +1102,22 @@ + /// @todo: validate + res.gravity_mode = body->GetGravityMode(); + ++ gazebo::physics::InertialPtr inertia = body->GetInertial(); ++ ++#if GAZEBO_MAJOR_VERSION >= 8 ++ res.mass = body->GetInertial()->Mass(); ++ ++ res.ixx = inertia->IXX(); ++ res.iyy = inertia->IYY(); ++ res.izz = inertia->IZZ(); ++ res.ixy = inertia->IXY(); ++ res.ixz = inertia->IXZ(); ++ res.iyz = inertia->IYZ(); ++ ++ ignition::math::Vector3d com = body->GetInertial()->CoG(); ++#else + res.mass = body->GetInertial()->GetMass(); + +- gazebo::physics::InertialPtr inertia = body->GetInertial(); + res.ixx = inertia->GetIXX(); + res.iyy = inertia->GetIYY(); + res.izz = inertia->GetIZZ(); +@@ -1045,6 +1126,7 @@ + res.iyz = inertia->GetIYZ(); + + ignition::math::Vector3d com = body->GetInertial()->GetCoG().Ign(); ++#endif + res.com.position.x = com.X(); + res.com.position.y = com.Y(); + res.com.position.z = com.Z(); +@@ -1062,8 +1144,13 @@ + bool GazeboRosApiPlugin::getLinkState(gazebo_msgs::GetLinkState::Request &req, + gazebo_msgs::GetLinkState::Response &res) + { ++#if GAZEBO_MAJOR_VERSION >= 8 ++ gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast<gazebo::physics::Link>(world_->EntityByName(req.link_name)); ++ gazebo::physics::EntityPtr frame = world_->EntityByName(req.reference_frame); ++#else + gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast<gazebo::physics::Link>(world_->GetEntity(req.link_name)); +- gazebo::physics::LinkPtr frame = boost::dynamic_pointer_cast<gazebo::physics::Link>(world_->GetEntity(req.reference_frame)); ++ gazebo::physics::EntityPtr frame = world_->GetEntity(req.reference_frame); ++#endif + + if (!body) + { +@@ -1073,24 +1160,33 @@ + } + + // get body pose +- ignition::math::Pose3d body_pose = body->GetWorldPose().Ign(); + // Get inertial rates ++#if GAZEBO_MAJOR_VERSION >= 8 ++ ignition::math::Pose3d body_pose = body->WorldPose(); ++ ignition::math::Vector3d body_vpos = body->WorldLinearVel(); // get velocity in gazebo frame ++ ignition::math::Vector3d body_veul = body->WorldAngularVel(); // get velocity in gazebo frame ++#else ++ ignition::math::Pose3d body_pose = body->GetWorldPose().Ign(); + ignition::math::Vector3d body_vpos = body->GetWorldLinearVel().Ign(); // get velocity in gazebo frame + ignition::math::Vector3d body_veul = body->GetWorldAngularVel().Ign(); // get velocity in gazebo frame ++#endif + + if (frame) + { +- // convert to relative pose ++ // convert to relative pose, rates ++#if GAZEBO_MAJOR_VERSION >= 8 ++ ignition::math::Pose3d frame_pose = frame->WorldPose(); ++ ignition::math::Vector3d frame_vpos = frame->WorldLinearVel(); // get velocity in gazebo frame ++ ignition::math::Vector3d frame_veul = frame->WorldAngularVel(); // get velocity in gazebo frame ++#else + ignition::math::Pose3d frame_pose = frame->GetWorldPose().Ign(); +- body_pose.Pos() = body_pose.Pos() - frame_pose.Pos(); +- body_pose.Pos() = frame_pose.Rot().RotateVectorReverse(body_pose.Pos()); +- body_pose.Rot() *= frame_pose.Rot().Inverse(); +- +- // convert to relative rates + ignition::math::Vector3d frame_vpos = frame->GetWorldLinearVel().Ign(); // get velocity in gazebo frame + ignition::math::Vector3d frame_veul = frame->GetWorldAngularVel().Ign(); // get velocity in gazebo frame +- body_vpos = frame_pose.Rot().RotateVector(body_vpos - frame_vpos); +- body_veul = frame_pose.Rot().RotateVector(body_veul - frame_veul); ++#endif ++ body_pose = body_pose - frame_pose; ++ ++ body_vpos = frame_pose.Rot().RotateVectorReverse(body_vpos - frame_vpos); ++ body_veul = frame_pose.Rot().RotateVectorReverse(body_veul - frame_veul); + } + /// @todo: FIXME map is really wrong, need to use tf here somehow + else if (req.reference_frame == "" || req.reference_frame == "world" || req.reference_frame == "map" || req.reference_frame == "/map") +@@ -1128,7 +1224,11 @@ + bool GazeboRosApiPlugin::getLightProperties(gazebo_msgs::GetLightProperties::Request &req, + gazebo_msgs::GetLightProperties::Response &res) + { ++#if GAZEBO_MAJOR_VERSION >= 8 ++ gazebo::physics::LightPtr phy_light = world_->LightByName(req.light_name); ++#else + gazebo::physics::LightPtr phy_light = world_->Light(req.light_name); ++#endif + + if (phy_light == NULL) + { +@@ -1158,7 +1258,11 @@ + bool GazeboRosApiPlugin::setLightProperties(gazebo_msgs::SetLightProperties::Request &req, + gazebo_msgs::SetLightProperties::Response &res) + { ++#if GAZEBO_MAJOR_VERSION >= 8 ++ gazebo::physics::LightPtr phy_light = world_->LightByName(req.light_name); ++#else + gazebo::physics::LightPtr phy_light = world_->Light(req.light_name); ++#endif + + if (phy_light == NULL) + { +@@ -1191,7 +1295,11 @@ + bool GazeboRosApiPlugin::setLinkProperties(gazebo_msgs::SetLinkProperties::Request &req, + gazebo_msgs::SetLinkProperties::Response &res) + { ++#if GAZEBO_MAJOR_VERSION >= 8 ++ gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast<gazebo::physics::Link>(world_->EntityByName(req.link_name)); ++#else + gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast<gazebo::physics::Link>(world_->GetEntity(req.link_name)); ++#endif + if (!body) + { + res.success = false; +@@ -1223,16 +1331,20 @@ + world_->SetGravity(ignition::math::Vector3d(req.gravity.x,req.gravity.y,req.gravity.z)); + + // supported updates ++#if GAZEBO_MAJOR_VERSION >= 8 ++ gazebo::physics::PhysicsEnginePtr pe = (world_->Physics()); ++#else + gazebo::physics::PhysicsEnginePtr pe = (world_->GetPhysicsEngine()); ++#endif + pe->SetMaxStepSize(req.time_step); + pe->SetRealTimeUpdateRate(req.max_update_rate); + +- if (world_->GetPhysicsEngine()->GetType() == "ode") ++ if (pe->GetType() == "ode") + { + // stuff only works in ODE right now + pe->SetAutoDisableFlag(req.ode_config.auto_disable_bodies); +- pe->SetParam("precon_iters", req.ode_config.sor_pgs_precon_iters); +- pe->SetParam("iters", req.ode_config.sor_pgs_iters); ++ pe->SetParam("precon_iters", int(req.ode_config.sor_pgs_precon_iters)); ++ pe->SetParam("iters", int(req.ode_config.sor_pgs_iters)); + pe->SetParam("sor", req.ode_config.sor_pgs_w); + pe->SetParam("cfm", req.ode_config.cfm); + pe->SetParam("erp", req.ode_config.erp); +@@ -1240,7 +1352,7 @@ + req.ode_config.contact_surface_layer); + pe->SetParam("contact_max_correcting_vel", + req.ode_config.contact_max_correcting_vel); +- pe->SetParam("max_contacts", req.ode_config.max_contacts); ++ pe->SetParam("max_contacts", int(req.ode_config.max_contacts)); + + world_->SetPaused(is_paused); + +@@ -1250,9 +1362,9 @@ + else + { + /// \TODO: add support for simbody, dart and bullet physics properties. +- ROS_ERROR_NAMED("api_plugin", "ROS set_physics_properties service call does not yet support physics engine [%s].", world_->GetPhysicsEngine()->GetType().c_str()); ++ ROS_ERROR_NAMED("api_plugin", "ROS set_physics_properties service call does not yet support physics engine [%s].", pe->GetType().c_str()); + res.success = false; +- res.status_message = "Physics engine [" + world_->GetPhysicsEngine()->GetType() + "]: set_physics_properties not supported."; ++ res.status_message = "Physics engine [" + pe->GetType() + "]: set_physics_properties not supported."; + } + return res.success; + } +@@ -1261,35 +1373,40 @@ + gazebo_msgs::GetPhysicsProperties::Response &res) + { + // supported updates +- res.time_step = world_->GetPhysicsEngine()->GetMaxStepSize(); ++#if GAZEBO_MAJOR_VERSION >= 8 ++ gazebo::physics::PhysicsEnginePtr pe = (world_->Physics()); ++#else ++ gazebo::physics::PhysicsEnginePtr pe = (world_->GetPhysicsEngine()); ++#endif ++ res.time_step = pe->GetMaxStepSize(); + res.pause = world_->IsPaused(); +- res.max_update_rate = world_->GetPhysicsEngine()->GetRealTimeUpdateRate(); ++ res.max_update_rate = pe->GetRealTimeUpdateRate(); + ignition::math::Vector3d gravity = world_->Gravity(); + res.gravity.x = gravity.X(); + res.gravity.y = gravity.Y(); + res.gravity.z = gravity.Z(); + + // stuff only works in ODE right now +- if (world_->GetPhysicsEngine()->GetType() == "ode") ++ if (pe->GetType() == "ode") + { + res.ode_config.auto_disable_bodies = +- world_->GetPhysicsEngine()->GetAutoDisableFlag(); ++ pe->GetAutoDisableFlag(); + res.ode_config.sor_pgs_precon_iters = boost::any_cast<int>( +- world_->GetPhysicsEngine()->GetParam("precon_iters")); ++ pe->GetParam("precon_iters")); + res.ode_config.sor_pgs_iters = boost::any_cast<int>( +- world_->GetPhysicsEngine()->GetParam("iters")); ++ pe->GetParam("iters")); + res.ode_config.sor_pgs_w = boost::any_cast<double>( +- world_->GetPhysicsEngine()->GetParam("sor")); ++ pe->GetParam("sor")); + res.ode_config.contact_surface_layer = boost::any_cast<double>( +- world_->GetPhysicsEngine()->GetParam("contact_surface_layer")); ++ pe->GetParam("contact_surface_layer")); + res.ode_config.contact_max_correcting_vel = boost::any_cast<double>( +- world_->GetPhysicsEngine()->GetParam("contact_max_correcting_vel")); ++ pe->GetParam("contact_max_correcting_vel")); + res.ode_config.cfm = boost::any_cast<double>( +- world_->GetPhysicsEngine()->GetParam("cfm")); ++ pe->GetParam("cfm")); + res.ode_config.erp = boost::any_cast<double>( +- world_->GetPhysicsEngine()->GetParam("erp")); ++ pe->GetParam("erp")); + res.ode_config.max_contacts = boost::any_cast<int>( +- world_->GetPhysicsEngine()->GetParam("max_contacts")); ++ pe->GetParam("max_contacts")); + + res.success = true; + res.status_message = "GetPhysicsProperties: got properties"; +@@ -1297,9 +1414,9 @@ + else + { + /// \TODO: add support for simbody, dart and bullet physics properties. +- ROS_ERROR_NAMED("api_plugin", "ROS get_physics_properties service call does not yet support physics engine [%s].", world_->GetPhysicsEngine()->GetType().c_str()); ++ ROS_ERROR_NAMED("api_plugin", "ROS get_physics_properties service call does not yet support physics engine [%s].", pe->GetType().c_str()); + res.success = false; +- res.status_message = "Physics engine [" + world_->GetPhysicsEngine()->GetType() + "]: get_physics_properties not supported."; ++ res.status_message = "Physics engine [" + pe->GetType() + "]: get_physics_properties not supported."; + } + return res.success; + } +@@ -1309,9 +1426,15 @@ + { + /// @todo: current settings only allows for setting of 1DOF joints (e.g. HingeJoint and SliderJoint) correctly. + gazebo::physics::JointPtr joint; ++#if GAZEBO_MAJOR_VERSION >= 8 ++ for (unsigned int i = 0; i < world_->ModelCount(); i ++) ++ { ++ joint = world_->ModelByIndex(i)->GetJoint(req.joint_name); ++#else + for (unsigned int i = 0; i < world_->GetModelCount(); i ++) + { + joint = world_->GetModel(i)->GetJoint(req.joint_name); ++#endif + if (joint) break; + } + +@@ -1360,7 +1483,11 @@ + ignition::math::Vector3d target_pos_dot(req.model_state.twist.linear.x,req.model_state.twist.linear.y,req.model_state.twist.linear.z); + ignition::math::Vector3d target_rot_dot(req.model_state.twist.angular.x,req.model_state.twist.angular.y,req.model_state.twist.angular.z); + ++#if GAZEBO_MAJOR_VERSION >= 8 ++ gazebo::physics::ModelPtr model = world_->ModelByName(req.model_state.model_name); ++#else + gazebo::physics::ModelPtr model = world_->GetModel(req.model_state.model_name); ++#endif + if (!model) + { + ROS_ERROR_NAMED("api_plugin", "Updating ModelState: model [%s] does not exist",req.model_state.model_name.c_str()); +@@ -1370,22 +1497,25 @@ + } + else + { +- gazebo::physics::LinkPtr relative_entity = boost::dynamic_pointer_cast<gazebo::physics::Link>(world_->GetEntity(req.model_state.reference_frame)); ++#if GAZEBO_MAJOR_VERSION >= 8 ++ gazebo::physics::EntityPtr relative_entity = world_->EntityByName(req.model_state.reference_frame); ++#else ++ gazebo::physics::EntityPtr relative_entity = world_->GetEntity(req.model_state.reference_frame); ++#endif + if (relative_entity) + { ++#if GAZEBO_MAJOR_VERSION >= 8 ++ ignition::math::Pose3d frame_pose = relative_entity->WorldPose(); // - myBody->GetCoMPose(); ++#else + ignition::math::Pose3d frame_pose = relative_entity->GetWorldPose().Ign(); // - myBody->GetCoMPose(); +- ignition::math::Vector3d frame_pos = frame_pose.Pos(); +- ignition::math::Quaterniond frame_rot = frame_pose.Rot(); ++#endif + +- //std::cout << " debug : " << relative_entity->GetName() << " : " << frame_pose << " : " << target_pose << std::endl; +- //target_pose = frame_pose + target_pose; // seems buggy, use my own +- target_pose.Pos() = model->GetWorldPose().Ign().Pos() + frame_rot.RotateVector(target_pos); +- target_pose.Rot() = frame_rot * target_pose.Rot(); ++ target_pose = target_pose + frame_pose; + + // Velocities should be commanded in the requested reference + // frame, so we need to translate them to the world frame +- target_pos_dot = frame_rot.RotateVector(target_pos_dot); +- target_rot_dot = frame_rot.RotateVector(target_rot_dot); ++ target_pos_dot = frame_pose.Rot().RotateVector(target_pos_dot); ++ target_rot_dot = frame_pose.Rot().RotateVector(target_rot_dot); + } + /// @todo: FIXME map is really wrong, need to use tf here somehow + else if (req.model_state.reference_frame == "" || req.model_state.reference_frame == "world" || req.model_state.reference_frame == "map" || req.model_state.reference_frame == "/map" ) +@@ -1406,7 +1536,11 @@ + world_->SetPaused(true); + model->SetWorldPose(target_pose); + world_->SetPaused(is_paused); ++#if GAZEBO_MAJOR_VERSION >= 8 ++ //ignition::math::Pose3d p3d = model->WorldPose(); ++#else + //ignition::math::Pose3d p3d = model->GetWorldPose().Ign(); ++#endif + //ROS_ERROR_NAMED("api_plugin", "model updated state: %f %f %f",p3d.Pos().X(),p3d.Pos().Y(),p3d.Pos().Z()); + + // set model velocity +@@ -1431,17 +1565,28 @@ + gazebo_msgs::ApplyJointEffort::Response &res) + { + gazebo::physics::JointPtr joint; ++#if GAZEBO_MAJOR_VERSION >= 8 ++ for (unsigned int i = 0; i < world_->ModelCount(); i ++) ++ { ++ joint = world_->ModelByIndex(i)->GetJoint(req.joint_name); ++#else + for (unsigned int i = 0; i < world_->GetModelCount(); i ++) + { + joint = world_->GetModel(i)->GetJoint(req.joint_name); ++#endif + if (joint) + { + GazeboRosApiPlugin::ForceJointJob* fjj = new GazeboRosApiPlugin::ForceJointJob; + fjj->joint = joint; + fjj->force = req.effort; + fjj->start_time = req.start_time; ++#if GAZEBO_MAJOR_VERSION >= 8 ++ if (fjj->start_time < ros::Time(world_->SimTime().Double())) ++ fjj->start_time = ros::Time(world_->SimTime().Double()); ++#else + if (fjj->start_time < ros::Time(world_->GetSimTime().Double())) + fjj->start_time = ros::Time(world_->GetSimTime().Double()); ++#endif + fjj->duration = req.duration; + lock_.lock(); + force_joint_jobs_.push_back(fjj); +@@ -1545,7 +1690,11 @@ + std::string gazebo_model_name = req.model_name; + + // search for model with name ++#if GAZEBO_MAJOR_VERSION >= 8 ++ gazebo::physics::ModelPtr gazebo_model = world_->ModelByName(req.model_name); ++#else + gazebo::physics::ModelPtr gazebo_model = world_->GetModel(req.model_name); ++#endif + if (!gazebo_model) + { + ROS_ERROR_NAMED("api_plugin", "SetModelConfiguration: model [%s] does not exist",gazebo_model_name.c_str()); +@@ -1586,8 +1735,13 @@ + bool GazeboRosApiPlugin::setLinkState(gazebo_msgs::SetLinkState::Request &req, + gazebo_msgs::SetLinkState::Response &res) + { ++#if GAZEBO_MAJOR_VERSION >= 8 ++ gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast<gazebo::physics::Link>(world_->EntityByName(req.link_state.link_name)); ++ gazebo::physics::LinkPtr frame = boost::dynamic_pointer_cast<gazebo::physics::Link>(world_->EntityByName(req.link_state.reference_frame)); ++#else + gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast<gazebo::physics::Link>(world_->GetEntity(req.link_state.link_name)); +- gazebo::physics::LinkPtr frame = boost::dynamic_pointer_cast<gazebo::physics::Link>(world_->GetEntity(req.link_state.reference_frame)); ++ gazebo::physics::EntityPtr frame = world_->GetEntity(req.link_state.reference_frame); ++#endif + if (!body) + { + ROS_ERROR_NAMED("api_plugin", "Updating LinkState: link [%s] does not exist",req.link_state.link_name.c_str()); +@@ -1607,17 +1761,21 @@ + + if (frame) + { ++#if GAZEBO_MAJOR_VERSION >= 8 ++ ignition::math::Pose3d frame_pose = frame->WorldPose(); // - myBody->GetCoMPose(); ++ ignition::math::Vector3d frame_linear_vel = frame->WorldLinearVel(); ++ ignition::math::Vector3d frame_angular_vel = frame->WorldAngularVel(); ++#else + ignition::math::Pose3d frame_pose = frame->GetWorldPose().Ign(); // - myBody->GetCoMPose(); ++ ignition::math::Vector3d frame_linear_vel = frame->GetWorldLinearVel().Ign(); ++ ignition::math::Vector3d frame_angular_vel = frame->GetWorldAngularVel().Ign(); ++#endif + ignition::math::Vector3d frame_pos = frame_pose.Pos(); + ignition::math::Quaterniond frame_rot = frame_pose.Rot(); + + //std::cout << " debug : " << frame->GetName() << " : " << frame_pose << " : " << target_pose << std::endl; +- //target_pose = frame_pose + target_pose; // seems buggy, use my own +- target_pose.Pos() = frame_pos + frame_rot.RotateVector(target_pos); +- target_pose.Rot() = frame_rot * target_pose.Rot(); ++ target_pose = target_pose + frame_pose; + +- ignition::math::Vector3d frame_linear_vel = frame->GetWorldLinearVel().Ign(); +- ignition::math::Vector3d frame_angular_vel = frame->GetWorldAngularVel().Ign(); + target_linear_vel -= frame_linear_vel; + target_angular_vel -= frame_angular_vel; + } +@@ -1627,7 +1785,7 @@ + } + else + { +- ROS_ERROR_NAMED("api_plugin", "Updating LinkState: reference_frame is not a valid link name"); ++ ROS_ERROR_NAMED("api_plugin", "Updating LinkState: reference_frame is not a valid entity name"); + res.success = false; + res.status_message = "SetLinkState: failed"; + return true; +@@ -1675,8 +1833,13 @@ + bool GazeboRosApiPlugin::applyBodyWrench(gazebo_msgs::ApplyBodyWrench::Request &req, + gazebo_msgs::ApplyBodyWrench::Response &res) + { ++#if GAZEBO_MAJOR_VERSION >= 8 ++ gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast<gazebo::physics::Link>(world_->EntityByName(req.body_name)); ++ gazebo::physics::EntityPtr frame = world_->EntityByName(req.reference_frame); ++#else + gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast<gazebo::physics::Link>(world_->GetEntity(req.body_name)); +- gazebo::physics::LinkPtr frame = boost::dynamic_pointer_cast<gazebo::physics::Link>(world_->GetEntity(req.reference_frame)); ++ gazebo::physics::EntityPtr frame = world_->GetEntity(req.reference_frame); ++#endif + if (!body) + { + ROS_ERROR_NAMED("api_plugin", "ApplyBodyWrench: body [%s] does not exist",req.body_name.c_str()); +@@ -1706,20 +1869,27 @@ + // transform wrench from reference_point in reference_frame + // into the reference frame of the body + // first, translate by reference point to the body frame +- ignition::math::Pose3d target_to_reference = frame->GetWorldPose().Ign() - body->GetWorldPose().Ign(); ++#if GAZEBO_MAJOR_VERSION >= 8 ++ ignition::math::Pose3d framePose = frame->WorldPose(); ++ ignition::math::Pose3d bodyPose = body->WorldPose(); ++#else ++ ignition::math::Pose3d framePose = frame->GetWorldPose().Ign(); ++ ignition::math::Pose3d bodyPose = body->GetWorldPose().Ign(); ++#endif ++ ignition::math::Pose3d target_to_reference = framePose - bodyPose; + ROS_DEBUG_NAMED("api_plugin", "reference frame for applied wrench: [%f %f %f, %f %f %f]-[%f %f %f, %f %f %f]=[%f %f %f, %f %f %f]", +- body->GetWorldPose().Ign().Pos().X(), +- body->GetWorldPose().Ign().Pos().Y(), +- body->GetWorldPose().Ign().Pos().Z(), +- body->GetWorldPose().Ign().Rot().Euler().X(), +- body->GetWorldPose().Ign().Rot().Euler().Y(), +- body->GetWorldPose().Ign().Rot().Euler().Z(), +- frame->GetWorldPose().Ign().Pos().X(), +- frame->GetWorldPose().Ign().Pos().Y(), +- frame->GetWorldPose().Ign().Pos().Z(), +- frame->GetWorldPose().Ign().Rot().Euler().X(), +- frame->GetWorldPose().Ign().Rot().Euler().Y(), +- frame->GetWorldPose().Ign().Rot().Euler().Z(), ++ bodyPose.Pos().X(), ++ bodyPose.Pos().Y(), ++ bodyPose.Pos().Z(), ++ bodyPose.Rot().Euler().X(), ++ bodyPose.Rot().Euler().Y(), ++ bodyPose.Rot().Euler().Z(), ++ framePose.Pos().X(), ++ framePose.Pos().Y(), ++ framePose.Pos().Z(), ++ framePose.Rot().Euler().X(), ++ framePose.Rot().Euler().Y(), ++ framePose.Rot().Euler().Z(), + target_to_reference.Pos().X(), + target_to_reference.Pos().Y(), + target_to_reference.Pos().Z(), +@@ -1750,14 +1920,18 @@ + { + ROS_INFO_NAMED("api_plugin", "ApplyBodyWrench: reference_frame is empty/world/map, using inertial frame, transferring from body relative to inertial frame"); + // FIXME: transfer to inertial frame ++#if GAZEBO_MAJOR_VERSION >= 8 ++ ignition::math::Pose3d target_to_reference = body->WorldPose(); ++#else + ignition::math::Pose3d target_to_reference = body->GetWorldPose().Ign(); ++#endif + target_force = reference_force; + target_torque = reference_torque; + + } + else + { +- ROS_ERROR_NAMED("api_plugin", "ApplyBodyWrench: reference_frame is not a valid link name"); ++ ROS_ERROR_NAMED("api_plugin", "ApplyBodyWrench: reference_frame is not a valid entity name"); + res.success = false; + res.status_message = "ApplyBodyWrench: reference_frame not found"; + return true; +@@ -1772,8 +1946,13 @@ + wej->force = target_force; + wej->torque = target_torque; + wej->start_time = req.start_time; ++#if GAZEBO_MAJOR_VERSION >= 8 ++ if (wej->start_time < ros::Time(world_->SimTime().Double())) ++ wej->start_time = ros::Time(world_->SimTime().Double()); ++#else + if (wej->start_time < ros::Time(world_->GetSimTime().Double())) + wej->start_time = ros::Time(world_->GetSimTime().Double()); ++#endif + wej->duration = req.duration; + lock_.lock(); + wrench_body_jobs_.push_back(wej); +@@ -1814,8 +1993,13 @@ + for (std::vector<GazeboRosApiPlugin::WrenchBodyJob*>::iterator iter=wrench_body_jobs_.begin();iter!=wrench_body_jobs_.end();) + { + // check times and apply wrench if necessary +- if (ros::Time(world_->GetSimTime().Double()) >= (*iter)->start_time) +- if (ros::Time(world_->GetSimTime().Double()) <= (*iter)->start_time+(*iter)->duration || ++#if GAZEBO_MAJOR_VERSION >= 8 ++ ros::Time simTime = ros::Time(world_->SimTime().Double()); ++#else ++ ros::Time simTime = ros::Time(world_->GetSimTime().Double()); ++#endif ++ if (simTime >= (*iter)->start_time) ++ if (simTime <= (*iter)->start_time+(*iter)->duration || + (*iter)->duration.toSec() < 0.0) + { + if ((*iter)->body) // if body exists +@@ -1827,7 +2011,7 @@ + (*iter)->duration.fromSec(0.0); // mark for delete + } + +- if (ros::Time(world_->GetSimTime().Double()) > (*iter)->start_time+(*iter)->duration && ++ if (simTime > (*iter)->start_time+(*iter)->duration && + (*iter)->duration.toSec() >= 0.0) + { + // remove from queue once expires +@@ -1848,8 +2032,13 @@ + for (std::vector<GazeboRosApiPlugin::ForceJointJob*>::iterator iter=force_joint_jobs_.begin();iter!=force_joint_jobs_.end();) + { + // check times and apply force if necessary +- if (ros::Time(world_->GetSimTime().Double()) >= (*iter)->start_time) +- if (ros::Time(world_->GetSimTime().Double()) <= (*iter)->start_time+(*iter)->duration || ++#if GAZEBO_MAJOR_VERSION >= 8 ++ ros::Time simTime = ros::Time(world_->SimTime().Double()); ++#else ++ ros::Time simTime = ros::Time(world_->GetSimTime().Double()); ++#endif ++ if (simTime >= (*iter)->start_time) ++ if (simTime <= (*iter)->start_time+(*iter)->duration || + (*iter)->duration.toSec() < 0.0) + { + if ((*iter)->joint) // if joint exists +@@ -1858,7 +2047,7 @@ + (*iter)->duration.fromSec(0.0); // mark for delete + } + +- if (ros::Time(world_->GetSimTime().Double()) > (*iter)->start_time+(*iter)->duration && ++ if (simTime > (*iter)->start_time+(*iter)->duration && + (*iter)->duration.toSec() >= 0.0) + { + // remove from queue once expires +@@ -1873,7 +2062,11 @@ + void GazeboRosApiPlugin::publishSimTime(const boost::shared_ptr<gazebo::msgs::WorldStatistics const> &msg) + { + ROS_ERROR_NAMED("api_plugin", "CLOCK2"); ++#if GAZEBO_MAJOR_VERSION >= 8 ++ gazebo::common::Time sim_time = world_->SimTime(); ++#else + gazebo::common::Time sim_time = world_->GetSimTime(); ++#endif + if (pub_clock_frequency_ > 0 && (sim_time - last_pub_clock_time_).Double() < 1.0/pub_clock_frequency_) + return; + +@@ -1886,11 +2079,19 @@ + } + void GazeboRosApiPlugin::publishSimTime() + { ++#if GAZEBO_MAJOR_VERSION >= 8 ++ gazebo::common::Time sim_time = world_->SimTime(); ++#else + gazebo::common::Time sim_time = world_->GetSimTime(); ++#endif + if (pub_clock_frequency_ > 0 && (sim_time - last_pub_clock_time_).Double() < 1.0/pub_clock_frequency_) + return; + ++#if GAZEBO_MAJOR_VERSION >= 8 ++ gazebo::common::Time currentTime = world_->SimTime(); ++#else + gazebo::common::Time currentTime = world_->GetSimTime(); ++#endif + rosgraph_msgs::Clock ros_time_; + ros_time_.clock.fromSec(currentTime.Double()); + // publish time to ros +@@ -1903,9 +2104,15 @@ + gazebo_msgs::LinkStates link_states; + + // fill link_states ++#if GAZEBO_MAJOR_VERSION >= 8 ++ for (unsigned int i = 0; i < world_->ModelCount(); i ++) ++ { ++ gazebo::physics::ModelPtr model = world_->ModelByIndex(i); ++#else + for (unsigned int i = 0; i < world_->GetModelCount(); i ++) + { + gazebo::physics::ModelPtr model = world_->GetModel(i); ++#endif + + for (unsigned int j = 0 ; j < model->GetChildCount(); j ++) + { +@@ -1915,7 +2122,15 @@ + { + link_states.name.push_back(body->GetScopedName()); + geometry_msgs::Pose pose; ++#if GAZEBO_MAJOR_VERSION >= 8 ++ ignition::math::Pose3d body_pose = body->WorldPose(); // - myBody->GetCoMPose(); ++ ignition::math::Vector3d linear_vel = body->WorldLinearVel(); ++ ignition::math::Vector3d angular_vel = body->WorldAngularVel(); ++#else + ignition::math::Pose3d body_pose = body->GetWorldPose().Ign(); // - myBody->GetCoMPose(); ++ ignition::math::Vector3d linear_vel = body->GetWorldLinearVel().Ign(); ++ ignition::math::Vector3d angular_vel = body->GetWorldAngularVel().Ign(); ++#endif + ignition::math::Vector3d pos = body_pose.Pos(); + ignition::math::Quaterniond rot = body_pose.Rot(); + pose.position.x = pos.X(); +@@ -1926,8 +2141,6 @@ + pose.orientation.y = rot.Y(); + pose.orientation.z = rot.Z(); + link_states.pose.push_back(pose); +- ignition::math::Vector3d linear_vel = body->GetWorldLinearVel().Ign(); +- ignition::math::Vector3d angular_vel = body->GetWorldAngularVel().Ign(); + geometry_msgs::Twist twist; + twist.linear.x = linear_vel.X(); + twist.linear.y = linear_vel.Y(); +@@ -1948,14 +2161,24 @@ + gazebo_msgs::ModelStates model_states; + + // fill model_states ++#if GAZEBO_MAJOR_VERSION >= 8 ++ for (unsigned int i = 0; i < world_->ModelCount(); i ++) ++ { ++ gazebo::physics::ModelPtr model = world_->ModelByIndex(i); ++ ignition::math::Pose3d model_pose = model->WorldPose(); // - myBody->GetCoMPose(); ++ ignition::math::Vector3d linear_vel = model->WorldLinearVel(); ++ ignition::math::Vector3d angular_vel = model->WorldAngularVel(); ++#else + for (unsigned int i = 0; i < world_->GetModelCount(); i ++) + { + gazebo::physics::ModelPtr model = world_->GetModel(i); +- model_states.name.push_back(model->GetName()); +- geometry_msgs::Pose pose; + ignition::math::Pose3d model_pose = model->GetWorldPose().Ign(); // - myBody->GetCoMPose(); ++ ignition::math::Vector3d linear_vel = model->GetWorldLinearVel().Ign(); ++ ignition::math::Vector3d angular_vel = model->GetWorldAngularVel().Ign(); ++#endif + ignition::math::Vector3d pos = model_pose.Pos(); + ignition::math::Quaterniond rot = model_pose.Rot(); ++ geometry_msgs::Pose pose; + pose.position.x = pos.X(); + pose.position.y = pos.Y(); + pose.position.z = pos.Z(); +@@ -1964,8 +2187,7 @@ + pose.orientation.y = rot.Y(); + pose.orientation.z = rot.Z(); + model_states.pose.push_back(pose); +- ignition::math::Vector3d linear_vel = model->GetWorldLinearVel().Ign(); +- ignition::math::Vector3d angular_vel = model->GetWorldAngularVel().Ign(); ++ model_states.name.push_back(model->GetName()); + geometry_msgs::Twist twist; + twist.linear.x = linear_vel.X(); + twist.linear.y = linear_vel.Y(); +@@ -2365,8 +2587,13 @@ + request_pub_->Publish(*entity_info_msg,true); + // todo: should wait for response response_sub_, check to see that if _msg->response == "nonexistant" + ++#if GAZEBO_MAJOR_VERSION >= 8 ++ gazebo::physics::ModelPtr model = world_->ModelByName(model_name); ++ gazebo::physics::LightPtr light = world_->LightByName(model_name); ++#else + gazebo::physics::ModelPtr model = world_->GetModel(model_name); + gazebo::physics::LightPtr light = world_->Light(model_name); ++#endif + if ((isLight && light != NULL) || (model != NULL)) + { + ROS_ERROR_NAMED("api_plugin", "SpawnModel: Failure - model name %s already exist.",model_name.c_str()); +@@ -2408,8 +2635,13 @@ + + { + //boost::recursive_mutex::scoped_lock lock(*world->GetMRMutex()); ++#if GAZEBO_MAJOR_VERSION >= 8 ++ if ((isLight && world_->LightByName(model_name) != NULL) ++ || (world_->ModelByName(model_name) != NULL)) ++#else + if ((isLight && world_->Light(model_name) != NULL) + || (world_->GetModel(model_name) != NULL)) ++#endif + break; + } + @@ -4,7 +4,7 @@ pkgdesc="ROS - Provides ROS plugins that offer message and service publishers fo url='http://gazebosim.org/tutorials?cat=connect_ros' pkgname='ros-lunar-gazebo-ros' -pkgver='2.7.3' +pkgver='2.7.4' _pkgver_patch=0 arch=('any') pkgrel=1 @@ -29,15 +29,21 @@ depends=(${ros_depends[@]} tinyxml) # Git version (e.g. for debugging) -# _tag=release/lunar/gazebo_ros/${pkgver}-${_pkgver_patch} +# _tag=release/lunar/gazebo_ros/2.7.3-${_pkgver_patch} # _dir=${pkgname} # source=("${_dir}"::"git+https://github.com/ros-gbp/gazebo_ros_pkgs-release.git"#tag=${_tag}) # sha256sums=('SKIP') # Tarball version (faster download) -_dir="gazebo_ros_pkgs-release-release-lunar-gazebo_ros-${pkgver}-${_pkgver_patch}" -source=("${pkgname}-${pkgver}-${_pkgver_patch}.tar.gz"::"https://github.com/ros-gbp/gazebo_ros_pkgs-release/archive/release/lunar/gazebo_ros/${pkgver}-${_pkgver_patch}.tar.gz") -sha256sums=('85b7b58bdc4575553204a25413a9835a881623a36bc77b0deeb0e7c17282f58f') +_dir="gazebo_ros_pkgs-release-release-lunar-gazebo_ros-2.7.3-${_pkgver_patch}" +source=("${pkgname}-2.7.3-${_pkgver_patch}.tar.gz"::"https://github.com/ros-gbp/gazebo_ros_pkgs-release/archive/release/lunar/gazebo_ros/2.7.3-${_pkgver_patch}.tar.gz" "2.7.4.patch") +sha256sums=('85b7b58bdc4575553204a25413a9835a881623a36bc77b0deeb0e7c17282f58f' + '7d211947fae04f2a45a22991980a817e7fe6457e7ac2495748c3e1147105bb63') + +prepare() { + cd "${srcdir}" + patch -Np1 -i "2.7.4.patch" +} build() { # Use ROS environment variables |