def action(name):
client = actionlib.SimpleActionClient('/play_motion', PlayMotionAction)
client.wait_for_server()
goal = PlayMotionGoal()
goal.motion_name = name
goal.skip_planning = False
goal.priority = 0 # Optional
rospy.loginfo("Sending goal with motion: " + name)
client.send_goal(goal)
action_ok = client.wait_for_result(rospy.Duration(30.0))
def reset(self):
goal = PlayMotionGoal()
goal.motion_name = 'home'
goal.skip_planning = False
goal.priority = 0 # Optional
rospy.loginfo("Sending goal with motion: " + sys.argv[1])
self.client.send_goal(goal)
rospy.loginfo("Waiting for result...")
action_ok = self.client.wait_for_result(rospy.Duration(30.0))
state = self.client.get_state()
if action_ok:
rospy.loginfo("Action finished succesfully with state: " +
str(get_status_string(state)))
else:
rospy.logwarn("Action failed with state: " +
str(get_status_string(state)))
def act(self, action="detectObject", objectPath=""):
print(action)
goal = PlayMotionGoal()
goal.motion_name = action
goal.skip_planning = False
goal.priority = 0 # Optional
self.objectPath = objectPath
if (action == 'detectObject'):
self.writeXML()
cli_args = ['objectDetectRoslaunch.launch']
roslaunch_file = [
(roslaunch.rlutil.resolve_launch_arguments(cli_args)[0])
]
print(roslaunch_file)
uuid = roslaunch.rlutil.get_or_generate_uuid(None, False)
roslaunch.configure_logging(uuid)
launch = roslaunch.parent.ROSLaunchParent(uuid, roslaunch_file)
launch.start()
image = rospy.wait_for_message("/xtion/rgb/image_raw", Image, 30.0)
object_detect = rospy.wait_for_message('/objectsStamped',
ObjectsStamped, 30.0)
self.doObjectDetect(image, object_detect)
self.exitRoslaunch()
launch.shutdown()
elif (action == 'preSearchObject'):
print("preSearchObject")
elif (action == 'wave'):
rospy.loginfo("Sending goal with motion: " + goal.motion_name)
self.client.send_goal(goal)
rospy.loginfo("Waiting for result...")
action_ok = self.client.wait_for_result(rospy.Duration(30.0))
state = self.client.get_state()
def play_motion_start_action(act_name):
client = actionlib.SimpleActionClient('/play_motion', PlayMotionAction)
goal = PlayMotionGoal()
if (act_name == "head_down" or act_name == "head_tilt_down"):
goal.motion_name = "head_tilt_down"
elif (act_name == "head_normal" or act_name == "head_straight"):
goal.motion_name = "head_normal"
elif (act_name == "look_around" or act_name == "head_tour"):
goal.motion_name = "head_pan_tilted"
elif (act_name == "give_hand" or act_name == "offer_hand"):
goal.motion_name = "reach_floor"
elif (act_name == "home"):
goal.motion_name = "home"
else:
# at the moment no more will be handled but there are plenty of more movements
print("\t Wrong play_motion name...")
return
goal.skip_planning = False
goal.priority = 0
client.wait_for_server()
client.send_goal(goal)
'''
action_ok = client.wait_for_result(rospy.Duration(2.5))
state = client.get_state()
if action_ok:
rospy.loginfo("Action finished succesfully with state: " + str(get_status_string(state)))
else:
rospy.logwarn("Action failed with state: " + str(get_status_string(state)))
'''
return client
if len(sys.argv) < 2:
show_usage()
exit(0)
rospy.loginfo("Starting run_motion_python application...")
wait_for_valid_time(10.0)
client = SimpleActionClient('/play_motion', PlayMotionAction)
rospy.loginfo("Waiting for Action Server...")
client.wait_for_server()
goal = PlayMotionGoal()
goal.motion_name = sys.argv[1]
goal.skip_planning = True
goal.priority = 0 # Optional
rospy.loginfo("Sending goal with motion: " + sys.argv[1])
client.send_goal(goal)
rospy.loginfo("Waiting for result...")
action_ok = client.wait_for_result(rospy.Duration(30.0))
state = client.get_state()
if action_ok:
rospy.loginfo("Action finished succesfully with state: " +
str(get_status_string(state)))
else:
rospy.logwarn("Action failed with state: " +
str(get_status_string(state)))
def callback(msg):
global flag
if 'msg.detections[0].x' in locals():
x = msg.detections[0].x
y = msg.detections[0].y
rospy.loginfo('X: {}, Y: {}'.format(x, y))
#rospy.loginfo('Y: {}'.format(y))
#do something with closest
if (msg.detections and flag==0):
#flag =1
client = SimpleActionClient('/play_motion', PlayMotionAction)
rospy.loginfo("Waiting for Action Server...")
client.wait_for_server()
goal = PlayMotionGoal()
goal.motion_name = "offer_hand"
goal.skip_planning = False
goal.priority = 0 # Optional
rospy.loginfo("Sending goal with motion: " + "offer_hand")
client.send_goal(goal)
rospy.loginfo("Waiting for result...")
action_ok = client.wait_for_result(rospy.Duration(30.0))
state = client.get_state()
if action_ok:
rospy.loginfo("Action finished succesfully with state: " + str(get_status_string(state)))
flag = 1
else:
rospy.logwarn("Action failed with state: " + str(get_status_string(state)))
elif(not msg.detections and flag ==1):
client = SimpleActionClient('/play_motion', PlayMotionAction)
rospy.loginfo("Waiting for Action Server...")
client.wait_for_server()
goal = PlayMotionGoal()
goal.motion_name = "home"
goal.skip_planning = False
goal.priority = 0 # Optional
rospy.loginfo("Sending goal with motion: " + "home")
client.send_goal(goal)
rospy.loginfo("Waiting for result...")
action_ok = client.wait_for_result(rospy.Duration(30.0))
state = client.get_state()
if action_ok:
rospy.loginfo("Action finished succesfully with state: " + str(get_status_string(state)))
flag = 0
else:
rospy.logwarn("Action failed with state: " + str(get_status_string(state)))
def play_motion(name, callback = None): goal = PlayMotionGoal() goal.skip_planning = False goal.motion_name = name goal.priority = 20 actions.send_goal(goal, callback)
rospy.init_node('run_motion_python')
if len(sys.argv) < 2:
show_usage()
exit(0)
rospy.loginfo("Starting run_motion_python application...")
wait_for_valid_time(10.0)
client = SimpleActionClient('/play_motion', PlayMotionAction)
rospy.loginfo("Waiting for Action Server...")
client.wait_for_server()
goal = PlayMotionGoal()
goal.motion_name = sys.argv[1]
goal.skip_planning = False
goal.priority = 0 # Optional
rospy.loginfo("Sending goal with motion: " + sys.argv[1])
client.send_goal(goal)
rospy.loginfo("Waiting for result...")
action_ok = client.wait_for_result(rospy.Duration(30.0))
state = client.get_state()
if action_ok:
rospy.loginfo("Action finished succesfully with state: " + str(get_status_string(state)))
else:
rospy.logwarn("Action failed with state: " + str(get_status_string(state)))
def planner(ts, init_pose, act, robot_task, robot_name='TIAGo'):
global robot_pose
robot_pose = [None, init_pose]
rospy.init_node('ltl_planner_%s' % robot_name)
print 'Robot %s: ltl_planner started!' % (robot_name)
###### publish to
#----------
#publish to
#----------
InitialPosePublisher = rospy.Publisher('initialpose',
PoseWithCovarianceStamped,
queue_size=100)
# for i in xrange(10):
# SendInitialPose(InitialPosePublisher, init_pose, rospy.Time.now())
# rospy.sleep(0.1)
# print('Initial pose set to %s.' %str(init_pose))
GoalPublisher = rospy.Publisher('move_base_simple/goal',
PoseStamped,
queue_size=100)
ang_pub = rospy.Publisher('mobile_base_controller/cmd_vel',
Twist,
queue_size=100)
ang = Twist()
#----------
#subscribe to
#----------
rospy.Subscriber('amcl_pose', PoseWithCovarianceStamped, PoseCallback)
####### robot information
full_model = MotActModel(ts, act)
planner = ltl_planner(full_model, robot_task, None)
####### initial plan synthesis
planner.optimal(5)
#######
reach_xy_bound = 0.5 # m
reach_yaw_bound = 0.1 * PI # rad
t0 = rospy.Time.now()
while not rospy.is_shutdown():
try:
t = rospy.Time.now() - t0
print '----------Time: %.2f----------' % t.to_sec()
next_move = planner.next_move
if isinstance(next_move, str):
print 'Robot %s next move is action %s' % (str(robot_name),
str(next_move))
client = SimpleActionClient('/play_motion', PlayMotionAction)
print 'Waiting for Action Server...'
client.wait_for_server()
goal = PlayMotionGoal()
goal.motion_name = next_move
goal.skip_planning = False
goal.priority = 0 # Optional
print 'Sending actionlib goal with motion: %s' % next_move
client.send_goal(goal)
print 'Waiting for result...'
action_ok = client.wait_for_result(rospy.Duration(30.0))
state = client.get_state()
if state >= 1:
print 'Successful execution of action %s' % next_move
planner.find_next_move()
else:
print 'Failed execution of action %s. Will retry' % next_move
else:
print 'Robot %s next move is motion to %s' % (str(robot_name),
str(next_move))
if ((norm2(robot_pose[1][0:2], next_move[0:2]) >
reach_xy_bound) or
(abs(robot_pose[1][2]) - next_move[2]) > reach_yaw_bound):
SendGoal(GoalPublisher, next_move, t)
print 'diff = %f' % (abs(robot_pose[1][2]) - next_move[2])
print('Goal %s sent to %s.' %
(str(next_move), str(robot_name)))
rospy.sleep(1)
else:
print('Goal %s reached by %s.' %
(str(next_move), str(robot_name)))
#ang.angular.z = 0
#ang_pub.publish(ang)
planner.find_next_move()
if next_move == planner.next_move:
break
except rospy.ROSInterruptException:
pass
