// 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;
}
