// Callback from tactile touch sensors void tactileCallBack(naoqi_msgs::TactileTouch msg){ headTouched = msg.state; ROS_INFO("Head: %d", headTouched); if(countStiffness > 20 && headTouched == 1){ std_srvs::Empty e; actionlib::SimpleActionClient<naoqi_msgs::BodyPoseAction> bodyPoseClient("body_pose", true); callBodyPoseClient("crouch"); sleep(5); stiffnessDisableClient.call(e); exited = true; } }
/** * \brief Callback for joystick messages * * @param joy */ void TeleopNaoJoy::joyCallback(const Joy::ConstPtr& joy) { initializePreviousJoystick(joy); // Buttons: // TODO: make buttons generally configurable by mapping btn_id => pose_string if (m_enabled && buttonTriggered(m_crouchBtn, joy) && m_bodyPoseClient.isServerConnected()) { if (callBodyPoseClient("crouch")) { std_srvs::Empty e; m_stiffnessDisableClient.call(e); } } if (m_enabled && buttonTriggered(m_initPoseBtn, joy) && m_bodyPoseClient.isServerConnected()) { callBodyPoseClient("init"); ROS_INFO("going to init pose"); } /*if(m_enabled && buttonTriggered(m_stopWalkBtn, joy)){ // not yet inhibited: publish zero walk command before inhibiting joystick m_motion.linear.x = 0.0; m_motion.linear.y = 0.0; m_motion.linear.z = 0.0; m_motion.angular.x = 0.0; m_motion.angular.y = 0.0; m_motion.angular.z = 0.0; naoqi_bridge_msgs::CmdVelService service_call; service_call.request.twist = m_motion; m_cmdVelClient.call(service_call); ROS_INFO("-- stop walk asked"); }*/ if (buttonTriggered(m_enableBtn, joy)) { std_msgs::String string; if (m_enabled) { m_enabled = false; string.data = "Gamepad control disabled"; } else { m_enabled = true; string.data = "Gamepad control enabled"; std_srvs::Empty e; m_stiffnessEnableClient.call(e); } m_speechPub.publish(string); ROS_INFO("%s", (string.data).c_str()); } // directional commands // walking velocities and head movements if (!axisValid(m_xAxis, joy) || !axisValid(m_yAxis, joy) || !axisValid(m_turnAxis, joy)) { m_motion.linear.x = m_motion.linear.y = m_motion.angular.z = 0.0f; m_headAngles.joint_angles[0] = m_headAngles.joint_angles[1] = 0.0f; ROS_WARN("Joystick message too short for Move or Turn axis!\n"); } else { if (buttonPressed(m_modifyHeadBtn, joy)) { ROS_INFO_STREAM("moving the head"); // move head m_headAngles.header.stamp = ros::Time::now(); m_headAngles.relative = 1; m_headAngles.joint_angles[0] = joy->axes[m_turnAxis]; m_headAngles.joint_angles[1] = joy->axes[m_xAxis]; } else { // stop head: //m_headAngles.joint_angles[0] = m_headAngles.joint_angles[1] = 0.0f; // move base: m_motion.linear.x = m_maxVx * std::max(std::min(joy->axes[m_xAxis], 1.0f), -1.0f); m_motion.linear.y = m_maxVy * std::max(std::min(joy->axes[m_yAxis], 1.0f), -1.0f); m_motion.angular.z = m_maxVw * std::max(std::min(joy->axes[m_turnAxis], 1.0f), -1.0f); } } /* // Head pos: if (!axisValid(m_headPitchAxis, joy) || !axisValid(m_headYawAxis, joy)){ m_headAngles.absolute = 0; m_headAngles.yaw = 0.0; m_headAngles.pitch = 0.0; } else { m_headAngles.absolute = 0; m_headAngles.yaw = joy->axes[m_headYawAxis]; m_headAngles.pitch = joy->axes[m_headPitchAxis]; } */ setPreviousJoystick(joy); }
// NAO teleoperation control void nao_gesture_control(){ actionlib::SimpleActionClient<naoqi_msgs::BodyPoseAction> bodyPoseClient("body_pose", true); // Teleop system starts when the operator stands up if(!isStandUp && left_hip_y > standUp_mark && right_hip_y > standUp_mark){ callBodyPoseClient("init"); //init countStand++; // Send standup pose command more than one, because sometimes it does not react at first time // Teleoperation starts after robot stands up if(countStand == 5){ isStandUp = true; countStand = 0; std_msgs::String text_msg; text_msg.data = "Active pour contrôler les mouvements de la"; ttsPub.publish(text_msg); ROS_INFO("J'ai leve"); countStiffness=0; teleopStarted = true; } } // Teleop system finishes when the operator takes crouch pose else if(isStandUp && left_hip_y < standUp_mark && right_hip_y < standUp_mark){ callBodyPoseClient("crouch"); countCrouch++; // Send crouch pose command more than one, because sometimes it does not react at first time if(countCrouch == 5){ ROS_INFO("Est-ce"); isStandUp = false; countCrouch = 0; } } // Eserita dagoela, esertzeko posearekin jarraituz gero stiffness desaktibatzen da else if(teleopStarted && !isStandUp && left_hip_y < standUp_mark && right_hip_y < standUp_mark){ countStiffness++; ROS_INFO("countStiffness: %d", countStiffness); //if(countStiffness > 30){ if (countStiffness == 21){ std_msgs::String text_msg; text_msg.data = "Le contrôle des mouvements est desactive"; ttsPub.publish(text_msg); } //} } // When NAO is stand up else if(isStandUp){ if(sonar_status == 0){ // Move forward if(left_foot_z < forward_mark && right_foot_z < forward_mark){ obstacleFront = false; countObstacle = 0; cmd.linear.x = forwardVel; cmd.linear.y = 0.0; // Move forward and turn left if(left_shoulder_x < turnLeft_mark){ cmd.angular.z = turnVel; } // Move forward and turn right else if(right_shoulder_x > turnRight_mark){ cmd.angular.z = -turnVel; } else{ cmd.angular.z = 0.0; } } // Move backward else if(left_foot_z > backward_mark && right_foot_z > backward_mark){ cmd.linear.x = backwardVel; cmd.linear.y = 0.0; // Move backward and turn left if(left_shoulder_x < turnLeft_mark){ cmd.angular.z = turnVel; } // Move backward and turn right else if(right_shoulder_x > turnRight_mark){ cmd.angular.z = -turnVel; } // Move backward without turn else{ cmd.angular.z = 0.0; } } // Move left - lateral displacement else if(left_foot_x < -0.3){ cmd.linear.x = 0.0; cmd.linear.y = lateralVel; } // Move right - lateral displacement else if(right_foot_x > 0.3){ cmd.linear.x = 0.0; cmd.linear.y = -lateralVel; } // Stop movements in the X and Y axes else{ cmd.linear.y = 0.0; cmd.linear.x = 0.0; // Turn left if(left_shoulder_x < turnLeft_mark){ cmd.angular.z = turnVel; } // Turn right else if(right_shoulder_x > turnRight_mark){ cmd.angular.z = -turnVel; } // NAO is completely stopped else{ cmd.angular.z = 0.0; } } } // if(sonar_status == 1){ else { if(leftSonar >= obstacleMinDist && rightSonar >= obstacleMinDist){ // Move forward if(left_foot_z < forward_mark && right_foot_z < forward_mark){ obstacleFront = false; countObstacle = 0; cmd.linear.x = forwardVel; cmd.linear.y = 0.0; // Move forward and turn left if(left_shoulder_x < turnLeft_mark){ cmd.angular.z = turnVel; } // Move forward and turn right else if(right_shoulder_x > turnRight_mark){ cmd.angular.z = -turnVel; } else{ cmd.angular.z = 0.0; } } // Move backward else if(left_foot_z > backward_mark && right_foot_z > backward_mark){ cmd.linear.x = backwardVel; cmd.linear.y = 0.0; // Move backward and turn left if(left_shoulder_x < turnLeft_mark){ cmd.angular.z = turnVel; } // Move backward and turn right else if(right_shoulder_x > turnRight_mark){ cmd.angular.z = -turnVel; } // Move backward without turn else{ cmd.angular.z = 0.0; } } // Move left - lateral displacement else if(left_foot_x < -0.3){ cmd.linear.x = 0.0; cmd.linear.y = lateralVel; } // Move right - lateral displacement else if(right_foot_x > 0.3){ cmd.linear.x = 0.0; cmd.linear.y = -lateralVel; } // Stop movements in the X and Y axes else{ cmd.linear.y = 0.0; cmd.linear.x = 0.0; // Turn left if(left_shoulder_x < turnLeft_mark){ cmd.angular.z = turnVel; } // Turn right else if(right_shoulder_x > turnRight_mark){ cmd.angular.z = -turnVel; } // NAO is completely stopped else{ cmd.angular.z = 0.0; } } } else{ obstacleFront = true; countObstacle++; // cmd.linear.y = 0.0; // cmd.linear.x = 0.0; // velPub.publish(cmd); // ros::spinOnce(); if(obstacleFront && countObstacle == 10){ std_msgs::String text_msg; text_msg.data = "Je ne peux pas proceder"; ttsPub.publish(text_msg); } // Move backward if(left_foot_z > backward_mark && right_foot_z > backward_mark){ cmd.linear.x = backwardVel; cmd.linear.y = 0.0; // Move backward and turn left if(left_shoulder_x < turnLeft_mark){ cmd.angular.z = turnVel; } // Move backward and turn right else if(right_shoulder_x > turnRight_mark){ cmd.angular.z = -turnVel; } // Move backward without turn else{ cmd.angular.z = 0.0; } } // Stop movements in the X and Y axes else{ cmd.linear.y = 0.0; cmd.linear.x = 0.0; // Turn left if(left_shoulder_x < turnLeft_mark){ cmd.angular.z = turnVel; } // Turn right else if(right_shoulder_x > turnRight_mark){ cmd.angular.z = -turnVel; } // NAO is completely stopped else{ cmd.angular.z = 0.0; } } } } } return; }