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


cob_teleop
Author(s): Florian Weisshardt
autogenerated on Sun Oct 5 2014 23:03:10