Commit ca96dd93 authored by Katie Winkle (Student)'s avatar Katie Winkle (Student)
Browse files

added pepper nodes and experiment script

parent d08306fd
#!/usr/bin/env python
#import ROS dependencies
import rospy
import time
import sys
#import NAO dependencies
from naoqi_driver.naoqi_node import NaoqiNode
from std_msgs.msg import String, Empty
class Experiment(NaoqiNode):
def __init__(self):
NaoqiNode.__init__(self, 'persuasion_experiment')
self.connectNaoQi()
#self.ledsProxy = self.get_proxy("ALLeds")
#self.ledsProxy.fadeRGB("FaceLeds","white",1) #different colour for each condition, maybe pink & blue or blue & green
#self.basicAwarenessProxy = self.get_proxy("ALBasicAwareness")
self.robotPostureProxy = self.get_proxy("ALRobotPosture")
self.motionProxy = self.get_proxy("ALMotion")
self.tabletProxy = self.get_proxy("ALTabletService")
#self.animspeechProxy = self.get_proxy("ALAnimatedSpeech")
self.robotPostureProxy.goToPosture("Stand", 0.5)
self.motionProxy.setAngles("HeadPitch", -0.36, 0.1)
#self.basicAwarenessProxy.setTrackingMode("Head")
self.start_sub = rospy.Subscriber('exp_start', Empty, self.start_CB)
self.rep_sub = rospy.Subscriber('exp_rep', Empty, self.rep_CB)
#self.setStart_sub = rospy.Subscriber('set_start', Empty, self.setStart_CB)
self.stop_sub = rospy.Subscriber('exp_stop', Empty, self.stop_CB)
self.continue_sub = rospy.Subscriber('exp_continue', Empty, self.continue_CB)
#self.setFinish_sub = rospy.Subscriber('set_finish', Empty, self.setFinish_CB)
self.dontknow_sub = rospy.Subscriber('exp_dontknow', Empty, self.dontknow_CB)
self.correction_sub = rospy.Subscriber('exp_correction', Empty, self.correction_CB)
self.finish_sub = rospy.Subscriber('exp_finish', Empty, self.finish_CB)
self.observations_sub = rospy.Subscriber('exp_observations', String, self.observations_CB)
self.complete_sub = rospy.Subscriber('exp_complete', Empty, self.complete_CB)
self.start_time = 0
self.finish_time = 0
#self.setList = []
self.numberOfEncouragements = 0
self.amountOfCorrection = 0
self.amountOfDontknow=0
#self.numberOfCompleteSets = 0
#self.numberOfStartedSets = 0
#self.set_durations = []
self.timeSpent = 0
self.numberOfReps = 0
self.forcedFinish = 0
self.observations = ""
def connectNaoQi(self):
rospy.loginfo("Connecting to NaoQi at %s:%d", self.pip, self.pport)
def start_CB(self, Empty):
self.start_time = rospy.get_rostime()
time.sleep(3)
#go to stand init too?
#self.motionProxy.setAngles("HeadPitch", 0.0005, 0.1)
self.motionProxy.setAngles("HeadPitch", -0.16, 0.1)
#print("Overall start time is: " + str(self.start_time.secs))
# def setStart_CB(self, Empty):
# self.setList.append(["start_set", rospy.get_rostime()])
# self.numberOfCompleteSets+=1
# self.numberOfStartedSets+=1
def rep_CB(self, Empty):
self.numberOfReps+=1
if self.numberOfReps == 5:
self.motionProxy.setAngles("HeadPitch", -0.26, 0.1) #may need to reduce this to ensure can clearly see reps, or may have to not move head at all if makes too difficult
#self.animspeechProxy.say("^mode(disabled) ^start(animations/Stand/Gestures/Explain_2) You're doing well, keep going")
time.sleep(1)
#self.robotPostureProxy.goToPosture("Stand", 0.3)
self.motionProxy.setAngles("HeadPitch", 0.0, 0.1)
self.numberOfEncouragements+=1
if self.numberOfReps == 10:
self.motionProxy.setAngles("HeadPitch", -0.26, 0.1)
#self.animspeechProxy.say("^mode(disabled) ^start(animations/Stand/Gestures/Explain_2) Excellent work, keep it up.")
time.sleep(1)
self.motionProxy.setAngles("HeadPitch", 0.0, 0.1)
self.numberOfEncouragements+=1
if self.numberOfReps == 15:
self.motionProxy.setAngles("HeadPitch", -0.26, 0.1)
#self.animspeechProxy.say("^mode(disabled) ^start(animations/Stand/Gestures/Explain_2) Great effort, you can do it")
time.sleep(1)
self.motionProxy.setAngles("HeadPitch", 0.0, 0.1)
self.numberOfEncouragements+=1
if self.numberOfReps == 20:
self.motionProxy.setAngles("HeadPitch", -0.26, 0.1)
#self.animspeechProxy.say("^mode(disabled) ^start(animations/Stand/Gestures/Explain_2) Good job, keep it going.")
time.sleep(1)
self.motionProxy.setAngles("HeadPitch", 0.0, 0.1)
self.numberOfEncouragements+=1
if self.numberOfReps == 25:
self.motionProxy.setAngles("HeadPitch", -0.26, 0.1)
#self.animspeechProxy.say("^mode(disabled) ^start(animations/Stand/Gestures/Explain_2) You're doing great.")
time.sleep(1)
self.motionProxy.setAngles("HeadPitch", 0.0, 0.1)
self.numberOfEncouragements+=1
if self.numberOfReps == 30: #maybe have a forced finish here but need to pilot to see if its enough
self.motionProxy.setAngles("HeadPitch", -0.36, 0.1)
self.forcedFinish = 1
#self.animspeechProxy.say("^mode(disabled) ^start(animations/Stand/Gestures/Explain_2) You've done a lot! I think that's enough for today.")
#self.motionProxy.setAngles("HeadPitch", 0.059, 0.1)
def continue_CB(self, Empty):
self.motionProxy.setAngles("HeadPitch", 0.059, 0.1)
def stop_CB(self, Empty):
self.motionProxy.setAngles("HeadPitch", -0.16, 0.1)
#self.setList.append(["interrupt", rospy.get_rostime()])
#self.numberOfCompleteSets-=1
# def setFinish_CB(self, Empty):
# self.setList.append(["finish_set", rospy.get_rostime()])
def dontknow_CB(self, Empty):
self.amountOfDontknow+=1
def correction_CB(self, Empty):
self.amountOfCorrection+=1
def finish_CB(self, Empty):
self.finish_time = rospy.get_rostime()
self.tabletProxy.hideImage()
timeSpent = self.finish_time - self.start_time
self.timeSpent = timeSpent.to_sec()
#self.setList.append(["finish_overall", rospy.get_rostime()])
#self.ledsProxy.fadeRGB("FaceLeds","orange",1)
#print("Overall duration is: %.2f seconds" %(time_on_task_seconds))
#print self.setList
#set_durations = []
#for item in self.setList:
#index = self.setList.index(item)
#event = self.setList[index]
#if event[0] == "start_set": #start will always indicate start of a set, even if after e.g. an interruption
#next_event = self.setList[index+1]
#start_time = event[1]
#end_time = next_event[1]
#set_duration = end_time - start_time
#self.set_durations.append(set_duration.to_sec())
#print set_durations
#self.overall_timeSpent = sum(self.set_durations) #overall time exercising is sum of set durations
def observations_CB(self, string):
self.observations = string.data
def complete_CB(self, Empty):
#self.robotPostureProxy.goToPosture("Stand", 0.6)
file = open("G29.csv","w")
file.write("G29" + ',' ) #participant id
#file.write("Condition: testing" + ',' )
file.write("%i" %(self.numberOfReps) + ',' ) #number of reps
file.write("%i" %(self.forcedFinish) + ',' ) #forced finish 0 = not
file.write("%i" %(self.numberOfEncouragements) + ',' ) #number of encouragements given
file.write("%.2f" %(self.timeSpent) + ',') #time spent exercising in seconds
file.write("%i" %(self.amountOfCorrection) + ',') #number of corrections given
file.write("%i" %(self.amountOfDontknow) + ',') #number of times had to use don't know
file.write("%s" %(self.observations)) #any observations
#file.write("List of events is: " + str(self.setList) + '\n')
#file.write("Number of started sets is: %i" %(self.numberOfStartedSets) + '\n') #indicates may want to add on a few extra reps (wizard should note any such)
#file.write("Number of completed sets is: %i" %(self.numberOfCompleteSets) + '\n')
#file.write("Duration of each set is: " + str(self.set_durations) + '\n')
#file.write("Time spent exercising is: %.2f seconds" %(self.overall_timeSpent) + '\n')
print("Experiment complete, ctrl+c to kill this script")
if __name__ == '__main__':
experiment_listener = Experiment()
experiment_listener.start()
rospy.spin()
\ No newline at end of file
<launch>
<arg name="nao_ip" default="$(optenv NAO_IP 127.0.0.1)" />
<arg name="nao_port" default="$(optenv NAO_PORT 9559)" />
<arg name="namespace" default="pepper_robot" />
<!-- Use CPP node by default for nao_sensors -->
<arg name="force_python" default="true" />
<include file="$(find pepper_description)/launch/pepper_publisher.launch" />
<!-- nao driver works for pepper -->
<include file="$(find naoqi_driver_py)/launch/naoqi_driver.launch">
<arg name="nao_ip" value="$(arg nao_ip)" />
</include>
<!--my own package of extra nodes-->
<include file="$(find my_pepper_nodes)/launch/my_naoqi_nodes.launch">
<arg name="nao_ip" value="$(arg nao_ip)" />
</include>
<!--
<include file="$(find pepper_sensors_py)/launch/sonar.launch" >
<arg name="nao_ip" value="$(arg nao_ip)" />
</include>
<include file="$(find pepper_sensors_py)/launch/laser.launch" >
<arg name="nao_ip" value="$(arg nao_ip)" />
</include>
-->
<include file="$(find pepper_sensors_py)/launch/camera.launch" ns="$(arg namespace)/camera/depth" >
<arg name="nao_ip" value="$(arg nao_ip)" />
<arg name="source" value="2" />
<arg name="color_space" value="17" />
<arg name="resolution" value="1" />
</include>
<include file="$(find pepper_sensors_py)/launch/camera.launch" ns="$(arg namespace)/camera/front" >
<arg name="nao_ip" value="$(arg nao_ip)" />
<arg name="source" value="0" />
<arg name="color_space" value="13" />
</include>
<!-- launch pose manager -->
<include file="$(find naoqi_pose)/launch/pose_manager.launch" ns="$(arg namespace)/pose" >
<arg name="nao_ip" value="$(arg nao_ip)" />
</include>
<!-- launch perception -->
<include file="$(find pepper_bringup)/launch/perception.launch.xml">
<arg name="namespace" value="$(arg namespace)" />
</include>
</launch>
cmake_minimum_required(VERSION 2.8.3)
project(my_pepper_nodes)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
catkin_python_setup()
################################################
## Declare ROS messages, services and actions ##
################################################
## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
## * add a build_depend tag for "message_generation"
## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
## but can be declared for certainty nonetheless:
## * add a run_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
## * add "message_generation" and every package in MSG_DEP_SET to
## find_package(catkin REQUIRED COMPONENTS ...)
## * add "message_runtime" and every package in MSG_DEP_SET to
## catkin_package(CATKIN_DEPENDS ...)
## * uncomment the add_*_files sections below as needed
## and list every .msg/.srv/.action file to be processed
## * uncomment the generate_messages entry below
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
# add_message_files(
# FILES
# Message1.msg
# Message2.msg
# )
## Generate services in the 'srv' folder
# add_service_files(
# FILES
# Service1.srv
# Service2.srv
# )
## Generate actions in the 'action' folder
# add_action_files(
# FILES
# Action1.action
# Action2.action
# )
## Generate added messages and services with any dependencies listed here
# generate_messages(
# DEPENDENCIES
# std_msgs
# )
################################################
## Declare ROS dynamic reconfigure parameters ##
################################################
## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
## * add a build_depend and a run_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
## * add "dynamic_reconfigure" to
## find_package(catkin REQUIRED COMPONENTS ...)
## * uncomment the "generate_dynamic_reconfigure_options" section below
## and list every .cfg file to be processed
## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
# cfg/DynReconf1.cfg
# cfg/DynReconf2.cfg
# )
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if you package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES my_pepper_nodes
# CATKIN_DEPENDS roscpp rospy std_msgs
# DEPENDS system_lib
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
# include_directories(include)
include_directories(
${catkin_INCLUDE_DIRS}
)
## Declare a C++ library
# add_library(my_pepper_nodes
# src/${PROJECT_NAME}/my_pepper_nodes.cpp
# )
## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
# add_dependencies(my_pepper_nodes ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Declare a C++ executable
# add_executable(my_pepper_nodes_node src/my_pepper_nodes_node.cpp)
## Add cmake target dependencies of the executable
## same as for the library above
# add_dependencies(my_pepper_nodes_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Specify libraries to link a library or executable target against
# target_link_libraries(my_pepper_nodes_node
# ${catkin_LIBRARIES}
# )
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# install(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables and/or libraries for installation
# install(TARGETS my_pepper_nodes my_pepper_nodes_node
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
# FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE
# )
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
# # myfile1
# # myfile2
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_my_pepper_nodes.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)
#!/usr/bin/env python
#import ROS dependencies
import rospy
#import NAO dependencies
from naoqi_driver.naoqi_node import NaoqiNode
from std_msgs.msg import String
class AnimSpeechListener(NaoqiNode):
def __init__(self):
NaoqiNode.__init__(self, 'naoqi_animspeech_listener')
self.connectNaoQi()
self.animspeech_sub = rospy.Subscriber('pepper_robot/animspeech', String, self.animspeech_CB)
def connectNaoQi(self):
rospy.loginfo("Connecting to NaoQi at %s:%d", self.pip, self.pport)
self.animspeechProxy = self.get_proxy("ALAnimatedSpeech")
if self.animspeechProxy is None:
exit(1)
def animspeech_CB(self, string):
#string = str(string)
self.animspeechProxy.say(string.data)
if __name__ == '__main__':
animspeech_listener = AnimSpeechListener()
animspeech_listener.start()
rospy.spin()
\ No newline at end of file
#!/usr/bin/env python
#import ROS dependencies
import rospy
#import NAO dependencies
from naoqi_driver.naoqi_node import NaoqiNode
from std_msgs.msg import String
import qi
import argparse
import sys
import time
class ImageListener(NaoqiNode):
def __init__(self):
NaoqiNode.__init__(self, 'naoqi_image_listener')
self.connectNaoQi()
self.animspeech_sub = rospy.Subscriber('pepper_robot/image', String, self.image_CB)
def connectNaoQi(self):
rospy.loginfo("Connecting to NaoQi at %s:%d", self.pip, self.pport)
self.session = qi.Session()
self.tabletService = self.session.service("ALTabletService")
def image_CB(self, string):
#string = str(string)
self.tabletService.showImage(string.data)
if __name__ == '__main__':
image_listener = ImageListener()
image_listener.start()
rospy.spin()
\ No newline at end of file
#!/usr/bin/env python
#import ROS dependencies
import rospy
#import NAO dependencies
from naoqi_driver.naoqi_node import NaoqiNode
from std_msgs.msg import String
class SpeechListener(NaoqiNode):
def __init__(self):
NaoqiNode.__init__(self, 'naoqi_tts_listener')
self.connectNaoQi()
self.tts_sub = rospy.Subscriber('pepper_robot/tts', String, self.tts_CB)
def connectNaoQi(self):
rospy.loginfo("Connecting to NaoQi at %s:%d", self.pip, self.pport)
self.ttsProxy = self.get_proxy("ALTextToSpeech")
if self.ttsProxy is None:
exit(1)
def tts_CB(self, string):
#string = str(string)
self.ttsProxy.say(string.data)
if __name__ == '__main__':
speech_listener = SpeechListener()
speech_listener.start()
rospy.spin()
\ No newline at end of file
<launch>
<!--
start Nao control and sensor nodes so that they connect to a local NaoQI
(e.g. a local simulated one or when running directly on the robot).
For a remote connection, change IP and port accordingly in the NAO_IP and NAO_PORT
environment variables.
-->
<!--
This pushes the local PYTHONPATH into the launch file, so that the NaoQI API is found.
You need to add the Nao's API dir to your PYTHONPATH so that the modules are found.
-->
<env name="PYTHONPATH" value="$(env PYTHONPATH)" />
<arg name="nao_ip" default="$(optenv NAO_IP 127.0.0.1)" doc="Robot IP address. On robot or Local NaoQI" />
<arg name="nao_port" default="$(optenv NAO_PORT 9559)" doc="NaoQI port number" />
<!-- start robot agnostic nodes -->
<node pkg="my_pepper_nodes" type="naoqi_anim_speech.py" name="naoqi_anim_speech" required="true" args="--pip=$(arg nao_ip) --pport=$(arg nao_port)" output="screen" />
<node pkg="my_pepper_nodes" type="naoqi_speech.py" name="naoqi_speech" required="true" args="--pip=$(arg nao_ip) --pport=$(arg nao_port)" output="screen" />
<node pkg="my_pepper_nodes" type="naoqi_image.py" name="naoqi_image" required="true" args="--pip=$(arg nao_ip) --pport=$(arg nao_port)" output="screen" />
</launch>
<?xml version="1.0"?>
<package>
<name>my_pepper_nodes</name>
<version>0.0.0</version>
<description>Launches additional nodes to be used with pepper bring up for ros</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="katiewinkle@todo.todo">katiewinkle</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but mutiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/my_pepper_nodes</url> -->
<!-- Author tags are optional, mutiple are allowed, one per tag -->
<!-- Authors do not have to be maintianers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *_depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use run_depend for packages you need at runtime: -->
<!-- <run_depend>message_runtime</run_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend>
<run_depend>roscpp</run_depend>
<run_depend>rospy</run_depend>
<run_depend>std_msgs</run_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>
\ No newline at end of file
## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD
from distutils.core import setup
from catkin_pkg.python_setup import generate_distutils_setup
# fetch values from package.xml
setup_args = generate_distutils_setup(
packages=['my_pepper_nodes'],
package_dir={'': 'include'},
)
setup(**setup_args)
\ No newline at end of file