def __init__( self ):
#Initialisation
NaoNode.__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" )
# 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):
NaoNode.__init__(self)
rospy.init_node('nao_camera')
self.camProxy = self.getProxy("ALVideoDevice")
if self.camProxy is None:
exit(1)
# ROS pub/sub
self.pub_img_ = rospy.Publisher('image_raw', Image)
self.pub_info_ = rospy.Publisher('camera_info', CameraInfo)
self.set_camera_info_service_ = rospy.Service('set_camera_info', SetCameraInfo, self.set_camera_info)
# Messages
self.info_ = CameraInfo()
self.set_default_params_qvga(self.info_) #default params should be overwritten by service call
# parameters
self.camera_switch = rospy.get_param('~camera_switch', 0)
if self.camera_switch == 0:
self.frame_id = "/CameraTop_frame"
elif self.camera_switch == 1:
self.frame_id = "/CameraBottom_frame"
else:
rospy.logerr('Invalid camera_switch. Must be 0 or 1')
exit(1)
print "Using namespace ", rospy.get_namespace()
print "using camera: ", self.camera_switch
#TODO: parameters
self.resolution = kQVGA
self.colorSpace = kBGRColorSpace
self.fps = 30
# init
self.nameId = ''
self.subscribe()
def __init__(self):
#Initialisation
NaoNode.__init__(self)
rospy.init_node(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.getProxy("ALBehaviorManager")
# 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
NaoNode.__init__( self )
rospy.init_node( self.NODE_NAME )
#Proxy to interface with LEDs
self.proxy = self.getProxy( "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):
# Initialisation
NaoNode.__init__(self)
rospy.init_node(Constants.NODE_NAME)
# State variables
self.conf = None
# Get Audio proxies
# Speech-recognition wrapper will be lazily initialized
self.audio = self.getProxy("ALAudioDevice")
self.tts = self.getProxy("ALTextToSpeech")
self.srw = None
# When using simulated naoqi, audio device is not available,
# Use a dummy instead
if self.audio is None:
rospy.logwarn("Using dummy audio device, volume controls disabled")
self.audio = DummyAudioDevice()
# Start reconfigure server
self.reconf_server = ReConfServer(NodeConfig, self.reconfigure)
#Subscribe to speech topic
self.sub = rospy.Subscriber("speech", String, self.say)
# Advertise word recognise topic
self.pub = rospy.Publisher("word_recognized", WordRecognized)
# Register ROS services
self.start_srv = rospy.Service("start_recognition", Empty, self.start)
self.stop_srv = rospy.Service("stop_recognition", Empty, self.stop)
def __init__(self):
NaoNode.__init__(self)
# ROS initialization:
rospy.init_node('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):
NaoNode.__init__(self, 'nao_sensors')
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 = ""
# To stop odometry tf being broadcast
self.broadcast_odometry = rospy.get_param('~broadcast_odometry', True)
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')
# 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, queue_size=10)
self.torsoIMUPub = rospy.Publisher("imu", Imu, queue_size=10)
self.jointStatePub = rospy.Publisher("joint_states", JointState, queue_size=10)
self.tf_br = tf.TransformBroadcaster()
rospy.loginfo("nao_sensors initialized")
def __init__(self, moduleName):
# ROS initialization
NaoNode.__init__(self)
rospy.init_node( 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):
#Initialisation
NaoNode.__init__(self)
rospy.init_node(self.NODE_NAME)
#Variable to keep track of subscription to Naoqi's speech recognition
self.subscribed = False
# Get text-to-speech proxy
# Speech-recognition wrapper will be lazily initialized
self.tts = self.getProxy("ALTextToSpeech")
self.srw = None
#Update parameters from ROS Parameter server
self.reconfigure(None)
#Subscribe to speech topic
self.sub = rospy.Subscriber("speech", String, self.say)
# Advertise word recognise topic
self.pub = rospy.Publisher("word_recognized", WordRecognized)
# Register ROS services
self.reconfigure_srv = rospy.Service("reconfigure", Empty,
self.reconfigure)
self.start_srv = rospy.Service("start_recognition", Empty, self.start)
self.stop_srv = rospy.Service("stop_recognition", Empty, self.stop)
def __init__(self):
NaoNode.__init__(self)
rospy.init_node('nao_camera')
self.camProxy = self.getProxy("ALVideoDevice")
if self.camProxy is None:
exit(1)
# ROS pub/sub
self.pub_img_ = rospy.Publisher('image_raw', Image)
self.pub_info_ = rospy.Publisher('camera_info', CameraInfo)
self.set_camera_info_service_ = rospy.Service('set_camera_info',
SetCameraInfo,
self.set_camera_info)
# Messages
self.info_ = CameraInfo()
self.set_default_params_qvga(
self.info_) #default params should be overwritten by service call
# parameters
self.camera_switch = rospy.get_param('~camera_switch', 0)
if self.camera_switch == 0:
self.frame_id = "/CameraTop_frame"
elif self.camera_switch == 1:
self.frame_id = "/CameraBottom_frame"
else:
rospy.logerr('Invalid camera_switch. Must be 0 or 1')
exit(1)
print "Using namespace ", rospy.get_namespace()
print "using camera: ", self.camera_switch
#TODO: parameters
self.resolution = kQVGA
self.colorSpace = kBGRColorSpace
self.fps = 30
# init
self.nameId = ''
self.subscribe()
def __init__(self):
NaoNode.__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 ):
#Initialisation
NaoNode.__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):
NaoNode.__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, queue_size=10)
# 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):
NaoNode.__init__(self)
rospy.init_node('nao_camera')
self.camProxy = self.getProxy("ALVideoDevice")
if self.camProxy is None:
exit(1)
# check if camera_switch is admissible
self.camera_switch = rospy.get_param('~camera_switch', 0)
if self.camera_switch == 0:
self.frame_id = "/CameraTop_frame"
elif self.camera_switch == 1:
self.frame_id = "/CameraBottom_frame"
else:
rospy.logerr('Invalid camera_switch. Must be 0 or 1')
exit(1)
rospy.loginfo('using camera: %d', self.camera_switch)
# Parameters for ALVideoDevice
self.resolution = rospy.get_param('~resolution', kVGA)
self.colorSpace = rospy.get_param('~color_space', kBGRColorSpace)
self.fps = rospy.get_param('~fps', 30)
# ROS publishers
self.pub_img_ = rospy.Publisher('~image_raw', Image)
self.pub_info_ = rospy.Publisher('~camera_info', CameraInfo)
# initialize camera info manager
self.cname = "nao_camera" # maybe it's a good idea to add _top and _bottom here ...
self.cim = camera_info_manager.CameraInfoManager(cname=self.cname)
self.calibration_file_bottom = rospy.get_param('~calibration_file_bottom', None)
self.calibration_file_top = rospy.get_param('~calibration_file_top', None)
if self.camera_switch == 0:
calibration_file = self.calibration_file_top
else:
calibration_file = self.calibration_file_bottom
if not self.cim.setURL( calibration_file ):
rospy.logerr('malformed URL for calibration file')
sys.exit(1)
else:
try:
self.cim.loadCameraInfo()
except IOExcept:
rospy.logerr('Could not read from existing calibration file')
exit(1)
if self.cim.isCalibrated():
rospy.loginfo('Successfully loaded camera info')
else:
rospy.logerr('There was a problem loading the calibration file. Check the URL!')
exit(1)
# subscription to camProxy
self.nameId = ''
self.subscribe()
def __init__(self):
NaoNode.__init__(self)
# ROS initialization:
rospy.init_node('nao_controller')
self.connectNaoQi()
# 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)
# start services / actions:
self.enableStiffnessSrv = rospy.Service("body_stiffness/enable", Empty, self.handleStiffnessSrv)
self.disableStiffnessSrv = rospy.Service("body_stiffness/disable", Empty, self.handleStiffnessOffSrv)
#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()
# subsribers 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, sonarSensorList, param_sonar_rate="~sonar_rate", sonar_rate=40):
NaoNode.__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):
NaoNode.__init__(self)
Thread.__init__(self)
# ROS initialization:
rospy.init_node('nao_diagnostic_updater')
self.connectNaoQi()
self.stopThread = False
# 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):
# 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,
sonarSensorList,
param_sonar_rate="~sonar_rate",
sonar_rate=40):
NaoNode.__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):
NaoNode.__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, queue_size=10)
rospy.loginfo("nao_diagnostic_updater initialized")
def __init__(self):
NaoNode.__init__(self)
rospy.init_node('nao_laser')
# for measureddata
self.startpoint = -30.0
self.endpoint = 210.0
self.scaninterval = 0
self.dataGroupingnumber = 1
self.dataInterval = 0.35
self.sensorstate = 'NORMAL'
self.distancenum = 682
# for rangedata
self.minangle = -0.49087
self.maxangle = 3.64474
self.maxdatanum = 768
self.angres = 0.0061
self.minrange = 0.06
self.maxrange = 5.6
self.rangeres = 0.01
self.frequency = 10
self.laserscan = LaserScan()
self.laserscan.header.frame_id = "/urg_frame"
self.laserscan.range_min = self.minrange
self.laserscan.range_max = self.maxrange
try:
self.alurg = self.getProxy("ALLaser")
self.alurg.setOpeningAngle(-2.09439, 2.09439)
self.alurg.setDetectingLength(20, 5600)
self.alurg.laserON()
except Exception as e:
print("Error when creating ALLaser proxy:")
print(str(e))
exit(1)
try:
self.almem = self.getProxy("ALMemory")
except Exception as e:
print("Error when creating ALMemory proxy:")
print(str(e))
exit(1)
self.pub_laser = rospy.Publisher('scan', LaserScan)
def __init__(self):
NaoNode.__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)
proxy._setObstacleModeForSafety(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):
NaoNode.__init__(self)
rospy.init_node('nao_camera')
self.camProxy = self.getProxy("ALVideoDevice")
if self.camProxy is None:
exit(1)
# ROS pub/sub
self.pub_img_ = []
self.pub_info_ = []
for i in ['/nao_cam/top/','/nao_cam/bottom/']:
self.pub_img_ += [rospy.Publisher(i+'image_raw', Image)]
self.pub_info_ += [rospy.Publisher(i+'camera_info', CameraInfo)]
# self.set_camera_info_service_ = rospy.Service('set_camera_info', SetCameraInfo, self.set_camera_info)
topCamParamsURL = rospy.get_param('~topCamParams','')
bottomCamParamsURL = rospy.get_param('~bottomCamParams','')
print "urls", topCamParamsURL, bottomCamParamsURL
self.tCIM = CameraInfoManager('/nao_cam/top/','narrow_stereo/top', topCamParamsURL)
self.bCIM = CameraInfoManager('/nao_cam/bottom/','narrow_stereo/bottom', bottomCamParamsURL)
self.tCIM.loadCameraInfo()
self.bCIM.loadCameraInfo()
# Messages
# self.info_ = CameraInfo()
# self.set_default_params_qvga(self.info_)
self.info_ = []
self.info_.append(self.tCIM.getCameraInfo())
self.info_.append(self.bCIM.getCameraInfo())
#TODO: parameters
self.resolution = rospy.get_param('~quality',30)
self.colorSpace = kBGRColorSpace
self.fps = rospy.get_param('~fps',20)
print "using fps: ", self.fps
# init
self.nameId = "rospy_gvm"
self.subscribe()
def __init__(self, moduleName):
# ROS initialization
NaoNode.__init__(self)
rospy.init_node( 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 ):
# Initialisation
NaoNode.__init__( self )
rospy.init_node( Constants.NODE_NAME )
# State variables
self.conf = None
# Get Audio proxies
# Speech-recognition wrapper will be lazily initialized
self.audio = self.getProxy("ALAudioDevice")
self.tts = self.getProxy("ALTextToSpeech")
self.srw = None
# When using simulated naoqi, audio device is not available,
# Use a dummy instead
if self.audio is None:
rospy.logwarn("Using dummy audio device, volume controls disabled")
self.audio = DummyAudioDevice()
# Start reconfigure server
self.reconf_server = ReConfServer(NodeConfig, self.reconfigure)
#Subscribe to speech topic
self.sub = rospy.Subscriber("speech", String, self.say )
# Advertise word recognise topic
self.pub = rospy.Publisher("word_recognized", WordRecognized )
# Register ROS services
self.start_srv = rospy.Service(
"start_recognition",
Empty,
self.start )
self.stop_srv = rospy.Service(
"stop_recognition",
Empty,
self.stop )
def __init__(self):
NaoNode.__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)
proxy._setObstacleModeForSafety(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 ):
#Initialisation
NaoNode.__init__( self )
rospy.init_node( self.NODE_NAME )
#Variable to keep track of subscription to Naoqi's speech recognition
self.subscribed = False
# Get text-to-speech proxy
# Speech-recognition wrapper will be lazily initialized
self.tts = self.getProxy("ALTextToSpeech")
self.srw = None
#Update parameters from ROS Parameter server
self.reconfigure( None )
#Subscribe to speech topic
self.sub = rospy.Subscriber("speech", String, self.say )
# Advertise word recognise topic
self.pub = rospy.Publisher("word_recognized", WordRecognized )
# Register ROS services
self.reconfigure_srv = rospy.Service(
self.NODE_NAME + "/reconfigure",
Empty,
self.reconfigure
)
self.start_srv = rospy.Service(
self.NODE_NAME + "/start_recognition",
Empty,
self.start )
self.stop_srv = rospy.Service(
self.NODE_NAME + "/stop_recognition",
Empty,
self.stop )
def __init__(self):
NaoNode.__init__(self)
rospy.init_node('nao_camera')
self.camProxy = self.getProxy("ALVideoDevice")
if self.camProxy is None:
exit(1)
# check if camera_switch is admissible
self.camera_switch = rospy.get_param('~camera_switch', 0)
if self.camera_switch == 0:
self.frame_id = "/CameraTop_frame"
elif self.camera_switch == 1:
self.frame_id = "/CameraBottom_frame"
else:
rospy.logerr('Invalid camera_switch. Must be 0 or 1')
exit(1)
rospy.loginfo('using camera: %d', self.camera_switch)
# Parameters for ALVideoDevice
self.resolution = rospy.get_param('~resolution', kVGA)
self.colorSpace = rospy.get_param('~color_space', kBGRColorSpace)
self.fps = rospy.get_param('~fps', 30)
# ROS publishers
self.pub_img_ = rospy.Publisher('~image_raw', Image)
self.pub_info_ = rospy.Publisher('~camera_info', CameraInfo)
# initialize camera info manager
self.cname = "nao_camera" # maybe it's a good idea to add _top and _bottom here ...
self.cim = camera_info_manager.CameraInfoManager(cname=self.cname)
self.calibration_file_bottom = rospy.get_param(
'~calibration_file_bottom', None)
self.calibration_file_top = rospy.get_param('~calibration_file_top',
None)
if self.camera_switch == 0:
calibration_file = self.calibration_file_top
else:
calibration_file = self.calibration_file_bottom
if not self.cim.setURL(calibration_file):
rospy.logerr('malformed URL for calibration file')
sys.exit(1)
else:
try:
self.cim.loadCameraInfo()
except IOExcept:
rospy.logerr('Could not read from existing calibration file')
exit(1)
if self.cim.isCalibrated():
rospy.loginfo('Successfully loaded camera info')
else:
rospy.logerr(
'There was a problem loading the calibration file. Check the URL!'
)
exit(1)
# subscription to camProxy
self.nameId = ''
self.subscribe()
def __init__(self):
NaoNode.__init__(self)
# ROS initialization:
rospy.init_node('nao_controller')
self.connectNaoQi()
# 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)
# start services / actions:
self.enableStiffnessSrv = rospy.Service("body_stiffness/enable", Empty,
self.handleStiffnessSrv)
self.disableStiffnessSrv = rospy.Service("body_stiffness/disable",
Empty,
self.handleStiffnessOffSrv)
#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()
# subsribers 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):
NaoNode.__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, queue_size=10)
# 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):
NaoNode.__init__(self)
Thread.__init__(self)
# ROS initialization:
rospy.init_node('nao_sensors')
self.connectNaoQi()
self.stopThread = False
# 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')
# 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.tf_br = tf.TransformBroadcaster()
rospy.loginfo("nao_sensors initialized")