romeo_driver.cpp
Go to the documentation of this file.
00001 
00023 #include <iostream>
00024 #include "romeo_dcm_driver/romeo.h"
00025 #include <alcommon/albroker.h>
00026 #include <alcommon/albrokermanager.h>
00027 #include <alproxies/almotionproxy.h>
00028 
00029 using std::string;
00030 using std::cerr;
00031 using std::endl;
00032 
00033 int main( int argc, char** argv )
00034 {
00035     int pport = 9559;
00036     string pip = "127.0.0.1";
00037     ros::init(argc, argv, "romeo_dcm_driver");
00038     ros::NodeHandle n;
00039     ros::NodeHandle n_p("~");
00040     if(!ros::master::check())
00041     {
00042         cerr<<"Could not contact master!\nQuitting... "<<endl;
00043         return -1;
00044     }
00045     // A broker needs a name, an IP and a port:
00046     string broker_name = "Romeo Driver Broker";
00047     // FIXME: would be a good idea to look for a free port first
00048     int broker_port = 54000;
00049     // listen port of the broker (here an anything)
00050     string broker_ip = "0.0.0.0";
00051 
00052     // Load Params from Parameter Server
00053     n_p.param("RobotIP", pip, string("127.0.0.1"));
00054     n_p.param("RobotPort", pport,9559);
00055     n_p.param("DriverBrokerPort", broker_port, 54000);
00056     n_p.param("DriverBrokerIP", broker_ip, string("0.0.0.0"));
00057 
00058     // Create your own broker
00059     boost::shared_ptr<AL::ALBroker> broker;
00060 
00061     try
00062     {
00063         broker = AL::ALBroker::createBroker(broker_name,broker_ip,broker_port,pip,pport,0);
00064 
00065         // Workaround because stiffness does not work well via DCM
00066         boost::shared_ptr<AL::ALMotionProxy> m_motionProxy;
00067         try
00068         {
00069            m_motionProxy = boost::shared_ptr<AL::ALMotionProxy>(new AL::ALMotionProxy(broker));
00070         }
00071         catch (const AL::ALError& e)
00072         {
00073            ROS_ERROR("Could not create ALMotionProxy.");
00074            return -1;
00075         }
00076         m_motionProxy->stiffnessInterpolation("LArm", 1.0f, 2.0f);
00077         m_motionProxy->stiffnessInterpolation("RArm", 1.0f, 2.0f);
00078 
00079         //m_motionProxy->stiffnessInterpolation("Head", 1.0f, 2.0f);
00080         //m_motionProxy->stiffnessInterpolation("Trunk", 1.0f, 2.0f);
00081 
00082     }
00083 
00084     catch(...)
00085     {
00086         ROS_ERROR("Failed to connect to Broker at %s:%d!",pip.c_str(),pport);
00087         return -1;
00088     }
00089 
00090     // Deal with ALBrokerManager singleton (add your broker into NAOqi)
00091     AL::ALBrokerManager::setInstance(broker->fBrokerManager.lock());
00092     AL::ALBrokerManager::getInstance()->addBroker(broker);
00093 
00094     // Now it's time to load your module
00095     boost::shared_ptr<Romeo> romeo = AL::ALModule::createModule<Romeo>(broker, "Romeo");
00096     romeo->connect(n);
00097     if(!romeo->connected())
00098     {
00099         ROS_ERROR("Could not connect to Romeo robot!");
00100         AL::ALBrokerManager::getInstance()->killAllBroker();
00101         AL::ALBrokerManager::kill();
00102         return -1;
00103     }
00104 
00105     // Run the spinner in a separate thread to prevent lockups
00106 
00107     ros::AsyncSpinner spinner(1);
00108     spinner.start();
00109     if(broker->isModulePresent("Romeo"))
00110         ROS_INFO("Romeo Module loaded succesfully!");
00111     else
00112     {
00113         ROS_ERROR("Romeo Module is not loaded!");
00114         return -1;
00115     }
00116 
00117     // Run Romeo Driver Loop
00118     romeo->run();
00119 
00120     AL::ALBrokerManager::getInstance()->killAllBroker();
00121     AL::ALBrokerManager::kill();
00122     spinner.stop();
00123 
00124     // Workaround because stiffness does not work well via DCM
00125 
00126     sleep(1);
00127     ROS_INFO( "finished sleeping... " );
00128     AL::ALProxy tempMotion("ALMotion", pip, pport);
00129     tempMotion.callVoid("setStiffnesses","LArm",0.0f);
00130     tempMotion.callVoid("setStiffnesses","RArm",0.0f);
00131     //tempMotion.callVoid("setStiffnesses","Head",0.0f);
00132     //tempMotion.callVoid("setStiffnesses","Trunk",0.0f);
00133 
00134 
00135     ROS_INFO( "Quitting... " );
00136     return 0;
00137 }


romeo_dcm_driver
Author(s): Konstantinos Chatzilygeroudis , Ha Dang
autogenerated on Fri Jun 24 2016 04:21:15