Joint_TEST.hh

Go to the documentation of this file.

/*
 * Copyright (C) 2012-2014 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 _JOINT_TEST_HH_
#define _JOINT_TEST_HH_

#include <string>
#include <sstream>

#include "test/ServerFixture.hh"

#include "gazebo/common/Time.hh"
#include "gazebo/physics/physics.hh"

using namespace gazebo;

typedef std::tr1::tuple<const char *, const char *> std_string2;

class Joint_TEST : public ServerFixture,
                   public ::testing::WithParamInterface<std_string2>
{
  protected: Joint_TEST() : ServerFixture(), spawnCount(0)
             {
             }

  public: void ForceTorque1(const std::string &_physicsEngine);

  public: void ForceTorque2(const std::string &_physicsEngine);

  public: void GetForceTorqueWithAppliedForce(
    const std::string &_physicsEngine);

  public: void JointTorqueTest(const std::string &_physicsEngine);

  public: void JointCreationDestructionTest(const std::string &_physicsEngine);

  public: virtual void SetUp()
          {
            const ::testing::TestInfo *const test_info =
              ::testing::UnitTest::GetInstance()->current_test_info();
            if (test_info->value_param())
            {
              gzdbg << "Params: " << test_info->value_param() << std::endl;
              this->physicsEngine = std::tr1::get<0>(GetParam());
              this->jointType = std::tr1::get<1>(GetParam());
            }
          }

  public: void SpawnJointTypes(const std::string &_physicsEngine,
                               const std::string &_jointType);

  public: void SpawnJointRotational(const std::string &_physicsEngine,
                                    const std::string &_jointType);

  public: void SpawnJointRotationalWorld(const std::string &_physicsEngine,
                                         const std::string &_jointType);

  public: class SpawnJointOptions
  {
    public: SpawnJointOptions() : worldChild(false), worldParent(false),
              wait(common::Time(99, 0)),
              noLinkPose(false), axis(math::Vector3(1, 0, 0))
            {
            }

    public: ~SpawnJointOptions()
            {
            }

    public: std::string type;

    public: bool worldChild;

    public: bool worldParent;

    public: common::Time wait;

    public: math::Pose modelPose;

    public: math::Pose childLinkPose;

    public: math::Pose parentLinkPose;

    public: bool noLinkPose;

    public: math::Pose jointPose;

    public: math::Vector3 axis;
  };

  public: physics::JointPtr SpawnJoint(const std::string &_type,
                                       bool _worldChild = false,
                                       bool _worldParent = false,
                                   common::Time _wait = common::Time(99, 0))
          {
            SpawnJointOptions opt;
            opt.type = _type;
            opt.worldChild = _worldChild;
            opt.worldParent = _worldParent;
            opt.wait = _wait;
            return SpawnJoint(opt);
          }

  public: physics::JointPtr SpawnJoint(const SpawnJointOptions &_opt)
          {
            msgs::Factory msg;
            std::ostringstream modelStr;
            std::ostringstream modelName;
            modelName << "joint_model" << this->spawnCount++;

            modelStr
              << "<sdf version='" << SDF_VERSION << "'>"
              << "<model name ='" << modelName.str() << "'>"
              << "  <pose>" << _opt.modelPose << "</pose>";
            if (!_opt.worldParent)
            {
              modelStr << "  <link name='parent'>";
              if (!_opt.noLinkPose)
              {
                modelStr << "    <pose>" << _opt.parentLinkPose << "</pose>";
              }
              modelStr << "  </link>";
            }
            if (!_opt.worldChild)
            {
              modelStr << "  <link name='child'>";
              if (!_opt.noLinkPose)
              {
                modelStr << "    <pose>" << _opt.childLinkPose << "</pose>";
              }
              modelStr << "  </link>";
            }
            modelStr
              << "  <joint name='joint' type='" << _opt.type << "'>"
              << "    <pose>" << _opt.jointPose << "</pose>";
            if (_opt.worldParent)
              modelStr << "    <parent>world</parent>";
            else
              modelStr << "    <parent>parent</parent>";
            if (_opt.worldChild)
              modelStr << "    <child>world</child>";
            else
              modelStr << "    <child>child</child>";
            modelStr
              << "    <axis>"
              << "      <xyz>" << _opt.axis << "</xyz>"
              << "    </axis>";
            modelStr
              << "  </joint>"
              << "</model>";

            msg.set_sdf(modelStr.str());
            this->factoryPub->Publish(msg);

            physics::JointPtr joint;
            if (_opt.wait != common::Time::Zero)
            {
              common::Time wallStart = common::Time::GetWallTime();
              unsigned int waitCount = 0;
              while (_opt.wait > (common::Time::GetWallTime() - wallStart) &&
                     !this->HasEntity(modelName.str()))
              {
                common::Time::MSleep(100);
                if (++waitCount % 10 == 0)
                {
                  gzwarn << "Waiting " << waitCount / 10 << " seconds for "
                         << _opt.type << " joint to spawn." << std::endl;
                }
              }
              if (this->HasEntity(modelName.str()) && waitCount >= 10)
                gzwarn << _opt.type << " joint has spawned." << std::endl;

              physics::WorldPtr world = physics::get_world("default");
              if (world != NULL)
              {
                physics::ModelPtr model = world->GetModel(modelName.str());
                if (model != NULL)
                  joint = model->GetJoint("joint");
              }
            }
            return joint;
          }

  protected: std::string physicsEngine;

  protected: std::string jointType;

  private: unsigned int spawnCount;
};
#endif