int main( int argc, char** argv )
{
if(argc != 3)
{
printf("Usage: %s <robot_xml> <controller_xml>\n",argv[0]);
exit(-1);
}
printf("robot file:: %s, controller file:: %s\n",argv[1],argv[2]);
/*********** Create the robot model ****************/
mechanism::Robot *robot_model = new mechanism::Robot;
controller::BaseControllerNode bc;
HardwareInterface hw(0);
robot_model->hw_ = &hw;
/*********** Initialize ROS ****************/
ros::init(argc,argv);
ros::node *node = new ros::node("test_base_controller");
/*********** Load the robot model and state file ************/
char *xml_robot_file = argv[1];
TiXmlDocument xml(xml_robot_file); // Load robot description
xml.LoadFile();
TiXmlElement *root = xml.FirstChildElement("robot");
urdf::normalizeXml(root);
robot_model->initXml(root);
mechanism::RobotState *robot_state = new mechanism::RobotState(robot_model, &hw);
/*********** Load the controller file ************/
char *xml_control_file = argv[2];
TiXmlDocument xml_control(xml_control_file); // Load robot description
xml_control.LoadFile();
TiXmlElement *root_control = xml_control.FirstChildElement("controllers");
TiXmlElement *root_controller = root_control->FirstChildElement("controller");
bc.initXml(robot_state,root_controller);
/************ Testing the odometry calculations themselves ******************/
/* NEWMAT::Matrix A(16,3);
A.Row(1) << 10 << 8.95 << 0.05;
A.Row(2) << 0 << -2 << 0;
A.Row(3) << 1 << -0.1 << 0.01;
A.Row(4) << 2 << 1.1 << 0;
A.Row(5) << 3 << 2 << -0.05;
A.Row(6) << 4 << 3 << 0.01;
A.Row(7) << 5 << 4.1 << 0.05;
A.Row(8) << -1 << -2 << 0.025;
A.Row(9) << 6.15 << 5.05 << 0.01;
A.Row(10) << 6.985 << 6.02 << 0.01;
A.Row(11) << 8.01 << 8.05 << -0.05;
A.Row(12) << 9.03 << 8.1 << -0.01;
A.Row(13) << -8.03 << -9.1 << 0.01;
A.Row(14) << -10.03 << -13.1 << 0.05;
A.Row(15) << -15.03 << -16.1 << -0.015;
A.Row(16) << -16.03 << -17.1 << -0.01;
NEWMAT::Matrix B(16,1);
B << 1.1 << 1 << 1.1 << 1.15 << 0.95 << 0.99 << 0.98 << 0.95 << 1.05 << 1.1 << 1.05 << 1 << 1.13 << 0.995 << 1.035 << 1.08;
NEWMAT::Matrix xfit(3,1);
xfit = bc.c_->iterativeLeastSquares(A,B,"Gaussian",10);
cout << "done" << xfit << endl;
*/
ros::fini();
delete robot_model;
delete robot_state;
}
void UserSettings::save()
{
//initInputCodeMap();
TiXmlDocument doc;
{
TiXmlElement xml_version("Version");
{
xml_version.SetAttribute("settingsVersion", VERSION_USERSETTINGS);
}
doc.InsertEndChild(xml_version);
TiXmlElement xml_system("System");
{
TiXmlElement xml_debugLog("DebugLog");
{
xml_debugLog.SetAttribute("on", system.debugLogOn);
}
xml_system.InsertEndChild(xml_debugLog);
TiXmlElement xml_locale("Locale");
{
xml_locale.SetAttribute("name", system.locale);
}
xml_system.InsertEndChild(xml_locale);
}
doc.InsertEndChild(xml_system);
TiXmlElement xml_audio("Audio");
{
TiXmlElement xml_microphone("Mic");
{
xml_microphone.SetAttribute("on", audio.micOn);
xml_microphone.SetAttribute("octave", audio.octave);
}
xml_audio.InsertEndChild(xml_microphone);
TiXmlElement xml_volume("Volume");
{
xml_volume.SetDoubleAttribute("sfx", double(audio.sfxvol));
xml_volume.SetDoubleAttribute("vox", double(audio.voxvol));
xml_volume.SetDoubleAttribute("mus", double(audio.musvol));
xml_volume.SetAttribute("subs", audio.subtitles);
}
xml_audio.InsertEndChild(xml_volume);
TiXmlElement xml_device("Device");
{
xml_device.SetAttribute("name", audio.deviceName);
}
xml_audio.InsertEndChild(xml_device);
TiXmlElement xml_prebuf("Prebuffer");
{
xml_prebuf.SetAttribute("on", audio.prebuffer);
}
xml_audio.InsertEndChild(xml_prebuf);
}
doc.InsertEndChild(xml_audio);
TiXmlElement xml_video("Video");
{
TiXmlElement xml_shader("Shader");
{
xml_shader.SetAttribute("num", video.shader);
}
xml_video.InsertEndChild(xml_shader);
TiXmlElement xml_blur("Blur");
{
xml_blur.SetAttribute("on", video.blur);
}
xml_video.InsertEndChild(xml_blur);
TiXmlElement xml_noteEffects("NoteEffects");
{
xml_noteEffects.SetAttribute("on", video.noteEffects);
}
xml_video.InsertEndChild(xml_noteEffects);
TiXmlElement xml_fpsSmoothing("FpsSmoothing");
{
xml_fpsSmoothing.SetAttribute("v", video.fpsSmoothing);
}
xml_video.InsertEndChild(xml_fpsSmoothing);
TiXmlElement xml_parallax("Parallax");
std::ostringstream os;
os << video.parallaxOn0 << " " << video.parallaxOn1 << " " << video.parallaxOn2;
xml_parallax.SetAttribute("on", os.str());
xml_video.InsertEndChild(xml_parallax);
TiXmlElement xml_numParticles("NumParticles");
xml_numParticles.SetAttribute("v", video.numParticles);
xml_video.InsertEndChild(xml_numParticles);
TiXmlElement xml_screenMode("ScreenMode");
{
xml_screenMode.SetAttribute("resx", video.resx);
xml_screenMode.SetAttribute("resy", video.resy);
xml_screenMode.SetAttribute("bits", video.bits);
xml_screenMode.SetAttribute("fbuffer", video.fbuffer);
xml_screenMode.SetAttribute("full", video.full);
xml_screenMode.SetAttribute("vsync", video.vsync);
xml_screenMode.SetAttribute("darkfbuffer", video.darkfbuffer);
xml_screenMode.SetAttribute("darkbuffersize", video.darkbuffersize);
xml_screenMode.SetAttribute("displaylists", video.displaylists);
}
xml_video.InsertEndChild(xml_screenMode);
TiXmlElement xml_saveSlotScreens("SaveSlotScreens");
{
xml_saveSlotScreens.SetAttribute("on", video.saveSlotScreens);
}
xml_video.InsertEndChild(xml_saveSlotScreens);
TiXmlElement xml_worldMap("WorldMap");
{
xml_worldMap.SetAttribute("revealMethod", video.worldMapRevealMethod);
}
xml_video.InsertEndChild(xml_worldMap);
}
doc.InsertEndChild(xml_video);
TiXmlElement xml_control("Control");
{
TiXmlElement xml_toolTipsOn("ToolTipsOn");
{
xml_toolTipsOn.SetAttribute("on", control.toolTipsOn);
}
xml_control.InsertEndChild(xml_toolTipsOn);
TiXmlElement xml_joystickEnabled("JoystickEnabled");
{
xml_joystickEnabled.SetAttribute("on", control.joystickEnabled);
}
xml_control.InsertEndChild(xml_joystickEnabled);
TiXmlElement xml_autoAim("AutoAim");
{
xml_autoAim.SetAttribute("on", control.autoAim);
}
xml_control.InsertEndChild(xml_autoAim);
TiXmlElement xml_targeting("Targeting");
{
xml_targeting.SetAttribute("on", control.targeting);
}
xml_control.InsertEndChild(xml_targeting);
TiXmlElement xml_joyCursorSpeed("JoyCursorSpeed");
{
xml_joyCursorSpeed.SetDoubleAttribute("v", double(control.joyCursorSpeed));
}
xml_control.InsertEndChild(xml_joyCursorSpeed);
TiXmlElement xml_joyAxes("JoyAxes");
{
xml_joyAxes.SetAttribute("s1ax", control.s1ax);
xml_joyAxes.SetAttribute("s1ay", control.s1ay);
xml_joyAxes.SetAttribute("s2ax", control.s2ax);
xml_joyAxes.SetAttribute("s2ay", control.s2ay);
xml_joyAxes.SetDoubleAttribute("s1dead", double(control.s1dead));
xml_joyAxes.SetDoubleAttribute("s2dead", double(control.s2dead));
}
xml_control.InsertEndChild(xml_joyAxes);
TiXmlElement xml_actionSet("ActionSet");
{
for (int i = 0; i < control.actionSet.inputSet.size(); i++)
{
TiXmlElement xml_action("Action");
ActionInput *actionInput = &control.actionSet.inputSet[i];
xml_action.SetAttribute("name", actionInput->name);
xml_action.SetAttribute("input", actionInput->toString());
xml_actionSet.InsertEndChild(xml_action);
}
}
xml_control.InsertEndChild(xml_actionSet);
}
doc.InsertEndChild(xml_control);
TiXmlElement xml_demo("Demo");
{
TiXmlElement xml_warpKeys("WarpKeys");
{
xml_warpKeys.SetAttribute("on", demo.warpKeys);
}
xml_demo.InsertEndChild(xml_warpKeys);
TiXmlElement xml_intro("Intro2");
{
xml_intro.SetAttribute("on", demo.intro);
}
xml_demo.InsertEndChild(xml_intro);
TiXmlElement xml_shortLogos("ShortLogos");
{
xml_shortLogos.SetAttribute("on", demo.shortLogos);
}
xml_demo.InsertEndChild(xml_shortLogos);
}
doc.InsertEndChild(xml_demo);
TiXmlElement xml_data("Data");
{
xml_data.SetAttribute("savePage", data.savePage);
xml_data.SetAttribute("saveSlot", data.saveSlot);
std::ostringstream ss;
for (std::set<std::string>::iterator it = dsq->activePatches.begin(); it != dsq->activePatches.end(); ++it)
ss << *it << " ";
xml_data.SetAttribute("activePatches", ss.str());
}
doc.InsertEndChild(xml_data);
TiXmlElement xml_net("Network");
{
xml_net.SetAttribute("masterServer", network.masterServer);
}
doc.InsertEndChild(xml_net);
}
#if defined(BBGE_BUILD_UNIX)
doc.SaveFile(dsq->getPreferencesFolder() + "/" + userSettingsFilename);
#elif defined(BBGE_BUILD_WINDOWS)
doc.SaveFile(userSettingsFilename);
#endif
//clearInputCodeMap();
}
