hand_controller.cpp
Go to the documentation of this file.
00001 #include <iostream>
00002 
00003 #include <ros/ros.h>
00004 #include <pr2_controller_manager/controller_manager.h>
00005 
00006 #include <osrf_msgs/JointCommands.h>
00007 #include <sensor_msgs/JointState.h>
00008 #include <roseus/StringString.h>
00009 
00010 class HandController {
00011 public:
00012   HandController() : nh_(), auto_start_(false) {
00013     hw_ = new pr2_hardware_interface::HardwareInterface();
00014     hw_->current_time_ = ros::Time::now();
00015 
00016     ros::NodeHandle pnh("~");
00017     pnh.getParam("auto_start", auto_start_);
00018 
00019     TiXmlElement *root;
00020     TiXmlElement *root_element;
00021     // Load robot description
00022     TiXmlDocument xml_doc;
00023 
00024     std::string robot_desc;
00025     if (pnh.getParam("robot_description", robot_desc)) {
00026       xml_doc.Parse(robot_desc.c_str());
00027     }  else {
00028       ROS_FATAL("Could not load the xml from parameter server");
00029       throw "end";
00030     }
00031 
00032     root_element = xml_doc.RootElement();
00033     root = xml_doc.FirstChildElement("robot");
00034 
00035     if (!root || !root_element) {
00036       ROS_FATAL("Could not parse the xml / %s", robot_desc.c_str());
00037       throw "end";
00038     }
00039 
00040     for (TiXmlElement *j = root_element->FirstChildElement("transmission"); j;
00041          j = j->NextSiblingElement("transmission")) {
00042       TiXmlElement *ael = j->FirstChildElement("actuator");
00043       const char *anm = ael ? ael->Attribute("name") : NULL;
00044 #if 0
00045       const char *tnm = j ? j->Attribute("name") : NULL;
00046       TiXmlElement *jel = j->FirstChildElement("joint");
00047       const char *jnm = jel ? jel->Attribute("name") : NULL;
00048       if(!!tnm) ROS_INFO("tnm: %s", tnm);
00049       if(!!jnm) ROS_INFO("jnm: %s", jnm);
00050       if(!!anm) ROS_INFO("anm: %s", anm);
00051 #endif
00052       if (!!anm) {
00053         hw_->addActuator(new pr2_hardware_interface::Actuator(anm));
00054       }
00055     }
00056 
00057     // Create controller manager
00058     cm_ = boost::shared_ptr<pr2_controller_manager::ControllerManager>
00059       (new pr2_controller_manager::ControllerManager(hw_, nh_));
00060 
00061     // Initialize the controller manager from robot description
00062     if (!cm_->initXml(root)) {
00063       ROS_FATAL("Could not initialize the controller manager");
00064       throw "end";
00065     } else {
00066       ROS_INFO("success to initialize the controller manager");
00067     }
00068 
00069     for (std::vector<pr2_mechanism_model::Transmission*>::iterator it = cm_->model_.transmissions_.begin();
00070          it != cm_->model_.transmissions_.end();
00071          ++it) { // *** js and ac must be consistent
00072       ROS_INFO_STREAM("JointState: " << (*it)->joint_names_[0] << " / Actuator: " << (*it)->actuator_names_[0]);
00073       pr2_mechanism_model::JointState *js = cm_->state_->getJointState((*it)->joint_names_[0]);
00074       pr2_hardware_interface::Actuator *ac = hw_->getActuator((*it)->actuator_names_[0]);
00075       if(!!ac) { ac->state_.is_enabled_ = true; }
00076       if(!!js) { js->calibrated_ = true; }
00077       if (!!ac && !!js) {
00078         jointactuator ja;
00079         ja.js = (void *)js;
00080         ja.ac = (void *)ac;
00081         jointactuator_map_.insert
00082           (std::map< std::string, jointactuator >::value_type ((*it)->joint_names_[0], ja));
00083       }
00084     }
00085 
00086     pub_ = nh_.advertise< osrf_msgs::JointCommands > ("commands", 1);
00087     srv_ = pnh.advertiseService ("query", &HandController::service_cb, this);
00088     if (auto_start_) {
00089       startSubscribe();
00090     }
00091 
00092     spinner_thread_ =
00093       boost::thread (boost::bind (&HandController::ManagerThread, this));
00094   }
00095 
00096   ~HandController() {
00097 
00098   }
00099 
00100   void startSubscribe() {
00101     sub_ = nh_.subscribe("in_joint_states", 100, &HandController::jointCallback, this);
00102   }
00103 
00104   bool service_cb (roseus::StringString::Request &req,
00105                    roseus::StringString::Response &res) {
00106     if (req.str == "stop") {
00107       if (sub_) {
00108         sub_.shutdown();
00109         return true;
00110       }
00111     } else if (req.str == "start") {
00112       if (!sub_) {
00113         startSubscribe();
00114         hw_->current_time_ = ros::Time::now();
00115         cm_->update();
00116         return true;
00117       }
00118     } else if (req.str == "query") {
00119       // return start or stop
00120     }
00121     res.str = "unknown_command";
00122     return false;
00123   }
00124 
00125   void jointCallback(const sensor_msgs::JointStateConstPtr& simjs) {
00126     ROS_DEBUG("callback");
00127     hw_->current_time_ = simjs->header.stamp;
00128 
00129     for(size_t i = 0; i < simjs->name.size(); i++) {
00130       jointactuator ja = jointactuator_map_[simjs->name[i]];
00131       //pr2_mechanism_model::JointState *js = (pr2_mechanism_model::JointState *)ja.js;
00132       pr2_hardware_interface::Actuator *ac = (pr2_hardware_interface::Actuator *)ja.ac;
00133 
00134       ac->state_.position_ = simjs->position[i];
00135       ac->state_.velocity_ = simjs->velocity[i];
00136     }
00137 #if 0
00138     pr2_mechanism_model::JointState *js = cm_->state_->getJointState("left_f0_j0");
00139     pr2_hardware_interface::Actuator *ac = hw_->getActuator("left_f0_j0_motor");
00140     ROS_INFO("%f A: %s, js pos: %f, vel %f, mes: %f, com %f / ac pos: %f, vel: %f, com: %f",
00141              simjs->header.stamp.toSec(),
00142              simjs->name[0].c_str(),
00143              js->position_,
00144              js->velocity_,
00145              js->measured_effort_,
00146              js->commanded_effort_,
00147              ac->state_.position_,
00148              ac->state_.velocity_,
00149              ac->command_.effort_);
00150 #endif
00151     //
00152     cm_->update();
00153 
00154 #if 0
00155     ROS_INFO("%f B: %s, js pos: %f, vel %f, mes: %f, com %f / ac pos: %f, vel: %f, com: %f",
00156              simjs->header.stamp.toSec(),
00157              simjs->name[0].c_str(),
00158              js->position_,
00159              js->velocity_,
00160              js->measured_effort_,
00161              js->commanded_effort_,
00162              ac->state_.position_,
00163              ac->state_.velocity_,
00164              ac->command_.effort_);
00165 #endif
00166 
00167     osrf_msgs::JointCommands joint_com;
00168     joint_com.name = simjs->name; //??
00169     joint_com.position.resize(joint_com.name.size());
00170 #if 0
00171     joint_com.velocity.resize(joint_com.name.size()); // 0
00172     joint_com.effort.resize(joint_com.name.size());   // 0
00173     joint_com.kp_position.resize(joint_com.name.size());
00174     joint_com.kd_position.resize(joint_com.name.size());
00175     joint_com.ki_position.resize(joint_com.name.size()); // 0
00176     joint_com.kp_velocity.resize(joint_com.name.size()); // 0
00177     joint_com.i_effort_min.resize(joint_com.name.size());// 0
00178     joint_com.i_effort_max.resize(joint_com.name.size());// 0
00179 #else
00180     joint_com.velocity.resize(0);
00181     joint_com.effort.resize(0);
00182     joint_com.kp_position.resize(0);
00183     joint_com.kd_position.resize(0);
00184     joint_com.ki_position.resize(0);
00185     joint_com.kp_velocity.resize(0);
00186     joint_com.i_effort_min.resize(0);
00187     joint_com.i_effort_max.resize(0);
00188 #endif
00189     for(size_t i = 0; i < joint_com.name.size(); i++) {
00190       jointactuator ja = jointactuator_map_[joint_com.name[i]];
00191       pr2_mechanism_model::JointState *js = (pr2_mechanism_model::JointState *)ja.js;
00192       pr2_hardware_interface::Actuator *ac = (pr2_hardware_interface::Actuator *)ja.ac;
00193 
00194       joint_com.position[i] = js->position_ + ac->command_.effort_; // joint Desired
00195 #if 0
00196       joint_com.velocity[i] = 0.0;   // 0
00197       joint_com.effort[i]   = 0.0;   // 0
00198       joint_com.kp_position[i] = 21.0;  // joint P gain
00199       joint_com.kd_position[i] = 0.004; // joint D gain
00200       joint_com.ki_position[i] = 0.0; // 0
00201       joint_com.kp_velocity[i] = 0.0; // 0
00202       joint_com.i_effort_min[i] = 0.0;// 0
00203       joint_com.i_effort_max[i] = 0.0;// 0
00204 #endif
00205     }
00206     pub_.publish(joint_com);
00207   }
00208 
00209   void ManagerThread() {
00210     ROS_INFO_STREAM("Callback thread id=" << boost::this_thread::get_id());
00211     while (ros::ok()) {
00212       usleep(1000);
00213       ros::spinOnce();
00214     }
00215   }
00216 
00217 protected:
00218   typedef struct jointactuator {
00219     void *js;
00220     void *ac;
00221   } jointactuator;
00222 
00223   ros::NodeHandle nh_;
00224   boost::thread spinner_thread_;
00225   pr2_hardware_interface::HardwareInterface* hw_;
00226   boost::shared_ptr<pr2_controller_manager::ControllerManager> cm_;
00227   ros::Publisher pub_;
00228   ros::Subscriber sub_;
00229   ros::ServiceServer srv_;
00230 
00231   // osrf_msgs::JointCommands joint_com_;
00232   std::map <std::string, jointactuator> jointactuator_map_; // name -> js,ac pointer
00233 
00234   bool auto_start_;
00235 private:
00236 };
00237 
00238 int main(int argc, char** argv) {
00239   ros::init(argc, argv, "sandia_hand_controller");
00240   HandController hcontroller;
00241 
00242   ros::spin();
00243 
00244   return 0;
00245 }


hrpsys_gazebo_atlas
Author(s): Yohei Kakiuchi
autogenerated on Wed Sep 16 2015 10:53:59