cob_teleop: cob_teleop: cob_teleop.cpp Source File

00001 
00064 #include <cstdlib>
00065 #include <cstdio>
00066 #include <unistd.h>
00067 #include <XmlRpcValue.h>
00068 #include <ros/ros.h>
00069 #include <sensor_msgs/Joy.h>
00070 #include <sensor_msgs/JointState.h>
00071 #include <trajectory_msgs/JointTrajectory.h>
00072 #include <geometry_msgs/Twist.h>
00073 
00074 #include <cob_srvs/Trigger.h>
00075 #include <brics_actuator/JointPositions.h>
00076 #include <brics_actuator/JointVelocities.h>
00077 
00078 const int PUBLISH_FREQ = 100.0;
00079 
00085 class TeleopCOB
00086 {
00087 public:
00088         struct joint_module{
00089                 std::string key;
00090                 std::vector<std::string> joint_names;
00091                 std::vector<double> req_joint_pos_;
00092                 std::vector<double> req_joint_vel_;
00093                 std::vector<double> steps;
00094                 ros::Publisher module_publisher_;
00095                 ros::Publisher module_publisher_brics_;
00096         };
00097 
00098         std::map<std::string,joint_module> joint_modules_; //std::vector<std::string> module_names;
00099 
00100         struct base_module_struct{
00101                 std::vector<double> req_vel_;
00102                 std::vector<double> vel_old_; //,vy_old_,vth_old_;
00103                 std::vector<double> max_vel_; //max_vx_,max_vy_,max_vth_;
00104                 std::vector<double> max_acc_; //max_ax_,max_ay_,max_ath_;
00105                 ros::Publisher base_publisher_;
00106                 ros::Publisher base_safety_publisher_;
00107         } base_module_;
00108 
00109         bool has_base_module_;
00110 
00111         int lower_neck_button_,upper_neck_button_; //buttons
00112         int tray_button_;
00113         int axis_vx_,axis_vy_,axis_vth_;
00114         int arm_joint12_button_;
00115         int arm_joint34_button_;
00116         int arm_joint56_button_;
00117         int arm_joint7_button_;
00118         //signs
00119         int up_down_, left_right_;   //sign for movements of upper_neck and tray
00120 
00121         //common
00122         int deadman_button_, run_button_, base_safety_button_, stop_base_button_, recover_base_button_;
00123         bool joy_active_, stopped_, base_safety_;
00124         double run_factor_, run_factor_param_;
00125 
00126 
00127         ros::NodeHandle n_;
00128         ros::Subscriber joy_sub_;  //subscribe topic joy
00129         ros::Subscriber joint_states_sub_;  //subscribe topic joint_states
00130 
00131         bool got_init_values_;
00132         double time_for_init_;
00133 
00134         struct combined_joints_struct{
00135                 std::vector<std::string> joint_names_;
00136                 std::vector<double> joint_init_values_;
00137                 std::vector<joint_module*> module_ref_;
00138         }combined_joints_;
00139         std::vector<std::string> joint_names_;
00140         std::vector<double> joint_init_values_;
00141 
00142         TeleopCOB();
00143         void getConfigurationFromParameters();
00144         void init();
00145         void joy_cb(const sensor_msgs::Joy::ConstPtr &joy_msg);
00146         void joint_states_cb(const sensor_msgs::JointState::ConstPtr &joint_states_msg);
00147         void update();
00148         void update_joint_modules();
00149         void update_base();
00150         void setInitValues();
00151         ~TeleopCOB();
00152 
00153 private:
00154         bool assign_joint_module(std::string,XmlRpc::XmlRpcValue);
00155         bool assign_base_module(XmlRpc::XmlRpcValue);
00156 };
00157 
00158 void TeleopCOB::getConfigurationFromParameters()
00159 {
00160         //std::map<std::string,joint_module> joint_modules; //std::vector<std::string> module_names;
00161         if(n_.hasParam("modules"))
00162         {
00163                 XmlRpc::XmlRpcValue modules;
00164                 ROS_DEBUG("modules found ");
00165                 n_.getParam("modules", modules);
00166                 if(modules.getType() == XmlRpc::XmlRpcValue::TypeStruct)
00167                 {
00168                         ROS_DEBUG("modules are of type struct with size %d",(int)modules.size());
00169 
00170                         for(std::map<std::string,XmlRpc::XmlRpcValue>::iterator p=modules.begin();p!=modules.end();++p)
00171                         {
00172                                 std::string mod_name = p->first;
00173                                 ROS_DEBUG("module name: %s",mod_name.c_str());
00174                                 XmlRpc::XmlRpcValue mod_struct = p->second;
00175                                 if(mod_struct.getType() != XmlRpc::XmlRpcValue::TypeStruct)
00176                                         ROS_WARN("invalid module, name: %s",mod_name.c_str());
00177                                 // search for joint_name parameter in current module struct to determine which type of module
00178                                 // only joint mods or wheel mods supported
00179                                 // which mens that is no joint names are found, then the module is a wheel module
00180                                 // TODO replace with build in find, but could not get it to work
00181                                 if(!assign_joint_module(mod_name, mod_struct))
00182                                 {
00183                                         // add wheel module struct
00184                                         ROS_DEBUG("wheel module found");
00185                                         assign_base_module(mod_struct);
00186                                 }
00187                         }
00188                 }
00189         }
00190 }
00191 
00203 bool TeleopCOB::assign_joint_module(std::string mod_name, XmlRpc::XmlRpcValue mod_struct)
00204 {
00205         // search for joint_name parameter in current module struct to determine which type of module
00206         // only joint mods or wheel mods supported
00207         // which mens that is no joint names are found, then the module is a wheel module
00208         // TODO replace with build in find, but could not get it to work
00209         bool is_joint_module = false;
00210         joint_module tempModule;
00211         for(std::map<std::string,XmlRpc::XmlRpcValue>::iterator ps=mod_struct.begin();ps!=mod_struct.end();++ps)
00212         {
00213                 std::string par_name = ps->first;
00214                 ROS_DEBUG("par name: %s",par_name.c_str());
00215 
00216                 if(par_name.compare("joint_names")==0)
00217                 {
00218                         ROS_DEBUG("joint names found");
00219                         XmlRpc::XmlRpcValue joint_names = ps->second;
00220 
00221                         ROS_ASSERT(joint_names.getType() == XmlRpc::XmlRpcValue::TypeArray);
00222                         ROS_DEBUG("joint_names.size: %d \n", joint_names.size());
00223                         for(int i=0;i<joint_names.size();i++)
00224                         {
00225                                 ROS_ASSERT(joint_names[i].getType() == XmlRpc::XmlRpcValue::TypeString);
00226                                 std::string s((std::string)joint_names[i]);
00227                                 ROS_DEBUG("joint_name found = %s",s.c_str());
00228                                 tempModule.joint_names.push_back(s);
00229                         }
00230                         // set size of other vectors according to the joint name vector
00231                         tempModule.req_joint_pos_.resize(joint_names.size());
00232                         tempModule.req_joint_vel_.resize(joint_names.size());
00233 
00234                         is_joint_module = true;
00235                         //break; // no need to continue searching if joint names are found
00236                 }else if(par_name.compare("joint_step")==0){
00237                         ROS_DEBUG("joint steps found");
00238                         XmlRpc::XmlRpcValue joint_steps = ps->second;
00239 
00240                         ROS_ASSERT(joint_steps.getType() == XmlRpc::XmlRpcValue::TypeArray);
00241                         ROS_DEBUG("joint_steps.size: %d \n", joint_steps.size());
00242                         for(int i=0;i<joint_steps.size();i++)
00243                         {
00244                                 ROS_ASSERT(joint_steps[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
00245                                 double step((double)joint_steps[i]);
00246                                 ROS_DEBUG("joint_step found = %f",step);
00247                                 tempModule.steps.push_back(step);
00248                         }
00249                 }
00250         }
00251         if(is_joint_module)
00252         {
00253                 // assign publisher
00254                 tempModule.module_publisher_ = n_.advertise<trajectory_msgs::JointTrajectory>(("/"+mod_name+"_controller/command"),1);
00255                 tempModule.module_publisher_brics_ = n_.advertise<brics_actuator::JointVelocities>(("/"+mod_name+"_controller/command_vel"),1);
00256                 // store joint module in collection
00257                 ROS_DEBUG("head module stored");
00258                 joint_modules_.insert(std::pair<std::string,joint_module>(mod_name,tempModule));
00259         }
00260         return is_joint_module;
00261 }
00273 bool TeleopCOB::assign_base_module(XmlRpc::XmlRpcValue mod_struct)
00274 {
00275         for(std::map<std::string,XmlRpc::XmlRpcValue>::iterator ps=mod_struct.begin();ps!=mod_struct.end();++ps)
00276         {
00277                 std::string par_name = ps->first;
00278                 ROS_DEBUG("par name: %s",par_name.c_str());
00279 
00280                 if(par_name.compare("max_velocity")==0)
00281                 {
00282                         ROS_DEBUG("max vel found");
00283                         XmlRpc::XmlRpcValue max_vel = ps->second;
00284 
00285                         ROS_ASSERT(max_vel.getType() == XmlRpc::XmlRpcValue::TypeArray);
00286                         if(max_vel.size()!=3){ROS_WARN("invalid base parameter size");}
00287                         ROS_DEBUG("max_vel.size: %d \n", max_vel.size());
00288                         for(int i=0;i<max_vel.size();i++)
00289                         {
00290                                 ROS_ASSERT(max_vel[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
00291                                 double val = (double)max_vel[i];
00292                                 ROS_DEBUG("max vel value = %f",val);
00293                                 base_module_.max_vel_.push_back(val);
00294                         }
00295                 }
00296                 else if(par_name.compare("max_acceleration")==0)
00297                 {
00298                         ROS_DEBUG("max acc found");
00299                         XmlRpc::XmlRpcValue max_acc = ps->second;
00300 
00301                         ROS_ASSERT(max_acc.getType() == XmlRpc::XmlRpcValue::TypeArray);
00302                         if(max_acc.size()!=3){ROS_DEBUG("invalid base parameter size");}
00303                         ROS_DEBUG("max_acc.size: %d \n", max_acc.size());
00304                         for(int i=0;i<max_acc.size();i++)
00305                         {
00306                                 ROS_ASSERT(max_acc[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
00307                                 double val = (double)max_acc[i];
00308                                 ROS_DEBUG("max acc value = %f", val);
00309                                 base_module_.max_acc_.push_back(val);
00310                         }
00311                 }
00312                 else
00313                 {
00314                         ROS_WARN("unsupported base module parameter read");
00315                 }
00316         }
00317         // make all the vectors the same length
00318         // the vector size is not completely safe since only warning is
00319         // issued if max value arrays has the wrong length
00320         base_module_.req_vel_.resize(base_module_.max_acc_.size());
00321         base_module_.vel_old_.resize(base_module_.max_acc_.size());
00322         base_module_.base_publisher_ = n_.advertise<geometry_msgs::Twist>("/cmd_vel",1);
00323         base_module_.base_safety_publisher_ = n_.advertise<geometry_msgs::Twist>("/cmd_vel_safety",1);
00324         ROS_DEBUG("base module stored");
00325         has_base_module_ = true;
00326         return true;
00327 }
00328 
00332 TeleopCOB::TeleopCOB()
00333 {
00334         getConfigurationFromParameters(); // assign configuration and subscribe to topics
00335         got_init_values_ = false;
00336         time_for_init_ = 0.0;
00337         joy_active_ = false;
00338         run_factor_ = 1.0;
00339 
00340         // add all found joint names to joint_names_vector, which is used to pass values to the state aggregator
00341         for(std::map<std::string,joint_module>::iterator module_it=joint_modules_.begin();module_it!=joint_modules_.end();++module_it){
00342                 std::vector<std::string> names = (module_it->second).joint_names;
00343                 for(unsigned int i=0; i<names.size();i++){
00344                         joint_names_.push_back(names[i]);
00345                         combined_joints_.joint_names_.push_back(names[i]); // puch joit name to combined collection
00346                         combined_joints_.joint_init_values_.push_back(0.0); // be sure that a init value is related to the joint name
00347                         combined_joints_.module_ref_.push_back((joint_module*)(&(module_it->second))); // store a reference to the module containing the joint
00348                 }
00349         }
00350         joint_init_values_.resize(joint_names_.size());
00351 }
00352 
00356 TeleopCOB::~TeleopCOB()
00357 {
00358 }
00359 
00363 void TeleopCOB::init()
00364 {
00365         // common
00366         n_.param("run_factor",run_factor_param_,1.5);
00367 
00368         // assign buttons
00369         n_.param("lower_neck_button",lower_neck_button_,6);
00370         n_.param("upper_neck_button",upper_neck_button_,4);
00371         n_.param("tray_button",tray_button_,3);
00372         n_.param("arm_joint12_button",arm_joint12_button_,0);
00373         n_.param("arm_joint34_button",arm_joint34_button_,1);
00374         n_.param("arm_joint56_button",arm_joint56_button_,2);
00375         n_.param("arm_joint7_button",arm_joint7_button_,3);
00376         n_.param("deadman_button",deadman_button_,5);
00377         n_.param("run_button",run_button_,7);
00378         n_.param("base_safety_button",base_safety_button_,4);
00379         n_.param("stop_base_button",stop_base_button_,8);
00380         n_.param("recover_base_button",recover_base_button_,9);
00381 
00382         // assign axis
00383         n_.param("axis_vx",axis_vx_,1);
00384         n_.param("axis_vy",axis_vy_,0);
00385         n_.param("axis_vth",axis_vth_,2);
00386         n_.param("up_down",up_down_,5); //axis[5] tray--up/down; tilt--front/back, here we just name up_down
00387         n_.param("left_right",left_right_,4);  //axis[4] pan--left/right
00388 
00389         // output for debugging
00390         ROS_DEBUG("init::lower_neck_button: %d",lower_neck_button_);
00391         ROS_DEBUG("init::upper_neck_button: %d",upper_neck_button_);
00392         ROS_DEBUG("init::tray_button: %d",tray_button_);
00393         ROS_DEBUG("init::arm_joint12_button: %d",arm_joint12_button_);
00394         ROS_DEBUG("init::arm_joint34_button: %d",arm_joint34_button_);
00395         ROS_DEBUG("init::arm_joint56_button: %d",arm_joint56_button_);
00396         ROS_DEBUG("init::arm_joint7_button: %d",arm_joint7_button_);
00397         ROS_DEBUG("init::deadman_button: %d",deadman_button_);
00398         ROS_DEBUG("init::run_button: %d",run_button_);
00399         ROS_DEBUG("init::stop_base_button: %d",stop_base_button_);
00400         ROS_DEBUG("init::recover_base_button: %d",recover_base_button_);
00401 
00402         ROS_DEBUG("init::axis_vx: %d",axis_vx_);
00403         ROS_DEBUG("init::axis_vy: %d",axis_vy_);
00404         ROS_DEBUG("init::axis_vth: %d",axis_vth_);
00405         ROS_DEBUG("init::up_down: %d",up_down_);
00406         ROS_DEBUG("init::left_right: %d",left_right_);
00407 
00408         // TODO general!!!
00409         joy_sub_ = n_.subscribe("/joy",1,&TeleopCOB::joy_cb,this);
00410         joint_states_sub_ = n_.subscribe("/joint_states",1,&TeleopCOB::joint_states_cb,this);
00411 }
00412 
00416 void TeleopCOB::setInitValues()
00417 {
00418 
00419         // loop trough all the joints in the combined collection
00420         for(unsigned int i=0; i<combined_joints_.joint_init_values_.size();i++){
00421                 //loop trough all the joints in module containing joint settings,
00422                 //and try to find the one with a name matching the currently browsed joint
00423                 //in the combined collection
00424                 for(unsigned int j=0; j<combined_joints_.module_ref_[i]->joint_names.size();j++){
00425                         // if the matching joint is found, assign value to pos command and stop looking for this name
00426                         if(combined_joints_.module_ref_[i]->joint_names[j].compare(combined_joints_.joint_names_[i])==0){
00427                                 combined_joints_.module_ref_[i]->req_joint_pos_[j] = combined_joints_.joint_init_values_[i];
00428                                 combined_joints_.module_ref_[i]->req_joint_vel_[j] = 0.0; // initalize velocity cmd to 0
00429                                 break; // node found, break the search
00430                         }
00431                 }
00432         }
00433 
00434         // base init values (velocities) are already set to 0 by default
00435 
00436         got_init_values_ = true;
00437 }
00438 
00446 void TeleopCOB::joint_states_cb(const sensor_msgs::JointState::ConstPtr &joint_states_msg)
00447 {
00448         if (!got_init_values_ && stopped_ && joy_active_)
00449         {
00450                 ROS_DEBUG("joint_states_cb: getting init values");
00451                 for (unsigned int j = 0; j<joint_names_.size(); j++ )
00452                 {
00453                         for (unsigned int i = 0; i<joint_states_msg->name.size(); i++ )
00454                         {
00455                                 ROS_DEBUG("joint names in init: %s should match %s",joint_names_[j].c_str(),joint_states_msg->name[i].c_str());
00456                                 if (joint_states_msg->name[i] == joint_names_[j])
00457                                 {
00458                                         joint_init_values_[j] = joint_states_msg->position[i];
00459                                         if(joint_names_[j]!=combined_joints_.joint_names_[j])
00460                                                 ROS_ERROR("error in new joint name collection, name miss match.");
00461                                         combined_joints_.joint_init_values_[j] = joint_states_msg->position[i]; //new
00462                                         ROS_DEBUG("joint %s found. init value = %f",joint_names_[j].c_str(),joint_init_values_[j]);
00463                                         break;
00464                                 }
00465                         }
00466                 }
00467                 setInitValues();
00468         }
00469 }
00470 
00478 void TeleopCOB::joy_cb(const sensor_msgs::Joy::ConstPtr &joy_msg)
00479 {
00480         // deadman button to activate joystick
00481         if(deadman_button_>=0 && deadman_button_<(int)joy_msg->buttons.size() && joy_msg->buttons[deadman_button_]==1)
00482         {
00483                 if (!joy_active_)
00484                 {
00485                         ROS_INFO("joystick is active");
00486                         joy_active_ = true;
00487                         got_init_values_ = false;
00488                 }
00489         }
00490         else
00491         {
00492                 ROS_DEBUG("joystick is not active");
00493                 joy_active_ = false;
00494                 return;
00495         }
00496 
00497         // run button
00498         if(run_button_>=0 && run_button_<(int)joy_msg->buttons.size() && joy_msg->buttons[run_button_]==1)
00499         {
00500                 run_factor_ = run_factor_param_;
00501         }
00502         else //button release
00503         {
00504                 run_factor_ = 1.0;
00505         }
00506         
00507         // base safety button
00508         if(base_safety_button_>=0 && base_safety_button_<(int)joy_msg->buttons.size() && joy_msg->buttons[base_safety_button_]==1)
00509         {
00510                 base_safety_ = false;
00511         }
00512         else //button release
00513         {
00514                 base_safety_ = true;
00515         }
00516         
00517         // recover base button
00518         if(recover_base_button_>=0 && recover_base_button_<(int)joy_msg->buttons.size() && joy_msg->buttons[recover_base_button_]==1)
00519         {
00520                 ros::ServiceClient client_init_base = n_.serviceClient<cob_srvs::Trigger>("/base_controller/init");
00521         
00522                 ROS_INFO("Init base");
00523                 cob_srvs::Trigger srv = cob_srvs::Trigger();
00524                 if (client_init_base.call(srv))
00525                 {
00526                         ROS_INFO("Base init successfully");
00527                 }
00528                 else
00529                 {
00530                         ROS_ERROR("Failed to call service /base_controller/init");
00531                 }
00532                 
00533                 ros::ServiceClient client_recover_base = n_.serviceClient<cob_srvs::Trigger>("/base_controller/recover");
00534         
00535                 ROS_INFO("Recover base");
00536                 if (client_recover_base.call(srv))
00537                 {
00538                         ROS_INFO("Base recovered successfully");
00539                 }
00540                 else
00541                 {
00542                         ROS_ERROR("Failed to call service /base_controller/recover");
00543                 }
00544         }
00545         
00546         // stop base button
00547         if(stop_base_button_>=0 && stop_base_button_<(int)joy_msg->buttons.size() && joy_msg->buttons[stop_base_button_]==1)
00548         {
00549                 ros::ServiceClient client_stop_base = n_.serviceClient<cob_srvs::Trigger>("/base_controller/stop");
00550         
00551                 ROS_INFO("Stop base");
00552                 cob_srvs::Trigger srv = cob_srvs::Trigger();
00553                 if (client_stop_base.call(srv))
00554                 {
00555                         ROS_INFO("Base stop successfully");
00556                 }
00557                 else
00558                 {
00559                         ROS_ERROR("Failed to call service /base_controller/stop");
00560                 }
00561         }
00562 
00563         // TODO add map for buttons
00564 
00565         // head
00566         if(joint_modules_.find("head")!=joint_modules_.end())
00567         {
00568                 if(upper_neck_button_>=0 &&
00569                                 upper_neck_button_<(int)joy_msg->buttons.size() &&
00570                                 joy_msg->buttons[upper_neck_button_]==1)
00571                 {
00572                         //pan (turn)
00573                         if(left_right_>=0 && left_right_<(int)joy_msg->axes.size() && joy_msg->axes[left_right_]<0.0)
00574                                 joint_modules_["head"].req_joint_vel_[0] = (int)joy_msg->buttons[upper_neck_button_]*joint_modules_["head"].steps[0]*run_factor_;//upper_pan_step_*run_factor_;
00575                         else if(left_right_>=0 && left_right_<(int)joy_msg->axes.size() && joy_msg->axes[left_right_]>0.0)
00576                                 joint_modules_["head"].req_joint_vel_[0] = -1*(int)joy_msg->buttons[upper_neck_button_]*joint_modules_["head"].steps[0]*run_factor_;//upper_pan_step_*run_factor_;
00577                         else
00578                                 joint_modules_["head"].req_joint_vel_[0]= 0.0;
00579                         ROS_DEBUG("cb::upper neck pan velocity: %f",joint_modules_["head"].req_joint_vel_[0]);
00580 
00581                         //tilt (nod)
00582                         if(up_down_>=0 && up_down_<(int)joy_msg->axes.size() && joy_msg->axes[up_down_]>0.0)
00583                                 joint_modules_["head"].req_joint_vel_[1] = -1*(int)joy_msg->buttons[upper_neck_button_]*joint_modules_["head"].steps[1]*run_factor_;//upper_tilt_step_*run_factor_;
00584                         else if(up_down_>=0 && up_down_<(int)joy_msg->axes.size() && joy_msg->axes[up_down_]<0.0)
00585                                 joint_modules_["head"].req_joint_vel_[1] = (int)joy_msg->buttons[upper_neck_button_]*joint_modules_["head"].steps[1]*run_factor_;//upper_tilt_step_*run_factor_;
00586                         else
00587                                 joint_modules_["head"].req_joint_vel_[1] = 0.0;
00588                         ROS_DEBUG("cb::upper neck tilt velocity: %f",joint_modules_["head"].req_joint_vel_[1]);
00589 
00590                 }
00591                 else
00592                 {
00593                         joint_modules_["head"].req_joint_vel_[0] = 0.0;
00594                         joint_modules_["head"].req_joint_vel_[1] = 0.0;
00595                 }
00596         }
00597         if(joint_modules_.find("torso")!=joint_modules_.end())
00598         {
00599                 // torso TODO update with general as well
00600                 //lower neck
00601                 if(lower_neck_button_>=0 &&
00602                                 lower_neck_button_<(int)joy_msg->buttons.size() &&
00603                                 joy_msg->buttons[lower_neck_button_]==1)
00604                 {
00605                         //pan
00606                         if(left_right_>=0 && left_right_<(int)joy_msg->axes.size() && joy_msg->axes[left_right_]<0.0)
00607                                 joint_modules_["torso"].req_joint_vel_[0] = (int)joy_msg->buttons[lower_neck_button_]*joint_modules_["torso"].steps[0]*run_factor_; //req_lower_pan_vel_
00608                         else if(left_right_>=0 && left_right_<(int)joy_msg->axes.size() && joy_msg->axes[left_right_]>0.0)
00609                                 joint_modules_["torso"].req_joint_vel_[0] = -1*(int)joy_msg->buttons[lower_neck_button_]*joint_modules_["torso"].steps[0]*run_factor_; //req_lower_pan_vel_
00610                         else
00611                                 joint_modules_["torso"].req_joint_vel_[0] = 0.0;
00612                         ROS_DEBUG("cb::lower neck pan velocity: %f",joint_modules_["torso"].req_joint_vel_[0]);
00613 
00614                         //tilt
00615                         if(up_down_>=0 && up_down_<(int)joy_msg->axes.size() && joy_msg->axes[up_down_]>0.0)
00616                                 joint_modules_["torso"].req_joint_vel_[1] = (int)joy_msg->buttons[lower_neck_button_]*joint_modules_["torso"].steps[1]*run_factor_; //req_lower_tilt_vel_
00617                         else if(up_down_>=0 && up_down_<(int)joy_msg->axes.size() && joy_msg->axes[up_down_]<0.0)
00618                                 joint_modules_["torso"].req_joint_vel_[1] = -1*(int)joy_msg->buttons[lower_neck_button_]*joint_modules_["torso"].steps[1]*run_factor_;
00619                         else
00620                                 joint_modules_["torso"].req_joint_vel_[1] = 0.0;
00621                         ROS_DEBUG("cb::lower neck tilt velocity: %f",joint_modules_["torso"].req_joint_vel_[1]);
00622                 }
00623                 else //button release
00624                 {
00625                         joint_modules_["torso"].req_joint_vel_[0] = 0.0;
00626                         joint_modules_["torso"].req_joint_vel_[1] = 0.0;
00627                 }
00628 
00629                 //upper neck
00630                 if(upper_neck_button_>=0 &&
00631                                 upper_neck_button_<(int)joy_msg->buttons.size() &&
00632                                 joy_msg->buttons[upper_neck_button_]==1)
00633                 {
00634                         //pan
00635                         if(left_right_>=0 && left_right_<(int)joy_msg->axes.size() && joy_msg->axes[left_right_]<0.0)
00636                                 joint_modules_["torso"].req_joint_vel_[2] = (int)joy_msg->buttons[upper_neck_button_]*joint_modules_["torso"].steps[2]*run_factor_;
00637                         else if(left_right_>=0 && left_right_<(int)joy_msg->axes.size() && joy_msg->axes[left_right_]>0.0)
00638                                 joint_modules_["torso"].req_joint_vel_[2] = -1*(int)joy_msg->buttons[upper_neck_button_]*joint_modules_["torso"].steps[2]*run_factor_;
00639                         else
00640                                 joint_modules_["torso"].req_joint_vel_[2] = 0.0;
00641                         ROS_DEBUG("cb::upper neck pan velocity: %f",joint_modules_["torso"].req_joint_vel_[2]);
00642 
00643                         //tilt
00644                         if(up_down_>=0 && up_down_<(int)joy_msg->axes.size() && joy_msg->axes[up_down_]>0.0)
00645                                 joint_modules_["torso"].req_joint_vel_[3] = (int)joy_msg->buttons[upper_neck_button_]*joint_modules_["torso"].steps[3]*run_factor_;
00646                         else if(up_down_>=0 && up_down_<(int)joy_msg->axes.size() && joy_msg->axes[up_down_]<0.0)
00647                                 joint_modules_["torso"].req_joint_vel_[3] = -1*(int)joy_msg->buttons[upper_neck_button_]*joint_modules_["torso"].steps[3]*run_factor_;
00648                         else
00649                                 joint_modules_["torso"].req_joint_vel_[3] = 0.0;
00650                         ROS_DEBUG("cb::upper neck tilt velocity: %f",joint_modules_["torso"].req_joint_vel_[3]);
00651                 }
00652                 else //button release
00653                 {
00654                         joint_modules_["torso"].req_joint_vel_[2] = 0.0;
00655                         joint_modules_["torso"].req_joint_vel_[3] = 0.0;
00656                 }
00657         }
00658 
00659         if(joint_modules_.find("tray")!=joint_modules_.end())
00660         {
00661                 //tray
00662                 if(tray_button_>=0 && tray_button_<(int)joy_msg->buttons.size() && joy_msg->buttons[tray_button_]==1)
00663                 {
00664                         if(up_down_>=0 && up_down_<(int)joy_msg->axes.size() && joy_msg->axes[up_down_]>0.0)
00665                                 joint_modules_["tray"].req_joint_vel_[0] = (int)joy_msg->buttons[tray_button_]*joint_modules_["tray"].steps[0]*run_factor_;
00666                         else if(up_down_>=0 && up_down_<(int)joy_msg->axes.size() && joy_msg->axes[up_down_]<0.0)
00667                                 joint_modules_["tray"].req_joint_vel_[0] = -1*(int)joy_msg->buttons[tray_button_]*joint_modules_["tray"].steps[0]*run_factor_;
00668                         else
00669                                 joint_modules_["tray"].req_joint_vel_[0] = 0.0;
00670                         ROS_DEBUG("cb::tray velocity: %f",joint_modules_["tray"].req_joint_vel_[0]);
00671                 }
00672                 else //button release
00673                 {
00674                         joint_modules_["tray"].req_joint_vel_[0] = 0.0;
00675                 }
00676         }
00677         if(joint_modules_.find("arm")!=joint_modules_.end())
00678         {
00679                 //arm
00680                 //publish_arm_ = false;
00681 
00682                 //joint12
00683                 if(joint_modules_["arm"].req_joint_vel_.size()>1 && arm_joint12_button_>=0 && arm_joint12_button_<(int)joy_msg->buttons.size() && joy_msg->buttons[arm_joint12_button_]==1)
00684                 {
00685                         //joint 1 left or right
00686                         if(left_right_>=0 && left_right_<(int)joy_msg->axes.size() && joy_msg->axes[left_right_]<0.0)
00687                                 joint_modules_["arm"].req_joint_vel_[0] = -1*(int)joy_msg->buttons[arm_joint12_button_]*joint_modules_["arm"].steps[0]*run_factor_;
00688                         else if(left_right_>=0 && left_right_<(int)joy_msg->axes.size() && joy_msg->axes[left_right_]>0.0)
00689                                 joint_modules_["arm"].req_joint_vel_[0] = (int)joy_msg->buttons[arm_joint12_button_]*joint_modules_["arm"].steps[0]*run_factor_;
00690                         else
00691                                 joint_modules_["arm"].req_joint_vel_[0] = 0.0;
00692                         ROS_DEBUG("cb::arm joint1 velocity: %f",joint_modules_["arm"].req_joint_vel_[0]);
00693 
00694                         //joint 2 up or down
00695                         if(up_down_>=0 && up_down_<(int)joy_msg->axes.size() && joy_msg->axes[up_down_]>0.0)
00696                                 joint_modules_["arm"].req_joint_vel_[1] = (int)joy_msg->buttons[arm_joint12_button_]*joint_modules_["arm"].steps[1]*run_factor_;
00697                         else if(up_down_>=0 && up_down_<(int)joy_msg->axes.size() && joy_msg->axes[up_down_]<0.0)
00698                                 joint_modules_["arm"].req_joint_vel_[1] = -1*(int)joy_msg->buttons[arm_joint12_button_]*joint_modules_["arm"].steps[1]*run_factor_;
00699                         else
00700                                 joint_modules_["arm"].req_joint_vel_[1] = 0.0;
00701                         ROS_DEBUG("cb::arm joint2 velocity: %f",joint_modules_["arm"].req_joint_vel_[1]);
00702                         //publish_arm_ = true;
00703                 }
00704                 else //button release
00705                 {
00706                         joint_modules_["arm"].req_joint_vel_[0] = 0.0;
00707                         joint_modules_["arm"].req_joint_vel_[1] = 0.0;
00708                 } //arm_joint12
00709 
00710                 //joint34
00711                 if(joint_modules_["arm"].req_joint_vel_.size()>3 && arm_joint34_button_>=0 && arm_joint34_button_<(int)joy_msg->buttons.size() && joy_msg->buttons[arm_joint34_button_]==1)
00712                 {
00713                         //joint 3 left or right
00714                         if(left_right_>=0 && left_right_<(int)joy_msg->axes.size() && joy_msg->axes[left_right_]<0.0)
00715                                 joint_modules_["arm"].req_joint_vel_[2] = -1*(int)joy_msg->buttons[arm_joint34_button_]*joint_modules_["arm"].steps[2]*run_factor_;
00716                         else if(left_right_>=0 && left_right_<(int)joy_msg->axes.size() && joy_msg->axes[left_right_]>0.0)
00717                                 joint_modules_["arm"].req_joint_vel_[2] = (int)joy_msg->buttons[arm_joint34_button_]*joint_modules_["arm"].steps[2]*run_factor_;
00718                         else
00719                                 joint_modules_["arm"].req_joint_vel_[2] = 0.0;
00720                         ROS_DEBUG("cb::arm joint3 velocity: %f",joint_modules_["arm"].req_joint_vel_[2]);
00721 
00722                         //joint 4 up or down
00723                         if(up_down_>=0 && up_down_<(int)joy_msg->axes.size() && joy_msg->axes[up_down_]>0.0)
00724                                 joint_modules_["arm"].req_joint_vel_[3] = (int)joy_msg->buttons[arm_joint34_button_]*joint_modules_["arm"].steps[3]*run_factor_;
00725                         else if(up_down_>=0 && up_down_<(int)joy_msg->axes.size() && joy_msg->axes[up_down_]<0.0)
00726                                 joint_modules_["arm"].req_joint_vel_[3] = -1*(int)joy_msg->buttons[arm_joint34_button_]*joint_modules_["arm"].steps[3]*run_factor_;
00727                         else
00728                                 joint_modules_["arm"].req_joint_vel_[3] = 0.0;
00729                         ROS_DEBUG("cb::arm joint4 velocity: %f",joint_modules_["arm"].req_joint_vel_[3]);
00730                         //publish_arm_ = true;
00731                 }
00732                 else //button release
00733                 {
00734                         joint_modules_["arm"].req_joint_vel_[2] = 0.0;
00735                         joint_modules_["arm"].req_joint_vel_[3] = 0.0;
00736                 } //arm_joint34
00737 
00738                 //joint56
00739                 if(joint_modules_["arm"].req_joint_vel_.size()>4 && arm_joint56_button_>=0 && arm_joint56_button_<(int)joy_msg->buttons.size() && joy_msg->buttons[arm_joint56_button_]==1)
00740                 {
00741                         //joint 5 left or right
00742                         if(left_right_>=0 && left_right_<(int)joy_msg->axes.size() && joy_msg->axes[left_right_]<0.0)
00743                                 joint_modules_["arm"].req_joint_vel_[4] = -1*(int)joy_msg->buttons[arm_joint56_button_]*joint_modules_["arm"].steps[4]*run_factor_;
00744                         else if(left_right_>=0 && left_right_<(int)joy_msg->axes.size() && joy_msg->axes[left_right_]>0.0)
00745                                 joint_modules_["arm"].req_joint_vel_[4] = (int)joy_msg->buttons[arm_joint56_button_]*joint_modules_["arm"].steps[4]*run_factor_;
00746                         else
00747                                 joint_modules_["arm"].req_joint_vel_[4] = 0.0;
00748                         ROS_DEBUG("cb::arm joint5 velocity: %f",joint_modules_["arm"].req_joint_vel_[4]);
00749 
00750                         //joint 6 up or down
00751                         if(up_down_>=0 && up_down_<(int)joy_msg->axes.size() && joy_msg->axes[up_down_]>0.0)
00752                                 joint_modules_["arm"].req_joint_vel_[5] = (int)joy_msg->buttons[arm_joint56_button_]*joint_modules_["arm"].steps[5]*run_factor_;
00753                         else if(up_down_>=0 && up_down_<(int)joy_msg->axes.size() && joy_msg->axes[up_down_]<0.0)
00754                                 joint_modules_["arm"].req_joint_vel_[5] = -1*(int)joy_msg->buttons[arm_joint56_button_]*joint_modules_["arm"].steps[5]*run_factor_;
00755                         else
00756                                 joint_modules_["arm"].req_joint_vel_[5] = 0.0;
00757                         ROS_DEBUG("cb::arm joint6 velocity: %f",joint_modules_["arm"].req_joint_vel_[5]);
00758                         //publish_arm_ = true;
00759                 }
00760                 else //button release
00761                 {
00762                         joint_modules_["arm"].req_joint_vel_[4] = 0.0;
00763                         joint_modules_["arm"].req_joint_vel_[5] = 0.0;
00764                 } //arm_joint56
00765 
00766                 //joint7
00767                 if(joint_modules_["arm"].req_joint_vel_.size()>5 && arm_joint7_button_>=0 && arm_joint7_button_<(int)joy_msg->buttons.size() && joy_msg->buttons[arm_joint7_button_]==1)
00768                 {
00769                         //joint 7 left or right
00770                         if(left_right_>=0 && left_right_<(int)joy_msg->axes.size() && joy_msg->axes[left_right_]<0.0)
00771                         {
00772                                 joint_modules_["arm"].req_joint_vel_[6] = -1*(int)joy_msg->buttons[arm_joint7_button_]*joint_modules_["arm"].steps[6]*run_factor_;
00773                                 //publish_arm_ = true;
00774                         }
00775                         else if(left_right_>=0 && left_right_<(int)joy_msg->axes.size() && joy_msg->axes[left_right_]>0.0)
00776                         {
00777                                 joint_modules_["arm"].req_joint_vel_[6] = (int)joy_msg->buttons[arm_joint7_button_]*joint_modules_["arm"].steps[6]*run_factor_;
00778                                 //publish_arm_ = true;
00779                         }
00780                         else
00781                                 joint_modules_["arm"].req_joint_vel_[6] = 0.0;
00782                         ROS_DEBUG("cb::arm joint7 velocity: %f",joint_modules_["arm"].req_joint_vel_[6]);
00783                 }
00784                 else //button release
00785                 {
00786                         joint_modules_["arm"].req_joint_vel_[6] = 0.0;
00787                 } //arm_joint7
00788         }
00789         //================base================
00790         if(has_base_module_ && base_module_.req_vel_.size()==3)
00791         {
00792                 if(axis_vx_>=0 && axis_vx_<(int)joy_msg->axes.size())
00793                         base_module_.req_vel_[0] = joy_msg->axes[axis_vx_]*base_module_.max_vel_[0]*run_factor_;
00794                 else
00795                         base_module_.req_vel_[0] = 0.0;
00796 
00797                 if(axis_vy_>=0 && axis_vy_<(int)joy_msg->axes.size())
00798                         base_module_.req_vel_[1] = joy_msg->axes[axis_vy_]*base_module_.max_vel_[1]*run_factor_;//req_vy_ = joy_msg->axes[axis_vy_]*max_vy_*run_factor_;
00799                 else
00800                         base_module_.req_vel_[1] = 0.0; //req_vy_ = 0.0;
00801 
00802                 if(axis_vth_>=0 && axis_vth_<(int)joy_msg->axes.size())
00803                         base_module_.req_vel_[2] = joy_msg->axes[axis_vth_]*base_module_.max_vel_[2]*run_factor_;//req_vth_ = joy_msg->axes[axis_vth_]*max_vth_*run_factor_;
00804                 else
00805                         base_module_.req_vel_[2] = 0.0; //req_vth_ = 0.0;
00806         }
00807 
00808 }//joy_cb
00809 
00813 void TeleopCOB::update()
00814 {
00815         if (!joy_active_)
00816         {
00817                 if (!stopped_)
00818                 {
00819                         // stop components: send zero for one time
00820                         for(std::map<std::string,joint_module>::iterator module_it=joint_modules_.begin();module_it!=joint_modules_.end();++module_it)
00821                         {
00822                                 for(unsigned int i=0; i<module_it->second.req_joint_vel_.size();i++)
00823                                 {
00824                                         module_it->second.req_joint_vel_[i] = 0.0;
00825                                 }
00826                         }
00827 
00828                         if(has_base_module_)
00829                         {
00830                                 for(unsigned int i=0; i<3; i++){
00831                                         base_module_.req_vel_[i]=0;
00832                                         base_module_.vel_old_[i]=0;
00833                                 }
00834                         }
00835 
00836                         update_joint_modules();
00837                         update_base();
00838                         stopped_ = true;
00839                         ROS_INFO("stopped all components");
00840                 }
00841                 return;
00842         }
00843 
00844         // set initial values
00845         if(!got_init_values_)
00846         {
00847                 if (time_for_init_ < 1.0) // wait for 1 sec, then set init values to 0.0
00848                 {
00849                         ROS_DEBUG("still waiting for initial values, time_for_init_ = %f",time_for_init_);
00850                         time_for_init_ = time_for_init_ + 1.0/PUBLISH_FREQ;
00851                         return;
00852                 }
00853                 else
00854                 {
00855                         ROS_WARN("Timeout waiting for /joint_states message. Setting all init values to 0.0");
00856                         setInitValues();
00857                 }
00858         }
00859 
00860         update_joint_modules();
00861         update_base();
00862         stopped_ = false;
00863 }
00864 
00865 void TeleopCOB::update_joint_modules()
00866 {
00867         double dt = 1.0/double(PUBLISH_FREQ);
00868         double horizon = 3.0*dt;
00869 
00870         joint_module* jointModule;
00871         for(std::map<std::string,joint_module>::iterator it = joint_modules_.begin();it!=joint_modules_.end();++it)
00872         {
00873                 jointModule = (joint_module*)(&(it->second));
00874 
00875                 trajectory_msgs::JointTrajectory traj;
00876                 traj.header.stamp = ros::Time::now()+ros::Duration(0.01);
00877                 traj.points.resize(1);
00878                 brics_actuator::JointVelocities cmd_vel;
00879                 brics_actuator::JointValue joint_vel;
00880                 joint_vel.timeStamp = traj.header.stamp;
00881                 joint_vel.unit = "rad";
00882                 for(unsigned int i = 0; i<jointModule->joint_names.size();i++)
00883                 {
00884                         // as trajectory message
00885                         traj.joint_names.push_back(jointModule->joint_names[i]);
00886                         traj.points[0].positions.push_back(jointModule->req_joint_pos_[i] + jointModule->req_joint_vel_[i]*horizon);
00887                         traj.points[0].velocities.push_back(jointModule->req_joint_vel_[i]);  //lower_neck_pan
00888                         // as brics message
00889                         joint_vel.value = jointModule->req_joint_vel_[i];
00890                         joint_vel.joint_uri = jointModule->joint_names[i];
00891                         cmd_vel.velocities.push_back(joint_vel);
00892                         // update current positions
00893                         jointModule->req_joint_pos_[i] += jointModule->req_joint_vel_[i]*horizon;
00894                 }
00895 
00896                 traj.points[0].time_from_start = ros::Duration(horizon);
00897 
00898                 jointModule->module_publisher_.publish(traj); // TODO, change
00899                 jointModule->module_publisher_brics_.publish(cmd_vel);
00900         }
00901 }
00902 
00906 void TeleopCOB::update_base()
00907 {
00908         if(!has_base_module_)
00909                 return;
00910         double dt = 1.0/double(PUBLISH_FREQ);
00911         double v[] = {0.0,0.0,0.0};
00912 
00913         for( int i =0; i<3; i++)
00914         {
00915                 // filter v with ramp
00916                 if ((base_module_.req_vel_[i]-base_module_.vel_old_[i])/dt > base_module_.max_acc_[i])
00917                 {
00918                         v[i] = base_module_.vel_old_[i] + base_module_.max_acc_[i]*dt;
00919                 }
00920                 else if((base_module_.req_vel_[i]-base_module_.vel_old_[i])/dt < -base_module_.max_acc_[i])
00921                 {
00922                         v[i] = base_module_.vel_old_[i] - base_module_.max_acc_[i]*dt;
00923                 }
00924                 else
00925                 {
00926                         v[i] = base_module_.req_vel_[i];
00927                 }
00928                 base_module_.vel_old_[i] = v[i];
00929         }
00930 
00931         geometry_msgs::Twist cmd;
00932         cmd.linear.x = v[0]; //vx;
00933         cmd.linear.y = v[1]; //vy;
00934         cmd.angular.z = v[2]; //vth;
00935 
00936         if (base_safety_)
00937         {
00938                 base_module_.base_safety_publisher_.publish(cmd);
00939         }
00940         else
00941         {
00942                 base_module_.base_publisher_.publish(cmd);
00943         }
00944 }
00945 
00951 int main(int argc,char **argv)
00952 {
00953         ros::init(argc,argv,"teleop_cob");
00954         TeleopCOB teleop_cob;
00955         teleop_cob.init();
00956 
00957         ros::Rate loop_rate(PUBLISH_FREQ); //Hz
00958         while(teleop_cob.n_.ok())
00959         {
00960                 ros::spinOnce();
00961                 teleop_cob.update();
00962                 loop_rate.sleep();
00963         }
00964 
00965         exit(0);
00966         return(0);
00967 }
00968