38 from naoqi_bridge_msgs.msg
import TactileTouch, Bumper
39 from std_msgs.msg
import Bool
41 from naoqi
import ( ALModule, ALBroker, ALProxy )
58 "Sends callbacks for tactile touch, bumper press and foot contact to ROS" 61 from optparse
import OptionParser
63 parser = OptionParser()
64 parser.add_option(
"--ip", dest=
"ip", default=
"",
65 help=
"IP/hostname of broker. Default is system's default IP address.", metavar=
"IP")
66 parser.add_option(
"--port", dest=
"port", default=0,
67 help=
"IP/hostname of broker. Default is automatic port.", metavar=
"PORT")
68 parser.add_option(
"--pip", dest=
"pip", default=
"127.0.0.1",
69 help=
"IP/hostname of parent broker. Default is 127.0.0.1.", metavar=
"IP")
70 parser.add_option(
"--pport", dest=
"pport", default=9559,
71 help=
"port of parent broker. Default is 9559.", metavar=
"PORT")
73 (options, args) = parser.parse_args()
75 self.
port = int(options.port)
76 self.
pip = options.pip
83 rospy.init_node(
'naoqi_tactile')
88 self.
tactilePub = rospy.Publisher(
"tactile_touch", TactileTouch, queue_size=1)
89 self.
bumperPub = rospy.Publisher(
"bumper", Bumper, queue_size=1)
92 footContact = self.memProxy.getData(
"footContact", 0)
96 if footContact
is None:
98 rospy.loginfo(
"Foot contact key is not present in ALMemory, will not publish to foot_contact topic.")
101 self.
footContactPub = rospy.Publisher(
"foot_contact", Bool, latch=
True, queue_size=1)
102 self.footContactPub.publish(footContact > 0.0)
115 rospy.loginfo(
"nao_tactile initialized")
119 rospy.loginfo(
"Connecting to NaoQi at %s:%d", self.
pip, self.
pport)
122 except RuntimeError,e:
123 print(
"Could not connect to NaoQi's main broker")
130 rospy.logerror(
"Could not get a proxy to ALMemory on %s:%d", self.
pip, self.
pport)
144 self.memProxy.subscribeToEvent(
"FrontTactilTouched", self.
moduleName,
"onTactileChanged")
145 self.memProxy.subscribeToEvent(
"MiddleTactilTouched", self.
moduleName,
"onTactileChanged")
146 self.memProxy.subscribeToEvent(
"RearTactilTouched", self.
moduleName,
"onTactileChanged")
147 self.memProxy.subscribeToEvent(
"RightBumperPressed", self.
moduleName,
"onBumperChanged")
148 self.memProxy.subscribeToEvent(
"LeftBumperPressed", self.
moduleName,
"onBumperChanged")
150 self.memProxy.subscribeToEvent(
"footContactChanged", self.
moduleName,
"onFootContactChanged")
154 self.memProxy.unsubscribeToEvent(
"FrontTactilTouched", self.
moduleName)
155 self.memProxy.unsubscribeToEvent(
"MiddleTactilTouched", self.
moduleName)
156 self.memProxy.unsubscribeToEvent(
"RearTactilTouched", self.
moduleName)
157 self.memProxy.unsubscribeToEvent(
"RightBumperPressed", self.
moduleName)
158 self.memProxy.unsubscribeToEvent(
"LeftBumperPressed", self.
moduleName)
160 self.memProxy.unsubscribeToEvent(
"footContactChanged", self.
moduleName)
164 "Called when tactile touch status changes in ALMemory" 165 if strVarName ==
"FrontTactilTouched":
167 elif strVarName ==
"MiddleTactilTouched":
169 elif strVarName ==
"RearTactilTouched":
172 self.tactile.state = int(value);
173 self.tactilePub.publish(self.
tactile)
174 rospy.logdebug(
"tactile touched: name=%s, value=%d, message=%s.", strVarName, value, strMessage);
177 "Called when bumper status changes in ALMemory" 178 if strVarName ==
"RightBumperPressed":
180 elif strVarName ==
"LeftBumperPressed":
183 self.bumper.state = int(value);
184 self.bumperPub.publish(self.
bumper)
185 rospy.logdebug(
"bumper pressed: name=%s, value=%d, message=%s.", strVarName, value, strMessage);
188 "Called when foot contact changes in ALMemory" 189 self.footContactPub.publish(value > 0.0)
191 if __name__ ==
'__main__':
196 rospy.loginfo(
"Stopping nao_tactile ...")
197 ROSNaoTactileModule.shutdown();
198 rospy.loginfo(
"nao_tactile stopped.")