romeo.cpp
Go to the documentation of this file.
00001 
00023 #include <iostream>
00024 #include "romeo_dcm_driver/romeo.h"
00025 #include <alerror/alerror.h>
00026 #include <alcommon/albroker.h>
00027 #include <sensor_msgs/JointState.h>
00028 #include <algorithm>
00029 
00030 #include <controller_manager_msgs/ControllerState.h>
00031 
00032 Romeo::Romeo(boost::shared_ptr<AL::ALBroker> broker, const string &name)
00033     : AL::ALModule(broker,name),is_connected_(false)
00034 {
00035     setModuleDescription("Romeo Robot Module");
00036 
00037     functionName("brokerDisconnected", getName(), "Callback when broker disconnects!");
00038     BIND_METHOD(Romeo::brokerDisconnected);
00039 }
00040 
00041 Romeo::~Romeo()
00042 {
00043     if(is_connected_)
00044         disconnect();
00045 }
00046 
00047 bool Romeo::initialize()
00048 {
00049 
00050     //Romeo Joints Initialization
00051     const char* joint[] = { /*"NeckYaw",
00052                             "NeckPitch",
00053                             "HeadPitch",
00054                             "HeadRoll",*/
00055                             "LShoulderPitch",
00056                             "LShoulderYaw",
00057                             "LElbowRoll",
00058                             "LElbowYaw",
00059                             "LWristRoll",
00060                             "LWristYaw",
00061                             "LWristPitch",
00062                             "LHand",
00063                             /*"TrunkYaw",
00064                             "LHipYaw",
00065                             "LHipRoll",
00066                             "LHipPitch",
00067                             "LKneePitch",
00068                             "LAnklePitch",
00069                             "LAnkleRoll",
00070                             "RHipYaw",
00071                             "RHipRoll",
00072                             "RHipPitch",
00073                             "RKneePitch",
00074                             "RAnklePitch",
00075                             "RAnkleRoll",*/
00076                             "RShoulderPitch",
00077                             "RShoulderYaw",
00078                             "RElbowRoll",
00079                             "RElbowYaw",
00080                             "RWristRoll",
00081                             "RWristYaw",
00082                             "RWristPitch",
00083                             "RHand" };
00084     joint_names_ = vector<string>(joint, end(joint));
00085     
00086     for(vector<string>::iterator it=joint_names_.begin();it!=joint_names_.end();it++)
00087     {
00088         joints_names_.push_back("Device/SubDeviceList/"+(*it)+"/Position/Sensor/Value");
00089     }
00090     number_of_joints_ = joint_names_.size();
00091 
00092     // DCM Motion Commands Initialization
00093     try
00094     {
00095         // Create Motion Command
00096         commands_.arraySetSize(4);
00097         commands_[0] = string("Joints");
00098         commands_[1] = string("ClearAll");
00099         commands_[2] = string("time-mixed");
00100         commands_[3].arraySetSize(number_of_joints_);
00101 
00102         // Create Joints Actuators Alias
00103         AL::ALValue commandAlias;
00104         commandAlias.arraySetSize(2);
00105         commandAlias[0] = string("Joints");
00106         commandAlias[1].arraySetSize(number_of_joints_);
00107         for(int i=0;i<number_of_joints_;i++)
00108         {
00109             commandAlias[1][i] = string("Device/SubDeviceList/"+joint_names_[i]+"/Position/Actuator/Value");
00110             commands_[3][i].arraySetSize(1);
00111             commands_[3][i][0].arraySetSize(2);
00112         }
00113         dcm_proxy_->callVoid("createAlias",commandAlias);
00114 
00115         // Create Joints Hardness Alias
00116         commandAlias[0] = string("JointsHardness");
00117         commandAlias[1].arraySetSize(number_of_joints_/*-1*/);
00118         for(int i=0;i<number_of_joints_;i++)
00119         {
00120             commandAlias[1][i] = string("Device/SubDeviceList/"+joint_names_[i]+"/Hardness/Actuator/Value");
00121         }
00122         dcm_proxy_->callVoid("createAlias",commandAlias);
00123 
00124     }
00125     catch(const AL::ALError& e)
00126     {
00127         ROS_ERROR("Could not initialize dcm aliases!\n\tTrace: %s",e.what());
00128         return false;
00129     }
00130 
00131     stiffnesses_enabled_ = true;
00132 
00133     m_jointState.name = m_motionProxy->getBodyNames("Body");
00134 
00135     std::stringstream ss;
00136     std::copy(m_jointState.name.begin(), m_jointState.name.end()-1, std::ostream_iterator<std::string>(ss,","));
00137     std::copy(m_jointState.name.end()-1, m_jointState.name.end(), std::ostream_iterator<std::string>(ss));
00138     ROS_INFO("Romeo joints found: %s",ss.str().c_str());
00139 
00140     return true;
00141 }
00142 
00143 bool Romeo::initializeControllers(controller_manager::ControllerManager& cm)
00144 {
00145     if(!initialize())
00146     {
00147         ROS_ERROR("Initialization method failed!");
00148         return false;
00149     }
00150 
00151     // Initialize Controllers' Interfaces
00152     joint_angles_.resize(number_of_joints_);
00153     joint_velocities_.resize(number_of_joints_);
00154     joint_efforts_.resize(number_of_joints_);
00155     joint_commands_.resize(number_of_joints_);
00156 
00157     try
00158     {
00159         for(int i=0;i<number_of_joints_;i++)
00160         {
00161             hardware_interface::JointStateHandle state_handle(joint_names_[i], &joint_angles_[i],
00162                                                               &joint_velocities_[i], &joint_efforts_[i]);
00163             jnt_state_interface_.registerHandle(state_handle);
00164 
00165             hardware_interface::JointHandle pos_handle(jnt_state_interface_.getHandle(joint_names_[i]),
00166                                                        &joint_commands_[i]);
00167             jnt_pos_interface_.registerHandle(pos_handle);
00168         }
00169 
00170         registerInterface(&jnt_state_interface_);
00171         registerInterface(&jnt_pos_interface_);
00172     }
00173     catch(const ros::Exception& e)
00174     {
00175         ROS_ERROR("Could not initialize hardware interfaces!\n\tTrace: %s",e.what());
00176         return false;
00177     }
00178     ROS_INFO("Romeo Module initialized!");
00179     return true;
00180 }
00181 
00182 bool Romeo::connect(const ros::NodeHandle nh)
00183 {
00184     // Initialize ROS nodes
00185     node_handle_ = nh;
00186 
00187     is_connected_ = false;
00188 
00189     // Load ROS Parameters
00190     loadParams();
00191 
00192     // Needed for Error Checking
00193     try
00194     {
00195         subscribeToMicroEvent("ClientDisconnected", "Romeo", "brokerDisconnected");
00196     }
00197     catch (const AL::ALError& e)
00198     {
00199         ROS_ERROR("Could not subscribe to brokerDisconnected!\n\tTrace: %s",e.what());
00200     }
00201 
00202     // Initialize DCM_motion Proxy
00203     try
00204     {
00205       dcm_proxy_  = boost::shared_ptr<AL::ALProxy>(new AL::ALProxy(getParentBroker(), "DCM_motion"));
00206 
00207     }
00208     catch (const AL::ALError& e)
00209     {
00210         ROS_ERROR("Failed to connect to DCM Proxy!\n\tTrace: %s",e.what());
00211         return false;
00212     }
00213 
00214     // Initialize Memory Proxy
00215     try
00216     {
00217         memory_proxy_ = AL::ALMemoryProxy(getParentBroker());
00218     }
00219     catch (const AL::ALError& e)
00220     {
00221         ROS_ERROR("Failed to connect to Memory Proxy!\n\tTrace: %s",e.what());
00222         return false;
00223     }
00224     try
00225     {
00226        m_motionProxy = boost::shared_ptr<AL::ALMotionProxy>(new AL::ALMotionProxy(getParentBroker()));
00227     }
00228     catch (const AL::ALError& e)
00229     {
00230        ROS_ERROR("Could not create ALMotionProxy.");
00231        return false;
00232     }
00233 
00234     is_connected_ = true;
00235 
00236     // Subscribe/Publish ROS Topics/Services
00237     subscribe();
00238 
00239     // Initialize Controller Manager and Controllers
00240     manager_ = new controller_manager::ControllerManager(this,node_handle_);
00241     if(!initializeControllers(*manager_))
00242     {
00243         ROS_ERROR("Could not load controllers!");
00244         return false;
00245     }
00246     ROS_INFO("Controllers successfully loaded!");
00247     return true;
00248 }
00249 
00250 void Romeo::disconnect()
00251 {
00252     if(!is_connected_)
00253         return;
00254     try
00255     {
00256         unsubscribeFromMicroEvent("ClientDisconnected", "Romeo");
00257     }
00258     catch (const AL::ALError& e)
00259     {
00260         ROS_ERROR("Failed to unsubscribe from subscribed events!\n\tTrace: %s",e.what());
00261     }
00262     is_connected_ = false;
00263 }
00264 
00265 void Romeo::subscribe()
00266 {
00267     // Subscribe/Publish ROS Topics/Services
00268     cmd_vel_sub_ = node_handle_.subscribe(prefix_+"cmd_vel", topic_queue_, &Romeo::commandVelocity, this);
00269 
00270     stiffness_pub_ = node_handle_.advertise<std_msgs::Float32>(prefix_+"stiffnesses", topic_queue_);
00271     stiffness_.data = 1.0f;
00272 /*
00273     //ROS service to enable robot's stiffness, currently not used.
00274     stiffness_switch_ = node_handle_.advertiseService<Romeo, romeo_dcm_msgs::BoolService::Request,
00275             romeo_dcm_msgs::BoolService::Response>(prefix_+"Stiffnesses/Enable", &Romeo::switchStiffnesses, this);
00276 */
00277     m_jointStatePub = node_handle_.advertise<sensor_msgs::JointState>("joint_states",5);
00278 }
00279 
00280 void Romeo::loadParams()
00281 {
00282     ros::NodeHandle n_p("~");
00283     // Load Server Parameters
00284     n_p.param("Version", version_, string("V4"));
00285     n_p.param("BodyType", body_type_, string("H21"));
00286 
00287     n_p.param("TopicQueue", topic_queue_, 50);
00288 
00289     n_p.param("Prefix", prefix_, string("romeo_dcm"));
00290     prefix_ = prefix_+"/";
00291 
00292     n_p.param("LowCommunicationFrequency", low_freq_, 10.0);
00293     n_p.param("HighCommunicationFrequency", high_freq_, 10.0);
00294     n_p.param("ControllerFrequency", controller_freq_, 15.0);
00295     n_p.param("JointPrecision", joint_precision_, 0.0174532925);
00296     n_p.param("OdomFrame", odom_frame_, string("odom"));
00297 }
00298 
00299 void Romeo::brokerDisconnected(const string& event_name, const string &broker_name, const string& subscriber_identifier)
00300 {
00301     if(broker_name == "Romeo Driver Broker")
00302         is_connected_ = false;
00303 }
00304 
00305 void Romeo::DCMTimedCommand(const string &key, const AL::ALValue &value, const int &timeOffset, const string &type)
00306 {
00307     try
00308     {
00309         // Create timed-command
00310         AL::ALValue command;
00311         command.arraySetSize(3);
00312         command[0] = key;
00313         command[1] = type;
00314         command[2].arraySetSize(1);
00315         command[2][0].arraySetSize(2);
00316         command[2][0][0] = value;
00317         command[2][0][1] = dcm_proxy_->call<int>("getTime",timeOffset);
00318 
00319         // Execute timed-command
00320         dcm_proxy_->callVoid("set",command);
00321     }
00322     catch(const AL::ALError& e)
00323     {
00324         ROS_ERROR("Could not execute DCM timed-command!\n\t%s\n\n\tTrace: %s", key.c_str(), e.what());
00325     }
00326 }
00327 
00328 void Romeo::DCMAliasTimedCommand(const string &alias, const vector<float> &values, const vector<int> &timeOffsets,
00329                                const string &type, const string &type2)
00330 {
00331     try
00332     {
00333         // Create Alias timed-command
00334         AL::ALValue command;
00335         command.arraySetSize(4);
00336         command[0] = alias;
00337         command[1] = type;
00338         command[2] = type2;
00339         command[3].arraySetSize(values.size());
00340         int T = dcm_proxy_->call<int>("getTime",0);
00341         for(int i=0;i<values.size();i++)
00342         {
00343             command[3][i].arraySetSize(1);
00344             command[3][i][0].arraySetSize(2);
00345             command[3][i][0][0] = values[i];
00346             command[3][i][0][1] = T+timeOffsets[i];
00347         }
00348 
00349         // Execute Alias timed-command
00350         dcm_proxy_->callVoid("setAlias",command);
00351     }
00352     catch(const AL::ALError& e)
00353     {
00354         ROS_ERROR("Could not execute DCM timed-command!\n\t%s\n\n\tTrace: %s", alias.c_str(), e.what());
00355     }
00356 }
00357 
00358 void Romeo::insertDataToMemory(const string &key, const AL::ALValue &value)
00359 {
00360     memory_proxy_.insertData(key,value);
00361 }
00362 
00363 AL::ALValue Romeo::getDataFromMemory(const string &key)
00364 {
00365     return memory_proxy_.getData(key);
00366 }
00367 
00368 void Romeo::subscribeToEvent(const string &name, const string &callback_module, const string &callback_method)
00369 {
00370     try
00371     {
00372         memory_proxy_.subscribeToEvent(name,callback_module,"",callback_method);
00373     }
00374     catch(const AL::ALError& e)
00375     {
00376         ROS_ERROR("Could not subscribe to event '%s'.\n\tTrace: %s",name.c_str(),e.what());
00377     }
00378 }
00379 
00380 void Romeo::subscribeToMicroEvent(const string &name, const string &callback_module,
00381                                 const string &callback_method, const string &callback_message)
00382 {
00383     try
00384     {
00385         memory_proxy_.subscribeToMicroEvent(name,callback_module,callback_message,callback_method);
00386     }
00387     catch(const AL::ALError& e)
00388     {
00389         ROS_ERROR("Could not subscribe to micro-event '%s'.\n\tTrace: %s",name.c_str(),e.what());
00390     }
00391 }
00392 
00393 void Romeo::unsubscribeFromEvent(const string &name, const string &callback_module)
00394 {
00395     try
00396     {
00397         memory_proxy_.unsubscribeToEvent(name,callback_module);
00398     }
00399     catch(const AL::ALError& e)
00400     {
00401         ROS_ERROR("Could not unsubscribe from event '%s'.\n\tTrace: %s",name.c_str(),e.what());
00402     }
00403 }
00404 
00405 void Romeo::unsubscribeFromMicroEvent(const string &name, const string &callback_module)
00406 {
00407     try
00408     {
00409         memory_proxy_.unsubscribeToMicroEvent(name,callback_module);
00410     }
00411     catch(const AL::ALError& e)
00412     {
00413         ROS_ERROR("Could not unsubscribe from micro-event '%s'.\n\tTrace: %s",name.c_str(),e.what());
00414     }
00415 }
00416 
00417 void Romeo::raiseEvent(const string &name, const AL::ALValue &value)
00418 {
00419     try
00420     {
00421         memory_proxy_.raiseEvent(name,value);
00422     }
00423     catch(const AL::ALError& e)
00424     {
00425         ROS_ERROR("Could not raise event '%s'.\n\tTrace: %s",name.c_str(),e.what());
00426     }
00427 }
00428 
00429 void Romeo::raiseMicroEvent(const string &name, const AL::ALValue &value)
00430 {
00431     try
00432     {
00433         memory_proxy_.raiseMicroEvent(name,value);
00434     }
00435     catch(const AL::ALError& e)
00436     {
00437         ROS_ERROR("Could not raise micro-event '%s'.\n\tTrace: %s",name.c_str(),e.what());
00438     }
00439 }
00440 
00441 void Romeo::declareEvent(const string &name)
00442 {
00443     try
00444     {
00445         memory_proxy_.declareEvent(name);
00446     }
00447     catch(AL::ALError& e)
00448     {
00449         ROS_ERROR("Could not declare event '%s'.\n\tTrace: %s",name.c_str(),e.what());
00450     }
00451 }
00452 
00453 void Romeo::run()
00454 {
00455     boost::thread t1(&Romeo::controllerLoop,this);
00456 //    boost::thread t2(&Romeo::lowCommunicationLoop,this);
00457 //    boost::thread t3(&Romeo::highCommunicationLoop,this);
00458     t1.join();
00459 //    t2.join();
00460 //    t3.join();
00461 }
00462 
00463 void Romeo::lowCommunicationLoop()
00464 {
00465     static ros::Rate rate(low_freq_);
00466     while(ros::ok())
00467     {
00468         ros::Time time = ros::Time::now();
00469 
00470         if(!is_connected_)
00471             break;
00472 
00473         if(stiffnesses_enabled_)
00474         {
00475             stiffness_.data = 1.0f;
00476         }
00477         else
00478         {
00479             stiffness_.data = 0.0f;
00480         }
00481         stiffness_pub_.publish(stiffness_);
00482 
00483         rate.sleep();
00484     }
00485 }
00486 
00487 void Romeo::highCommunicationLoop()
00488 {
00489     static ros::Rate rate(high_freq_);
00490     while(ros::ok())
00491     {
00492         ros::Time time = ros::Time::now();
00493 
00494         if(!is_connected_)
00495             break;
00496 
00497         //You can add publisher for sensors value here
00498 
00499         try
00500         {
00501             dcm_proxy_->callVoid("ping");
00502         }
00503         catch(const AL::ALError& e)
00504         {
00505             ROS_ERROR("Could not ping DCM proxy.\n\tTrace: %s",e.what());
00506             is_connected_ = false;
00507         }
00508         rate.sleep();
00509     }
00510 }
00511 
00512 void Romeo::controllerLoop()
00513 {
00514     static ros::Rate rate(controller_freq_+10.0);
00515     while(ros::ok())
00516     {
00517 
00518         
00519         ros::Time time = ros::Time::now();
00520 
00521         if(!is_connected_)
00522             break;
00523 
00524         //You can add publisher for sensors value here
00525 
00526         try
00527         {
00528             dcm_proxy_->callVoid("ping");
00529         }
00530         catch(const AL::ALError& e)
00531         {
00532             ROS_ERROR("Could not ping DCM proxy.\n\tTrace: %s",e.what());
00533             is_connected_ = false;
00534             rate.sleep();
00535             continue;
00536         }
00537 
00538         readJoints();
00539 
00540         manager_->update(time,ros::Duration(1.0f/controller_freq_));
00541 
00542         writeJoints();
00543 
00544         publishJointStateFromAlMotion();
00545 
00546         rate.sleep();
00547     }
00548 }
00549 
00550 bool Romeo::connected()
00551 {
00552     return is_connected_;
00553 }
00554 
00555 void Romeo::commandVelocity(const geometry_msgs::TwistConstPtr &msg)
00556 {
00557     ROS_WARN("This function does nothing at the moment..");
00558 }
00559 
00560 void Romeo::readJoints()
00561 {
00562     vector<float> jointData;
00563     try
00564     {
00565         jointData = memory_proxy_.getListData(joints_names_);
00566     }
00567     catch(const AL::ALError& e)
00568     {
00569         ROS_ERROR("Could not get joint data from Romeo.\n\tTrace: %s",e.what());
00570         return;
00571     }
00572 
00573     for(short i = 0; i<jointData.size(); i++)
00574     {
00575         joint_angles_[i] = jointData[i];
00576         // Set commands to the read angles for when no command specified
00577         joint_commands_[i] = jointData[i];
00578     }
00579 
00580 
00581 }
00582 
00583 void Romeo::publishJointStateFromAlMotion(){
00584   std::vector<float> positionData;
00585   positionData = m_motionProxy->getAngles("Body", true);
00586   m_jointState.header.stamp = ros::Time::now();
00587   m_jointState.header.frame_id = "base_link";
00588   m_jointState.position.resize(positionData.size());
00589   for(unsigned i = 0; i<positionData.size(); ++i)
00590   {
00591      m_jointState.position[i] = positionData[i];
00592   }
00593 
00594   m_jointStatePub.publish(m_jointState);
00595 }
00596 
00597 void Romeo::writeJoints()
00598 {
00599     // Update joints only when actual command is issued
00600     bool changed = false;
00601     for(int i=0;i<number_of_joints_;i++)
00602     {
00603         if(fabs(joint_commands_[i]-joint_angles_[i])>joint_precision_)
00604         {
00605             changed = true;
00606             break;
00607         }
00608     }
00609     // Do not write joints if no change in joint values
00610     if(!changed)
00611     {
00612         return;
00613     }
00614 
00615     try
00616     {
00617         int T = dcm_proxy_->call<int>("getTime",0);
00618         for(int i=0;i<number_of_joints_;i++)
00619         {
00620             commands_[3][i][0][0] = float(joint_commands_[i]);
00621             commands_[3][i][0][1] = T+(int)(800.0f/controller_freq_);
00622        //ROS_INFO(<<i<<"\t"<<commands_[3][i][0][0]<<"\n");
00623         }
00624         
00625         dcm_proxy_->callVoid("setAlias",commands_);
00626     }
00627     catch(const AL::ALError& e)
00628     {
00629         ROS_ERROR("Could not send joint commands to Romeo.\n\tTrace: %s",e.what());
00630         return;
00631     }
00632 }
00633 
00634 
00635 bool Romeo::switchStiffnesses(romeo_dcm_msgs::BoolService::Request &req, romeo_dcm_msgs::BoolService::Response &res)
00636 {
00637     if(stiffnesses_enabled_!=req.enable && req.enable)
00638     {
00639         DCMAliasTimedCommand("JointsHardness",vector<float>(number_of_joints_,1.0f), vector<int>(number_of_joints_,0));
00640     }
00641     else if(stiffnesses_enabled_!=req.enable)
00642     {
00643         DCMAliasTimedCommand("JointsHardness",vector<float>(number_of_joints_,0.0f), vector<int>(number_of_joints_,0));
00644     }
00645     stiffnesses_enabled_ = req.enable;
00646 }
00647 


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