Hello, I am creating something similiar to the move_group_tutorial, the robot should do the sequence automatically, but the robot both displays and executes the planned path synchronously. How can I make the robot wait for the display of the path to finish before executing? This is my code:

!/usr/bin/env python

import sys import rospy import copy import moveit_commander import moveit_msgs.msg import geometry_msgs.msg from math import pi from moveit_commander.conversions import pose_to_list from test.msg import square_size

def move_panda_square(): #set up subscriber node rospy.init_node('move_panda_square', anonymous = True) rospy.Subscriber('square_size', square_size, callback) rospy.spin()

def callback(data): rospy.loginfo(data) robot = MoveGroupInterface() go_to_start_config(robot) counter=0 plan, fraction = plan_cartesian_path(robot, data) display_trajectory(robot, plan) #wait for trajectory to be displayed while(counter<25000000): counter+=1

execute_planned_trajectory(robot, plan)

class MoveGroupInterface(object): """MoveGroupInterface""" def __init__(self): super(MoveGroupInterface, self).__init__()

#Initialize panda robot, scene and a publisher used to display trajetories
moveit_commander.roscpp_initialize(sys.argv)    
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
group_name = "panda_arm"
move_group = moveit_commander.MoveGroupCommander(group_name)
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20)

#Basic information
planning_frame = move_group.get_planning_frame()
eef_link = move_group.get_end_effector_link()
group_names = robot.get_group_names()

# Misc variables
self.box_name = ''
self.robot = robot
self.scene = scene
self.move_group = move_group
self.display_trajectory_publisher = display_trajectory_publisher
self.planning_frame = planning_frame
self.eef_link = eef_link
self.group_names = group_names

def go_to_start_config(self): #set starting pos joint_goal = self.move_group.get_current_joint_values() joint_goal[0] = 0 joint_goal[1] = -pi/4 joint_goal[2] = 0 joint_goal[3] = -pi/2 joint_goal[4] = 0 joint_goal[5] = pi/3 joint_goal[6] = 0

#move robot
self.move_group.go(joint_goal, wait = True)
self.move_group.stop()

def plan_cartesian_path(self, square_size):

#plan a Cartesian path of a square in the x-y plane
waypoints = []

wpose = self.move_group.get_current_pose().pose
wpose.position.x += square_size.size

waypoints.append(copy.deepcopy(wpose))

wpose.position.y += square_size.size
waypoints.append(copy.deepcopy(wpose))

wpose.position.x -= square_size.size
waypoints.append(copy.deepcopy(wpose))

wpose.position.y -= square_size.size
waypoints.append(copy.deepcopy(wpose))

(plan, fraction) = self.move_group.compute_cartesian_path(waypoints, 0.01, 0.0) 

return plan, fraction

def display_trajectory(self, plan): #Select trajectory_start to current position and add the calculated cartesian plan before publishing display_trajectory = moveit_msgs.msg.DisplayTrajectory() display_trajectory.trajectory_start = self.robot.get_current_state() display_trajectory.trajectory.append(plan) self.display_trajectory_publisher.publish(display_trajectory)

def execute_planned_trajectory(self, plan): self.move_group.execute(plan, wait=True)

if __name__ == '__main__': move_panda_square()