Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041 #include <YouBotOODLWrapper.h>
00042 #include <stdlib.h>
00043
00044 int main(int argc, char **argv)
00045 {
00046
00047 youbot::Logger::toConsole = false;
00048 youbot::Logger::toFile = false;
00049 youbot::Logger::toROS = true;
00050 ros::init(argc, argv, "youbot_wrapper");
00051 ros::NodeHandle n;
00052 youBot::YouBotOODLWrapper youBot(n);
00053 std::vector<std::string> armNames;
00054
00055
00056 bool youBotHasBase;
00057 bool youBotHasArms;
00058 double youBotDriverCycleFrequencyInHz;
00059 n.param("youBotHasBase", youBotHasBase, true);
00060 n.param("youBotHasArms", youBotHasArms, true);
00061 n.param("youBotDriverCycleFrequencyInHz", youBotDriverCycleFrequencyInHz, 50.0);
00062
00063
00064 char* configLocation = getenv("YOUBOT_CONFIG_FOLDER_LOCATION");
00065 if (configLocation == NULL)
00066 throw std::runtime_error("youbot_wrapper.cpp: Could not find environment variable YOUBOT_CONFIG_FOLDER_LOCATION");
00067
00068 n.param < std::string
00069 > ("youBotConfigurationFilePath", youBot.youBotConfiguration.configurationFilePath, configLocation);
00070
00071 n.param < std::string > ("youBotBaseName", youBot.youBotConfiguration.baseConfiguration.baseID, "youbot-base");
00072
00073
00074 int i = 1;
00075 std::stringstream armNameParam;
00076 armNameParam << "youBotArmName" << i;
00077 while (n.hasParam(armNameParam.str()))
00078 {
00079 std::string armName;
00080 n.getParam(armNameParam.str(), armName);
00081 armNames.push_back(armName);
00082 armNameParam.str("");
00083 armNameParam << "youBotArmName" << (++i);
00084 }
00085
00086 ros::ServiceServer reconnectService = n.advertiseService("reconnect", &youBot::YouBotOODLWrapper::reconnectCallback,
00087 &youBot);
00088
00089 ROS_ASSERT((youBotHasBase == true) || (youBotHasArms == true));
00090 if (youBotHasBase == true)
00091 {
00092 youBot.initializeBase(youBot.youBotConfiguration.baseConfiguration.baseID);
00093 }
00094
00095 if (youBotHasArms == true)
00096 {
00097 std::vector<std::string>::iterator armNameIter;
00098 for (armNameIter = armNames.begin(); armNameIter != armNames.end(); ++armNameIter)
00099 {
00100 youBot.initializeArm(*armNameIter);
00101 }
00102 }
00103
00104
00105 ros::Rate rate(youBotDriverCycleFrequencyInHz);
00106 while (n.ok())
00107 {
00108 ros::spinOnce();
00109 youBot.computeOODLSensorReadings();
00110 youBot.publishOODLSensorReadings();
00111 youBot.publishArmAndBaseDiagnostics(2.0);
00112 rate.sleep();
00113
00114
00115 if (!youBot.youBotConfiguration.isEtherCATOkay()) {
00116 std_srvs::Empty::Request req;
00117 std_srvs::Empty::Response res;
00118 youBot.reconnectCallback(req,res);
00119 }
00120
00121 }
00122
00123 youBot.stop();
00124
00125 return 0;
00126 }
00127