00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032 #include <nao_teleop/teleop_nao_joy.h>
00033 #include <naoqi_bridge_msgs/BodyPoseActionGoal.h>
00034 #include <naoqi_bridge_msgs/CmdVelService.h>
00035 #include <naoqi_bridge_msgs/JointTrajectoryAction.h>
00036
00037 using sensor_msgs::Joy;
00038
00039 namespace nao_teleop{
00040 TeleopNaoJoy::TeleopNaoJoy()
00041 : privateNh("~"), m_enabled(false),
00042 m_xAxis(3), m_yAxis(2), m_turnAxis(0), m_headYawAxis(4), m_headPitchAxis(5),
00043 m_crouchBtn(8), m_initPoseBtn(0), m_enableBtn(9), m_modifyHeadBtn(5),
00044 m_maxVx(1.0), m_maxVy(1.0), m_maxVw(0.5),
00045 m_maxHeadYaw(2.0943), m_maxHeadPitch(0.7853),
00046 m_bodyPoseTimeOut(5.0),
00047 m_inhibitCounter(0), m_previousJoystick_initialized(false),
00048 m_bodyPoseClient("body_pose", true)
00049 {
00050 privateNh.param("axis_x", m_xAxis, m_xAxis);
00051 privateNh.param("axis_y", m_yAxis, m_yAxis);
00052 privateNh.param("axis_turn", m_turnAxis, m_turnAxis);
00053 privateNh.param("axis_head_yaw", m_headYawAxis, m_headYawAxis);
00054 privateNh.param("axis_head_pitch", m_headPitchAxis, m_headPitchAxis);
00055 privateNh.param("btn_crouch", m_crouchBtn, m_crouchBtn);
00056 privateNh.param("btn_init_pose", m_initPoseBtn, m_initPoseBtn);
00057 privateNh.param("btn_enable_control", m_enableBtn, m_enableBtn);
00058 privateNh.param("btn_head_mod", m_modifyHeadBtn, m_modifyHeadBtn);
00059 privateNh.param("max_vx", m_maxVx, m_maxVx);
00060 privateNh.param("max_vy", m_maxVy, m_maxVy);
00061 privateNh.param("max_vw", m_maxVw, m_maxVw);
00062
00063 privateNh.param("max_head_yaw", m_maxHeadYaw, m_maxHeadYaw);
00064 privateNh.param("max_head_pitch", m_maxHeadPitch, m_maxHeadPitch);
00065
00066 m_motion.linear.x = m_motion.linear.y = m_motion.angular.z = 0.0;
00067 m_headAngles.joint_names.push_back("HeadYaw");
00068 m_headAngles.joint_names.push_back("HeadPitch");
00069 m_headAngles.joint_angles.resize(2, 0.0f);
00070 m_headAngles.relative = 0;
00071 m_headAngles.speed = 0.2;
00072
00073 m_movePub = nh.advertise<geometry_msgs::Twist>("cmd_vel", 10);
00074 m_headPub = nh.advertise<naoqi_bridge_msgs::JointAnglesWithSpeed>("joint_angles", 1);
00075 m_speechPub = nh.advertise<std_msgs::String>("speech", 1);
00076 m_inhibitWalkSrv = nh.advertiseService("inhibit_walk", &TeleopNaoJoy::inhibitWalk, this);
00077 m_uninhibitWalkSrv = nh.advertiseService("uninhibit_walk", &TeleopNaoJoy::uninhibitWalk, this);
00078 m_cmdVelClient = nh.serviceClient<naoqi_bridge_msgs::CmdVelService>("cmd_vel_srv");
00079 m_stiffnessDisableClient = nh.serviceClient<std_srvs::Empty>("body_stiffness/disable");
00080 m_stiffnessEnableClient = nh.serviceClient<std_srvs::Empty>("body_stiffness/enable");
00081
00082
00083 if (!m_bodyPoseClient.waitForServer(ros::Duration(3.0))){
00084 ROS_WARN_STREAM("Could not connect to \"body_pose\" action server, "
00085 << "there will be no body poses available on button presses.\n"
00086 << "Is the pose_manager node running?");
00087 }
00088
00089 std::cout << "starting button is set to " << m_enableBtn << std::endl;
00090 }
00091
00092
00093 ros::Subscriber TeleopNaoJoy::subscribeToJoystick() {
00094 return subscribeToJoystick(&TeleopNaoJoy::joyCallback, this);
00095 }
00096
00097 bool TeleopNaoJoy::callBodyPoseClient(const std::string& poseName){
00098 if (!m_bodyPoseClient.isServerConnected()){
00099 return false;
00100 }
00101
00102 naoqi_bridge_msgs::BodyPoseGoal goal;
00103 goal.pose_name = poseName;
00104 m_bodyPoseClient.sendGoalAndWait(goal, m_bodyPoseTimeOut);
00105 actionlib::SimpleClientGoalState state = m_bodyPoseClient.getState();
00106 if (state != actionlib::SimpleClientGoalState::SUCCEEDED){
00107 ROS_ERROR("Pose action \"%s\" did not succeed (%s): %s", goal.pose_name.c_str(), state.toString().c_str(), state.text_.c_str());
00108 return false;
00109 } else{
00110 ROS_INFO("Pose action \"%s\" succeeded", goal.pose_name.c_str());
00111 return true;
00112 }
00113
00114 }
00115
00116
00117 void TeleopNaoJoy::initializePreviousJoystick(const Joy::ConstPtr& joy) {
00118 if(!m_previousJoystick_initialized) {
00119
00120
00121
00122 Joy::Ptr pJoy(new Joy());
00123 pJoy->buttons.resize( joy->buttons.size(), 0);
00124 pJoy->axes.resize( joy->axes.size(), 0.0);
00125 m_previousJoystick = pJoy;
00126 m_previousJoystick_initialized = true;
00127 }
00128
00129 }
00130
00131
00137 void TeleopNaoJoy::joyCallback(const Joy::ConstPtr& joy){
00138 initializePreviousJoystick(joy);
00139
00140
00141
00142
00143 if (m_enabled && buttonTriggered(m_crouchBtn, joy) && m_bodyPoseClient.isServerConnected()){
00144 if (callBodyPoseClient("crouch")){
00145 std_srvs::Empty e;
00146 m_stiffnessDisableClient.call(e);
00147 }
00148 }
00149
00150 if (m_enabled && buttonTriggered(m_initPoseBtn, joy) && m_bodyPoseClient.isServerConnected()){
00151 callBodyPoseClient("init");
00152 }
00153
00154 if (buttonTriggered(m_enableBtn, joy)){
00155 std_msgs::String string;
00156 if (m_enabled){
00157 m_enabled = false;
00158 string.data = "Gamepad control disabled";
00159
00160 } else{
00161 m_enabled = true;
00162 string.data = "Gamepad control enabled";
00163 std_srvs::Empty e;
00164 m_stiffnessEnableClient.call(e);
00165 }
00166 m_speechPub.publish(string);
00167 ROS_INFO("%s", (string.data).c_str());
00168
00169 }
00170
00171
00172
00173
00174 if (!axisValid(m_xAxis, joy) || !axisValid(m_yAxis, joy) || !axisValid(m_turnAxis, joy)){
00175 m_motion.linear.x = m_motion.linear.y = m_motion.angular.z = 0.0f;
00176 m_headAngles.joint_angles[0] = m_headAngles.joint_angles[1] = 0.0f;
00177 ROS_WARN("Joystick message too short for Move or Turn axis!\n");
00178 } else{
00179 if (buttonPressed(m_modifyHeadBtn, joy)){
00180
00181 m_headAngles.header.stamp = ros::Time::now();
00182 m_headAngles.relative = 1;
00183 m_headAngles.joint_angles[0] = joy->axes[m_turnAxis];
00184 m_headAngles.joint_angles[1] = joy->axes[m_xAxis];
00185
00186 } else {
00187
00188 m_headAngles.joint_angles[0] = m_headAngles.joint_angles[1] = 0.0f;
00189
00190 m_motion.linear.x = m_maxVx * std::max(std::min(joy->axes[m_xAxis], 1.0f), -1.0f);
00191 m_motion.linear.y = m_maxVy * std::max(std::min(joy->axes[m_yAxis], 1.0f), -1.0f);
00192 m_motion.angular.z = m_maxVw * std::max(std::min(joy->axes[m_turnAxis], 1.0f), -1.0f);
00193
00194 }
00195 }
00196
00197
00198
00199
00200
00201
00202
00203
00204
00205
00206
00207
00208
00209
00210 setPreviousJoystick(joy);
00211
00212 }
00213
00221 bool TeleopNaoJoy::buttonPressed(int button, const Joy::ConstPtr& joy) const{
00222 return (button >= 0 && unsigned(button) < joy->buttons.size() && joy->buttons[button] == 1);
00223 }
00224
00232 bool TeleopNaoJoy::buttonTriggered(int button, const Joy::ConstPtr& joy) const{
00233 return (buttonPressed(button, joy) && buttonChanged(button, joy, m_previousJoystick));
00234 }
00235
00236
00237
00247 bool TeleopNaoJoy::buttonChanged(int button, const Joy::ConstPtr& joy, const Joy::ConstPtr& prevJoy) const{
00248 return (unsigned(button) < joy->buttons.size() && joy->buttons[button] != prevJoy->buttons[button] );
00249 }
00250
00258 bool TeleopNaoJoy::axisValid(int axis, const Joy::ConstPtr& joy) const{
00259 return (axis >= 0 && unsigned(axis) < joy->buttons.size());
00260 }
00261
00265 void TeleopNaoJoy::pubMsg(){
00266 if (m_enabled && m_inhibitCounter == 0)
00267 {
00268
00269 if (m_headAngles.joint_angles[0] != 0.0f || m_headAngles.joint_angles[1] != 0.0f )
00270 {
00271 m_headPub.publish(m_headAngles);
00272 std::cout << "going to publish head angles" << std::endl;
00273 }
00274 if (m_motion.linear.x != 0.0f || m_motion.linear.y != 0.0f || m_motion.angular.z != 0.0f)
00275 {
00276 m_movePub.publish(m_motion);
00277 std::cout << "going to publish motion commands" << std::endl;
00278 }
00279
00280 }
00281 }
00282
00295 bool TeleopNaoJoy::inhibitWalk(std_srvs::EmptyRequest& , std_srvs::EmptyResponse& res) {
00296 if(m_inhibitCounter == 0) {
00297
00298 m_motion.linear.x = 0.0;
00299 m_motion.linear.y = 0.0;
00300 m_motion.linear.z = 0.0;
00301 m_motion.angular.x = 0.0;
00302 m_motion.angular.y = 0.0;
00303 m_motion.angular.z = 0.0;
00304 naoqi_bridge_msgs::CmdVelService service_call;
00305 service_call.request.twist = m_motion;
00306 m_cmdVelClient.call(service_call);
00307 }
00308 m_inhibitCounter++;
00309 ROS_DEBUG("Inhibit counter: %d", m_inhibitCounter);
00310 return true;
00311 }
00312
00323 bool TeleopNaoJoy::uninhibitWalk(std_srvs::EmptyRequest &req, std_srvs::EmptyResponse& res) {
00324 if(m_inhibitCounter > 0) {
00325 m_inhibitCounter--;
00326 ROS_DEBUG("Inhibit counter: %d", m_inhibitCounter);
00327 } else {
00328 m_inhibitCounter = 0;
00329 ROS_WARN("/uninhibit_walk called more times than /inhibit_walk - ignoring");
00330 }
00331 return true;
00332 }
00333 }
00334