diff --git a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch index 4d280e75c2..31c3c7e896 100644 --- a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch +++ b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch @@ -25,7 +25,7 @@ - + lang: japanese diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/pr2_2dnav.launch b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/pr2_2dnav.launch index 3b12e8e988..e06259fa25 100644 --- a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/pr2_2dnav.launch +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/pr2_2dnav.launch @@ -40,5 +40,12 @@ args="$(find jsk_pr2_startup)/jsk_pr2_move_base/look-forward-in-nav.l" machine="c2" respawn="true" if="$(arg launch_look_forward)"/> + + + + + lang: japanese + + diff --git a/jsk_fetch_robot/jsk_fetch_startup/scripts/nav_speak.py b/jsk_robot_common/jsk_robot_startup/scripts/nav_speak.py similarity index 82% rename from jsk_fetch_robot/jsk_fetch_startup/scripts/nav_speak.py rename to jsk_robot_common/jsk_robot_startup/scripts/nav_speak.py index ecbbc6aeb1..eadf364e73 100755 --- a/jsk_fetch_robot/jsk_fetch_startup/scripts/nav_speak.py +++ b/jsk_robot_common/jsk_robot_startup/scripts/nav_speak.py @@ -4,10 +4,11 @@ import rospy import time +from actionlib_msgs.msg import GoalStatus +from move_base_msgs.msg import MoveBaseActionGoal +from move_base_msgs.msg import MoveBaseActionResult from sound_play.libsoundplay import SoundClient from sound_play.msg import SoundRequestActionGoal -from move_base_msgs.msg import MoveBaseActionGoal, MoveBaseActionResult -from actionlib_msgs.msg import GoalStatus def goal_status(status): @@ -22,15 +23,21 @@ def goal_status(status): GoalStatus.RECALLED: 'RECALLED', GoalStatus.LOST: 'LOST'}[status] -class NavSpeak: + +class NavSpeak(object): def __init__(self): - self.move_base_goal_sub = rospy.Subscriber("/move_base/goal", MoveBaseActionGoal, self.move_base_goal_callback, queue_size = 1) - self.move_base_result_sub = rospy.Subscriber("/move_base/result", MoveBaseActionResult, self.move_base_result_callback, queue_size = 1) + self.move_base_goal_sub = rospy.Subscriber( + "/move_base/goal", MoveBaseActionGoal, + self.move_base_goal_callback, queue_size=1) + self.move_base_result_sub = rospy.Subscriber( + "/move_base/result", MoveBaseActionResult, + self.move_base_result_callback, queue_size=1) self.sound = SoundClient() self.lang = "japanese" # speak japanese by default if rospy.has_param("/nav_speak/lang"): self.lang = rospy.get_param("/nav_speak/lang") - self.pub = rospy.Publisher('/robotsound_jp/goal', SoundRequestActionGoal, queue_size=1) + self.pub = rospy.Publisher( + '/robotsound_jp/goal', SoundRequestActionGoal, queue_size=1) def move_base_goal_callback(self, msg): self.sound.play(2) @@ -45,6 +52,7 @@ def move_base_result_callback(self, msg): sound_goal.goal.sound_request.command = 1 sound_goal.goal.sound_request.volume = 1.0 sound_goal.goal.sound_request.arg2 = "jp" + if msg.status.status == GoalStatus.SUCCEEDED: self.sound.play(1) time.sleep(1) @@ -75,12 +83,6 @@ def move_base_result_callback(self, msg): self.sound.say(text) if __name__ == "__main__": - global sound rospy.init_node("nav_speak") n = NavSpeak() rospy.spin() - - - - -