nao_teleop: nao_teleop: teleop_nao

00001 // SVN $HeadURL: http://alufr-ros-pkg.googlecode.com/svn/trunk/humanoid_stacks/nao_common/nao_teleop/src/teleop_nao_joy.cpp $
00002 // SVN $Id: teleop_nao_joy.cpp 2804 2012-06-12 13:14:39Z hornunga@informatik.uni-freiburg.de $
00003 
00004 /*
00005  * Nao Joystick / Gamepad teleoperation
00006  *
00007  * Copyright 2009-2012 Armin Hornung, University of Freiburg
00008  * http://www.ros.org/wiki/nao
00009  *
00010  * Redistribution and use in source and binary forms, with or without
00011  * modification, are permitted provided that the following conditions are met:
00012  *
00013  *     * Redistributions of source code must retain the above copyright
00014  *       notice, this list of conditions and the following disclaimer.
00015  *     * Redistributions in binary form must reproduce the above copyright
00016  *       notice, this list of conditions and the following disclaimer in the
00017  *       documentation and/or other materials provided with the distribution.
00018  *     * Neither the name of the University of Freiburg nor the names of its
00019  *       contributors may be used to endorse or promote products derived from
00020  *       this software without specific prior written permission.
00021  *
00022  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00023  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00024  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00025  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00026  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00027  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00028  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00029  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00030  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00031  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  * POSSIBILITY OF SUCH DAMAGE.
00033  */
00034 
00035 #include <nao_teleop/teleop_nao_joy.h>
00036 
00037 // switch between diamondback /electric:
00038 #if ROS_VERSION_MINIMUM(1,6,0)
00039 #include <sensor_msgs/Joy.h>
00040 using sensor_msgs::Joy;
00041 #else
00042 #include <joy/Joy.h>
00043 using joy::Joy;
00044 #endif
00045 
00046 namespace nao_teleop{
00047 TeleopNaoJoy::TeleopNaoJoy()
00048 : privateNh("~"), m_enabled(false),
00049   m_xAxis(3), m_yAxis(2), m_turnAxis(0), m_headYawAxis(4),      m_headPitchAxis(5),
00050   m_crouchBtn(9), m_initPoseBtn(0), m_enableBtn(8), m_modifyHeadBtn(5),
00051   m_maxVx(1.0), m_maxVy(1.0), m_maxVw(0.5),
00052   m_maxHeadYaw(2.0943), m_maxHeadPitch(0.7853),
00053   m_bodyPoseTimeOut(5.0),
00054   m_inhibitCounter(0), m_previousJoystick_initialized(false),
00055   m_bodyPoseClient("body_pose", true)
00056 {
00057   privateNh.param("axis_x", m_xAxis, m_xAxis);
00058   privateNh.param("axis_y", m_yAxis, m_yAxis);
00059   privateNh.param("axis_turn", m_turnAxis, m_turnAxis);
00060   privateNh.param("axis_head_yaw", m_headYawAxis, m_headYawAxis);
00061   privateNh.param("axis_head_pitch", m_headPitchAxis, m_headPitchAxis);
00062   privateNh.param("btn_crouch", m_crouchBtn, m_crouchBtn);
00063   privateNh.param("btn_init_pose", m_initPoseBtn, m_initPoseBtn);
00064   privateNh.param("btn_enable_control", m_enableBtn, m_enableBtn);
00065   privateNh.param("btn_head_mod", m_modifyHeadBtn, m_modifyHeadBtn);
00066   privateNh.param("max_vx", m_maxVx, m_maxVx);
00067   privateNh.param("max_vy", m_maxVy, m_maxVy);
00068   privateNh.param("max_vw", m_maxVw, m_maxVw);
00069 
00070   privateNh.param("max_head_yaw", m_maxHeadYaw, m_maxHeadYaw);
00071   privateNh.param("max_head_pitch", m_maxHeadPitch, m_maxHeadPitch);
00072 
00073   m_motion.linear.x = m_motion.linear.y = m_motion.angular.z = 0.0;
00074   m_headAngles.joint_names.push_back("HeadYaw");
00075   m_headAngles.joint_names.push_back("HeadPitch");
00076   m_headAngles.joint_angles.resize(2, 0.0f);
00077   m_headAngles.relative = 0;
00078   m_headAngles.speed = 0.2; // TODO: param
00079 
00080   m_movePub = nh.advertise<geometry_msgs::Twist>("cmd_vel", 10);
00081   m_headPub = nh.advertise<nao_msgs::JointAnglesWithSpeed>("joint_angles", 1);
00082   m_speechPub = nh.advertise<std_msgs::String>("speech", 1);
00083   m_inhibitWalkSrv = nh.advertiseService("inhibit_walk", &TeleopNaoJoy::inhibitWalk, this);
00084   m_uninhibitWalkSrv = nh.advertiseService("uninhibit_walk", &TeleopNaoJoy::uninhibitWalk, this);
00085   m_cmdVelClient = nh.serviceClient<nao_msgs::CmdVelService>("cmd_vel_srv");
00086   m_stiffnessDisableClient = nh.serviceClient<std_srvs::Empty>("body_stiffness/disable");
00087   m_stiffnessEnableClient = nh.serviceClient<std_srvs::Empty>("body_stiffness/enable");
00088 
00089 
00090   if (!m_bodyPoseClient.waitForServer(ros::Duration(3.0))){
00091     ROS_WARN_STREAM("Could not connect to \"body_pose\" action server, "
00092         << "there will be no body poses available on button presses.\n"
00093         << "Is the pose_manager node running?");
00094   }
00095 
00096 }
00097 
00098 
00099 ros::Subscriber TeleopNaoJoy::subscribeToJoystick() {
00100   return subscribeToJoystick(&TeleopNaoJoy::joyCallback, this);
00101 }
00102 
00103 bool TeleopNaoJoy::callBodyPoseClient(const std::string& poseName){
00104   if (!m_bodyPoseClient.isServerConnected()){
00105     return false;
00106   }
00107 
00108   nao_msgs::BodyPoseGoal goal;
00109   goal.pose_name = poseName;
00110   m_bodyPoseClient.sendGoalAndWait(goal, m_bodyPoseTimeOut);
00111   actionlib::SimpleClientGoalState state = m_bodyPoseClient.getState();
00112   if (state != actionlib::SimpleClientGoalState::SUCCEEDED){
00113     ROS_ERROR("Pose action \"%s\" did not succeed (%s): %s", goal.pose_name.c_str(), state.toString().c_str(), state.text_.c_str());
00114     return false;
00115   } else{
00116     ROS_INFO("Pose action \"%s\" succeeded", goal.pose_name.c_str());
00117     return true;
00118   }
00119 
00120 }
00121 
00122 
00123 void TeleopNaoJoy::initializePreviousJoystick(const Joy::ConstPtr& joy) {
00124   if(!m_previousJoystick_initialized) {
00125     // if no previous joystick message has been received
00126     // assume all buttons and axes have been zero
00127     //
00128     Joy::Ptr pJoy(new Joy());
00129     pJoy->buttons.resize( joy->buttons.size(), 0);
00130     pJoy->axes.resize( joy->axes.size(), 0.0);
00131     m_previousJoystick = pJoy;
00132     m_previousJoystick_initialized = true;
00133   }
00134 
00135 }
00136 
00137 
00143 void TeleopNaoJoy::joyCallback(const Joy::ConstPtr& joy){
00144   initializePreviousJoystick(joy);
00145 
00146   // Buttons:
00147   // TODO: make buttons generally configurable by mapping btn_id => pose_string
00148 
00149   if (m_enabled && buttonTriggered(m_crouchBtn, joy) && m_bodyPoseClient.isServerConnected()){
00150     if (callBodyPoseClient("crouch")){
00151       std_srvs::Empty e;
00152       m_stiffnessDisableClient.call(e);
00153     }
00154   }
00155 
00156   if (m_enabled && buttonTriggered(m_initPoseBtn, joy) && m_bodyPoseClient.isServerConnected()){
00157     callBodyPoseClient("init");
00158   }
00159 
00160   if (buttonTriggered(m_enableBtn, joy)){
00161     std_msgs::String string;
00162     if (m_enabled){
00163       m_enabled = false;
00164       string.data = "Gamepad control disabled";
00165 
00166     } else{
00167       m_enabled = true;
00168       string.data = "Gamepad control enabled";
00169       std_srvs::Empty e;
00170       m_stiffnessEnableClient.call(e);
00171     }
00172     m_speechPub.publish(string);
00173     ROS_INFO("%s", (string.data).c_str());
00174 
00175   }
00176 
00177 
00178   // directional commands
00179   // walking velocities and head movements
00180   if (!axisValid(m_xAxis, joy) ||  !axisValid(m_yAxis, joy) || !axisValid(m_turnAxis, joy)){
00181     m_motion.linear.x = m_motion.linear.y = m_motion.angular.z = 0.0;
00182     m_headAngles.joint_angles[0] = m_headAngles.joint_angles[1] = 0.0;
00183     ROS_WARN("Joystick message too short for Move or Turn axis!\n");
00184   } else{
00185     if (buttonPressed(m_modifyHeadBtn, joy)){
00186       // move head
00187       m_headAngles.header.stamp = ros::Time::now();
00188       m_headAngles.relative = 1;
00189       m_headAngles.joint_angles[0] = joy->axes[m_turnAxis];
00190       m_headAngles.joint_angles[1] = joy->axes[m_xAxis];
00191 
00192     } else {
00193       // stop head:
00194       m_headAngles.joint_angles[0] = m_headAngles.joint_angles[1] = 0.0;
00195       // move base:
00196       m_motion.linear.x = m_maxVx * std::max(std::min(joy->axes[m_xAxis], 1.0f), -1.0f);
00197       m_motion.linear.y = m_maxVy * std::max(std::min(joy->axes[m_yAxis], 1.0f), -1.0f);
00198       m_motion.angular.z = m_maxVw * std::max(std::min(joy->axes[m_turnAxis], 1.0f), -1.0f);
00199     }
00200   }
00201 
00202   /*
00203       // Head pos:
00204       if (!axisValid(m_headPitchAxis, joy) || !axisValid(m_headYawAxis, joy)){
00205       m_headAngles.absolute = 0;
00206       m_headAngles.yaw = 0.0;
00207       m_headAngles.pitch = 0.0;
00208       } else {
00209       m_headAngles.absolute = 0;
00210       m_headAngles.yaw = joy->axes[m_headYawAxis];
00211       m_headAngles.pitch = joy->axes[m_headPitchAxis];
00212       }
00213    */
00214 
00215   setPreviousJoystick(joy);
00216 
00217 }
00218 
00226 bool TeleopNaoJoy::buttonPressed(int button, const Joy::ConstPtr& joy) const{
00227   return (button >= 0 && unsigned(button) < joy->buttons.size() && joy->buttons[button] == 1);
00228 }
00229 
00237 bool TeleopNaoJoy::buttonTriggered(int button, const Joy::ConstPtr& joy) const{
00238   return (buttonPressed(button, joy) && buttonChanged(button, joy, m_previousJoystick));
00239 }
00240 
00241 
00242 
00252 bool TeleopNaoJoy::buttonChanged(int button, const Joy::ConstPtr& joy, const Joy::ConstPtr& prevJoy) const{
00253   return (unsigned(button) < joy->buttons.size() && joy->buttons[button] != prevJoy->buttons[button] );
00254 }
00255 
00263 bool TeleopNaoJoy::axisValid(int axis, const Joy::ConstPtr& joy) const{
00264   return (axis >= 0 && unsigned(axis) < joy->buttons.size());
00265 }
00266 
00270 void TeleopNaoJoy::pubMsg(){
00271   if (m_enabled && m_inhibitCounter == 0) {
00272     m_movePub.publish(m_motion);
00273     m_headPub.publish(m_headAngles);
00274   }
00275 }
00276 
00289 bool TeleopNaoJoy::inhibitWalk(std_srvs::EmptyRequest& /*req*/, std_srvs::EmptyResponse& res) {
00290   if(m_inhibitCounter == 0) {
00291     // not yet inhibited: publish zero walk command before inhibiting joystick
00292     m_motion.linear.x = 0.0;
00293     m_motion.linear.y = 0.0;
00294     m_motion.linear.z = 0.0;
00295     m_motion.angular.x = 0.0;
00296     m_motion.angular.y = 0.0;
00297     m_motion.angular.z = 0.0;
00298     nao_msgs::CmdVelService service_call;
00299     service_call.request.twist = m_motion;
00300     m_cmdVelClient.call(service_call);
00301   }
00302   m_inhibitCounter++;
00303   ROS_DEBUG("Inhibit counter: %d", m_inhibitCounter);
00304   return true;
00305 }
00306 
00317 bool TeleopNaoJoy::uninhibitWalk(std_srvs::EmptyRequest &req, std_srvs::EmptyResponse& res) {
00318   if(m_inhibitCounter > 0) {
00319     m_inhibitCounter--;
00320     ROS_DEBUG("Inhibit counter: %d", m_inhibitCounter);
00321   } else {
00322     m_inhibitCounter = 0;
00323     ROS_WARN("/uninhibit_walk called more times than /inhibit_walk - ignoring");
00324   }
00325   return true;
00326 }
00327 }
00328