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));
};
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_));
};
std::string ImageManager::nameToFile(std::string name) {
return res_path(name + ".png");
}