Commits

Ian Chen committed 4915e08

Changes based on pull request feedback

  • Participants
  • Parent commits 7b3a4fb
  • Branches issue_495

Comments (0)

Files changed (4)

File gazebo/physics/bullet/BulletRayShape.cc

  * Date: 24 May 2009
  */
 
+#include "gazebo/common/Assert.hh"
+
 #include "gazebo/physics/World.hh"
 #include "gazebo/physics/bullet/BulletLink.hh"
 #include "gazebo/physics/bullet/BulletPhysics.hh"
     BulletCollisionPtr collision =
       boost::shared_static_cast<BulletCollision>(this->collisionParent);
 
-    this->globalStartPos =
-      this->collisionParent->GetLink()->GetWorldPose().CoordPositionAdd(
+    LinkPtr link = this->collisionParent->GetLink();
+    GZ_ASSERT(link != NULL, "Bullet link is NULL");
+
+    this->globalStartPos = link->GetWorldPose().CoordPositionAdd(
           this->relativeStartPos);
 
-    this->globalEndPos =
-      this->collisionParent->GetLink()->GetWorldPose().CoordPositionAdd(
+    this->globalEndPos = link->GetWorldPose().CoordPositionAdd(
           this->relativeEndPos);
   }
 
       this->globalEndPos.z);
 
   btCollisionWorld::ClosestRayResultCallback rayCallback(start, end);
-  this->physicsEngine->GetPhysicsUpdateMutex()->lock();
+
+  boost::recursive_mutex::scoped_lock lock(
+      *this->physicsEngine->GetPhysicsUpdateMutex());
+
   this->physicsEngine->GetDynamicsWorld()->rayTest(
       start, end, rayCallback);
 
                          rayCallback.m_hitPointWorld.getZ());
     this->SetLength(this->globalStartPos.Distance(result));
   }
-  this->physicsEngine->GetPhysicsUpdateMutex()->unlock();
 }
 
 //////////////////////////////////////////////////
                            rayCallback.m_hitPointWorld.getY(),
                            rayCallback.m_hitPointWorld.getZ());
       _dist = this->globalStartPos.Distance(result);
-      _entity = static_cast<BulletLink*>(
-          rayCallback.m_collisionObject->getUserPointer())->GetName();
+
+      BulletLink *link = static_cast<BulletLink *>(
+          rayCallback.m_collisionObject->getUserPointer());
+      GZ_ASSERT(link != NULL, "Bullet link is NULL");
+      _entity = link->GetName();
     }
   }
 }

File gazebo/physics/bullet/BulletRayShape.hh

       public: void SetPoints(const math::Vector3 &_posStart,
                              const math::Vector3 &_posEnd);
 
+      /// \brief Pointer to the Bullet physics engine
       private: BulletPhysicsPtr physicsEngine;
     };
     /// \}

File test/ServerFixture.hh

                msg.set_sdf(newModelStr.str());
                this->factoryPub->Publish(msg);
 
+               int i = 0;
                // Wait for the entity to spawn
-               while (!this->HasEntity(_modelName))
-                 common::Time::MSleep(10);
+               while (!this->HasEntity(_modelName) && i < 50)
+               {
+                 common::Time::MSleep(20);
+                 ++i;
+               }
+               EXPECT_LT(i, 50);
              }
 
   protected: void SpawnCylinder(const std::string &_name,

File test/regression/laser.cc

 #include "common/common.hh"
 #include "scans_cmp.h"
 
-#define LASER_TOL 1e-2
+#define LASER_TOL 1e-5
 #define DOUBLE_TOL 1e-6
 
 using namespace gazebo;
 class LaserTest : public ServerFixture
 {
   public: void Stationary_EmptyWorld(const std::string &_physicsEngine);
-  public: void LaserUnitCylinder(const std::string &_physicsEngine);
+  public: void LaserUnitBox(const std::string &_physicsEngine);
 };
 
 void LaserTest::Stationary_EmptyWorld(const std::string &_physicsEngine)
 //  Stationary_EmptyWorld("bullet");
 }
 
-void LaserTest::LaserUnitCylinder(const std::string &_physicsEngine)
+void LaserTest::LaserUnitBox(const std::string &_physicsEngine)
 {
-  // Test ray sensor with 3 cylinders in the world.
-  // First place 2 of 3 cylinders within range and verify range values,
-  // then move all 3 cylinder out of range and verify range values
+  // Test ray sensor with 3 boxes in the world.
+  // First place 2 of 3 boxes within range and verify range values,
+  // then move all 3 boxes out of range and verify range values
 
   Load("worlds/empty.world", true, _physicsEngine);
 
   std::string raySensorName = "ray_sensor";
   double hMinAngle = -M_PI/2.0;
   double hMaxAngle = M_PI/2.0;
-  double minRange = 0.1;
+  double minRange = 0.0;
   double maxRange = 5.0;
   double rangeResolution = 0.02;
   unsigned int samples = 320;
       testPose.rot.GetAsEuler(), hMinAngle, hMaxAngle, minRange, maxRange,
       rangeResolution, samples);
 
-  std::string cylinder01 = "cylinder_01";
-  std::string cylinder02 = "cylinder_02";
-  std::string cylinder03 = "cylinder_03";
+  std::string box01 = "box_01";
+  std::string box02 = "box_02";
+  std::string box03 = "box_03";
 
-  // cylinder in front of ray sensor
-  math::Pose cylinder01Pose(math::Vector3(1, 0, 0),
-      math::Quaternion(0, 0, 0));
-  // cylinder on the right of ray sensor
-  math::Pose cylinder02Pose(math::Vector3(0, -1, 0),
-      math::Quaternion(0, 0, 0));
-  // cylinder on the left of the ray sensor but out of range
-  math::Pose cylinder03Pose(math::Vector3(0, maxRange + 1, 0),
+  // box in front of ray sensor
+  math::Pose box01Pose(math::Vector3(1, 0, 0.5), math::Quaternion(0, 0, 0));
+  // box on the right of ray sensor
+  math::Pose box02Pose(math::Vector3(0, -1, 0.5), math::Quaternion(0, 0, 0));
+  // box on the left of the ray sensor but out of range
+  math::Pose box03Pose(math::Vector3(0, maxRange + 1, 0.5),
       math::Quaternion(0, 0, 0));
 
-  SpawnCylinder(cylinder01, cylinder01Pose.pos,
-      cylinder01Pose.rot.GetAsEuler());
+  SpawnBox(box01, math::Vector3(1, 1, 1), box01Pose.pos,
+      box01Pose.rot.GetAsEuler());
 
-  SpawnCylinder(cylinder02, cylinder02Pose.pos,
-      cylinder02Pose.rot.GetAsEuler());
+  SpawnBox(box02, math::Vector3(1, 1, 1), box02Pose.pos,
+      box02Pose.rot.GetAsEuler());
 
-  SpawnCylinder(cylinder03, cylinder03Pose.pos,
-      cylinder03Pose.rot.GetAsEuler());
+  SpawnBox(box03, math::Vector3(1, 1, 1), box03Pose.pos,
+      box03Pose.rot.GetAsEuler());
 
   sensors::SensorPtr sensor = sensors::get_sensor(raySensorName);
   sensors::RaySensorPtr raySensor =
   raySensor->Update(true);
 
   int mid = samples / 2;
-  double unitCylinderRadius = 0.5;
-  double expectedRangeAtMidPoint = cylinder01Pose.pos.x - unitCylinderRadius;
+  double unitBoxSize = 1.0;
+  double expectedRangeAtMidPoint = box01Pose.pos.x - unitBoxSize/2;
 
   // WARNING: gazebo returns distance to object from min range
   // issue #503
   // than +inf. issue #124
   EXPECT_NEAR(raySensor->GetRange(samples-1), maxRange, LASER_TOL);
 
-  // Move all cylinders out of range
+  // Move all boxes out of range
   physics::WorldPtr world = physics::get_world("default");
-  ASSERT_TRUE(world !=NULL);
+  ASSERT_TRUE(world != NULL);
 
-  world->GetModel(cylinder01)->SetWorldPose(
-        math::Pose(math::Vector3(maxRange + 1, 0, 0),
-        math::Quaternion(0, 0, 0)));
-  world->GetModel(cylinder02)->SetWorldPose(
-        math::Pose(math::Vector3(0, -(maxRange + 1), 0),
-        math::Quaternion(0, 0, 0)));
+  world->GetModel(box01)->SetWorldPose(
+      math::Pose(math::Vector3(maxRange + 1, 0, 0), math::Quaternion(0, 0, 0)));
+  world->GetModel(box02)->SetWorldPose(
+      math::Pose(math::Vector3(0, -(maxRange + 1), 0),
+      math::Quaternion(0, 0, 0)));
   world->StepWorld(1);
   raySensor->Update(true);
 
   }
 }
 
-TEST_F(LaserTest, LaserCylindereODE)
+TEST_F(LaserTest, LaserBoxODE)
 {
-  LaserUnitCylinder("ode");
+  LaserUnitBox("ode");
 }
 
-TEST_F(LaserTest, LaserCylinderBullet)
+#ifdef HAVE_BULLET
+TEST_F(LaserTest, LaserBoxBullet)
 {
-  LaserUnitCylinder("bullet");
+  LaserUnitBox("bullet");
 }
+#endif  // HAVE_BULLET
 
 int main(int argc, char **argv)
 {