[gazebo] 03/04: Fix problems that appeared during the merge
Jose Luis Rivero
jrivero-guest at moszumanska.debian.org
Mon Feb 2 16:45:42 UTC 2015
This is an automated email from the git hooks/post-receive script.
jrivero-guest pushed a commit to branch dfsg_clean
in repository gazebo.
commit a830beb3ab4778618c088644880570a6a2e36987
Author: Jose Luis Rivero <jrivero at osrfoundation.org>
Date: Mon Feb 2 17:43:23 2015 +0100
Fix problems that appeared during the merge
Now all the files are synced to real gazebo 5.0.1 release
---
gazebo/gui/building/BuildingEditorEvents.hh | 394 +++++++++++++++++++
gazebo/physics/bullet/BulletMeshShape.cc | 66 ++++
test/integration/factory.cc | 314 +++++++++++++++
test/integration/imu.cc | 581 ++++++++++++++++++++++++++++
4 files changed, 1355 insertions(+)
diff --git a/gazebo/gui/building/BuildingEditorEvents.hh b/gazebo/gui/building/BuildingEditorEvents.hh
new file mode 100644
index 0000000..0ba2977
--- /dev/null
+++ b/gazebo/gui/building/BuildingEditorEvents.hh
@@ -0,0 +1,394 @@
+/*
+ * Copyright (C) 2015 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+#ifndef _BUILDING_EDITOR_EVENTS_HH_
+#define _BUILDING_EDITOR_EVENTS_HH_
+
+#include <string>
+#include "gazebo/gui/qt.h"
+#include "gazebo/common/Event.hh"
+#include "gazebo/util/system.hh"
+
+namespace gazebo
+{
+ namespace gui
+ {
+ namespace editor
+ {
+ class GAZEBO_VISIBLE Events
+ {
+ /// \brief Connect a Gazebo event to the toggle edit mode signal
+ /// \param[in] _subscriber the subscriber to this event
+ /// \return a connection
+ public: template<typename T>
+ static event::ConnectionPtr
+ ConnectToggleEditMode(T _subscriber)
+ { return toggleEditMode.Connect(_subscriber); }
+
+ /// \brief Disconnect a Gazebo event from the toggle edit mode signal
+ /// \param[in] _subscriber the subscriber to this event
+ public: static void DisconnectToggleEditMode(
+ event::ConnectionPtr _subscriber)
+ { toggleEditMode.Disconnect(_subscriber); }
+
+ /// \brief Connect a Gazebo event to the create editor item signal
+ /// \param[in] _subscriber the subscriber to this event
+ /// \return a connection
+ public: template<typename T>
+ static event::ConnectionPtr
+ ConnectCreateBuildingEditorItem(T _subscriber)
+ { return createBuildingEditorItem.Connect(_subscriber); }
+
+ /// \brief Disconnect a Gazebo event from the create editor item signal
+ /// \param[in] _subscriber the subscriber to this event
+ public: static void DisconnectCreateBuildingEditorItem(
+ event::ConnectionPtr _subscriber)
+ { createBuildingEditorItem.Disconnect(_subscriber); }
+
+ /// \brief Connect a Gazebo event to the color selected signal
+ /// \param[in] _subscriber the subscriber to this event
+ /// \return a connection
+ public: template<typename T>
+ static event::ConnectionPtr
+ ConnectColorSelected(T _subscriber)
+ { return colorSelected.Connect(_subscriber); }
+
+ /// \brief Disconnect a Gazebo event from the color selected signal
+ /// \param[in] _subscriber the subscriber to this event
+ public: static void DisconnectColorSelected(
+ event::ConnectionPtr _subscriber)
+ { colorSelected.Disconnect(_subscriber); }
+
+ /// \brief Connect a Gazebo event to the texture selected signal
+ /// \param[in] _subscriber the subscriber to this event
+ /// \return a connection
+ public: template<typename T>
+ static event::ConnectionPtr
+ ConnectTextureSelected(T _subscriber)
+ { return textureSelected.Connect(_subscriber); }
+
+ /// \brief Disconnect a Gazebo event from the texture selected signal
+ /// \param[in] _subscriber the subscriber to this event
+ public: static void DisconnectTextureSelected(
+ event::ConnectionPtr _subscriber)
+ { textureSelected.Disconnect(_subscriber); }
+
+ /// \brief Connect a Gazebo event to the save model signal
+ /// \param[in] _subscriber the subscriber to this event
+ /// \return a connection
+ public: template<typename T>
+ static event::ConnectionPtr ConnectSaveBuildingModel(T _subscriber)
+ { return saveBuildingModel.Connect(_subscriber); }
+
+ /// \brief Disconnect a Gazebo event from the save model signal
+ /// \param[in] _subscriber the subscriber to this event
+ public: static void DisconnectSaveBuildingModel(
+ event::ConnectionPtr _subscriber)
+ { saveBuildingModel.Disconnect(_subscriber); }
+
+ /// \brief Connect a Gazebo event to the finish model signal
+ /// \param[in] _subscriber the subscriber to this event
+ /// \return a connection
+ public: template<typename T>
+ static event::ConnectionPtr
+ ConnectFinishBuildingModel(T _subscriber)
+ { return finishBuildingModel.Connect(_subscriber); }
+
+ /// \brief Disconnect a Gazebo event from the finish model signal
+ /// \param[in] _subscriber the subscriber to this event
+ public: static void DisconnectFinishBuildingModel(
+ event::ConnectionPtr _subscriber)
+ { finishBuildingModel.Disconnect(_subscriber); }
+
+ /// \brief Connect a Gazebo event to the new model signal
+ /// \param[in] _subscriber the subscriber to this event
+ /// \return a connection
+ public: template<typename T>
+ static event::ConnectionPtr
+ ConnectNewBuildingModel(T _subscriber)
+ { return newBuildingModel.Connect(_subscriber); }
+
+ /// \brief Disconnect a Gazebo event from the new model signal
+ /// \param[in] _subscriber the subscriber to this event
+ public: static void DisconnectNewBuildingModel(
+ event::ConnectionPtr _subscriber)
+ { newBuildingModel.Disconnect(_subscriber); }
+
+ /// \brief Connect a Gazebo event to the change model signal
+ /// \param[in] _subscriber the subscriber to this event
+ /// \return a connection
+ public: template<typename T>
+ static event::ConnectionPtr
+ ConnectChangeBuildingLevel(T _subscriber)
+ { return changeBuildingLevel.Connect(_subscriber); }
+
+ /// \brief Disconnect a Gazebo event from the change level signal
+ /// \param[in] _subscriber the subscriber to this event
+ public: static void DisconnectChangeBuildingLevel(
+ event::ConnectionPtr _subscriber)
+ { changeBuildingLevel.Disconnect(_subscriber); }
+
+ /// \brief Connect a Gazebo event to the add level signal
+ /// \param[in] _subscriber the subscriber to this event
+ /// \return a connection
+ public: template<typename T>
+ static event::ConnectionPtr ConnectAddBuildingLevel(T _subscriber)
+ { return addBuildingLevel.Connect(_subscriber); }
+
+ /// \brief Disconnect a Gazebo event from the add level signal
+ /// \param[in] _subscriber the subscriber to this event
+ public: static void DisconnectAddBuildingLevel(
+ event::ConnectionPtr _subscriber)
+ { addBuildingLevel.Disconnect(_subscriber); }
+
+ /// \brief Connect a Gazebo event to the delete level signal
+ /// \param[in] _subscriber the subscriber to this event
+ /// \return a connection
+ public: template<typename T>
+ static event::ConnectionPtr
+ ConnectDeleteBuildingLevel(T _subscriber)
+ { return deleteBuildingLevel.Connect(_subscriber); }
+
+ /// \brief Disconnect a Gazebo event from the delete level signal
+ /// \param[in] _subscriber the subscriber to this event
+ public: static void DisconnectDeleteBuildingLevel(
+ event::ConnectionPtr _subscriber)
+ { deleteBuildingLevel.Disconnect(_subscriber); }
+
+ /// \brief Connect a Gazebo event to the show floorplan signal
+ /// \param[in] _subscriber the subscriber to this event
+ /// \return a connection
+ public: template<typename T>
+ static event::ConnectionPtr ConnectShowFloorplan(T _subscriber)
+ { return showFloorplan.Connect(_subscriber); }
+
+ /// \brief Disconnect a Gazebo event from the show floorplan signal
+ /// \param[in] _subscriber the subscriber to this event
+ public: static void DisconnectShowFloorplan(
+ event::ConnectionPtr _subscriber)
+ { showFloorplan.Disconnect(_subscriber); }
+
+ /// \brief Connect a Gazebo event to the trigger show floorplan
+ /// signal
+ /// \param[in] _subscriber the subscriber to this event
+ /// \return a connection
+ public: template<typename T>
+ static event::ConnectionPtr
+ ConnectTriggerShowFloorplan(T _subscriber)
+ { return triggerShowFloorplan.Connect(_subscriber); }
+
+ /// \brief Disconnect a Gazebo event from the trigger show floorplan
+ /// signal
+ /// \param[in] _subscriber the subscriber to this event
+ public: static void DisconnectTriggerShowFloorplan(
+ event::ConnectionPtr _subscriber)
+ { triggerShowFloorplan.Disconnect(_subscriber); }
+
+ /// \brief Connect a Gazebo event to the show elements signal
+ /// \param[in] _subscriber the subscriber to this event
+ /// \return a connection
+ public: template<typename T>
+ static event::ConnectionPtr ConnectShowElements(T _subscriber)
+ { return showElements.Connect(_subscriber); }
+
+ /// \brief Disconnect a Gazebo event from the show elements signal
+ /// \param[in] _subscriber the subscriber to this event
+ public: static void DisconnectShowElements(
+ event::ConnectionPtr _subscriber)
+ { showElements.Disconnect(_subscriber); }
+
+ /// \brief Connect a Gazebo event to the trigger show elements
+ /// signal
+ /// \param[in] _subscriber the subscriber to this event
+ /// \return a connection
+ public: template<typename T>
+ static event::ConnectionPtr
+ ConnectTriggerShowElements(T _subscriber)
+ { return triggerShowElements.Connect(_subscriber); }
+
+ /// \brief Disconnect a Gazebo event from the trigger show elements
+ /// signal
+ /// \param[in] _subscriber the subscriber to this event
+ public: static void DisconnectTriggerShowElements(
+ event::ConnectionPtr _subscriber)
+ { triggerShowElements.Disconnect(_subscriber); }
+
+ /// \brief Connect a Gazebo event to the update level widget signal
+ /// \param[in] _subscriber the subscriber to this event
+ /// \return a connection
+ public: template<typename T>
+ static event::ConnectionPtr
+ ConnectUpdateLevelWidget(T _subscriber)
+ { return updateLevelWidget.Connect(_subscriber); }
+
+ /// \brief Disconnect a Gazebo event from the update level widget signal
+ /// \param[in] _subscriber the subscriber to this event
+ public: static void DisconnectUpdateLevelWidget(
+ event::ConnectionPtr _subscriber)
+ { updateLevelWidget.Disconnect(_subscriber); }
+
+ /// \brief Connect a Gazebo event to the change zoom signal
+ /// \param[in] _subscriber the subscriber to this event
+ /// \return a connection
+ public: template<typename T>
+ static event::ConnectionPtr
+ ConnectChangeBuildingEditorZoom(T _subscriber)
+ { return changeBuildingEditorZoom.Connect(_subscriber); }
+
+ /// \brief Disconnect a Gazebo event from the change zoom level signal
+ /// \param[in] _subscriber the subscriber to this event
+ public: static void DisconnectChangeBuildingEditorZoom(
+ event::ConnectionPtr _subscriber)
+ { changeBuildingEditorZoom.Disconnect(_subscriber); }
+
+ /// \brief Connect a Gazebo event to the save signal
+ /// \param[in] _subscriber the subscriber to this event
+ /// \return a connection
+ public: template<typename T>
+ static event::ConnectionPtr ConnectSaveBuildingEditor(T _subscriber)
+ { return saveBuildingEditor.Connect(_subscriber); }
+
+ /// \brief Connect a Gazebo event to the save signal
+ /// \param[in] _subscriber the subscriber to this event
+ /// \return a connection
+ public: template<typename T>
+ static event::ConnectionPtr ConnectSaveAsBuildingEditor
+ (T _subscriber)
+ { return saveAsBuildingEditor.Connect(_subscriber); }
+
+ /// \brief Disconnect a Gazebo event from the save signal
+ /// \param[in] _subscriber the subscriber to this event
+ public: static void DisconnectSaveBuildingEditor(
+ event::ConnectionPtr _subscriber)
+ { saveBuildingEditor.Disconnect(_subscriber); }
+
+ /// \brief Disconnect a Gazebo event from the save as signal
+ /// \param[in] _subscriber the subscriber to this event
+ public: static void DisconnectSaveAsBuildingEditor(
+ event::ConnectionPtr _subscriber)
+ { saveAsBuildingEditor.Disconnect(_subscriber); }
+
+ /// \brief Connect a Gazebo event to the new signal
+ /// \param[in] _subscriber the subscriber to this event
+ /// \return a connection
+ public: template<typename T>
+ static event::ConnectionPtr
+ ConnectNewBuildingEditor(T _subscriber)
+ { return newBuildingEditor.Connect(_subscriber); }
+
+ /// \brief Disconnect a Gazebo event from the new signal
+ /// \param[in] _subscriber the subscriber to this event
+ public: static void DisconnectNewBuildingEditor(
+ event::ConnectionPtr _subscriber)
+ { newBuildingEditor.Disconnect(_subscriber); }
+
+ /// \brief Connect a Gazebo event to the exit signal
+ /// \param[in] _subscriber the subscriber to this event
+ /// \return a connection
+ public: template<typename T>
+ static event::ConnectionPtr ConnectExitBuildingEditor(T _subscriber)
+ { return exitBuildingEditor.Connect(_subscriber); }
+
+ /// \brief Disconnect a Gazebo event from the exit signal
+ /// \param[in] _subscriber the subscriber to this event
+ public: static void DisconnectExitBuildingEditor(
+ event::ConnectionPtr _subscriber)
+ { exitBuildingEditor.Disconnect(_subscriber); }
+
+ /// \brief Toggle if the edit mode was checked or not.
+ public: static event::EventT<void (bool)> toggleEditMode;
+
+ /// \brief Connect a Gazebo event to the name changed signal
+ /// \param[in] _subscriber the subscriber to this event
+ /// \return a connection
+ public: template<typename T>
+ static event::ConnectionPtr ConnectBuildingNameChanged
+ (T _subscriber)
+ { return buildingNameChanged.Connect(_subscriber); }
+
+ /// \brief Disconnect a Gazebo event from the exit signal
+ /// \param[in] _subscriber the subscriber to this event
+ public: static void ConnectBuildingNameChanged(
+ event::ConnectionPtr _subscriber)
+ { buildingNameChanged.Disconnect(_subscriber); }
+
+ /// \brief An editor item is to be created
+ public: static event::EventT<void (std::string)>
+ createBuildingEditorItem;
+
+ /// \brief A color has been selected.
+ public: static event::EventT<void (QColor)> colorSelected;
+
+ /// \brief A texture has been selected.
+ public: static event::EventT<void (QString)> textureSelected;
+
+ /// \brief A model has been saved with a name and a location
+ public: static event::EventT<void (std::string, std::string)>
+ saveBuildingModel;
+
+ /// \brief A model has been completed and uploaded onto the server.
+ public: static event::EventT<void ()> finishBuildingModel;
+
+ /// \brief A new model has been started
+ public: static event::EventT<void ()> newBuildingModel;
+
+ /// \brief The current level has been changed
+ public: static event::EventT<void (int)> changeBuildingLevel;
+
+ /// \brief A new level has been added
+ public: static event::EventT<void ()> addBuildingLevel;
+
+ /// \brief A level has been deleted
+ public: static event::EventT<void ()> deleteBuildingLevel;
+
+ /// \brief Show or hide floorplan
+ public: static event::EventT<void ()> showFloorplan;
+
+ /// \brief Trigger show floorplan
+ public: static event::EventT<void ()> triggerShowFloorplan;
+
+ /// \brief Show or hide building elements
+ public: static event::EventT<void ()> showElements;
+
+ /// \brief Trigger show elements
+ public: static event::EventT<void ()> triggerShowElements;
+
+ /// \brief The levels have been changed
+ public: static event::EventT<void (int, std::string)>
+ updateLevelWidget;
+
+ /// \brief The current zoom level has been changed
+ public: static event::EventT<void (double)> changeBuildingEditorZoom;
+
+ /// \brief Save the model
+ public: static event::EventT<bool (std::string)> saveBuildingEditor;
+
+ /// \brief Save the model as
+ public: static event::EventT<bool (std::string)> saveAsBuildingEditor;
+
+ /// \brief Make a new model
+ public: static event::EventT<void ()> newBuildingEditor;
+
+ /// \brief Exit the editor mode with the option to save
+ public: static event::EventT<void ()> exitBuildingEditor;
+
+ /// \brief Name was changed in the editor palette
+ public: static event::EventT<void (std::string)> buildingNameChanged;
+ };
+ }
+ }
+}
+#endif
diff --git a/gazebo/physics/bullet/BulletMeshShape.cc b/gazebo/physics/bullet/BulletMeshShape.cc
new file mode 100644
index 0000000..88524c6
--- /dev/null
+++ b/gazebo/physics/bullet/BulletMeshShape.cc
@@ -0,0 +1,66 @@
+/*
+ * Copyright (C) 2012-2015 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#include "gazebo/common/Mesh.hh"
+
+#include "gazebo/physics/bullet/BulletMesh.hh"
+#include "gazebo/physics/bullet/BulletTypes.hh"
+#include "gazebo/physics/bullet/BulletCollision.hh"
+#include "gazebo/physics/bullet/BulletPhysics.hh"
+#include "gazebo/physics/bullet/BulletMeshShape.hh"
+
+using namespace gazebo;
+using namespace physics;
+
+//////////////////////////////////////////////////
+BulletMeshShape::BulletMeshShape(CollisionPtr _parent)
+ : MeshShape(_parent)
+{
+ this->bulletMesh = new BulletMesh();
+}
+
+//////////////////////////////////////////////////
+BulletMeshShape::~BulletMeshShape()
+{
+ delete this->bulletMesh;
+}
+
+//////////////////////////////////////////////////
+void BulletMeshShape::Load(sdf::ElementPtr _sdf)
+{
+ MeshShape::Load(_sdf);
+}
+
+//////////////////////////////////////////////////
+void BulletMeshShape::Init()
+{
+ MeshShape::Init();
+
+ BulletCollisionPtr bParent =
+ boost::static_pointer_cast<BulletCollision>(this->collisionParent);
+
+ if (this->submesh)
+ {
+ this->bulletMesh->Init(this->submesh, bParent,
+ this->sdf->Get<math::Vector3>("scale"));
+ }
+ else
+ {
+ this->bulletMesh->Init(this->mesh, bParent,
+ this->sdf->Get<math::Vector3>("scale"));
+ }
+}
diff --git a/test/integration/factory.cc b/test/integration/factory.cc
new file mode 100644
index 0000000..02bc637
--- /dev/null
+++ b/test/integration/factory.cc
@@ -0,0 +1,314 @@
+/*
+ * Copyright (C) 2012-2015 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+#include <string.h>
+#include "gazebo/math/Helpers.hh"
+#include "gazebo/transport/TransportTypes.hh"
+#include "gazebo/transport/Node.hh"
+
+#include "gazebo/rendering/RenderEngine.hh"
+#include "gazebo/rendering/Camera.hh"
+#include "gazebo/sensors/SensorsIface.hh"
+#include "gazebo/sensors/CameraSensor.hh"
+#include "ServerFixture.hh"
+#include "images_cmp.h"
+#include "helper_physics_generator.hh"
+
+using namespace gazebo;
+class FactoryTest : public ServerFixture,
+ public testing::WithParamInterface<const char*>
+{
+ public: void BoxSdf(const std::string &_physicsEngine);
+ public: void Box(const std::string &_physicsEngine);
+ public: void Sphere(const std::string &_physicsEngine);
+ public: void Cylinder(const std::string &_physicsEngine);
+ public: void Clone(const std::string &_physicsEngine);
+};
+
+///////////////////////////////////////////////////
+// Verify that sdf is retained by entities spawned
+// via factory messages. A change between 1.6, 1.7
+// caused entities to lose their sdf data
+// (see issue #651)
+void FactoryTest::BoxSdf(const std::string &_physicsEngine)
+{
+ math::Pose setPose, testPose;
+ Load("worlds/empty.world", true, _physicsEngine);
+ physics::WorldPtr world = physics::get_world("default");
+ ASSERT_TRUE(world != NULL);
+
+ unsigned int entityCount = 6;
+
+ for (unsigned int i = 0; i < entityCount; ++i)
+ {
+ std::ostringstream name;
+ name << "test_box_" << i;
+ setPose.Set(math::Vector3(0, 0, i+0.5), math::Quaternion(0, 0, 0));
+ SpawnBox(name.str(), math::Vector3(1, 1, 1), setPose.pos,
+ setPose.rot.GetAsEuler());
+ }
+
+ // This loop must be separate from the previous loop to cause
+ // the failure.
+ for (unsigned int i = 0; i < entityCount; ++i)
+ {
+ std::ostringstream name;
+ name << "test_box_" << i;
+
+ physics::ModelPtr model = world->GetModel(name.str());
+ ASSERT_TRUE(model != NULL);
+ msgs::Model msg;
+ model->FillMsg(msg);
+ EXPECT_TRUE(msg.has_pose());
+ // gzerr << msg.DebugString() << '\n';
+ }
+}
+
+/////////////////////////////////////////////////
+TEST_P(FactoryTest, BoxSdf)
+{
+ BoxSdf(GetParam());
+}
+
+/////////////////////////////////////////////////
+void FactoryTest::Box(const std::string &_physicsEngine)
+{
+ math::Pose setPose, testPose;
+ Load("worlds/empty.world", true, _physicsEngine);
+
+ for (unsigned int i = 0; i < 100; i++)
+ {
+ std::ostringstream name;
+ name << "test_box_" << i;
+ setPose.Set(math::Vector3(0, 0, i+0.5), math::Quaternion(0, 0, 0));
+ SpawnBox(name.str(), math::Vector3(1, 1, 1), setPose.pos,
+ setPose.rot.GetAsEuler());
+ testPose = GetEntityPose(name.str());
+ EXPECT_TRUE(math::equal(testPose.pos.x, setPose.pos.x, 0.1));
+ EXPECT_TRUE(math::equal(testPose.pos.y, setPose.pos.y, 0.1));
+ EXPECT_TRUE(math::equal(testPose.pos.z, setPose.pos.z, 0.1));
+ }
+}
+
+/////////////////////////////////////////////////
+TEST_P(FactoryTest, Box)
+{
+ Box(GetParam());
+}
+
+/////////////////////////////////////////////////
+void FactoryTest::Sphere(const std::string &_physicsEngine)
+{
+ math::Pose setPose, testPose;
+ Load("worlds/empty.world", true, _physicsEngine);
+
+ for (unsigned int i = 0; i < 100; i++)
+ {
+ std::ostringstream name;
+ name << "test_sphere_" << i;
+ setPose.Set(math::Vector3(0, 0, i+0.5), math::Quaternion(0, 0, 0));
+ SpawnSphere(name.str(), setPose.pos, setPose.rot.GetAsEuler());
+ testPose = GetEntityPose(name.str());
+ EXPECT_TRUE(math::equal(testPose.pos.x, setPose.pos.x, 0.1));
+ EXPECT_TRUE(math::equal(testPose.pos.y, setPose.pos.y, 0.1));
+ EXPECT_TRUE(math::equal(testPose.pos.z, setPose.pos.z, 0.1));
+ }
+}
+
+/////////////////////////////////////////////////
+TEST_P(FactoryTest, Sphere)
+{
+ Sphere(GetParam());
+}
+
+/////////////////////////////////////////////////
+void FactoryTest::Cylinder(const std::string &_physicsEngine)
+{
+ math::Pose setPose, testPose;
+ Load("worlds/empty.world", true, _physicsEngine);
+
+ for (unsigned int i = 0; i < 100; i++)
+ {
+ std::ostringstream name;
+ name << "test_cylinder_" << i;
+ setPose.Set(math::Vector3(0, 0, i+0.5), math::Quaternion(0, 0, 0));
+ SpawnCylinder(name.str(), setPose.pos, setPose.rot.GetAsEuler());
+ testPose = GetEntityPose(name.str());
+ EXPECT_TRUE(math::equal(testPose.pos.x, setPose.pos.x, 0.1));
+ EXPECT_TRUE(math::equal(testPose.pos.y, setPose.pos.y, 0.1));
+ EXPECT_TRUE(math::equal(testPose.pos.z, setPose.pos.z, 0.1));
+ }
+}
+
+/////////////////////////////////////////////////
+TEST_P(FactoryTest, Cylinder)
+{
+ Cylinder(GetParam());
+}
+
+/////////////////////////////////////////////////
+void FactoryTest::Clone(const std::string &_physicsEngine)
+{
+ math::Pose setPose, testPose;
+ Load("worlds/pr2.world", true, _physicsEngine);
+
+ // clone the pr2
+ std::string name = "pr2";
+ msgs::Factory msg;
+ math::Pose clonePose;
+ clonePose.Set(math::Vector3(2, 3, 0.5), math::Quaternion(0, 0, 0));
+ msgs::Set(msg.mutable_pose(), clonePose);
+ msg.set_clone_model_name(name);
+ this->factoryPub->Publish(msg);
+
+ // Wait for the pr2 clone to spawn
+ std::string cloneName = name + "_clone";
+ this->WaitUntilEntitySpawn(cloneName, 100, 100);
+
+ EXPECT_TRUE(this->HasEntity(cloneName));
+ testPose = GetEntityPose(cloneName);
+ EXPECT_TRUE(math::equal(testPose.pos.x, clonePose.pos.x, 0.1));
+ EXPECT_TRUE(math::equal(testPose.pos.y, clonePose.pos.y, 0.1));
+ EXPECT_TRUE(math::equal(testPose.pos.z, clonePose.pos.z, 0.1));
+
+ // Verify properties of the pr2 clone with the original model.
+ physics::WorldPtr world = physics::get_world("default");
+ ASSERT_TRUE(world != NULL);
+
+ // Check model
+ physics::ModelPtr model = world->GetModel(name);
+ ASSERT_TRUE(model != NULL);
+ physics::ModelPtr modelClone = world->GetModel(cloneName);
+ ASSERT_TRUE(modelClone != NULL);
+ EXPECT_EQ(model->GetJointCount(), modelClone->GetJointCount());
+ EXPECT_EQ(model->GetLinks().size(), modelClone->GetLinks().size());
+ EXPECT_EQ(model->GetSensorCount(), modelClone->GetSensorCount());
+ EXPECT_EQ(model->GetPluginCount(), modelClone->GetPluginCount());
+ EXPECT_EQ(model->GetAutoDisable(), modelClone->GetAutoDisable());
+
+ // Check links
+ physics::Link_V links = model->GetLinks();
+ physics::Link_V linkClones = modelClone->GetLinks();
+ for (unsigned int i = 0; i < links.size(); ++i)
+ {
+ physics::LinkPtr link = links[i];
+ physics::LinkPtr linkClone = linkClones[i];
+
+ EXPECT_EQ(link->GetSensorCount(), linkClone->GetSensorCount());
+ EXPECT_EQ(link->GetKinematic(), linkClone->GetKinematic());
+
+ // Check collisions
+ physics::Collision_V collisions = link->GetCollisions();
+ physics::Collision_V collisionClones = linkClone->GetCollisions();
+ EXPECT_EQ(collisions.size(), collisionClones.size());
+ for (unsigned int j = 0; j < collisions.size(); ++j)
+ {
+ physics::CollisionPtr collision = collisions[j];
+ physics::CollisionPtr collisionClone = collisionClones[j];
+ EXPECT_EQ(collision->GetShapeType(), collisionClone->GetShapeType());
+ EXPECT_EQ(collision->GetMaxContacts(), collisionClone->GetMaxContacts());
+
+ // Check surface
+ physics::SurfaceParamsPtr surface = collision->GetSurface();
+ physics::SurfaceParamsPtr cloneSurface = collisionClone->GetSurface();
+ EXPECT_EQ(surface->collideWithoutContact,
+ cloneSurface->collideWithoutContact);
+ EXPECT_EQ(surface->collideWithoutContactBitmask,
+ cloneSurface->collideWithoutContactBitmask);
+ }
+
+ // Check inertial
+ physics::InertialPtr inertial = link->GetInertial();
+ physics::InertialPtr inertialClone = linkClone->GetInertial();
+ EXPECT_EQ(inertial->GetMass(), inertialClone->GetMass());
+ EXPECT_EQ(inertial->GetCoG(), inertialClone->GetCoG());
+ EXPECT_EQ(inertial->GetPrincipalMoments(),
+ inertialClone->GetPrincipalMoments());
+ EXPECT_EQ(inertial->GetProductsofInertia(),
+ inertialClone->GetProductsofInertia());
+ }
+
+ // Check joints
+ physics::Joint_V joints = model->GetJoints();
+ physics::Joint_V jointClones = modelClone->GetJoints();
+ for (unsigned int i = 0; i < joints.size(); ++i)
+ {
+ physics::JointPtr joint = joints[i];
+ physics::JointPtr jointClone = jointClones[i];
+ EXPECT_EQ(joint->GetAngleCount(), jointClone->GetAngleCount());
+ for (unsigned j = 0; j < joint->GetAngleCount(); ++j)
+ {
+ EXPECT_EQ(joint->GetUpperLimit(j), jointClone->GetUpperLimit(j));
+ EXPECT_EQ(joint->GetLowerLimit(j), jointClone->GetLowerLimit(j));
+ EXPECT_EQ(joint->GetEffortLimit(j), jointClone->GetEffortLimit(j));
+ EXPECT_EQ(joint->GetVelocityLimit(j), jointClone->GetVelocityLimit(j));
+ EXPECT_EQ(joint->GetStopStiffness(j), jointClone->GetStopStiffness(j));
+ EXPECT_EQ(joint->GetStopDissipation(j),
+ jointClone->GetStopDissipation(j));
+ EXPECT_EQ(joint->GetLocalAxis(j), jointClone->GetLocalAxis(j));
+ EXPECT_EQ(joint->GetDamping(j), jointClone->GetDamping(j));
+ }
+ }
+}
+
+/////////////////////////////////////////////////
+TEST_P(FactoryTest, Clone)
+{
+ Clone(GetParam());
+}
+
+// Disabling this test for now. Different machines return different
+// camera images. Need a better way to evaluate rendered content.
+// TEST_F(FactoryTest, Camera)
+// {
+/*
+ if (rendering::RenderEngine::Instance()->GetRenderPathType() ==
+ rendering::RenderEngine::NONE)
+ return;
+
+ math::Pose setPose, testPose;
+ Load("worlds/empty.world");
+ setPose.Set(math::Vector3(-5, 0, 5), math::Quaternion(0, GZ_DTOR(15), 0));
+ SpawnCamera("camera_model", "camera_sensor2", setPose.pos,
+ setPose.rot.GetAsEuler());
+
+ unsigned char *img = NULL;
+ unsigned int width;
+ unsigned int height;
+ GetFrame("camera_sensor2", &img, width, height);
+ ASSERT_EQ(width, static_cast<unsigned int>(320));
+ ASSERT_EQ(height, static_cast<unsigned int>(240));
+
+ unsigned int diffMax = 0;
+ unsigned int diffSum = 0;
+ double diffAvg = 0;
+ ImageCompare(&img, &empty_world_camera1,
+ width, height, 3, diffMax, diffSum, diffAvg);
+ // PrintImage("empty_world_camera1", &img, width, height, 3);
+ ASSERT_LT(diffSum, static_cast<unsigned int>(100));
+ ASSERT_EQ(static_cast<unsigned int>(0), diffMax);
+ ASSERT_EQ(0.0, diffAvg);
+ */
+// }
+
+INSTANTIATE_TEST_CASE_P(PhysicsEngines, FactoryTest, PHYSICS_ENGINE_VALUES);
+
+/////////////////////////////////////////////////
+int main(int argc, char **argv)
+{
+ ::testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/integration/imu.cc b/test/integration/imu.cc
new file mode 100644
index 0000000..d8620ae
--- /dev/null
+++ b/test/integration/imu.cc
@@ -0,0 +1,581 @@
+/*
+ * Copyright (C) 2012-2015 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#include "ServerFixture.hh"
+#include "gazebo/sensors/sensors.hh"
+#include "gazebo/common/common.hh"
+#include "helper_physics_generator.hh"
+
+// How tightly to compare for deterministic values
+#define IMU_TOL 1e-5
+
+using namespace gazebo;
+class ImuTest : public ServerFixture,
+ public testing::WithParamInterface<const char*>
+{
+ /// \brief start imu_sensor_test.world, which contains a pendulum,
+ /// a sphere with frictional contact, a sphere with frictionless
+ /// contact and a ramp. Each model has an IMU attached.
+ /// This test check results to make sure the readings adhere to
+ /// each simple model under gravity.
+ public: void ImuSensorTestWorld(const std::string &_physicsEngine);
+
+ /// \brief Spawn a static model with an ImuSensor attached
+ /// in the empty world. Test basic IMU outputs.
+ public: void Stationary_EmptyWorld(const std::string &_physicsEngine);
+
+ /// \brief Spawn a static model with an ImuSensor attached
+ /// in the empty world. Test basic IMU outputs with noise enabled.
+ public: void Stationary_EmptyWorld_Noise(const std::string &_physicsEngine);
+
+ /// \brief Spawn a static model with an ImuSensor attached
+ /// in the empty world. Test basic IMU outputs with bias enabled.
+ public: void Stationary_EmptyWorld_Bias(const std::string &_physicsEngine);
+
+ /// \breif Return gravity rotated by some orientation
+ /// \param[in] _rot User specified rotation
+ /// \param[out] _g gravity in user specified orientation
+ private: void GetGravity(const math::Quaternion& _rot, math::Vector3 &_g);
+
+ /// \breif Collect a number of samples and return the average
+ /// rate and accel values
+ /// \param[in] _imu Pointer to sensor
+ /// \param[in] _cnt number of samples to tak
+ /// \param[out] _rateMean average angular rates over samples
+ /// \param[out] _accelMean average accelerations over samples
+ /// \param[out] _orientation orientation of the imu at the end of sample
+ /// period
+ private: void GetImuData(sensors::ImuSensorPtr _imu, unsigned int _cnt,
+ math::Vector3 &_rateMean,
+ math::Vector3 &_accelMean,
+ math::Quaternion &_orientation);
+};
+
+void ImuTest::GetGravity(const math::Quaternion &_rot, math::Vector3 &_g)
+{
+ physics::WorldPtr world = physics::get_world("default");
+ ASSERT_TRUE(world != NULL);
+ physics::PhysicsEnginePtr physics = world->GetPhysicsEngine();
+ ASSERT_TRUE(physics != NULL);
+ // Rotate into IMU's frame
+ _g = _rot.GetInverse().RotateVector(physics->GetGravity());
+}
+
+void ImuTest::GetImuData(sensors::ImuSensorPtr _imu,
+ unsigned int _cnt,
+ math::Vector3 &_rateMean,
+ math::Vector3 &_accelMean,
+ math::Quaternion& _orientation)
+{
+ physics::WorldPtr world = physics::get_world("default");
+ ASSERT_TRUE(world != NULL);
+ // Collect a number of samples and return the average rate and accel values
+ math::Vector3 rateSum, accelSum;
+ for (unsigned int i = 0; i < _cnt; ++i)
+ {
+ world->Step(1);
+
+ int j = 0;
+ while (_imu->GetLastMeasurementTime() == gazebo::common::Time::Zero &&
+ j < 100)
+ {
+ _imu->Update(true);
+ gazebo::common::Time::MSleep(100);
+ ++j;
+ }
+
+ EXPECT_LT(j, 100);
+
+ rateSum += _imu->GetAngularVelocity();
+ accelSum += _imu->GetLinearAcceleration();
+ }
+ _rateMean = rateSum / _cnt;
+ _accelMean = accelSum / _cnt;
+ _orientation = _imu->GetOrientation();
+}
+
+void ImuTest::ImuSensorTestWorld(const std::string &_physicsEngine)
+{
+ if (_physicsEngine != "ode")
+ {
+ gzerr << "not working yet for anything other than ode. see issue #9999.\n";
+ return;
+ }
+
+ Load("worlds/imu_sensor_test.world", true, _physicsEngine);
+
+ // get world
+ physics::WorldPtr world = physics::get_world("default");
+ ASSERT_TRUE(world != NULL);
+
+ // get physics engine
+ physics::PhysicsEnginePtr physics = world->GetPhysicsEngine();
+ ASSERT_TRUE(physics != NULL);
+
+ // get pendulum
+ std::string pendulumName = "model_pendulum";
+ physics::ModelPtr pendulumModel = world->GetModel(pendulumName);
+ ASSERT_TRUE(pendulumModel != NULL);
+
+ std::string pendulumSensorName = "pendulum_imu_sensor";
+ sensors::ImuSensorPtr pendulumImu =
+ boost::static_pointer_cast<sensors::ImuSensor>(
+ sensors::SensorManager::Instance()->GetSensor(pendulumSensorName));
+ ASSERT_TRUE(pendulumImu != NULL);
+ pendulumImu->Init();
+
+ // get friction ball
+ std::string ballFrictionName = "model_ball";
+ physics::ModelPtr ballFrictionModel = world->GetModel(ballFrictionName);
+ ASSERT_TRUE(ballFrictionModel != NULL);
+
+ std::string ballFrictionSensorName = "ball_imu_sensor";
+ sensors::ImuSensorPtr ballFrictionImu =
+ boost::static_pointer_cast<sensors::ImuSensor>(
+ sensors::SensorManager::Instance()->GetSensor(ballFrictionSensorName));
+ ASSERT_TRUE(ballFrictionImu != NULL);
+ ballFrictionImu->Init();
+
+ // get frictionless ball
+ std::string ballNoFrictionName = "model_ball_no_friction";
+ physics::ModelPtr ballNoFrictionModel = world->GetModel(ballNoFrictionName);
+ ASSERT_TRUE(ballNoFrictionModel != NULL);
+
+ std::string ballNoFrictionSensorName = "ball_no_friction_imu_sensor";
+ sensors::ImuSensorPtr ballNoFrictionImu =
+ boost::static_pointer_cast<sensors::ImuSensor>(
+ sensors::SensorManager::Instance()->GetSensor(ballNoFrictionSensorName));
+ ASSERT_TRUE(ballNoFrictionImu != NULL);
+ ballNoFrictionImu->Init();
+
+ // get gravity
+ math::Vector3 g = physics->GetGravity();
+
+ // run for 1900 steps (Step(1) each time), or 1.9 seconds, enough
+ // to capture what we need from this experiment.
+ for (unsigned n = 0; n < 1900; ++n)
+ {
+ world->Step(1);
+ // gzdbg << "time: " << world->GetSimTime().Double() << "\n";
+
+ // pendulum
+ // on startup
+ // sensor linear accel [0 0 0]
+ // Link::GetRelativeLinearAccel() [0 0 -9.81]
+ // Link::GetWorldLinearAccel() [0 0 -9.81]
+ // @T=1.872 sec, at max lowest position
+ // sensor linear accel [-0 -0.041216 29.4258]
+ // Link::GetRelativeLinearAccel() [-0 -0.008923 19.6159]
+ // Link::GetWorldLinearAccel() [-0 0.055649 19.6158]
+ {
+ // get states from imu sensor
+ math::Vector3 imuLinearAccel = pendulumImu->GetLinearAcceleration();
+ // get states from link
+ math::Vector3 relativeLinearAccel =
+ pendulumModel->GetRelativeLinearAccel();
+ math::Vector3 worldLinearAccel =
+ pendulumModel->GetWorldLinearAccel();
+
+ if (world->GetSimTime().Double() == 1.872)
+ {
+ // initial values
+ EXPECT_NEAR(imuLinearAccel.x, 0, IMU_TOL);
+ EXPECT_NEAR(imuLinearAccel.y, -0.041216, IMU_TOL);
+ EXPECT_NEAR(imuLinearAccel.z, 29.42581726, IMU_TOL);
+ EXPECT_NEAR(relativeLinearAccel.x, 0, IMU_TOL);
+ EXPECT_NEAR(relativeLinearAccel.y, -0.036397, IMU_TOL);
+ EXPECT_NEAR(relativeLinearAccel.z, 19.6158848, IMU_TOL);
+ EXPECT_NEAR(worldLinearAccel.x, 0, IMU_TOL);
+ EXPECT_NEAR(worldLinearAccel.y, -0.0267709, IMU_TOL);
+ EXPECT_NEAR(worldLinearAccel.z, 19.6159003, IMU_TOL);
+ }
+ else
+ {
+ // initial values
+ EXPECT_LE(imuLinearAccel.z, 29.4259);
+ EXPECT_LE(relativeLinearAccel.z, 19.616);
+ EXPECT_LE(worldLinearAccel.z, 19.616);
+ }
+ }
+
+ // friction ball
+ // before contact
+ // sensor linear accel [0 0 0]
+ // Link::GetRelativeLinearAccel() [0 0 -9.81]
+ // Link::GetWorldLinearAccel() [0 0 -9.81]
+ //
+ // @T=1.2 sec, on ramp - varies, e.g.
+ // sensor linear accel [-7.81558 0 3.71003]
+ // Link::GetRelativeLinearAccel() [1.98569 0 3.29613]
+ // Link::GetWorldLinearAccel() [3.37698 0 -1.84485]
+ //
+ // @T=1.849 sec, on ground - sensor vector rotates
+ // sensor linear accel [-2.93844 0 9.35958]
+ // Link::GetRelativeLinearAccel() [0 0 0]
+ // Link::GetWorldLinearAccel() [0 0 0]
+ {
+ // get states from imu sensor
+ math::Vector3 imuLinearAccel = ballFrictionImu->GetLinearAcceleration();
+ // get states from link
+ math::Vector3 relativeLinearAccel =
+ ballFrictionModel->GetRelativeLinearAccel();
+ math::Vector3 worldLinearAccel =
+ ballFrictionModel->GetWorldLinearAccel();
+
+ if (world->GetSimTime().Double() <= 1.0)
+ {
+ // freefall
+ EXPECT_NEAR(imuLinearAccel.x, 0, IMU_TOL);
+ EXPECT_NEAR(imuLinearAccel.y, 0, IMU_TOL);
+ EXPECT_NEAR(imuLinearAccel.z, 0, IMU_TOL);
+ EXPECT_NEAR(relativeLinearAccel.x, g.x, IMU_TOL);
+ EXPECT_NEAR(relativeLinearAccel.y, g.y, IMU_TOL);
+ EXPECT_NEAR(relativeLinearAccel.z, g.z, IMU_TOL);
+ EXPECT_NEAR(worldLinearAccel.x, g.x, IMU_TOL);
+ EXPECT_NEAR(worldLinearAccel.y, g.y, IMU_TOL);
+ EXPECT_NEAR(worldLinearAccel.z, g.z, IMU_TOL);
+ }
+ // should use contact detector for these timing stuff
+ else if (world->GetSimTime().Double() >= 1.2 &&
+ world->GetSimTime().Double() <= 1.84)
+ {
+ // on ramp
+ // ...hm, not much can be said in simple terms, leave out for now.
+ }
+ else if (world->GetSimTime().Double() >= 1.85)
+ {
+ // on the ground
+ double imuMag = imuLinearAccel.GetLength();
+ double gMag = g.GetLength();
+ EXPECT_NEAR(imuMag, gMag, IMU_TOL);
+ EXPECT_NEAR(relativeLinearAccel.x, 0, IMU_TOL);
+ EXPECT_NEAR(relativeLinearAccel.y, 0, IMU_TOL);
+ EXPECT_NEAR(relativeLinearAccel.z, 0, IMU_TOL);
+ EXPECT_NEAR(worldLinearAccel.x, 0, IMU_TOL);
+ EXPECT_NEAR(worldLinearAccel.y, 0, IMU_TOL);
+ EXPECT_NEAR(worldLinearAccel.z, 0, IMU_TOL);
+ }
+ }
+
+ // frictionless ball
+ // before contact
+ // sensor linear accel [0 0 0]
+ // Link::GetRelativeLinearAccel() [0 0 -9.81]
+ // Link::GetWorldLinearAccel() [0 0 -9.81]
+ // @T=1.2 sec, on ramp - constant
+ // sensor linear accel [4.12742 0 7.55518]
+ // Link::GetRelativeLinearAccel() [4.12742 0 -2.25482]
+ // Link::GetWorldLinearAccel() [4.12742 0 -2.25482]
+ // @T=1.8 sec, on ground - constant
+ // sensor linear accel [0 0 9.81]
+ // Link::GetRelativeLinearAccel() [0 0 0]
+ // Link::GetWorldLinearAccel() [0 0 0]
+ {
+ // get states from imu sensor
+ math::Vector3 imuLinearAccel =
+ ballNoFrictionImu->GetLinearAcceleration();
+ // get states from link
+ math::Vector3 relativeLinearAccel =
+ ballNoFrictionModel->GetRelativeLinearAccel();
+ math::Vector3 worldLinearAccel =
+ ballNoFrictionModel->GetWorldLinearAccel();
+
+ if (world->GetSimTime().Double() <= 1.0)
+ {
+ // freefall
+ EXPECT_NEAR(imuLinearAccel.x, 0, IMU_TOL);
+ EXPECT_NEAR(imuLinearAccel.y, 0, IMU_TOL);
+ EXPECT_NEAR(imuLinearAccel.z, 0, IMU_TOL);
+ EXPECT_NEAR(relativeLinearAccel.x, g.x, IMU_TOL);
+ EXPECT_NEAR(relativeLinearAccel.y, g.y, IMU_TOL);
+ EXPECT_NEAR(relativeLinearAccel.z, g.z, IMU_TOL);
+ EXPECT_NEAR(worldLinearAccel.x, g.x, IMU_TOL);
+ EXPECT_NEAR(worldLinearAccel.y, g.y, IMU_TOL);
+ EXPECT_NEAR(worldLinearAccel.z, g.z, IMU_TOL);
+ }
+ else if (world->GetSimTime().Double() >= 1.3 &&
+ world->GetSimTime().Double() <= 1.751)
+ {
+ // on the ramp
+ const double rampAngle = 0.5;
+ double gMag = g.GetLength();
+ double imuMag = imuLinearAccel.GetLength();
+ EXPECT_NEAR(imuMag, gMag*cos(rampAngle), IMU_TOL);
+
+ double relMag = relativeLinearAccel.GetLength();
+ EXPECT_NEAR(relMag, gMag*sin(rampAngle), IMU_TOL);
+ double worMag = worldLinearAccel.GetLength();
+ EXPECT_NEAR(worMag, gMag*sin(rampAngle), IMU_TOL);
+ }
+ else if (world->GetSimTime().Double() >= 1.8)
+ {
+ // on the ground
+ double imuMag = imuLinearAccel.GetLength();
+ double gMag = g.GetLength();
+ EXPECT_NEAR(imuMag, gMag, IMU_TOL);
+ EXPECT_NEAR(relativeLinearAccel.x, 0, IMU_TOL);
+ EXPECT_NEAR(relativeLinearAccel.y, 0, IMU_TOL);
+ EXPECT_NEAR(relativeLinearAccel.z, 0, IMU_TOL);
+ EXPECT_NEAR(worldLinearAccel.x, 0, IMU_TOL);
+ EXPECT_NEAR(worldLinearAccel.y, 0, IMU_TOL);
+ EXPECT_NEAR(worldLinearAccel.z, 0, IMU_TOL);
+ }
+ }
+ }
+}
+
+TEST_P(ImuTest, ImuSensorTestWorld)
+{
+ ImuSensorTestWorld(GetParam());
+}
+
+void ImuTest::Stationary_EmptyWorld(const std::string &_physicsEngine)
+{
+ // static models not fully working in simbody yet
+ if (_physicsEngine == "simbody")
+ {
+ gzerr << "Aborting test for Simbody, see issue #860.\n";
+ return;
+ }
+
+ Load("worlds/empty.world", true, _physicsEngine);
+
+ std::string modelName = "imu_model";
+ std::string imuSensorName = "imu_sensor";
+ math::Pose testPose(math::Vector3(0, 0, 0.05),
+ math::Quaternion(0.5, -1.0, 0.2));
+
+ SpawnImuSensor(modelName, imuSensorName, testPose.pos,
+ testPose.rot.GetAsEuler());
+
+ sensors::ImuSensorPtr imu =
+ boost::static_pointer_cast<sensors::ImuSensor>(
+ sensors::SensorManager::Instance()->GetSensor(imuSensorName));
+
+ ASSERT_TRUE(imu != NULL);
+ imu->Init();
+ math::Vector3 rateMean, accelMean;
+ math::Quaternion orientation;
+ this->GetImuData(imu, 1, rateMean, accelMean, orientation);
+
+ EXPECT_NEAR(rateMean.x, 0.0, IMU_TOL);
+ EXPECT_NEAR(rateMean.y, 0.0, IMU_TOL);
+ EXPECT_NEAR(rateMean.z, 0.0, IMU_TOL);
+
+ math::Vector3 g;
+ this->GetGravity(testPose.rot, g);
+ EXPECT_NEAR(accelMean.x, -g.x, IMU_TOL);
+ EXPECT_NEAR(accelMean.y, -g.y, IMU_TOL);
+ EXPECT_NEAR(accelMean.z, -g.z, IMU_TOL);
+
+ // Orientation should be identity, since it is reported relative
+ // to reference pose.
+ EXPECT_NEAR(orientation.x, 0, IMU_TOL);
+ EXPECT_NEAR(orientation.y, 0, IMU_TOL);
+ EXPECT_NEAR(orientation.z, 0, IMU_TOL);
+ EXPECT_NEAR(orientation.w, 1, IMU_TOL);
+}
+
+TEST_P(ImuTest, EmptyWorld)
+{
+ Stationary_EmptyWorld(GetParam());
+}
+
+void ImuTest::Stationary_EmptyWorld_Noise(const std::string &_physicsEngine)
+{
+ // static models not fully working in simbody yet
+ if (_physicsEngine == "simbody")
+ {
+ gzerr << "Aborting test for Simbody, see issue #860.\n";
+ return;
+ }
+
+ Load("worlds/empty.world", true, _physicsEngine);
+
+ std::string modelName = "imu_model";
+ std::string imuSensorName = "imu_sensor";
+ math::Pose testPose(math::Vector3(0, 0, 0.05),
+ math::Quaternion(0.3, -1.4, 2.0));
+
+ double rateNoiseMean = 1.0;
+ double rateNoiseStddev = 0.1;
+ double rateBiasMean = 0.0;
+ double rateBiasStddev = 0.0;
+ double accelNoiseMean = -10.0;
+ double accelNoiseStddev = 0.1;
+ double accelBiasMean = 0.0;
+ double accelBiasStddev = 0.0;
+ SpawnImuSensor(modelName, imuSensorName, testPose.pos,
+ testPose.rot.GetAsEuler(), "gaussian",
+ rateNoiseMean, rateNoiseStddev,
+ rateBiasMean, rateBiasStddev,
+ accelNoiseMean, accelNoiseStddev,
+ accelBiasMean, accelBiasStddev);
+
+ sensors::ImuSensorPtr imu =
+ boost::static_pointer_cast<sensors::ImuSensor>(
+ sensors::SensorManager::Instance()->GetSensor(imuSensorName));
+
+ ASSERT_TRUE(imu != NULL);
+ imu->Init();
+ math::Vector3 rateMean, accelMean;
+ math::Quaternion orientation;
+ this->GetImuData(imu, 1000, rateMean, accelMean, orientation);
+
+ double d1, d2;
+ // Have to account for the fact that the bias might be sampled as positive
+ // or negative
+ d1 = fabs(rateMean.x - (rateNoiseMean + rateBiasMean));
+ d2 = fabs(rateMean.x - (rateNoiseMean - rateBiasMean));
+ EXPECT_NEAR(0.0, std::min(d1, d2),
+ 3*rateNoiseStddev + 3*rateBiasStddev);
+ d1 = fabs(rateMean.y - (rateNoiseMean + rateBiasMean));
+ d2 = fabs(rateMean.y - (rateNoiseMean - rateBiasMean));
+ EXPECT_NEAR(0.0, std::min(d1, d2),
+ 3*rateNoiseStddev + 3*rateBiasStddev);
+ d1 = fabs(rateMean.z - (rateNoiseMean + rateBiasMean));
+ d2 = fabs(rateMean.z - (rateNoiseMean - rateBiasMean));
+ EXPECT_NEAR(0.0, std::min(d1, d2),
+ 3*rateNoiseStddev + 3*rateBiasStddev);
+
+ math::Vector3 g;
+ this->GetGravity(testPose.rot, g);
+ // Have to account for the fact that the bias might be sampled as positive
+ // or negative
+ d1 = fabs(accelMean.x - (accelNoiseMean + accelBiasMean) + g.x);
+ d2 = fabs(accelMean.x - (accelNoiseMean - accelBiasMean) + g.x);
+ EXPECT_NEAR(0.0, std::min(d1, d2),
+ 3*accelNoiseStddev + 3*accelBiasStddev);
+ d1 = fabs(accelMean.y - (accelNoiseMean + accelBiasMean) + g.y);
+ d2 = fabs(accelMean.y - (accelNoiseMean - accelBiasMean) + g.y);
+ EXPECT_NEAR(0.0, std::min(d1, d2),
+ 3*accelNoiseStddev + 3*accelBiasStddev);
+ d1 = fabs(accelMean.z - (accelNoiseMean + accelBiasMean) + g.z);
+ d2 = fabs(accelMean.z - (accelNoiseMean - accelBiasMean) + g.z);
+ EXPECT_NEAR(0.0, std::min(d1, d2),
+ 3*accelNoiseStddev + 3*accelBiasStddev);
+
+ // Orientation should be identity, since it is reported relative
+ // to reference pose.
+ EXPECT_NEAR(orientation.x, 0, IMU_TOL);
+ EXPECT_NEAR(orientation.y, 0, IMU_TOL);
+ EXPECT_NEAR(orientation.z, 0, IMU_TOL);
+ EXPECT_NEAR(orientation.w, 1, IMU_TOL);
+}
+
+TEST_P(ImuTest, EmptyWorldNoise)
+{
+ Stationary_EmptyWorld_Noise(GetParam());
+}
+
+void ImuTest::Stationary_EmptyWorld_Bias(const std::string &_physicsEngine)
+{
+ // static models not fully working in simbody yet
+ if (_physicsEngine == "simbody")
+ {
+ gzerr << "Aborting test for Simbody, see issue #860.\n";
+ return;
+ }
+
+ Load("worlds/empty.world", true, _physicsEngine);
+
+ std::string modelName = "imu_model";
+ std::string imuSensorName = "imu_sensor";
+ math::Pose testPose(math::Vector3(0, 0, 0.05),
+ math::Quaternion(-0.3, 0.5, 1.0));
+
+ double rateNoiseMean = 0.0;
+ double rateNoiseStddev = 0.0;
+ double rateBiasMean = 1.0;
+ double rateBiasStddev = 0.1;
+ double accelNoiseMean = 0.0;
+ double accelNoiseStddev = 0.0;
+ double accelBiasMean = 5.0;
+ double accelBiasStddev = 0.1;
+ SpawnImuSensor(modelName, imuSensorName, testPose.pos,
+ testPose.rot.GetAsEuler(), "gaussian",
+ rateNoiseMean, rateNoiseStddev,
+ rateBiasMean, rateBiasStddev,
+ accelNoiseMean, accelNoiseStddev,
+ accelBiasMean, accelBiasStddev);
+
+ sensors::ImuSensorPtr imu =
+ boost::static_pointer_cast<sensors::ImuSensor>(
+ sensors::SensorManager::Instance()->GetSensor(imuSensorName));
+
+ ASSERT_TRUE(imu != NULL);
+ imu->Init();
+ math::Vector3 rateMean, accelMean;
+ math::Quaternion orientation;
+ this->GetImuData(imu, 1000, rateMean, accelMean, orientation);
+
+ double d1, d2;
+ // Have to account for the fact that the bias might be sampled as positive
+ // or negative
+ d1 = fabs(rateMean.x - (rateNoiseMean + rateBiasMean));
+ d2 = fabs(rateMean.x - (rateNoiseMean - rateBiasMean));
+ EXPECT_NEAR(0.0, std::min(d1, d2),
+ 3*rateNoiseStddev + 3*rateBiasStddev);
+ d1 = fabs(rateMean.y - (rateNoiseMean + rateBiasMean));
+ d2 = fabs(rateMean.y - (rateNoiseMean - rateBiasMean));
+ EXPECT_NEAR(0.0, std::min(d1, d2),
+ 3*rateNoiseStddev + 3*rateBiasStddev);
+ d1 = fabs(rateMean.z - (rateNoiseMean + rateBiasMean));
+ d2 = fabs(rateMean.z - (rateNoiseMean - rateBiasMean));
+ EXPECT_NEAR(0.0, std::min(d1, d2),
+ 3*rateNoiseStddev + 3*rateBiasStddev);
+
+ math::Vector3 g;
+ this->GetGravity(testPose.rot, g);
+ // Have to account for the fact that the bias might be sampled as positive
+ // or negative
+ d1 = fabs(accelMean.x - (accelNoiseMean + accelBiasMean) + g.x);
+ d2 = fabs(accelMean.x - (accelNoiseMean - accelBiasMean) + g.x);
+ EXPECT_NEAR(0.0, std::min(d1, d2),
+ 3*accelNoiseStddev + 3*accelBiasStddev);
+ d1 = fabs(accelMean.y - (accelNoiseMean + accelBiasMean) + g.y);
+ d2 = fabs(accelMean.y - (accelNoiseMean - accelBiasMean) + g.y);
+ EXPECT_NEAR(0.0, std::min(d1, d2),
+ 3*accelNoiseStddev + 3*accelBiasStddev);
+ d1 = fabs(accelMean.z - (accelNoiseMean + accelBiasMean) + g.z);
+ d2 = fabs(accelMean.z - (accelNoiseMean - accelBiasMean) + g.z);
+ EXPECT_NEAR(0.0, std::min(d1, d2),
+ 3*accelNoiseStddev + 3*accelBiasStddev);
+
+ // Orientation should be identity, since it is reported relative
+ // to reference pose.
+ EXPECT_NEAR(orientation.x, 0, IMU_TOL);
+ EXPECT_NEAR(orientation.y, 0, IMU_TOL);
+ EXPECT_NEAR(orientation.z, 0, IMU_TOL);
+ EXPECT_NEAR(orientation.w, 1, IMU_TOL);
+}
+
+TEST_P(ImuTest, EmptyWorldBias)
+{
+ Stationary_EmptyWorld_Bias(GetParam());
+}
+
+INSTANTIATE_TEST_CASE_P(PhysicsEngines, ImuTest, PHYSICS_ENGINE_VALUES);
+
+int main(int argc, char **argv)
+{
+ // Set a specific seed to avoid occasional test failures due to
+ // statistically unlikely, but possible results.
+ math::Rand::SetSeed(42);
+ ::testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
--
Alioth's /usr/local/bin/git-commit-notice on /srv/git.debian.org/git/debian-science/packages/gazebo.git
More information about the debian-science-commits
mailing list