def __init__(self, moduleName):
# ROS initialization
NaoqiNode.__init__(self, Constants.NODE_NAME)
# NAOQi initialization
self.ip = ""
self.port = 10602
self.moduleName = moduleName
self.init_almodule()
#~ ROS initializations
self.playFileSrv = rospy.Service("nao_audio/play_file", AudioPlayback,
self.playFileSrv)
self.masterVolumeSrv = rospy.Service("nao_audio/master_volume",
AudioMasterVolume,
self.handleAudioMasterVolumeSrv)
self.enableRecordSrv = rospy.Service("nao_audio/record", AudioRecorder,
self.handleRecorderSrv)
self.audioSourceLocalizationPub = rospy.Publisher(
"nao_audio/audio_source_localization", AudioSourceLocalization)
self.subscribe()
rospy.loginfo(Constants.NODE_NAME + " initialized")
def __init__(self):
NaoqiNode.__init__(self, 'naoqi_moveto_listener')
self.connectNaoQi()
self.listener = tf.TransformListener()
self.goal_sub = rospy.Subscriber("/move_base_simple/goal", PoseStamped, self.goalCB)
self.cvel_sub = rospy.Subscriber("/cmd_vel", Twist, self.cvelCB)
def __init__(self):
NaoqiNode.__init__(self, 'naoqi_pod_node')
self.markerpublisher = rospy.Publisher('naoqi_pod_publisher', PoseWithConfidenceStamped, queue_size=5)
self.rate = rospy.Rate(2)
self.init = False
self.connectNaoQi()
def __init__( self ):
#Initialisation
NaoqiNode.__init__( self, self.NODE_NAME )
#Proxy to interface with LEDs
self.proxy = self.get_proxy( "ALLeds" )
#Seed python's random number generator
random.seed( rospy.Time.now().to_nsec() )
#Create a subscriber for the fade_rgb topic
self.subscriber = rospy.Subscriber(
"fade_rgb",
FadeRGB,
self.fade_rgb)
#Prepare and start actionlib server
self.actionlib = actionlib.SimpleActionServer(
"blink",
BlinkAction,
self.run_blink,
False
)
self.actionlib.start()
def __init__(self):
NaoqiNode.__init__(self, 'naoqi_joint_states')
self.connectNaoQi()
# default sensor rate: 25 Hz (50 is max, stresses Nao's CPU)
self.sensorRate = rospy.Rate(rospy.get_param('~sensor_rate', 25.0))
self.dataNamesList = ["DCM/Time",
"Device/SubDeviceList/InertialSensor/AngleX/Sensor/Value","Device/SubDeviceList/InertialSensor/AngleY/Sensor/Value",
"Device/SubDeviceList/InertialSensor/AngleZ/Sensor/Value",
"Device/SubDeviceList/InertialSensor/GyroscopeX/Sensor/Value", "Device/SubDeviceList/InertialSensor/GyroscopeY/Sensor/Value",
"Device/SubDeviceList/InertialSensor/GyroscopeZ/Sensor/Value",
"Device/SubDeviceList/InertialSensor/AccelerometerX/Sensor/Value", "Device/SubDeviceList/InertialSensor/AccelerometerY/Sensor/Value",
"Device/SubDeviceList/InertialSensor/AccelerometerZ/Sensor/Value"]
tf_prefix_param_name = rospy.search_param('tf_prefix')
if tf_prefix_param_name:
self.tf_prefix = rospy.get_param(tf_prefix_param_name)
else:
self.tf_prefix = ""
self.base_frameID = rospy.get_param('~base_frame_id', "base_link")
if not(self.base_frameID[0] == '/'):
self.base_frameID = self.tf_prefix + '/' + self.base_frameID
# use sensor values or commanded (open-loop) values for joint angles
self.useJointSensors = rospy.get_param('~use_joint_sensors', True) # (set to False in simulation!)
# init. messages:
self.torsoOdom = Odometry()
self.torsoOdom.header.frame_id = rospy.get_param('~odom_frame_id', "odom")
if not(self.torsoOdom.header.frame_id[0] == '/'):
self.torsoOdom.header.frame_id = self.tf_prefix + '/' + self.torsoOdom.header.frame_id
self.torsoIMU = Imu()
self.torsoIMU.header.frame_id = self.base_frameID
self.jointState = JointState()
self.jointState.name = self.motionProxy.getJointNames('Body')
self.jointStiffness = JointState()
self.jointStiffness.name = self.motionProxy.getJointNames('Body')
# simluated model misses some joints, we need to fill:
if (len(self.jointState.name) == 22):
self.jointState.name.insert(6,"LWristYaw")
self.jointState.name.insert(7,"LHand")
self.jointState.name.append("RWristYaw")
self.jointState.name.append("RHand")
msg = "Nao joints found: "+ str(self.jointState.name)
rospy.logdebug(msg)
self.torsoOdomPub = rospy.Publisher("odom_raw", Odometry, queue_size=1)
self.torsoIMUPub = rospy.Publisher("imu", Imu, queue_size=1)
self.jointStatePub = rospy.Publisher("joint_states", JointState, queue_size=1)
self.jointStiffnessPub = rospy.Publisher("joint_stiffness", JointState, queue_size=1)
self.tf_br = tf.TransformBroadcaster()
rospy.loginfo("nao_joint_states initialized")
def __init__(self):
NaoqiNode.__init__(self, 'naoqi_joint_states')
self.connectNaoQi()
# default sensor rate: 25 Hz (50 is max, stresses Nao's CPU)
self.sensorRate = rospy.Rate(rospy.get_param('~sensor_rate', 25.0))
self.dataNamesList = ["DCM/Time",
"Device/SubDeviceList/InertialSensor/AngleX/Sensor/Value","Device/SubDeviceList/InertialSensor/AngleY/Sensor/Value",
"Device/SubDeviceList/InertialSensor/AngleZ/Sensor/Value",
"Device/SubDeviceList/InertialSensor/GyroscopeX/Sensor/Value", "Device/SubDeviceList/InertialSensor/GyroscopeY/Sensor/Value",
"Device/SubDeviceList/InertialSensor/GyroscopeZ/Sensor/Value",
"Device/SubDeviceList/InertialSensor/AccelerometerX/Sensor/Value", "Device/SubDeviceList/InertialSensor/AccelerometerY/Sensor/Value",
"Device/SubDeviceList/InertialSensor/AccelerometerZ/Sensor/Value"]
tf_prefix_param_name = rospy.search_param('tf_prefix')
if tf_prefix_param_name:
self.tf_prefix = rospy.get_param(tf_prefix_param_name)
else:
self.tf_prefix = ""
self.base_frameID = rospy.get_param('~base_frame_id', "base_link")
if not(self.base_frameID[0] == '/'):
self.base_frameID = self.tf_prefix + '/' + self.base_frameID
# use sensor values or commanded (open-loop) values for joint angles
self.useJointSensors = rospy.get_param('~use_joint_sensors', True) # (set to False in simulation!)
# init. messages:
self.torsoOdom = Odometry()
self.torsoOdom.header.frame_id = rospy.get_param('~odom_frame_id', "odom")
if not(self.torsoOdom.header.frame_id[0] == '/'):
self.torsoOdom.header.frame_id = self.tf_prefix + '/' + self.torsoOdom.header.frame_id
self.torsoIMU = Imu()
self.torsoIMU.header.frame_id = self.base_frameID
self.jointState = JointState()
self.jointState.name = self.motionProxy.getJointNames('Body')
self.jointStiffness = JointState()
self.jointStiffness.name = self.motionProxy.getJointNames('Body')
# simluated model misses some joints, we need to fill:
if (len(self.jointState.name) == 22):
self.jointState.name.insert(6,"LWristYaw")
self.jointState.name.insert(7,"LHand")
self.jointState.name.append("RWristYaw")
self.jointState.name.append("RHand")
msg = "Nao joints found: "+ str(self.jointState.name)
rospy.logdebug(msg)
self.torsoOdomPub = rospy.Publisher("odom", Odometry)
self.torsoIMUPub = rospy.Publisher("imu", Imu)
self.jointStatePub = rospy.Publisher("joint_states", JointState)
self.jointStiffnessPub = rospy.Publisher("joint_stiffness", JointState)
self.tf_br = tf.TransformBroadcaster()
rospy.loginfo("nao_joint_states initialized")
def __init__(self):
NaoqiNode.__init__(self, 'naoqi_moveto_listener')
self.connectNaoQi()
self.listener = tf.TransformListener()
self.subscriber = rospy.Subscriber("/move_base_simple/goal",
PoseStamped, self.callback)
def __init__(self):
NaoqiNode.__init__(self, 'audio_server')
self.pip = rospy.get_param('/pepper_ip')
self.pport = rospy.get_param('/pepper_port')
self.pname = rospy.get_param('/pepper_name')
self.connectNaoQi()
# advertise topics:
self.audioBuffer = rospy.Publisher(self.pname + "/audio_buffer",
AudioBuffer,
queue_size=10)
#self.soundLocated = rospy.Publisher(self.pname+"/sound/located", SoundLocated, queue_size=10) definirano u drugom serveru
# start services / actions:
self.enableStiffnessSrv = rospy.Service(
self.pname + "/energy_computation/enable", Empty,
self.energy_computation)
self.disableStiffnessSrv = rospy.Service(
self.pname + "/energy_computation/disable", Empty,
self.energy_computation_off)
self.setOutputVolume = rospy.Service(self.pname + "/output_volume/set",
GetFloat, self.set_output_volume)
rospy.loginfo("sound initialized")
def __init__(self):
NaoqiNode.__init__(self, 'nao_footsteps')
self.connectNaoQi()
# initial stiffness (defaults to 0 so it doesn't strain the robot when
# no teleoperation is running)
# set to 1.0 if you want to control the robot immediately
initStiffness = rospy.get_param('~init_stiffness', 0.0)
# TODO: parameterize
if initStiffness > 0.0 and initStiffness <= 1.0:
self.motionProxy.stiffnessInterpolation('Body', initStiffness, 0.5)
# last: ROS subscriptions (after all vars are initialized)
rospy.Subscriber("footstep", StepTarget, self.handleStep, queue_size=50)
# ROS services (blocking functions)
self.stepToSrv = rospy.Service("footstep_srv", StepTargetService,
self.handleStepSrv)
self.clipSrv = rospy.Service("clip_footstep_srv", ClipFootstep,
self.handleClipSrv)
# Initialize action server
self.actionServer = actionlib.SimpleActionServer(
"footsteps_execution",
ExecFootstepsAction,
execute_cb=self.footstepsExecutionCallback,
auto_start=False)
self.actionServer.start()
rospy.loginfo("nao_footsteps initialized")
def __init__(self):
NaoqiNode.__init__(self, 'nao_motion_bridge')
self.connectNaoQi()
self.moveTest()
rospy.Subscriber("joint_position_cmd", JointState,
self.sendJointsToNAO)
def __init__(self, event_watcher_session):
NaoqiNode.__init__(self, 'naoqi_asr_tts')
self.is_vocabulary_filled = False
self.proxy = event_watcher_session
self.pname = rospy.get_param('/pepper_name')
# Servisi
#self.setVocabularySrv = rospy.Service("set_vocabulary", SetString, self.handleSetVocabulary) #koristi se akcija, njen goal
self.TTSSpeechSrv = rospy.Service(self.pname+"/tts/motionless", SetString, self.handleTTSSpeech)
self.TTSAnimationSrv = rospy.Service(self.pname+"/tts/animation", SetString, self.handleTTSAnimation)
self.startASRSrv = rospy.Service(self.pname+"/asr/enable", Empty, self.handleStartASR)
self.stopASRSrv = rospy.Service(self.pname+"/asr/disable", Empty, self.handleStopASR)
#self.setLanguageSrv = rospy.Service("set_language", SetString, self.handleSetLanguage) ima vec servis /naoqi_driver/set_language
rospy.loginfo("naoqi_asr_tts is initialized")
#Akcije
# Actionlib server for altering the speech recognition vocabulary
self.setSpeechVocabularyServer = actionlib.SimpleActionServer(self.pname+"/asr/vocabulary/set", SetSpeechVocabularyAction,
execute_cb=self.executeSpeechVocabularyAction,
auto_start=False)
# Actionlib server for having speech with feedback
#self.speechWithFeedbackServer = actionlib.SimpleActionServer(self.pname+"/asr/vocabulary/set/2", SpeechWithFeedbackAction,
# execute_cb=self.executeSpeechWithFeedbackAction,
# auto_start=False)
# Start both actionlib servers
self.setSpeechVocabularyServer.start()
def __init__(self):
#Initialisation
NaoqiNode.__init__(self, self.NODE_NAME)
#We need this variable to be able to call stop behavior when preempted
self.behavior = None
self.lock = threading.RLock()
#Proxy for listingBehaviors and stopping them
#IP = "192.168.1.111"
#PORT = 9559
ip = rospy.get_param('/behaviours_server/pepper_ip')
port = rospy.get_param('/behaviours_server/pepper_port')
self.behaviorProxy = ALProxy("ALBehaviorManager", ip, port)
# Register ROS services
self.getInstalledBehaviorsService = rospy.Service(
"get_installed_behaviors", GetInstalledBehaviors,
self.getInstalledBehaviors)
#Prepare and start actionlib server
self.actionlibServer = actionlib.SimpleActionServer(
"run_behavior", RunBehaviorAction, self.runBehavior, False)
#self.actionlibServer.register_preempt_callback( self.stopBehavior )
self.actionlibServer.start()
def __init__( self ):
#Initialisation
NaoqiNode.__init__( self, self.NODE_NAME )
#We need this variable to be able to call stop behavior when preempted
self.behavior = None
self.lock = threading.RLock()
#Proxy for listingBehaviors and stopping them
self.behaviorProxy = self.get_proxy( "ALBehaviorManager" )
if self.behaviorProxy is None:
exit(1)
# Register ROS services
self.getInstalledBehaviorsService = rospy.Service(
"get_installed_behaviors",
GetInstalledBehaviors,
self.getInstalledBehaviors
)
#Prepare and start actionlib server
self.actionlibServer = actionlib.SimpleActionServer(
"run_behavior",
RunBehaviorAction,
self.runBehavior,
False
)
self.actionlibServer.register_preempt_callback( self.stopBehavior )
self.actionlibServer.start()
def __init__(self, event_watcher_session):
NaoqiNode.__init__(self, 'naoqi_asr_tts')
self.is_vocabulary_filled = False
self.proxy = event_watcher_session
# Servisi
#self.setVocabularySrv = rospy.Service("set_vocabulary", SetString, self.handleSetVocabulary) #koristi se akcija, njen goal
self.TTSSpeechSrv = rospy.Service("tts_speech", SetString, self.handleTTSSpeech)
self.TTSAnimationSrv = rospy.Service("tts_animation", SetString, self.handleTTSAnimation)
self.startASRSrv = rospy.Service("start_asr", SetBool, self.handleStartASR)
#self.setLanguageSrv = rospy.Service("set_language", SetString, self.handleSetLanguage) ima vec servis /naoqi_driver/set_language
rospy.loginfo("naoqi_asr_tts is initialized")
#Akcije
# Actionlib server for altering the speech recognition vocabulary
self.setSpeechVocabularyServer = actionlib.SimpleActionServer("speech_vocabulary_action", SetSpeechVocabularyAction,
execute_cb=self.executeSpeechVocabularyAction,
auto_start=False)
# Actionlib server for having speech with feedback
self.speechWithFeedbackServer = actionlib.SimpleActionServer("speech_action", SpeechWithFeedbackAction,
execute_cb=self.executeSpeechWithFeedbackAction,
auto_start=False)
# Start both actionlib servers
self.setSpeechVocabularyServer.start()
self.speechWithFeedbackServer.start() # koristi se servis umjesto akcije
def __init__(self):
#Initialisation
NaoqiNode.__init__(self, self.NODE_NAME)
#We need this variable to be able to call stop behavior when preempted
self.behavior = None
self.lock = threading.RLock()
#Proxy for listingBehaviors and stopping them
self.behaviorProxy = self.get_proxy("ALBehaviorManager")
if self.behaviorProxy is None:
exit(1)
# Register ROS services
self.getInstalledBehaviorsService = rospy.Service(
"get_installed_behaviors", GetInstalledBehaviors,
self.getInstalledBehaviors)
#Prepare and start actionlib server
self.actionlibServer = actionlib.SimpleActionServer(
"run_behavior", RunBehaviorAction, self.runBehavior, False)
self.actionlibServer.register_preempt_callback(self.stopBehavior)
self.actionlibServer.start()
def __init__(self, node_name='naoqi_camera'):
NaoqiNode.__init__(self, node_name)
self.camProxy = self.get_proxy("ALVideoDevice")
if self.camProxy is None:
exit(1)
self.nameId = None
self.camera_infos = {}
def returnNone():
return None
self.config = defaultdict(returnNone)
# ROS publishers
self.pub_img_ = rospy.Publisher('~image_raw', Image, queue_size=5)
self.pub_info_ = rospy.Publisher('~camera_info',
CameraInfo,
queue_size=5)
# initialize the parameter server
self.srv = Server(NaoqiCameraConfig, self.reconfigure)
# initial load from param server
self.init_config()
# initially load configurations
self.reconfigure(self.config, 0)
def __init__( self ):
#Initialisation
NaoqiNode.__init__( self, self.NODE_NAME )
#Proxy to interface with LEDs
self.proxy = self.get_proxy( "ALLeds" )
#Seed python's random number generator
random.seed( rospy.Time.now().to_nsec() )
#Create a subscriber for the fade_rgb topic
self.subscriber = rospy.Subscriber(
"fade_rgb",
FadeRGB,
self.fade_rgb)
#Prepare and start actionlib server
self.actionlib = actionlib.SimpleActionServer(
"blink",
BlinkAction,
self.run_blink,
False
)
self.actionlib.start()
def __init__(self, moduleName):
# ROS initialization
NaoqiNode.__init__(self, Constants.NODE_NAME )
# NAOQi initialization
self.ip = ""
self.port = 10601
self.moduleName = moduleName
self.init_almodule()
#~ Variable initialization
self.faces = FaceDetected()
face_detection_enabled = False
motion_detection_enabled = False
landmark_detection_enabled = False
#~ ROS initializations
self.subscribeFaceSrv = rospy.Service("nao_vision/face_detection/enable", Empty, self.serveSubscribeFaceSrv)
self.unsubscribeFaceSrv = rospy.Service("nao_vision/face_detection/disable", Empty, self.serveUnsubscribeFaceSrv)
self.subscribeMotionSrv = rospy.Service("nao_vision/motion_detection/enable", Empty, self.serveSubscribeMotionSrv)
self.unsubscribeMotionSrv = rospy.Service("nao_vision/motion_detection/disable", Empty, self.serveUnsubscribeMotionSrv)
self.subscribeLandmarkSrv = rospy.Service("nao_vision/landmark_detection/enable", Empty, self.serveSubscribeLandmarkSrv)
self.unsubscribeLandmarkSrv = rospy.Service("nao_vision/landmark_detection/disable", Empty, self.serveUnsubscribeLandmarkSrv)
self.subscribe()
rospy.loginfo("nao_vision_interface initialized")
def __init__(self):
NaoqiNode.__init__(self, 'naoqi_background_movement')
self.connectNaoQi()
self.SetEnabledSrv = rospy.Service("background_movement/set_enabled", SetBool, self.handleSetEnabledSrv)
self.IsEnabledSrv = rospy.Service("background_movement/is_enabled", Trigger, self.handleIsEnabledSrv)
rospy.loginfo("naoqi_background_movement initialized")
def __init__(self, moduleName):
NaoqiNode.__init__(self, Constants.NODE_NAME)
self.moduleName = moduleName
self.ip = ""
self.port = 0
self.init_almodule()
self.speech_with_feedback_flag = False # Used for speech with feedback mode only
self.conf = None
self.srw = None
self.subscribe()
self.reconf_server = ReConfServer(NodeConfig, self.reconfigure)
self.reconf_client = dynamic_reconfigure.client.Client(
rospy.get_name())
self.sub = rospy.Subscriber("speech", String, self.say)
self.speechWithFeedbackServer = actionlib.SimpleActionServer(
"render_speech",
RenderItemAction,
self.executeSpeechWithFeedbackAction,
auto_start=False)
self.speechWithFeedbackServer.start()
def __init__(self):
NaoqiNode.__init__(self, 'nao_footsteps')
self.connectNaoQi()
# initial stiffness (defaults to 0 so it doesn't strain the robot when
# no teleoperation is running)
# set to 1.0 if you want to control the robot immediately
initStiffness = rospy.get_param('~init_stiffness', 0.0)
# TODO: parameterize
if initStiffness > 0.0 and initStiffness <= 1.0:
self.motionProxy.stiffnessInterpolation('Body', initStiffness, 0.5)
# last: ROS subscriptions (after all vars are initialized)
rospy.Subscriber("footstep",
StepTarget,
self.handleStep,
queue_size=50)
# ROS services (blocking functions)
self.stepToSrv = rospy.Service("footstep_srv", StepTargetService,
self.handleStepSrv)
self.clipSrv = rospy.Service("clip_footstep_srv", ClipFootstep,
self.handleClipSrv)
# Initialize action server
self.actionServer = actionlib.SimpleActionServer(
"footsteps_execution",
ExecFootstepsAction,
execute_cb=self.footstepsExecutionCallback,
auto_start=False)
self.actionServer.start()
rospy.loginfo("nao_footsteps initialized")
def __init__(self):
NaoqiNode.__init__(self, 'nao_diagnostic_updater')
# ROS initialization:
self.connectNaoQi()
# Read parameters:
self.sleep = 1.0 / rospy.get_param('~update_rate',
0.5) # update rate in Hz
self.warningThreshold = rospy.get_param(
'~warning_threshold',
68) # warning threshold for joint temperatures
self.errorThreshold = rospy.get_param(
'~error_threshold', 74) # error threshold for joint temperatures
# check if NAOqi runs on the robot by checking whether the OS has aldebaran in its name
self.runsOnRobot = "aldebaran" in os.uname(
)[2] # if temperature device files cannot be opened, this flag will be set to False later.
# Lists with interesting ALMemory keys
self.jointNamesList = self.motionProxy.getJointNames('Body')
self.jointTempPathsList = []
self.jointStiffPathsList = []
for i in range(0, len(self.jointNamesList)):
self.jointTempPathsList.append(
"Device/SubDeviceList/%s/Temperature/Sensor/Value" %
self.jointNamesList[i])
self.jointStiffPathsList.append(
"Device/SubDeviceList/%s/Hardness/Actuator/Value" %
self.jointNamesList[i])
self.batteryNamesList = ["Percentage", "Status"]
self.batteryPathsList = [
"Device/SubDeviceList/Battery/Charge/Sensor/Value",
"Device/SubDeviceList/Battery/Charge/Sensor/Status",
"Device/SubDeviceList/Battery/Current/Sensor/Value"
]
self.deviceInfoList = ["Device/DeviceList/ChestBoard/BodyId"]
deviceInfoData = self.memProxy.getListData(self.deviceInfoList)
if (len(deviceInfoData) > 1 and isinstance(deviceInfoData[1], list)):
deviceInfoData = deviceInfoData[1]
if (deviceInfoData[0] is None or deviceInfoData[0] == 0):
# No device info -> running in a simulation
self.isSimulator = True
if (self.pip.startswith("127.") or self.pip == "localhost"):
# Replace localhost with hostname of the machine
f = open("/etc/hostname")
self.hardwareID = "%s:%d" % (f.readline().rstrip(), self.pport)
f.close()
else:
self.hardwareID = "%s:%d" % (self.pip, self.pport)
else:
self.hardwareID = deviceInfoData[0].rstrip()
self.isSimulator = False
# init. messages:
self.diagnosticPub = rospy.Publisher("diagnostics", DiagnosticArray)
rospy.loginfo("nao_diagnostic_updater initialized")
def __init__(self):
NaoqiNode.__init__(self, 'nao_walker')
self.connectNaoQi()
# walking pattern params:
self.stepFrequency = rospy.get_param('~step_frequency', 0.5)
self.useStartWalkPose = rospy.get_param('~use_walk_pose', False)
self.needsStartWalkPose = True
# other params
self.maxHeadSpeed = rospy.get_param('~max_head_speed', 0.2)
# initial stiffness (defaults to 0 so it doesn't strain the robot when no teleoperation is running)
# set to 1.0 if you want to control the robot immediately
initStiffness = rospy.get_param('~init_stiffness', 0.0)
self.useFootGaitConfig = rospy.get_param('~use_foot_gait_config', False)
rospy.loginfo("useFootGaitConfig = %d" % self.useFootGaitConfig)
if self.useFootGaitConfig:
self.footGaitConfig = rospy.get_param('~foot_gait_config', self.motionProxy.getFootGaitConfig("Default"))
else:
self.footGaitConfig = self.motionProxy.getFootGaitConfig("Default")
# TODO: parameterize
if initStiffness > 0.0 and initStiffness <= 1.0:
self.motionProxy.stiffnessInterpolation('Body', initStiffness, 0.5)
try:
enableFootContactProtection = rospy.get_param('~enable_foot_contact_protection')
self.motionProxy.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION", enableFootContactProtection]])
if enableFootContactProtection:
rospy.loginfo("Enabled foot contact protection")
else:
rospy.loginfo("Disabled foot contact protection")
except KeyError:
# do not change foot contact protection
pass
# last: ROS subscriptions (after all vars are initialized)
rospy.Subscriber("cmd_vel", Twist, self.handleCmdVel, queue_size=1)
rospy.Subscriber("cmd_pose", Pose2D, self.handleTargetPose, queue_size=1)
rospy.Subscriber("cmd_step", StepTarget, self.handleStep, queue_size=50)
# Create ROS publisher for speech
self.pub = rospy.Publisher("speech", String, latch = True)
# ROS services (blocking functions)
self.cmdPoseSrv = rospy.Service("cmd_pose_srv", CmdPoseService, self.handleTargetPoseService)
self.cmdVelSrv = rospy.Service("cmd_vel_srv", CmdVelService, self.handleCmdVelService)
self.stepToSrv = rospy.Service("cmd_step_srv", StepTargetService, self.handleStepSrv)
self.stopWalkSrv = rospy.Service("stop_walk_srv", Empty, self.handleStopWalkSrv)
self.needsStartWalkPoseSrv = rospy.Service("needs_start_walk_pose_srv", Empty, self.handleNeedsStartWalkPoseSrv)
self.readFootGaitConfigSrv = rospy.Service("read_foot_gait_config_srv", Empty, self.handleReadFootGaitConfigSrv)
self.setArmsEnabledSrv = rospy.Service("enable_arms_walking_srv", SetArmsEnabled, self.handleSetArmsEnabledSrv)
self.say("Walker online")
rospy.loginfo("nao_walker initialized")
def __init__(self, param_sonar_rate="~sonar_rate", sonar_rate=10):
NaoqiNode.__init__(self, "sonar_publisher")
self.sonarRate = rospy.Rate(rospy.get_param(param_sonar_rate, sonar_rate))
self.connectNaoQi()
memory_key = rospy.get_param("~memory_key")
frame_id = rospy.get_param("~frame_id")
self.sonarSensor = SonarSensor( memory_key, frame_id, "sonar" )
self.publisher = rospy.Publisher( self.sonarSensor.rosTopic, Range, queue_size=5)
def __init__(self):
NaoqiNode.__init__(self, 'naoqi_pod_node')
self.markerpublisher = rospy.Publisher('naoqi_pod_publisher',
PoseWithConfidenceStamped,
queue_size=5)
self.rate = rospy.Rate(2)
self.init = False
self.connectNaoQi()
def __init__(self):
NaoqiNode.__init__(self, 'naoqi_listening_movement')
self.connectNaoQi()
self.SetEnabledSrv = rospy.Service("listening_movement/set_enabled",
SetBool, self.handleSetEnabledSrv)
self.IsEnabledSrv = rospy.Service("listening_movement/is_enabled",
Trigger, self.handleIsEnabledSrv)
rospy.loginfo("naoqi_listening_movement initialized")
def __init__(self, param_sonar_rate="~sonar_rate", sonar_rate=10):
NaoqiNode.__init__(self, "sonar_publisher")
self.sonarRate = rospy.Rate(rospy.get_param(param_sonar_rate, sonar_rate))
self.connectNaoQi()
memory_key = rospy.get_param("~memory_key")
frame_id = rospy.get_param("~frame_id")
self.sonarSensor = SonarSensor( memory_key, frame_id, "sonar" )
self.publisher = rospy.Publisher( self.sonarSensor.rosTopic, Range, queue_size=5)
def __init__(self):
NaoqiNode.__init__(self, 'go_to_rest')
self.connectNaoQi()
#self.postureProxy.goToPosture("StandInit", 0.5)
#self.postureProxy.goToPosture("Crouch", 0.5)
self.motionProxy.rest()
rospy.loginfo("Pepper resting")
sys.exit(0)
def __init__(self):
NaoqiNode.__init__(self, 'naoqi_gaze_analysis')
self.connectNaoQi()
self.setToleranceSrv = rospy.Service(
"set_gaze_analysis_tolerance", SetFloat,
self.handleSetGazeAnalysisTolerance)
self.getToleranceSrv = rospy.Service(
"get_gaze_analysis_tolerance", GetFloat,
self.handleGetGazeAnalysisTolerance)
rospy.loginfo("naoqi_gaze_analysis is initialized")
def __init__(self, sonarSensorList, param_sonar_rate="~sonar_rate", sonar_rate=40):
NaoqiNode.__init__(self, "sonar_publisher")
self.sonarRate = rospy.Rate(rospy.get_param(param_sonar_rate, sonar_rate))
self.connectNaoQi()
self.sonarSensorList = sonarSensorList
self.publisherList = []
for sonarSensor in sonarSensorList:
self.publisherList.append(
rospy.Publisher(sonarSensor.rosTopic, Range, queue_size=5))
def __init__(self):
#Create a Naoqui node called 'animated_speech' and connect it
NaoqiNode.__init__(self, 'animated_speech')
parser = ArgumentParser()
parser.add_argument("--ip", dest="ip", default="10.0.1.230")
args, unknown = parser.parse_known_args(args=rospy.myargv(argv=sys.argv)[1:])
ip = args.ip
self.pip = ip
self.connectNaoQi()
pass
def __init__(self):
NaoqiNode.__init__(self, 'naoqi_localization')
self.navigation = None
self.connectNaoQi()
self.publishers = {}
self.publishers["uncertainty"] = rospy.Publisher('localization_uncertainty', Marker)
self.publishers["map_tf"] = rospy.Publisher('/tf', TFMessage, latch=True)
self.publishers["map"] = rospy.Publisher('naoqi_exploration_map', OccupancyGrid , queue_size=None)
self.publishers["exploration_path"] = rospy.Publisher('naoqi_exploration_path', Path, queue_size=None)
self.frequency = 2
self.rate = rospy.Rate(self.frequency)
def __init__(self, pointcloud=True, laserscan=False):
self.pointcloud = pointcloud
self.laserscan = laserscan
NaoqiNode.__init__(self, 'pepper_node')
# ROS initialization;
self.connectNaoQi()
# default sensor rate: 25 Hz (50 is max, stresses Nao's CPU)
self.laserRate = rospy.Rate(rospy.get_param(
self.PARAM_LASER_RATE,
self.PARAM_LASER_RATE_DEFAULT))
self.laserShovelFrame = rospy.get_param(
self.PARAM_LASER_SHOVEL_FRAME,
self.PARAM_LASER_SHOVEL_FRAME_DEFAULT)
self.laserGroundLeftFrame = rospy.get_param(
self.PARAM_LASER_GROUND_LEFT_FRAME,
self.PARAM_LASER_GROUND_LEFT_FRAME_DEFAULT)
self.laserGroundRightFrame = rospy.get_param(
self.PARAM_LASER_GROUND_RIGHT_FRAME,
self.PARAM_LASER_GROUND_RIGHT_FRAME_DEFAULT)
self.laserSRDFrontFrame = rospy.get_param(
self.PARAM_LASER_SRD_FRONT_FRAME,
self.PARAM_LASER_SRD_FRONT_FRAME_DEFAULT)
self.laserSRDLeftFrame = rospy.get_param(
self.PARAM_LASER_SRD_LEFT_FRAME,
self.PARAM_LASER_SRD_LEFT_FRAME_DEFAULT)
self.laserSRDRightFrame = rospy.get_param(
self.PARAM_LASER_SRD_RIGHT_FRAME,
self.PARAM_LASER_SRD_RIGHT_FRAME_DEFAULT)
if self.pointcloud:
self.pcShovelPublisher = rospy.Publisher(self.TOPIC_LASER_SHOVEL+'pointcloud', PointCloud2, queue_size=1)
self.pcGroundLeftPublisher = rospy.Publisher(self.TOPIC_LASER_GROUND_LEFT+'pointcloud', PointCloud2, queue_size=1)
self.pcGroundRightPublisher = rospy.Publisher(self.TOPIC_LASER_GROUND_RIGHT+'pointcloud', PointCloud2, queue_size=1)
self.pcSRDFrontPublisher = rospy.Publisher(self.TOPIC_LASER_SRD_FRONT+'pointcloud', PointCloud2, queue_size=1)
self.pcSRDLeftPublisher = rospy.Publisher(self.TOPIC_LASER_SRD_LEFT+'pointcloud', PointCloud2, queue_size=1)
self.pcSRDRightPublisher = rospy.Publisher(self.TOPIC_LASER_SRD_RIGHT+'pointcloud', PointCloud2, queue_size=1)
if self.laserscan:
self.laserShovelPublisher = rospy.Publisher(self.TOPIC_LASER_SHOVEL+'scan', LaserScan, queue_size=1)
self.laserGroundLeftPublisher = rospy.Publisher(self.TOPIC_LASER_GROUND_LEFT+'scan', LaserScan, queue_size=1)
self.laserGroundRightPublisher = rospy.Publisher(self.TOPIC_LASER_GROUND_RIGHT+'scan', LaserScan, queue_size=1)
self.laserSRDFrontPublisher = rospy.Publisher(self.TOPIC_LASER_SRD_FRONT+'scan', LaserScan, queue_size=1)
self.laserSRDLeftPublisher = rospy.Publisher(self.TOPIC_LASER_SRD_LEFT+'scan', LaserScan, queue_size=1)
self.laserSRDRightPublisher = rospy.Publisher(self.TOPIC_LASER_SRD_RIGHT+'scan', LaserScan, queue_size=1)
self.laserSRDFrontPublisher_test = rospy.Publisher("~/pepper_navigation/front", LaserScan, queue_size=1)
def __init__(self, pointcloud=True, laserscan=False):
self.pointcloud = pointcloud
self.laserscan = laserscan
NaoqiNode.__init__(self, 'pepper_node')
# ROS initialization;
self.connectNaoQi()
# default sensor rate: 25 Hz (50 is max, stresses Nao's CPU)
self.laserRate = rospy.Rate(rospy.get_param(
self.PARAM_LASER_RATE,
self.PARAM_LASER_RATE_DEFAULT))
self.laserShovelFrame = rospy.get_param(
self.PARAM_LASER_SHOVEL_FRAME,
self.PARAM_LASER_SHOVEL_FRAME_DEFAULT)
self.laserGroundLeftFrame = rospy.get_param(
self.PARAM_LASER_GROUND_LEFT_FRAME,
self.PARAM_LASER_GROUND_LEFT_FRAME_DEFAULT)
self.laserGroundRightFrame = rospy.get_param(
self.PARAM_LASER_GROUND_RIGHT_FRAME,
self.PARAM_LASER_GROUND_RIGHT_FRAME_DEFAULT)
self.laserSRDFrontFrame = rospy.get_param(
self.PARAM_LASER_SRD_FRONT_FRAME,
self.PARAM_LASER_SRD_FRONT_FRAME_DEFAULT)
self.laserSRDLeftFrame = rospy.get_param(
self.PARAM_LASER_SRD_LEFT_FRAME,
self.PARAM_LASER_SRD_LEFT_FRAME_DEFAULT)
self.laserSRDRightFrame = rospy.get_param(
self.PARAM_LASER_SRD_RIGHT_FRAME,
self.PARAM_LASER_SRD_RIGHT_FRAME_DEFAULT)
if self.pointcloud:
self.pcShovelPublisher = rospy.Publisher(self.TOPIC_LASER_SHOVEL+'pointcloud', PointCloud2, queue_size=1)
self.pcGroundLeftPublisher = rospy.Publisher(self.TOPIC_LASER_GROUND_LEFT+'pointcloud', PointCloud2, queue_size=1)
self.pcGroundRightPublisher = rospy.Publisher(self.TOPIC_LASER_GROUND_RIGHT+'pointcloud', PointCloud2, queue_size=1)
self.pcSRDFrontPublisher = rospy.Publisher(self.TOPIC_LASER_SRD_FRONT+'pointcloud', PointCloud2, queue_size=1)
self.pcSRDLeftPublisher = rospy.Publisher(self.TOPIC_LASER_SRD_LEFT+'pointcloud', PointCloud2, queue_size=1)
self.pcSRDRightPublisher = rospy.Publisher(self.TOPIC_LASER_SRD_RIGHT+'pointcloud', PointCloud2, queue_size=1)
if self.laserscan:
self.laserShovelPublisher = rospy.Publisher(self.TOPIC_LASER_SHOVEL+'scan', LaserScan, queue_size=1)
self.laserGroundLeftPublisher = rospy.Publisher(self.TOPIC_LASER_GROUND_LEFT+'scan', LaserScan, queue_size=1)
self.laserGroundRightPublisher = rospy.Publisher(self.TOPIC_LASER_GROUND_RIGHT+'scan', LaserScan, queue_size=1)
self.laserSRDFrontPublisher = rospy.Publisher(self.TOPIC_LASER_SRD_FRONT+'scan', LaserScan, queue_size=1)
self.laserSRDLeftPublisher = rospy.Publisher(self.TOPIC_LASER_SRD_LEFT+'scan', LaserScan, queue_size=1)
self.laserSRDRightPublisher = rospy.Publisher(self.TOPIC_LASER_SRD_RIGHT+'scan', LaserScan, queue_size=1)
self.laserSRDFrontPublisher_test = rospy.Publisher("~/pepper_navigation/front", LaserScan, queue_size=1)
def __init__(self, human_tracked_event_watcher_session):
NaoqiNode.__init__(self, 'naoqi_awareness')
#sad ide poziv druge klase
self.proxy = human_tracked_event_watcher_session
self.pname = rospy.get_param('/pepper_name')
self.setEngagementSrv = rospy.Service(self.pname+"/basic_awareness/set/EngagementMode", SetString, self.handleSetEngagementMode)
self.setTrackingSrv = rospy.Service(self.pname+"/basic_awareness/set/TrackingMode", SetString, self.handleSetTrackingMode)
self.enable_awarenesSrv = rospy.Service(self.pname+"/basic_awareness/enable", Empty, self.handleEnableBasicAwareness)
self.disable_awarenesSrv = rospy.Service(self.pname+"/basic_awareness/disable", Empty, self.handleDisableBasicAwareness)
#self.getHumanSrv = rospy.Service(self.pname+"/getHumanDistance", GetFloat, self.handleGetHumanDistance)
rospy.loginfo("naoqi_basic_awareness is initialized")
def __init__(self):
NaoqiNode.__init__(self, 'nao_head_control')
self.connectNaoQi()
#rate = rospy.Rate(1) # 1 Hz
rospy.Subscriber("nao_head_rtc",
HeadControl,
self.handleHeadControl,
queue_size=1)
# Create ROS publisher for speech
self.pub = rospy.Publisher("speech", String, latch=True, queue_size=1)
self.say("Nao Head Control online")
def __init__(self, sound_tracked_event_watcher_session):
NaoqiNode.__init__(self, 'naoqi_sound_local')
#sad ide poziv druge klase
self.proxy = sound_tracked_event_watcher_session
self.pname = rospy.get_param('/pepper_name')
self.enable_soundloc = rospy.Service(self.pname+"/sound_localization/enable", Empty, self.handleEnableSoundLoca)
self.disable_soundloc = rospy.Service(self.pname+"/sound_localization/disable", Empty, self.handleDisableSoundLoca)
rospy.loginfo("naoqi_sound_loc is initialized")
global sound_located
sound_located = ALProxy("ALSoundLocalization")
global memory
memory = ALProxy("ALMemory")
def __init__(self):
NaoqiNode.__init__(self, 'nao_diagnostic_updater')
# ROS initialization:
self.connectNaoQi()
# Read parameters:
self.sleep = 1.0/rospy.get_param('~update_rate', 0.5) # update rate in Hz
self.warningThreshold = rospy.get_param('~warning_threshold', 68) # warning threshold for joint temperatures
self.errorThreshold = rospy.get_param('~error_threshold', 74) # error threshold for joint temperatures
# check if NAOqi runs on the robot by checking whether the OS has aldebaran in its name
self.runsOnRobot = "aldebaran" in os.uname()[2] # if temperature device files cannot be opened, this flag will be set to False later.
# Lists with interesting ALMemory keys
self.jointNamesList = self.motionProxy.getJointNames('Body')
self.jointTempPathsList = []
self.jointStiffPathsList = []
for i in range(0, len(self.jointNamesList)):
self.jointTempPathsList.append("Device/SubDeviceList/%s/Temperature/Sensor/Value" % self.jointNamesList[i])
self.jointStiffPathsList.append("Device/SubDeviceList/%s/Hardness/Actuator/Value" % self.jointNamesList[i])
self.batteryNamesList = ["Percentage", "Status"]
self.batteryPathsList = ["Device/SubDeviceList/Battery/Charge/Sensor/Value",
"Device/SubDeviceList/Battery/Charge/Sensor/Status",
"Device/SubDeviceList/Battery/Current/Sensor/Value"]
self.deviceInfoList = ["Device/DeviceList/ChestBoard/BodyId"]
deviceInfoData = self.memProxy.getListData(self.deviceInfoList)
if(len(deviceInfoData) > 1 and isinstance(deviceInfoData[1], list)):
deviceInfoData = deviceInfoData[1]
if(deviceInfoData[0] is None or deviceInfoData[0] == 0):
# No device info -> running in a simulation
self.isSimulator = True
if(self.pip.startswith("127.") or self.pip == "localhost"):
# Replace localhost with hostname of the machine
f = open("/etc/hostname")
self.hardwareID = "%s:%d"%(f.readline().rstrip(), self.pport)
f.close()
else:
self.hardwareID = "%s:%d"%(self.pip, self.pport)
else:
self.hardwareID = deviceInfoData[0].rstrip()
self.isSimulator = False
# init. messages:
self.diagnosticPub = rospy.Publisher("diagnostics", DiagnosticArray)
rospy.loginfo("nao_diagnostic_updater initialized")
def __init__(self):
NaoqiNode.__init__(self, self.NODE_NAME)
self.proxy = self.get_proxy("ALLeds")
random.seed(rospy.Time.now().to_nsec())
self.current_eye_color = self.eye_color['neutral']
self.last_eye_color = self.current_eye_color
self.server = actionlib.SimpleActionServer('render_facial_expression', RenderItemAction, self.execute_callback, False)
self.server.start()
self.t1 = Thread(target=self.handle_blink_thread)
self.t1.start()
rospy.loginfo("nao_render_expression initialized")
rospy.spin()
def __init__( self ):
#Initialisation
NaoqiNode.__init__( self, self.NODE_NAME )
if self.get_version() < distutils.version.LooseVersion('2.0'):
rospy.logwarn("{} is only available on NaoQi version 2.0 or higher, your version is {}".format(self.NODE_NAME, self.get_version()))
exit(1)
#Proxy to interface with LEDs
self.proxy = self.get_proxy( "ALAutonomousLife" )
# Register ROS services
self.disabled_srv = rospy.Service("~disabled", Empty, self.disabled )
self.solitary_srv = rospy.Service("~solitary", Empty, self.solitary )
self.interactive_srv = rospy.Service("~interactive", Empty, self.interactive )
self.safeguard_srv = rospy.Service("~safeguard", Empty, self.safeguard )
def __init__(self):
NaoqiNode.__init__(self, 'naoqi_localization')
self.navigation = None
self.connectNaoQi()
self.publishers = {}
self.publishers["uncertainty"] = rospy.Publisher(
'localization_uncertainty', Marker)
self.publishers["map_tf"] = rospy.Publisher('/tf',
TFMessage,
latch=True)
self.publishers["map"] = rospy.Publisher('naoqi_exploration_map',
OccupancyGrid,
queue_size=None)
self.publishers["exploration_path"] = rospy.Publisher(
'naoqi_exploration_path', Path, queue_size=None)
self.frequency = 2
self.rate = rospy.Rate(self.frequency)
def __init__(self):
NaoqiNode.__init__(self, 'nao_hands_control')
self.connectNaoQi()
#rate = rospy.Rate(1) # 1 Hz
# start services / actions:
self.openHandsControlSrv = rospy.Service("nao_hands_control/open_hand",
HandsControl,
self.handleOpenHandSrv)
self.closeHandsControlSrv = rospy.Service(
"nao_hands_control/close_hand", HandsControl,
self.handleCloseHandSrv)
# Create ROS publisher for speech
self.pub = rospy.Publisher("speech", String, latch=True, queue_size=1)
self.say("Nao Hands Control online")
def __init__(self, moduleName):
# ROS initialization
NaoqiNode.__init__(self, Constants.NODE_NAME)
# NAOQi initialization
self.ip = ""
self.port = 10601
self.moduleName = moduleName
self.init_almodule()
#~ Variable initialization
self.faces = FaceDetected()
self.face_detection_enabled = False
self.motion_detection_enabled = False
self.landmark_detection_enabled = False
#~ ROS initializations
self.subscribeFaceSrv = rospy.Service(
"nao_vision/face_detection/enable", Empty,
self.serveSubscribeFaceSrv)
self.unsubscribeFaceSrv = rospy.Service(
"nao_vision/face_detection/disable", Empty,
self.serveUnsubscribeFaceSrv)
self.subscribeMotionSrv = rospy.Service(
"nao_vision/motion_detection/enable", Empty,
self.serveSubscribeMotionSrv)
self.unsubscribeMotionSrv = rospy.Service(
"nao_vision/motion_detection/disable", Empty,
self.serveUnsubscribeMotionSrv)
self.subscribeLandmarkSrv = rospy.Service(
"nao_vision/landmark_detection/enable", Empty,
self.serveSubscribeLandmarkSrv)
self.unsubscribeLandmarkSrv = rospy.Service(
"nao_vision/landmark_detection/disable", Empty,
self.serveUnsubscribeLandmarkSrv)
self.learnFaceSrv = rospy.Service(
'nao_vision/face_detection/learn_face', LearnFace,
self.learnFaceSrv)
self.forgetPersonSrv = rospy.Service(
'nao_vision/face_detection/forget_person', LearnFace,
self.forgetPersonSrv)
self.subscribe()
rospy.loginfo("nao_vision_interface initialized")
def __init__(self):
NaoqiNode.__init__(self, 'nao_octomap')
if self.get_version() < LooseVersion('2.0'):
rospy.loginfo('NAOqi version < 2.0, Octomap is not used')
exit(0)
proxy = self.get_proxy("ALNavigation")
if proxy is None:
rospy.loginfo('Could not get access to the ALNavigation proxy')
exit(1)
# Create ROS publisher
self.pub = rospy.Publisher("octomap", Octomap, latch = True, queue_size=1)
self.fps = 1
rospy.loginfo("nao_octomap initialized")
def __init__( self ):
#Initialization
NaoqiNode.__init__( self, self.NODE_NAME )
from distutils.version import LooseVersion
if self.get_version() < LooseVersion('2.0.0'):
rospy.loginfo('The NAOqi version is inferior to 2.0, hence no log bridge possible')
exit(0)
rospy.init_node( self.NODE_NAME )
# the log manager is only avaiable through a session (NAOqi 2.0)
self.session = qi.Session()
self.session.connect("tcp://%s:%s" % (self.pip, self.pport))
self.logManager = self.session.service("LogManager")
self.listener = self.logManager.getListener()
self.listener.onLogMessage.connect(onMessageCallback)
rospy.loginfo('Logger initialized')
def __init__(self):
NaoqiNode.__init__(self, 'nao_depth')
self.camProxy = self.get_proxy("ALVideoDevice")
if self.camProxy is None:
exit(1)
self.nameId = None
self.camera_infos = {}
def returnNone():
return None
self.bridge = CvBridge()
# ROS publishers
self.pub_img_ = rospy.Publisher('~image_raw', Image)
self.pub_cimg_ = rospy.Publisher('~image_color', Image)
self.pub_info_ = rospy.Publisher('~camera_info', CameraInfo)
# initialize the parameter of the depth camera
self.setup_cam()
def __init__(self, moduleName):
# ROS initialization
NaoqiNode.__init__(self, Constants.NODE_NAME )
# NAOQi initialization
self.ip = ""
self.port = 10602
self.moduleName = moduleName
self.init_almodule()
#~ ROS initializations
self.playFileSrv = rospy.Service("nao_audio/play_file", AudioPlayback, self.playFileSrv )
self.masterVolumeSrv = rospy.Service("nao_audio/master_volume", AudioMasterVolume, self.handleAudioMasterVolumeSrv)
self.enableRecordSrv = rospy.Service("nao_audio/record", AudioRecorder, self.handleRecorderSrv)
self.audioSourceLocalizationPub = rospy.Publisher("nao_audio/audio_source_localization", AudioSourceLocalization)
self.subscribe()
rospy.loginfo(Constants.NODE_NAME + " initialized")
def __init__(self, node_name='naoqi_camera'):
NaoqiNode.__init__(self, node_name)
self.camProxy = self.get_proxy("ALVideoDevice")
if self.camProxy is None:
exit(1)
self.nameId = None
self.camera_infos = {}
def returnNone():
return None
self.config = defaultdict(returnNone)
# ROS publishers
self.pub_img_ = rospy.Publisher('~image_raw', Image, queue_size=5)
self.pub_info_ = rospy.Publisher('~camera_info', CameraInfo, queue_size=5)
# initialize the parameter server
self.srv = Server(NaoqiCameraConfig, self.reconfigure)
# initial load from param server
self.init_config()
# initially load configurations
self.reconfigure(self.config, 0)
def __init__(self):
NaoqiNode.__init__(self, 'pose_controller')
self.connectNaoQi()
self.rate = rospy.Rate(10)
# store the number of joints in each motion chain and collection, used for sanity checks
self.collectionSize = {}
for collectionName in ['Head', 'LArm', 'LLeg', 'RLeg', 'RArm', 'Body', 'BodyJoints', 'BodyActuators']:
try:
self.collectionSize[collectionName] = len(self.motionProxy.getJointNames(collectionName));
except RuntimeError:
# the following is useful for old NAOs with no legs/arms
rospy.logwarn('Collection %s not found on your robot.' % collectionName)
# Get poll rate for actionlib (ie. how often to check whether currently running task has been preempted)
# Defaults to 200ms
self.poll_rate = int(rospy.get_param("~poll_rate", 0.2)*1000)
# initial stiffness (defaults to 0 so it doesn't strain the robot when no teleoperation is running)
# set to 1.0 if you want to control the robot immediately
initStiffness = rospy.get_param('~init_stiffness', 0.0)
# TODO: parameterize
if initStiffness > 0.0 and initStiffness <= 1.0:
self.motionProxy.stiffnessInterpolation('Body', initStiffness, 0.5)
# advertise topics:
self.getLifeStatePub = rospy.Publisher("get_life_state", String, queue_size=10)
# start services / actions:
self.enableStiffnessSrv = rospy.Service("body_stiffness/enable", Empty, self.handleStiffnessSrv)
self.disableStiffnessSrv = rospy.Service("body_stiffness/disable", Empty, self.handleStiffnessOffSrv)
self.wakeUpSrv = rospy.Service("wakeup", Empty, self.handleWakeUpSrv)
self.restSrv = rospy.Service("rest", Empty, self.handleRestSrv)
self.enableLifeSrv = rospy.Service("life/enable", Empty, self.handleLifeSrv)
self.disableLifeSrv = rospy.Service("life/disable", Empty, self.handleLifeOffSrv)
self.getLifeSrv = rospy.Service("life/get_state", Trigger, self.handleGetLifeSrv)
#Start simple action servers
self.jointTrajectoryServer = actionlib.SimpleActionServer("joint_trajectory", JointTrajectoryAction,
execute_cb=self.executeJointTrajectoryAction,
auto_start=False)
self.jointStiffnessServer = actionlib.SimpleActionServer("joint_stiffness_trajectory", JointTrajectoryAction,
execute_cb=self.executeJointStiffnessAction,
auto_start=False)
self.jointAnglesServer = actionlib.SimpleActionServer("joint_angles_action", JointAnglesWithSpeedAction,
execute_cb=self.executeJointAnglesWithSpeedAction,
auto_start=False)
#Start action servers
self.jointTrajectoryServer.start()
self.jointStiffnessServer.start()
self.jointAnglesServer.start()
# only start when ALRobotPosture proxy is available
if not (self.robotPostureProxy is None):
self.bodyPoseWithSpeedServer = actionlib.SimpleActionServer("body_pose_naoqi", BodyPoseWithSpeedAction,
execute_cb=self.executeBodyPoseWithSpeed,
auto_start=False)
self.bodyPoseWithSpeedServer.start()
else:
rospy.logwarn("Proxy to ALRobotPosture not available, requests to body_pose_naoqi will be ignored.")
# subscribers last:
rospy.Subscriber("joint_angles", JointAnglesWithSpeed, self.handleJointAngles, queue_size=10)
rospy.Subscriber("joint_stiffness", JointState, self.handleJointStiffness, queue_size=10)
rospy.loginfo("nao_controller initialized")
def __init__(self):
NaoqiNode.__init__(self, 'naoqi_moveto_listener')
self.connectNaoQi()
self.subscriber = rospy.Subscriber("/move_base_simple/goal", PoseStamped, self.callback)
def __init__(self):
NaoqiNode.__init__(self, 'nao_motion_bridge')
self.connectNaoQi()
self.moveTest()
rospy.Subscriber("joint_position_cmd", JointState, self.sendJointsToNAO)
def __init__(self):
NaoqiNode.__init__(self, 'naoqi_gaze_analysis')
self.connectNaoQi()
self.setToleranceSrv = rospy.Service("set_gaze_analysis_tolerance", SetFloat, self.handleSetGazeAnalysisTolerance)
self.getToleranceSrv = rospy.Service("get_gaze_analysis_tolerance", GetFloat, self.handleGetGazeAnalysisTolerance)
rospy.loginfo("naoqi_gaze_analysis is initialized")
def __init__(self):
NaoqiNode.__init__(self, 'naoqi_exploration')
self.navigation = None
self.connectNaoQi()
