diff --git a/nao_apps/nodes/nao_tactile.py b/nao_apps/nodes/nao_tactile.py index 5bfdf07..97c56c3 100755 --- a/nao_apps/nodes/nao_tactile.py +++ b/nao_apps/nodes/nao_tactile.py @@ -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 ) @@ -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: @@ -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; @@ -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: @@ -156,6 +170,12 @@ 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) @@ -163,14 +183,28 @@ def unsubscribe(self): 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):