Esempio n. 1
0
  virtual void SetUp()
  {
    std::string resource_dir = ros::package::getPath("moveit_resources");
    if(resource_dir == "")
    {
      FAIL() << "Failed to find package moveit_resources.";
      return;
    }
    boost::filesystem::path res_path(resource_dir);

    srdf_model.reset(new srdf::Model());
    std::string xml_string;
    std::fstream xml_file((res_path / "test/urdf/robot.xml").string().c_str(), std::fstream::in);
    if (xml_file.is_open())
    {
      while (xml_file.good())
      {
        std::string line;
        std::getline(xml_file, line);
        xml_string += (line + "\n");
      }
      xml_file.close();
      urdf_model = urdf::parseURDF(xml_string);
    }
    srdf_model->initFile(*urdf_model, (res_path / "test/srdf/robot.xml").string());
    robot_model.reset(new moveit::core::RobotModel(urdf_model, srdf_model));
  };
  virtual void SetUp()
  {
    std::string resource_dir = ros::package::getPath("moveit_resources");
    if(resource_dir == "")
    {
      FAIL() << "Failed to find package moveit_resources.";
      return;
    }
    boost::filesystem::path res_path(resource_dir);

    srdf_model.reset(new srdf::Model());
    std::string xml_string;
    std::fstream xml_file((res_path / "test/urdf/robot.xml").string().c_str(), std::fstream::in);
    if (xml_file.is_open())
    {
      while ( xml_file.good() )
      {
        std::string line;
        std::getline( xml_file, line);
        xml_string += (line + "\n");
      }
      xml_file.close();
      urdf_model = urdf::parseURDF(xml_string);
    }
    srdf_model->initFile(*urdf_model, (res_path / "test/srdf/robot.xml").string());
    kmodel.reset(new robot_model::RobotModel(urdf_model, srdf_model));

    pr2_kinematics_plugin_right_arm_.reset(new pr2_arm_kinematics::PR2ArmKinematicsPlugin);

    pr2_kinematics_plugin_right_arm_->setRobotModel(urdf_model);
    pr2_kinematics_plugin_right_arm_->initialize("",
                                                 "right_arm",
                                                 "torso_lift_link",
                                                 "r_wrist_roll_link",
                                                 .01);

    pr2_kinematics_plugin_left_arm_.reset(new pr2_arm_kinematics::PR2ArmKinematicsPlugin);

    pr2_kinematics_plugin_left_arm_->setRobotModel(urdf_model);
    pr2_kinematics_plugin_left_arm_->initialize("",
                                                "left_arm",
                                                "torso_lift_link",
                                                "l_wrist_roll_link",
                                                .01);

    func_right_arm.reset(new robot_model::SolverAllocatorFn(boost::bind(&LoadPlanningModelsPr2::getKinematicsSolverRightArm, this, _1)));
    func_left_arm.reset(new robot_model::SolverAllocatorFn(boost::bind(&LoadPlanningModelsPr2::getKinematicsSolverLeftArm, this, _1)));

    std::map<std::string, robot_model::SolverAllocatorFn> allocators;
    allocators["right_arm"] = *func_right_arm;
    allocators["left_arm"] = *func_left_arm;
    allocators["whole_body"] = *func_right_arm;
    allocators["base"] = *func_left_arm;

    kmodel->setKinematicsAllocators(allocators);

    ps.reset(new planning_scene::PlanningScene(kmodel));

  };
Esempio n. 3
0
Game::Game(Application *app, Widget *parent) : Widget(parent) {
  this->app = app;

  radius = 3;
  techTree = TechTree::fromFile(res_path("techtree.json"), app->getImgMgr());
  map = Map::fromFile(res_path("map"), app->getImgMgr());

  players.push_back(new Player("Raymond", techTree, map));
  players.push_back(new Player("Jean-Pierre", techTree, map));

  fogTileMap = new TileMap("fow", app->getImgMgr());
  fog = new FoW(map, fogTileMap);
  foglight = new FoW(map, fogTileMap, FoW::LIGHT);

  EventCallback *func;
  func = new EventMethodCallback<Game>(this, &Game::onMousePressed);
  addEventCallback("MousePressed", func);
}
  virtual void SetUp()
  {
    std::string resource_dir = ros::package::getPath("moveit_resources");
    if(resource_dir == "")
    {
      FAIL() << "Failed to find package moveit_resources.";
      return;
    }
    boost::filesystem::path res_path(resource_dir);
    std::string urdf_file = (res_path / "test/urdf/robot.xml").string();
    std::string srdf_file = (res_path / "test/srdf/robot.xml").string();
    kinect_dae_resource_ = "package://moveit_resources/test/urdf/meshes/sensors/kinect_v0/kinect.dae";

    srdf_model_.reset(new srdf::Model());
    std::string xml_string;
    std::fstream xml_file(urdf_file.c_str(), std::fstream::in);

    if (xml_file.is_open())
    {
      while ( xml_file.good() )
      {
        std::string line;
        std::getline( xml_file, line);
        xml_string += (line + "\n");
      }
      xml_file.close();
      urdf_model_ = urdf::parseURDF(xml_string);
      urdf_ok_ = urdf_model_;
    }
    else
    {
      EXPECT_EQ("FAILED TO OPEN FILE", urdf_file);
      urdf_ok_ = false;
    }
    srdf_ok_ = srdf_model_->initFile(*urdf_model_, srdf_file);

    kmodel_.reset(new robot_model::RobotModel(urdf_model_, srdf_model_));

    acm_.reset(new collision_detection::AllowedCollisionMatrix(kmodel_->getLinkModelNames(), true));

    crobot_.reset(new DefaultCRobotType(kmodel_));
    cworld_.reset(new DefaultCWorldType());
  }
  void SetUp() override
  {
    boost::filesystem::path res_path(ros::package::getPath("moveit_resources"));

    srdf_model_.reset(new srdf::Model());
    std::string xml_string;
    std::fstream xml_file((res_path / "pr2_description/urdf/robot.xml").string().c_str(), std::fstream::in);
    if (xml_file.is_open())
    {
      while (xml_file.good())
      {
        std::string line;
        std::getline(xml_file, line);
        xml_string += (line + "\n");
      }
      xml_file.close();
      urdf_model_ = urdf::parseURDF(xml_string);
    }
    srdf_model_->initFile(*urdf_model_, (res_path / "pr2_description/srdf/robot.xml").string());
    robot_model_.reset(new moveit::core::RobotModel(urdf_model_, srdf_model_));
  };
Esempio n. 6
0
std::string ImageManager::nameToFile(std::string name) {
  return res_path(name + ".png");
}