epos_hardware.cpp
Go to the documentation of this file.
00001 #include "epos_hardware/epos_hardware.h"
00002 #include <boost/foreach.hpp>
00003 
00004 namespace epos_hardware {
00005 
00006 EposHardware::EposHardware(ros::NodeHandle& nh, ros::NodeHandle& pnh, const std::vector<std::string>& motor_names)
00007   : epos_manager_(asi, avi, api, nh, pnh, motor_names) {
00008 
00009   // TODO throw exception or something
00010   try {
00011     transmission_loader.reset(new transmission_interface::TransmissionInterfaceLoader(this, &robot_transmissions));
00012   }
00013   catch(const std::invalid_argument& ex){
00014     ROS_ERROR_STREAM("Failed to create transmission interface loader. " << ex.what());
00015     return;
00016   }
00017   catch(const pluginlib::LibraryLoadException& ex){
00018     ROS_ERROR_STREAM("Failed to create transmission interface loader. " << ex.what());
00019     return;
00020   }
00021   catch(...){
00022     ROS_ERROR_STREAM("Failed to create transmission interface loader. ");
00023     return;
00024   }
00025 
00026   registerInterface(&asi);
00027   registerInterface(&avi);
00028   registerInterface(&api);
00029 
00030   std::string urdf_string;
00031   nh.getParam("robot_description", urdf_string);
00032   while (urdf_string.empty() && ros::ok()){
00033     ROS_INFO_STREAM_ONCE("Waiting for robot_description");
00034     nh.getParam("robot_description", urdf_string);
00035     ros::Duration(0.1).sleep();
00036   }
00037 
00038   transmission_interface::TransmissionParser parser;
00039   std::vector<transmission_interface::TransmissionInfo> infos;
00040   // TODO: throw exception
00041   if (!parser.parse(urdf_string, infos)) {
00042     ROS_ERROR("Error parsing URDF");
00043     return;
00044   }
00045 
00046   // build a list of all loaded actuator names
00047   std::vector<std::string> actuator_names;
00048   std::vector<boost::shared_ptr<Epos> > motors = epos_manager_.motors();
00049   BOOST_FOREACH(const boost::shared_ptr<Epos>& motor, motors) {
00050     actuator_names.push_back(motor->actuator_name());
00051   }
00052 
00053   // Load all transmissions that are for the loaded motors
00054   BOOST_FOREACH(const transmission_interface::TransmissionInfo& info, infos) {
00055     bool found_some = false;
00056     bool found_all = true;
00057     BOOST_FOREACH(const transmission_interface::ActuatorInfo& actuator, info.actuators_) {
00058       if(std::find(actuator_names.begin(), actuator_names.end(), actuator.name_) != actuator_names.end())
00059         found_some = true;
00060       else
00061         found_all = false;
00062     }
00063     if(found_all) {
00064       if (!transmission_loader->load(info)) {
00065         ROS_ERROR_STREAM("Error loading transmission: " << info.name_);
00066         return;
00067       }
00068       else
00069         ROS_INFO_STREAM("Loaded transmission: " << info.name_);
00070     }
00071     else if(found_some){
00072       ROS_ERROR_STREAM("Do not support transmissions that contain only some EPOS actuators: " << info.name_);
00073     }
00074   }
00075 
00076 }
00077 
00078 bool EposHardware::init() {
00079   return epos_manager_.init();
00080 }
00081 
00082 void EposHardware::update_diagnostics() {
00083   epos_manager_.update_diagnostics();
00084 }
00085 
00086 void EposHardware::read() {
00087   epos_manager_.read();
00088   if(robot_transmissions.get<transmission_interface::ActuatorToJointStateInterface>())
00089     robot_transmissions.get<transmission_interface::ActuatorToJointStateInterface>()->propagate();
00090 }
00091 
00092 void EposHardware::write() {
00093   if(robot_transmissions.get<transmission_interface::JointToActuatorVelocityInterface>())
00094     robot_transmissions.get<transmission_interface::JointToActuatorVelocityInterface>()->propagate();
00095   if(robot_transmissions.get<transmission_interface::JointToActuatorPositionInterface>())
00096     robot_transmissions.get<transmission_interface::JointToActuatorPositionInterface>()->propagate();
00097   epos_manager_.write();
00098 }
00099 
00100 }


epos_hardware
Author(s): Mitchell Wills
autogenerated on Thu Jun 6 2019 20:43:10