Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

nao_apps/nodes/nao_tactile.py support head/hand #47

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
Open

nao_apps/nodes/nao_tactile.py support head/hand #47

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
58 changes: 46 additions & 12 deletions nao_apps/nodes/nao_tactile.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -35,7 +35,7 @@
import rospy

import naoqi
from naoqi_bridge_msgs.msg import HeadTouch, Bumper
from naoqi_bridge_msgs.msg import HandTouch, HeadTouch, Bumper
from std_msgs.msg import Bool
from naoqi_driver.naoqi_node import NaoqiNode
from naoqi import ( ALModule, ALBroker, ALProxy )
Expand Down Expand Up @@ -83,9 +83,11 @@ def __init__(self, moduleName):
rospy.init_node('nao_tactile')

# init. messages:
self.tactile = HeadTouch()
self.headTactile = HeadTouch()
self.handTactile = HandTouch()
self.bumper = Bumper()
self.tactilePub = rospy.Publisher("tactile_touch", HeadTouch, queue_size=10)
self.headTactilePub = rospy.Publisher("head_touch", HeadTouch, queue_size=10)
self.handTactilePub = rospy.Publisher("hand_touch", HandTouch, queue_size=10)
self.bumperPub = rospy.Publisher("bumper", Bumper, queue_size=10)

try:
Expand All @@ -104,9 +106,15 @@ def __init__(self, moduleName):
# constants in HeadTouch and Bumper will not be available in callback functions
# as they are executed in the parent broker context (i.e. on robot),
# so they have to be copied to member variables
self.tactileTouchFrontButton = HeadTouch.buttonFront;
self.tactileTouchMiddleButton = HeadTouch.buttonMiddle;
self.tactileTouchRearButton = HeadTouch.buttonRear;
self.headTactileTouchFrontButton = HeadTouch.buttonFront;
self.headTactileTouchMiddleButton = HeadTouch.buttonMiddle;
self.headTactileTouchRearButton = HeadTouch.buttonRear;
self.handTactileTouchRightBack = HandTouch.RIGHT_BACK;
self.handTactileTouchRightLeft = HandTouch.RIGHT_LEFT;
self.handTactileTouchRightRight = HandTouch.RIGHT_RIGHT;
self.handTactileTouchLeftBack = HandTouch.LEFT_BACK;
self.handTactileTouchLeftLeft = HandTouch.LEFT_LEFT;
self.TactileTouchLeftRight = HandTouch.LEFT_RIGHT;
self.bumperRightButton = Bumper.right;
self.bumperLeftButton = Bumper.left;

Expand Down Expand Up @@ -144,6 +152,12 @@ def subscribe(self):
self.memProxy.subscribeToEvent("FrontTactilTouched", self.moduleName, "onTactileChanged")
self.memProxy.subscribeToEvent("MiddleTactilTouched", self.moduleName, "onTactileChanged")
self.memProxy.subscribeToEvent("RearTactilTouched", self.moduleName, "onTactileChanged")
self.memProxy.subscribeToEvent("HandLeftBackTouched", self.moduleName, "onTactileChanged")
self.memProxy.subscribeToEvent("HandLeftLeftTouched", self.moduleName, "onTactileChanged")
self.memProxy.subscribeToEvent("HandLeftRightTouched", self.moduleName, "onTactileChanged")
self.memProxy.subscribeToEvent("HandRightBackTouched", self.moduleName, "onTactileChanged")
self.memProxy.subscribeToEvent("HandRightLeftTouched", self.moduleName, "onTactileChanged")
self.memProxy.subscribeToEvent("HandRightRightTouched", self.moduleName, "onTactileChanged")
self.memProxy.subscribeToEvent("RightBumperPressed", self.moduleName, "onBumperChanged")
self.memProxy.subscribeToEvent("LeftBumperPressed", self.moduleName, "onBumperChanged")
if self.hasFootContactKey:
Expand All @@ -156,21 +170,41 @@ def unsubscribe(self):
self.memProxy.unsubscribeToEvent("RearTactilTouched", self.moduleName)
self.memProxy.unsubscribeToEvent("RightBumperPressed", self.moduleName)
self.memProxy.unsubscribeToEvent("LeftBumperPressed", self.moduleName)
self.memProxy.unsubscribeToEvent("HandLeftBackTouched", self.moduleName)
self.memProxy.unsubscribeToEvent("HandLeftLeftTouched", self.moduleName)
self.memProxy.unsubscribeToEvent("HandLeftRightTouched", self.moduleName)
self.memProxy.unsubscribeToEvent("HandRightBackTouched", self.moduleName)
self.memProxy.unsubscribeToEvent("HandRightLeftTouched", self.moduleName)
self.memProxy.unsubscribeToEvent("HandRightRightTouched", self.moduleName)
if self.hasFootContactKey:
self.memProxy.unsubscribeToEvent("footContactChanged", self.moduleName)


def onTactileChanged(self, strVarName, value, strMessage):
"Called when tactile touch status changes in ALMemory"
if strVarName == "FrontTactilTouched":
self.tactile.button = self.tactileTouchFrontButton
self.headTactile.button = self.headTactileTouchFrontButton
elif strVarName == "MiddleTactilTouched":
self.tactile.button = self.tactileTouchMiddleButton
self.headTactile.button = self.headTactileTouchMiddleButton
elif strVarName == "RearTactilTouched":
self.tactile.button = self.tactileTouchRearButton

self.tactile.state = int(value);
self.tactilePub.publish(self.tactile)
self.headTactile.button = self.headTactileTouchRearButton
elif strVarName == "HandLeftBackTouched":
self.handTactile.hand = self.handTactileTouchRightBack
elif strVarName == "HandLeftLeftTouched":
self.handTactile.hand = self.handTactileTouchRightLeft
elif strVarName == "HandLeftRightTouched":
self.handTactile.hand = self.handTactileTouchRightRight
elif strVarName == "HandRightBackTouched":
self.handTactile.hand = self.handTactileTouchLeftBack
elif strVarName == "HandRightLeftTouched":
self.handTactile.hand = self.handTactileTouchLeftLeft
elif strVarName == "HandRightRightTouched":
self.handTactile.hand = self.handTactileTouchLeftRight

self.handTactile.state = int(value);
self.headTactile.state = int(value);
self.headTactilePub.publish(self.headTactile)
self.handTactilePub.publish(self.handTactile)
rospy.logdebug("tactile touched: name=%s, value=%d, message=%s.", strVarName, value, strMessage);

def onBumperChanged(self, strVarName, value, strMessage):
Expand Down