cob_teleop.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *   http://www.apache.org/licenses/LICENSE-2.0
00009 
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  */
00016 
00017 #include <ros/ros.h>
00018 
00019 #include <actionlib/client/simple_action_client.h>
00020 #include <cob_light/LightModes.h>
00021 #include <cob_light/SetLightModeAction.h>
00022 #include <cob_script_server/ScriptAction.h>
00023 #include <cob_sound/SayAction.h>
00024 #include <geometry_msgs/Twist.h>
00025 #include <sensor_msgs/JointState.h>
00026 #include <sensor_msgs/Joy.h>
00027 #include <sensor_msgs/JoyFeedbackArray.h>
00028 #include <std_msgs/Float64MultiArray.h>
00029 #include <std_msgs/ColorRGBA.h>
00030 #include <std_srvs/Trigger.h>
00031 #include <XmlRpcValue.h>
00032 
00033 class CobTeleop
00034 {
00035 public:
00036 
00037   struct component_config
00038   {
00039     std::string key;
00040     std::string twist_topic_name;
00041     std::string twist_safety_topic_name;
00042     std::string vel_group_topic_name;
00043     std::string sss_default_target;
00044     std::vector<double> joint_velocity;
00045     std::vector<double> twist_max_vel; //max_vx_, max_vy_, max_vz_, max_rotx_, max_roty_, max_rotz_
00046     std::vector<double> twist_max_acc; //max_ax_, max_ay_, max_arotz_
00047     ros::Publisher vel_group_controller_publisher_;
00048     ros::Publisher twist_controller_publisher_;
00049     ros::Publisher twist_safety_controller_publisher_;
00050   };
00051 
00052   std::map<std::string,component_config> component_config_;
00053 
00054   //axis
00055   int axis_vx_, axis_vy_, axis_vz_, axis_roll_, axis_pitch_, axis_yaw_;
00056 
00057   //buttons
00058   //mode 1: Base
00059   int run_button_;
00060   //mode 2: Trajectory controller (to default target position using sss.move)
00061   //mode 3: Velocity group controller
00062   int right_indicator_button_;
00063   int left_indicator_button_;
00064   int up_down_button_;
00065   int right_left_button_;
00066   //mode 4: Twist controller
00067 
00068   //common
00069   int deadman_button_;
00070   int safety_button_;
00071   int init_button_;
00072   bool joy_active_;
00073   bool safe_mode_;
00074   double publish_freq_;
00075   double run_factor_, run_factor_param_;
00076   int joy_num_modes_;
00077   int mode_switch_button_;
00078   int mode_;
00079   XmlRpc::XmlRpcValue LEDS_;
00080   XmlRpc::XmlRpcValue led_mode_;
00081 
00082   XmlRpc::XmlRpcValue components_;
00083   ros::NodeHandle n_;
00084   ros::Subscriber joy_sub_;  //subscribe topic joy
00085   ros::Subscriber joint_states_sub_;  //subscribe topic joint_states
00086 
00087   typedef actionlib::SimpleActionClient<cob_script_server::ScriptAction> ScriptClient_;
00088   ScriptClient_ * sss_client_;
00089   cob_script_server::ScriptGoal script_goal_;
00090 
00091   typedef actionlib::SimpleActionClient<cob_sound::SayAction> SayClient_;
00092   SayClient_ * say_client_;
00093   bool enable_sound_;
00094   std::string sound_action_name_;
00095 
00096   typedef actionlib::SimpleActionClient<cob_light::SetLightModeAction> SetLightClient_;
00097   SetLightClient_ * setlight_client_;
00098   bool enable_light_;
00099   std::string light_action_name_;
00100 
00101   std::vector<double> vel_old_;
00102   std::vector<double> vel_req_;
00103   std::vector<double> vel_base_;
00104   bool apply_ramp_;
00105 
00106   void getConfigurationFromParameters();
00107   void init();
00108   void updateBase();
00109   void say(std::string text, bool blocking);
00110   void setLight(int mode);
00111   void joy_cb(const sensor_msgs::Joy::ConstPtr &joy_msg);
00112   sensor_msgs::JoyFeedbackArray switch_mode();
00113 };
00114 
00115 void CobTeleop::getConfigurationFromParameters()
00116 {
00117   if(n_.hasParam("components"))
00118   {
00119     ROS_DEBUG("components found ");
00120     n_.getParam("components", components_);
00121     if(components_.getType() == XmlRpc::XmlRpcValue::TypeStruct)
00122     {
00123       ROS_DEBUG("components are of type struct with size %d",(int)components_.size());
00124       for(std::map<std::string,XmlRpc::XmlRpcValue>::iterator p=components_.begin();p!=components_.end();++p)
00125       {
00126         std::string comp_name = p->first;
00127         ROS_DEBUG("component name: %s",comp_name.c_str());
00128         XmlRpc::XmlRpcValue comp_struc = p->second;
00129         if(comp_struc.getType() != XmlRpc::XmlRpcValue::TypeStruct)
00130         {
00131           ROS_WARN("invalid component, name: %s",comp_name.c_str());
00132         }
00133         component_config tempComponent;
00134         for(std::map<std::string,XmlRpc::XmlRpcValue>::iterator ps=comp_struc.begin();ps!=comp_struc.end();++ps)
00135         {
00136           std::string par_name = ps->first;
00137           ROS_DEBUG("par name: %s",par_name.c_str());
00138           if(par_name.compare("twist_topic_name")==0)
00139           {
00140             ROS_DEBUG("twist topic name found");
00141             XmlRpc::XmlRpcValue twist_topic_name = ps->second;
00142             ROS_ASSERT(twist_topic_name.getType() == XmlRpc::XmlRpcValue::TypeString);
00143             std::string s((std::string)twist_topic_name);
00144             ROS_DEBUG("twist_topic_name found = %s",s.c_str());
00145             tempComponent.twist_topic_name = s;
00146             tempComponent.twist_controller_publisher_ = n_.advertise<geometry_msgs::Twist>((s),1);
00147           }
00148           if(par_name.compare("twist_safety_topic_name")==0)
00149           {
00150             ROS_DEBUG("twist saftey topic name found");
00151             XmlRpc::XmlRpcValue twist_safety_topic_name= ps->second;
00152             ROS_ASSERT(twist_safety_topic_name.getType() == XmlRpc::XmlRpcValue::TypeString);
00153             std::string s((std::string)twist_safety_topic_name);
00154             ROS_DEBUG("twist_safety_topic_name found = %s",s.c_str());
00155             tempComponent.twist_safety_topic_name = s;
00156             tempComponent.twist_safety_controller_publisher_ = n_.advertise<geometry_msgs::Twist>((s),1);
00157           }
00158           else if(par_name.compare("velocity_topic_name")==0)
00159           {
00160             ROS_DEBUG("topic name found");
00161             XmlRpc::XmlRpcValue vel_group_topic_name = ps->second;
00162             ROS_ASSERT(vel_group_topic_name.getType() == XmlRpc::XmlRpcValue::TypeString);
00163             std::string s((std::string)vel_group_topic_name);
00164             ROS_DEBUG("topic_name found = %s",s.c_str());
00165             tempComponent.vel_group_topic_name = s;
00166             tempComponent.vel_group_controller_publisher_ = n_.advertise<std_msgs::Float64MultiArray>((s),1);
00167           }
00168           else if(par_name.compare("sss_default_target")==0)
00169           {
00170             ROS_DEBUG("default target position found");
00171             XmlRpc::XmlRpcValue sss_default_target = ps->second;
00172             ROS_ASSERT(sss_default_target.getType() == XmlRpc::XmlRpcValue::TypeString);
00173             std::string s((std::string)sss_default_target);
00174             ROS_DEBUG("sss_default_target found = %s",s.c_str());
00175             tempComponent.sss_default_target = s;
00176           }
00177           else if(par_name.compare("joint_velocity")==0)
00178           {
00179             ROS_DEBUG("joint vels found");
00180             XmlRpc::XmlRpcValue joint_velocity = ps->second;
00181             ROS_ASSERT(joint_velocity.getType() == XmlRpc::XmlRpcValue::TypeArray);
00182             ROS_DEBUG("joint_velocity.size: %d \n", joint_velocity.size());
00183             for(int i=0;i<joint_velocity.size();i++)
00184             {
00185               ROS_ASSERT(joint_velocity[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
00186               double vel((double)joint_velocity[i]);
00187               ROS_DEBUG("joint_velocity found = %f",vel);
00188               tempComponent.joint_velocity.push_back(vel);
00189             }
00190           }
00191           else if(par_name.compare("twist_max_velocity")==0)
00192           {
00193             ROS_DEBUG("max Velocity found");
00194             XmlRpc::XmlRpcValue twist_max_velocity = ps->second;
00195             ROS_ASSERT(twist_max_velocity.getType() == XmlRpc::XmlRpcValue::TypeArray);
00196             ROS_DEBUG("twist_max_velocity.size: %d \n", twist_max_velocity.size());
00197             for(int i=0;i<twist_max_velocity.size();i++)
00198             {
00199               ROS_ASSERT(twist_max_velocity[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
00200               double vel((double)twist_max_velocity[i]);
00201               ROS_DEBUG("twist_max_velocity found = %f",vel);
00202               tempComponent.twist_max_vel.push_back(vel);
00203             }
00204           }
00205           else if(par_name.compare("twist_max_acc")==0)
00206           {
00207             ROS_DEBUG("max Velocity found");
00208             XmlRpc::XmlRpcValue twist_max_acc = ps->second;
00209             ROS_ASSERT(twist_max_acc.getType() == XmlRpc::XmlRpcValue::TypeArray);
00210             ROS_DEBUG("twist_max_acc.size: %d \n", twist_max_acc.size());
00211             for(int i=0;i<twist_max_acc.size();i++)
00212             {
00213               ROS_ASSERT(twist_max_acc[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
00214               double vel((double)twist_max_acc[i]);
00215               ROS_DEBUG("twist_max_acc found = %f",vel);
00216               tempComponent.twist_max_acc.push_back(vel);
00217             }
00218           }
00219         }
00220         ROS_DEBUG("%s module stored",comp_name.c_str());
00221         component_config_.insert(std::pair<std::string,component_config>(comp_name,tempComponent));
00222       }
00223     }
00224   }
00225 
00226   sss_client_ = new ScriptClient_("/script_server", true);
00227 
00228   n_.param<bool>("enable_sound", enable_sound_, false);
00229   n_.param<std::string>("sound_action_name", sound_action_name_, "/sound/say");
00230   say_client_ = new SayClient_(sound_action_name_, true);
00231 
00232   n_.param<bool>("enable_light", enable_light_, false);
00233   n_.param<std::string>("light_action_name", light_action_name_, "set_light");
00234   setlight_client_ = new SetLightClient_(light_action_name_, true);
00235 
00236   vel_req_.resize(component_config_["base"].twist_max_acc.size());
00237   vel_old_.resize(component_config_["base"].twist_max_acc.size());
00238   vel_base_.resize(component_config_["base"].twist_max_acc.size());
00239   for(unsigned int i=0; i<component_config_["base"].twist_max_acc.size(); i++)
00240   {
00241     vel_old_[i]=0;
00242     vel_req_[i]=0;
00243   }
00244 }
00245 
00246 sensor_msgs::JoyFeedbackArray CobTeleop::switch_mode()
00247 {
00248   ++mode_;
00249 
00250   if (mode_ > joy_num_modes_)
00251   {
00252     mode_ = 1;
00253   }
00254 
00255   std::string saytext;
00256   if (mode_ == 1)
00257   {
00258     ROS_INFO("Switched to mode 1: move the base using twist controller");
00259     saytext = "Base mode";
00260   }
00261   if (mode_ == 2)
00262   {
00263     ROS_INFO("Switched to mode 2: move the actuators to a default position (Trajectory controller)");
00264     saytext = "Default position mode";
00265   }
00266   if(mode_ == 3)
00267   {
00268     ROS_INFO("Switched to mode 3: move the actuators using joint group velocity controller");
00269     saytext = "Velocity mode";
00270   }
00271   if(mode_ == 4)
00272   {
00273     ROS_INFO("Switched to mode 4: move the actuators in cartesian mode using twist controller");
00274     saytext = "Cartesian mode";
00275   }
00276 
00277   setLight(mode_);
00278   say(saytext, false);
00279 
00280   LEDS_=led_mode_[mode_];
00281 
00282   sensor_msgs::JoyFeedbackArray joy_fb;
00283   joy_fb.array.resize(4);
00284   for (unsigned int i=0; i<4; i++)
00285   {
00286       joy_fb.array[i].type=0;
00287       joy_fb.array[i].id=i;
00288       joy_fb.array[i].intensity=static_cast<int>(LEDS_[i]);
00289   }
00290   return joy_fb;
00291 }
00292 
00293 void CobTeleop::updateBase()
00294 {
00295   if (joy_active_)
00296   {
00297     if(mode_==1)
00298     {
00299       double dt = 1.0/double(publish_freq_);
00300       geometry_msgs::Twist base_cmd;
00301       if(!joy_active_)
00302       {
00303         for(unsigned int i=0; i<3; i++)
00304         {
00305           vel_old_[i]=0;
00306           vel_req_[i]=0;
00307         }
00308       }
00309 
00310       if(apply_ramp_)
00311       {
00312         for( int i =0; i<3; i++)
00313         {
00314           // filter v with ramp
00315           if ((vel_req_[i]-vel_old_[i])/dt > component_config_["base"].twist_max_acc[i])
00316           {
00317             vel_base_[i] = vel_old_[i] + component_config_["base"].twist_max_acc[i]*dt;
00318           }
00319           else if((vel_req_[i]-vel_old_[i])/dt < -component_config_["base"].twist_max_acc[i])
00320           {
00321             vel_base_[i] = vel_old_[i] - component_config_["base"].twist_max_acc[i]*dt;
00322           }
00323           else
00324           {
00325             vel_base_[i] = vel_req_[i];
00326           }
00327           vel_old_[i] = vel_base_[i];
00328         }
00329       }
00330       else
00331       {
00332         vel_base_[0] = vel_req_[0];
00333         vel_base_[1] = vel_req_[1];
00334         vel_base_[2] = vel_req_[2];
00335       }
00336       base_cmd.linear.x = vel_base_[0];
00337       base_cmd.linear.y = vel_base_[1];
00338       base_cmd.angular.z = vel_base_[2];
00339       if (safe_mode_)
00340       {
00341         component_config_["base"].twist_safety_controller_publisher_.publish(base_cmd);
00342       }
00343       else
00344       {
00345         component_config_["base"].twist_controller_publisher_.publish(base_cmd);
00346       }
00347     }
00348   }
00349 }
00350 
00351 void CobTeleop::say(std::string text, bool blocking)
00352 {
00353   if(enable_sound_)
00354   {
00355     cob_sound::SayGoal say_goal;
00356     say_goal.text = text;
00357     say_client_->sendGoal(say_goal);
00358     if (blocking)
00359     {
00360       say_client_->waitForResult(ros::Duration(5));
00361     }
00362   }
00363 }
00364 
00365 void CobTeleop::setLight(int mode)
00366 {
00367   if(enable_light_)
00368   {
00369     cob_light::LightMode light;
00370     light.mode = cob_light::LightModes::FLASH;
00371     light.frequency = 5;
00372     light.priority = 2;
00373     std_msgs::ColorRGBA color;
00374     color.r = 0;
00375     color.g = 1;
00376     color.b = 0;
00377     color.a = 1;
00378     light.pulses = mode;
00379     light.colors.push_back(color);
00380     cob_light::SetLightModeGoal light_goal;
00381     light_goal.mode = light;
00382     setlight_client_->sendGoal(light_goal);
00383   }
00384 }
00385 
00386 void CobTeleop::joy_cb(const sensor_msgs::Joy::ConstPtr &joy_msg)
00387 {
00388   if(deadman_button_>=0 && deadman_button_<(int)joy_msg->buttons.size() && joy_msg->buttons[deadman_button_]==1)
00389   {
00390     ROS_DEBUG("joystick is active");
00391   }
00392   else
00393   {
00394     for(unsigned int i=0; i<component_config_["base"].twist_max_acc.size(); i++)
00395     {
00396       vel_req_[i]=0;
00397       vel_old_[i]=0;
00398     }
00399     ROS_DEBUG("joystick is not active");
00400     joy_active_ = false;
00401     return;
00402   }
00403 
00404   if(mode_switch_button_>=0 && mode_switch_button_<(int)joy_msg->buttons.size() && joy_msg->buttons[mode_switch_button_]==1)
00405   {
00406     ROS_INFO("Switch mode button pressed");
00407     switch_mode();
00408   }
00409 
00410   if(safety_button_>=0 && safety_button_<(int)joy_msg->buttons.size() && joy_msg->buttons[safety_button_]==1)
00411   {
00412     safe_mode_ = false;
00413   }else
00414   {
00415     safe_mode_ = true;
00416   }
00417 
00418   if(run_button_>=0 && run_button_<(int)joy_msg->buttons.size() && joy_msg->buttons[run_button_]==1)
00419   {
00420     run_factor_ = run_factor_param_;
00421   }
00422   else //button release
00423   {
00424     run_factor_ = 1.0;
00425   }
00426 
00427   if(!sss_client_->waitForServer(ros::Duration(3.0)))
00428   {
00429     ROS_INFO("Waiting for script server");
00430     return;
00431   }
00432 
00433   if(init_button_>=0 && init_button_<(int)joy_msg->buttons.size() && joy_msg->buttons[init_button_]==1)
00434   {
00435     ROS_INFO("Init and recover");
00436     say("init and recover", true);
00437 
00438     for(std::map<std::string,XmlRpc::XmlRpcValue>::iterator p=components_.begin();p!=components_.end();++p)
00439     {
00440       std::string comp_name = p->first;
00441 
00442       ROS_INFO("Init %s",comp_name.c_str());
00443       std::string saytext = comp_name;
00444       std::replace(saytext.begin(), saytext.end(), '_', ' ');
00445       say(saytext, true);
00446       script_goal_.component_name = comp_name.c_str();
00447       script_goal_.function_name="init";
00448       sss_client_->sendGoal(script_goal_);
00449       sss_client_->waitForResult();
00450       if(sss_client_->getResult()->error_code != 0)
00451       {
00452         std::string saytext = "Init " + comp_name + " failed";
00453         std::replace(saytext.begin(), saytext.end(), '_', ' ');
00454         say(saytext, true);
00455       }
00456 
00457       ROS_INFO("Recover %s",comp_name.c_str());
00458       script_goal_.function_name="recover";
00459       sss_client_->sendGoal(script_goal_);
00460       sss_client_->waitForResult();
00461       if(sss_client_->getResult()->error_code != 0)
00462       {
00463         std::string saytext = "Recover " + comp_name + " failed";
00464         std::replace(saytext.begin(), saytext.end(), '_', ' ');
00465         say(saytext, true);
00466 
00467       }
00468     }
00469 
00470     say("go", true);
00471   }
00472 
00473   //-------MODE 1
00474   if (mode_==1)
00475   {
00476     ROS_DEBUG("Mode 1: Move the base using twist controller");
00477     if(axis_vx_>=0 && axis_vx_<(int)joy_msg->axes.size())
00478     {
00479       joy_active_ = true;
00480       vel_req_[0] = joy_msg->axes[axis_vx_]*component_config_["base"].twist_max_vel[0]*run_factor_;
00481     }
00482     else
00483     {
00484       vel_req_[0] =0.0;
00485     }
00486     if(axis_vy_>=0 && axis_vy_<(int)joy_msg->axes.size())
00487     {
00488       joy_active_ = true;
00489       vel_req_[1] = joy_msg->axes[axis_vy_]*component_config_["base"].twist_max_vel[1]*run_factor_;
00490     }
00491     else
00492     {
00493       vel_req_[1] = 0.0;
00494     }
00495     if(axis_yaw_>=0 && axis_yaw_<(int)joy_msg->axes.size())
00496     {
00497       joy_active_ = true;
00498       vel_req_[2] = joy_msg->axes[axis_yaw_]*component_config_["base"].twist_max_vel[2]*run_factor_;
00499     }
00500     else
00501     {
00502       vel_req_[2] = 0.0;
00503     }
00504   }
00505 
00506   //-------MODE 2
00507   if (mode_==2)
00508   {
00509     ROS_DEBUG("Mode 2: Move the actuators to a default position (Trajectory controller)");
00510 
00511     for(std::map<std::string,XmlRpc::XmlRpcValue>::iterator p=components_.begin();p!=components_.end();++p)
00512     {
00513       int component_sss_default_target_button_temp = -1;
00514       std::string comp_name = p->first;
00515       std::string component_sss_default_target_button = comp_name + "_sss_default_target_button";
00516       n_.getParam(component_sss_default_target_button,component_sss_default_target_button_temp);
00517       if (component_sss_default_target_button_temp == -1)
00518       {
00519         ROS_DEBUG("%s_sss_default_target_button parameter not defined",comp_name.c_str());
00520       }
00521       if(component_sss_default_target_button_temp>=0 && component_sss_default_target_button_temp<(int)joy_msg->buttons.size() && joy_msg->buttons[component_sss_default_target_button_temp]==1)
00522       {
00523         script_goal_.component_name = comp_name;
00524         script_goal_.function_name = "move";
00525         script_goal_.parameter_name = component_config_[comp_name].sss_default_target.c_str();
00526         ROS_INFO("Move %s to %s",comp_name.c_str(), component_config_[comp_name].sss_default_target.c_str());
00527         std::string saytext = "Move " + comp_name + " to " + script_goal_.parameter_name;
00528         std::replace(saytext.begin(), saytext.end(), '_', ' ');
00529         say(saytext, true);
00530         sss_client_->sendGoal(script_goal_);
00531       }
00532     }
00533   }
00534 
00535   //-------MODE 3
00536   if (mode_==3)
00537   {
00538     ROS_DEBUG("Mode 3: Move the actuators using the group velocity controller");
00539       for(std::map<std::string,XmlRpc::XmlRpcValue>::iterator p=components_.begin();p!=components_.end();++p)
00540       {
00541         int count = 0;
00542         int component_joint_button_temp = -1;
00543         std::string comp_name = p->first;
00544         int size = ceil((component_config_[comp_name].joint_velocity.size()+1)/2);
00545         for(int i=0; i < size; ++i)
00546         {
00547           std::ostringstream component_joint_button_stream;
00548           component_joint_button_stream << comp_name << "_joint" << i+1 << "_button";
00549           std::string component_joint_button = component_joint_button_stream.str();
00550           n_.getParam(component_joint_button,component_joint_button_temp);
00551           std_msgs::Float64MultiArray vel_cmd;
00552           count++;
00553           if(!(comp_name.find("left") != std::string::npos) && !(comp_name.find("right") != std::string::npos))
00554           {
00555             if(component_joint_button_temp>=0 && component_joint_button_temp<(int)joy_msg->buttons.size() && joy_msg->buttons[component_joint_button_temp]==1 && joy_msg->buttons[right_indicator_button_]==0 && joy_msg->buttons[left_indicator_button_]==0)
00556             {
00557               joy_active_ = true;
00558               ROS_INFO("%s velocity mode",comp_name.c_str());
00559               vel_cmd.data.resize(component_config_[comp_name].joint_velocity.size());
00560               if(up_down_button_>=0 && up_down_button_<(int)joy_msg->axes.size())
00561               {
00562                 vel_cmd.data[count+i-1]=joy_msg->axes[up_down_button_]*component_config_[comp_name].joint_velocity[count+i-1];
00563               }
00564               if((i+1)*2 <= component_config_[comp_name].joint_velocity.size())
00565               {
00566                 if(right_left_button_>=0 && right_left_button_<(int)joy_msg->axes.size())
00567                 {
00568                   vel_cmd.data[count+i]=joy_msg->axes[right_left_button_]*component_config_[comp_name].joint_velocity[count+i];
00569                 }
00570               }
00571             component_config_[comp_name].vel_group_controller_publisher_.publish(vel_cmd);
00572           }
00573         }
00574         else if(!(comp_name.find("left") != std::string::npos) && (comp_name.find("right") != std::string::npos))
00575         {
00576           if(component_joint_button_temp>=0 && component_joint_button_temp<(int)joy_msg->buttons.size() && joy_msg->buttons[component_joint_button_temp]==1 && joy_msg->buttons[right_indicator_button_]==1)
00577           {
00578             joy_active_ = true;
00579             ROS_INFO("%s velocity mode",comp_name.c_str());
00580             vel_cmd.data.resize(component_config_[comp_name].joint_velocity.size());
00581             if(up_down_button_>=0 && up_down_button_<(int)joy_msg->axes.size())
00582             {
00583               vel_cmd.data[count+i-1]=joy_msg->axes[up_down_button_]*component_config_[comp_name].joint_velocity[count+i-1];
00584             }
00585             if((i+1)*2 <= component_config_[comp_name].joint_velocity.size())
00586             {
00587               if(right_left_button_>=0 && right_left_button_<(int)joy_msg->axes.size())
00588               {
00589                 vel_cmd.data[count+i]=joy_msg->axes[right_left_button_]*component_config_[comp_name].joint_velocity[count+i];
00590               }
00591             }
00592             component_config_[comp_name].vel_group_controller_publisher_.publish(vel_cmd);
00593           }
00594         }
00595         else if((comp_name.find("left") != std::string::npos) && !(comp_name.find("right") != std::string::npos))
00596         {
00597           if(component_joint_button_temp>=0 && component_joint_button_temp<(int)joy_msg->buttons.size() && joy_msg->buttons[component_joint_button_temp]==1 && joy_msg->buttons[left_indicator_button_]==1)
00598           {
00599             joy_active_ = true;
00600             ROS_INFO("%s velocity mode",comp_name.c_str());
00601             vel_cmd.data.resize(component_config_[comp_name].joint_velocity.size());
00602             if(up_down_button_>=0 && up_down_button_<(int)joy_msg->axes.size())
00603             {
00604               vel_cmd.data[count+i-1]=joy_msg->axes[up_down_button_]*component_config_[comp_name].joint_velocity[count+i-1];
00605             }
00606             if((i+1)*2 <= component_config_[comp_name].joint_velocity.size())
00607             {
00608               if(right_left_button_>=0 && right_left_button_<(int)joy_msg->axes.size())
00609               {
00610                 vel_cmd.data[count+i]=joy_msg->axes[right_left_button_]*component_config_[comp_name].joint_velocity[count+i];
00611               }
00612             }
00613             component_config_[comp_name].vel_group_controller_publisher_.publish(vel_cmd);
00614           }
00615         }
00616       }
00617     }
00618   }
00619 
00620   //-------MODE 4
00621   if (mode_==4)
00622   {
00623     ROS_DEBUG("Mode 4: Move the actuators using the twist controller");
00624     for(std::map<std::string,XmlRpc::XmlRpcValue>::iterator p=components_.begin();p!=components_.end();++p)
00625     {
00626       int component_twist_button_temp = -1;
00627       std::string comp_name = p->first;
00628       std::string component_twist_button = comp_name + "_twist_button";
00629       n_.getParam(component_twist_button,component_twist_button_temp);
00630       geometry_msgs::Twist twist_cmd;
00631       if (component_twist_button_temp>=0 && component_twist_button_temp<(int)joy_msg->buttons.size() && joy_msg->buttons[component_twist_button_temp]==1)
00632       {
00633         joy_active_ = true;
00634         ROS_INFO("%s twist mode",comp_name.c_str());
00635         if(axis_vx_>=0 && axis_vx_<(int)joy_msg->axes.size())
00636           twist_cmd.linear.x = joy_msg->axes[axis_vx_]*component_config_[comp_name].twist_max_vel[0]; //*run_factor_;
00637         else
00638           twist_cmd.linear.x =0.0;
00639         if(axis_vy_>=0 && axis_vy_<(int)joy_msg->axes.size())
00640           twist_cmd.linear.y = joy_msg->axes[axis_vy_]*component_config_[comp_name].twist_max_vel[1]; //*run_factor_;
00641         else
00642           twist_cmd.linear.y =0.0;
00643         if(axis_vz_>=0 && axis_vz_<(int)joy_msg->axes.size())
00644           twist_cmd.linear.z = joy_msg->axes[axis_vz_]*component_config_[comp_name].twist_max_vel[2]; //*run_factor_;
00645         else
00646           twist_cmd.linear.z =0.0;
00647         if(axis_roll_>=0 && axis_roll_<(int)joy_msg->axes.size())
00648           twist_cmd.angular.x = joy_msg->axes[axis_roll_]*component_config_[comp_name].twist_max_vel[3]; //*run_factor_;
00649         else
00650           twist_cmd.angular.x =0.0;
00651         if(axis_pitch_>=0 && axis_pitch_<(int)joy_msg->axes.size())
00652           twist_cmd.angular.y = joy_msg->axes[axis_pitch_]*component_config_[comp_name].twist_max_vel[4]; //*run_factor_;
00653         else
00654           twist_cmd.angular.y =0.0;
00655         if(axis_yaw_>=0 && axis_yaw_<(int)joy_msg->axes.size())
00656           twist_cmd.angular.z = joy_msg->axes[axis_yaw_]*component_config_[comp_name].twist_max_vel[5]; //*run_factor_;
00657         else
00658           twist_cmd.angular.z =0.0;
00659         component_config_[comp_name].twist_controller_publisher_.publish(twist_cmd);
00660       }
00661     }
00662   }
00663 }
00664 
00668 void CobTeleop::init()
00669 {
00670   n_ = ros::NodeHandle("~");
00671   if(!n_.hasParam("components"))
00672   {
00673     ROS_ERROR("parameter components does not exist on ROS Parameter Server, aborting...");
00674     exit(0);
00675   }
00676   // common
00677   n_.param("publish_freq",publish_freq_, 30.0);
00678   n_.param("run_factor",run_factor_param_,1.5);
00679   n_.param("apply_ramp",apply_ramp_,true);
00680 
00681   // joy config
00682   n_.param("joy_num_modes",joy_num_modes_,2);
00683   n_.param("mode_switch_button",mode_switch_button_,0);
00684 
00685   // assign axis
00686   n_.param("axis_vx",axis_vx_,17);
00687   n_.param("axis_vy",axis_vy_,16);
00688   n_.param("axis_vz",axis_vz_,17);
00689   n_.param("axis_roll",axis_roll_,16);
00690   n_.param("axis_pitch",axis_pitch_,19);
00691   n_.param("axis_yaw",axis_yaw_,19);
00692 
00693   // assign buttons
00694   n_.param("deadman_button",deadman_button_,11);
00695   n_.param("safety_button",safety_button_,10);
00696   n_.param("init_button",init_button_,3);
00697 
00698   n_.param("run_button",run_button_,9);
00699 
00700   n_.param("right_indicator_button",right_indicator_button_,9);
00701   n_.param("left_indicator_button",left_indicator_button_,8);
00702   n_.param("up_down_button",up_down_button_,4);
00703   n_.param("right_left_button",right_left_button_,5);
00704 
00705   // output for debugging
00706   ROS_DEBUG("init::axis_vx: %d",axis_vx_);
00707   ROS_DEBUG("init::axis_vy: %d",axis_vy_);
00708   ROS_DEBUG("init::axis_vz: %d",axis_vz_);
00709   ROS_DEBUG("init::axis_roll: %d",axis_roll_);
00710   ROS_DEBUG("init::axis_pitch: %d",axis_pitch_);
00711   ROS_DEBUG("init::axis_yaw: %d",axis_yaw_);
00712 
00713   ROS_DEBUG("init::deadman_button: %d",deadman_button_);
00714   ROS_DEBUG("init::safety_button: %d",safety_button_);
00715   ROS_DEBUG("init::init_button: %d",init_button_);
00716   ROS_DEBUG("init::run_button: %d",run_button_);
00717 
00718   ROS_DEBUG("init::right_indicator_button: %d",right_indicator_button_);
00719   ROS_DEBUG("init::left_indicator_button: %d",left_indicator_button_);
00720   ROS_DEBUG("init::up_down_button: %d",up_down_button_);
00721   ROS_DEBUG("init::right_left_button: %d",right_left_button_);
00722 
00723   joy_sub_ = n_.subscribe("/joy",1,&CobTeleop::joy_cb,this);
00724   mode_ = 1;
00725   LEDS_=led_mode_[mode_];
00726   joy_active_ = false;
00727   safe_mode_ = true;
00728 }
00729 
00730 
00731 int main(int argc, char** argv)
00732 {
00733   ros::init(argc, argv, "cob_teleop");
00734   CobTeleop  cob_teleop;
00735   cob_teleop.init();
00736   cob_teleop.getConfigurationFromParameters();
00737   ros::Rate loop_rate(cob_teleop.publish_freq_); //Hz
00738   while(cob_teleop.n_.ok())
00739   {
00740     cob_teleop.updateBase();
00741     ros::spinOnce();
00742     loop_rate.sleep();
00743   }
00744 
00745   exit(0);
00746   return(0);
00747 }


cob_teleop
Author(s): Florian Weisshardt, Maximilian Sieber
autogenerated on Sun Apr 7 2019 03:08:59