def execute(self, ud):
self.move_base_client.wait_for_server()
goal = MoveBaseGoal()
goal.target_pose = ud.move_waypoints[ud.move_target_wp]
goal.target_pose.header.stamp = rospy.Time.now()
self.move_base_client.send_goal(goal)
self.move_base_client.wait_for_result()
status = self.move_base_client.get_state()
tts_msg = String()
if status == GoalStatus.SUCCEEDED:
tts_msg.data = "I have reached " + ud.move_wp_str[ud.move_target_wp] + "."
self.tts_pub.publish(tts_msg)
if ud.move_target_wp == 0:
return 'test_finished'
else:
ud.move_target_wp -= 1
return 'succeeded'
else:
return 'wp_not_reached'
def _move_to_marker(self, trans_pose):
pose = trans_pose
goal = MoveBaseGoal()
goal.target_pose = pose
# The robot can't move in these directions. Clear them so it's not confused
goal.target_pose.pose.position.z = 0
goal.target_pose.pose.orientation.x = 0
goal.target_pose.pose.orientation.y = 0
# If the marker position is fresh, stay behind it so we don't collide.
# Use trans_pose to get original time
if self._last_trans_pose.header.stamp + rospy.Duration(2) > rospy.Time.now():
goal.target_pose.pose.position.x = goal.target_pose.pose.position.x - self.FOLLOW_DIST
rospy.logdebug(
"Sending new goal. x: "
+ str(goal.target_pose.pose.position.x)
+ ", y: "
+ str(goal.target_pose.pose.position.y)
)
# Cancel old goal if it exists.
self.base_client.cancel_all_goals()
self.base_client.send_goal(goal)
def navigate(self, goalHandle, statusCB, doneCB):
positions = goalHandle.get_goal().jointPositions
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = "map"
pose.pose.position.x = positions[0]
pose.pose.position.y = positions[1]
pose.pose.position.z = 0.0
q = Rotation.RotZ(positions[2]).GetQuaternion()
pose.pose.orientation.x = q[0]
pose.pose.orientation.y = q[1]
pose.pose.orientation.z = q[2]
pose.pose.orientation.w = q[3]
client_goal = MoveBaseGoal()
client_goal.target_pose = pose
client = self._jointControllers.get('base', None)
if client:
client.wait_for_server()
rospy.loginfo("%s: Navigating to %s",
rospy.get_name(),
pose)
client.send_goal(client_goal, done_cb=doneCB)
else:
goalHandle.set_rejected()
def go_to(self, goal_posestamped, timeout=0):
goal = MoveBaseGoal()
goal.target_pose = goal_posestamped
self.movebase.send_goal(goal)
self.movebase.wait_for_result() # timeout maybe?
# result = self.movebase.get_goal_status_text()
# print 'move_base ' + result
resultcode = self.movebase.get_state()
# print 'result code ' + repr(resultcode)
return resultcode
def move_to(self, stamped, done_cb, timeout_cb): goal = MoveBaseGoal() goal.target_pose = stamped goal.target_pose.header.stamp = rospy.Time(0) msg = MarkerArray() msg.markers.append(self.tools.create_pose_marker(stamped)) self.tools.visualization_publisher.publish(msg) self.cancel_all_actions() self.cancel_standstill_timer() self._action = ActionWrapper(done_cb=done_cb, timeout_cb=timeout_cb, timeout=Params().move_base_timeout, action_client=self.move_base_client, goal=goal)
def go_to(self, pose_stamped):
"""Goes to a location in the world.
Args:
pose_stamped: geometry_msgs/PoseStamped. The location to go to.
"""
self._move_base_client.wait_for_server()
goal = MoveBaseGoal()
goal.target_pose = pose_stamped
self._move_base_client.send_goal(goal)
self._move_base_client.wait_for_result()
def goalcallback(self, data):
#we'll send a goal to the robot to move 3 meters forward
goal = MoveBaseGoal()
# goal.target_pose.header.frame_id = 'base_link'
# goal.target_pose.header.stamp = rospy.Time.now()
# goal.target_pose.pose.position.x = 0.2
# goal.target_pose.pose.orientation.w = 1.0
goal.target_pose = data
#start moving
rospy.logerr(goal)
# self.move_base.send_goal(goal)
#TODO check status of route, if on another route, cancel it first
# self.cancelMovement()
t = rospy.Time.now()
#draws the path
req = GetPlanRequest()
req.start = self.robot_pose_stamped
req.start.header.stamp = t
req.goal = data
req.goal.header.stamp = t
req.tolerance = .02
# return_path = self.get_path_to_goal_srv(req)
# self.next_path_pub.publish(return_path)
self.move_to_goal_count += 1
g = MoveBaseActionGoal();
t = rospy.Time.now()
g.header = Header(self.move_to_goal_count, t, "map")
g.goal_id.stamp = t
g.goal_id.id = "movement_num:"+str(self.move_to_goal_count)
g.goal.target_pose = data
# newnppose = vector3_to_numpy(data.pose.position)
# if np.linalg.norm(newnppose - self.lastnppose) >.05 :
# self.goal_pose_stamped_pub.publish(data)
self.move_to_goal_pub.publish(g)
rospy.logerr("published move_base goal")
def retreat(self, done_cb, timeout_cb):
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = 'base_footprint'
goal.target_pose.pose.position.x = -(Params().approach_distance - Params().ring_distance)
goal.target_pose.header.stamp = rospy.Time(0)
goal.target_pose.pose.orientation.w = 1
goal.target_pose = self.tools.transform_pose('map', goal.target_pose)
msg = MarkerArray()
msg.markers.append(self.tools.create_pose_marker(goal.target_pose))
msg.markers[-1].color.r = 0.8
msg.markers[-1].color.g = 0.5
self.tools.visualization_publisher.publish(msg)
self.cancel_all_actions()
self.cancel_standstill_timer()
self._action = ActionWrapper(done_cb=done_cb, timeout_cb=timeout_cb, timeout=Params().approach_timeout, action_client=self.retreat_client, goal=goal)
def move_to_pose(self, pos, frame="/odom_combined"):
"""
Move the robot to pos, using the move_base actions.
@param pos: A PoseStamped
@param frame: Check that the frame is allowed by move_base
"""
if not self.use_move_base:
rospy.logerr("Trying to call a move_base service without \
initializing it")
self.listener.waitForTransform(frame, pos.header.frame_id,
rospy.Time(0), rospy.Duration((1.0)))
target = self.listener.transformPose(frame, pos)
goal= MoveBaseGoal()
goal.target_pose = target
self.move_action.send_goal(goal)
return self.move_action.wait_for_result()
def driveTo(x,y, theta):
rospy.loginfo("Drive to (%f, %f, %f)", x, y, theta)
newPose = generate_pose(x, y, 0)
#print newPose
actionGoal = MoveBaseActionGoal()
actionGoal.header = genHeader()
actionGoal.goal_id.id = str(driveTo.goalID)
actionGoal.goal_id.stamp = rospy.Time.now()
goal = MoveBaseGoal()
goal.target_pose = newPose
actionGoal.goal = goal
# Publish the goal to the robot
global actionGoalPublisher
actionGoalPublisher.publish(actionGoal)
global soundClient
# Wait for the robot's status to to have reached the goal
timeOutCounter = 0
while not rospy.is_shutdown(): # This is done so that status can be checked and used
rospy.sleep(4.)
timeOutCounter += 1
currentStatus = getStatus(driveTo.goalID)
global cant_reach_list
print "Status: %d, GoalID: %d, Driving to: (%f, %f, %f), # unreachable: %d" % (currentStatus, driveTo.goalID, x, y, theta, len(cant_reach_list ))
if currentStatus == GoalStatus.ABORTED or timeOutCounter > 20:
soundClient.say("Abort driving to goal.")
print "The goal was aborted"
cant_reach_list.append((x, y))
break
elif currentStatus == GoalStatus.REJECTED:
soundClient.say("Goal rejected")
print "The goal was rejected"
break
elif currentStatus == GoalStatus.LOST:
soundClient.say("Robot is lost")
print "The robot is lost, exiting driving"
#TODO Should we send a cancel message?
exit(1)
break
elif currentStatus == GoalStatus.SUCCEEDED:
soundClient.say("Goal reached. Moving on.")
print "Drive to complete!"
break
driveTo.goalID += 1
def execute(self, userdata):
rospy.loginfo("MoveBase called.")
try:
client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
client.wait_for_server(rospy.Duration.from_sec(10.0))
goal = MoveBaseGoal()
goal.target_pose = userdata.goal_pose
client.send_goal(goal)
client.wait_for_result(rospy.Duration.from_sec(80.0))
if client.get_state() == 3:
return 'succeeded'
else:
return 'failed'
except actionlib.ActionException, e:
rospy.logwarn("MoveBase action failed: %s" %e)
return 'failed'
def move(self, goal_x=0, goal_y=0, goal_yaw=0, x_lower_bound=-1, x_upper_bound=1, y_lower_bound=-1, y_upper_bound=1):
# To prevent robot getting stuck or drifting towards just one specific area over course of experiment
# Randomly send to middle of allowable area with 1% probability
rand = random.uniform(0, 100)
if rand == 1:
goal_x = (x_lower_bound + x_upper_bound) / 2
goal_y = (y_lower_bound + y_upper_bound) / 2
goal_pose = PoseStamped()
goal_pose.header.stamp = rospy.Time.now()
goal_pose.header.frame_id = 'map'
goal_pose.pose.position.x = goal_x
goal_pose.pose.position.y = goal_y
goal_pose.pose.orientation = self.continuous_pose_wrt_map.pose.pose.orientation # Always keep our orientation the same
goal = MoveBaseGoal()
goal.target_pose = goal_pose
try:
rospy.logdebug('Waiting for move_base server')
self.move_base_client.wait_for_server()
rospy.logdebug('Sending goal to move_base server')
self.move_base_client.send_goal(goal)
rospy.logdebug('Waiting for goal result from move_base server')
self.move_base_client.wait_for_result()
success = self.move_base_client.get_result()
if not success:
rospy.logwarn(self.namespace + ': move_base was not able to successfully complete the request action')
else:
continuous_x = self.continuous_pose_wrt_map.pose.pose.position.x
continuous_y = self.continuous_pose_wrt_map.pose.pose.position.y
discrete_x = self.discrete_pose_wrt_map.pose.pose.position.x
discrete_y = self.discrete_pose_wrt_map.pose.pose.position.y
gazebo_x = self.gazebo_pose_wrt_map.pose.pose.position.x
gazebo_y = self.gazebo_pose_wrt_map.pose.pose.position.y
rospy.loginfo(self.namespace + ': requested move to (' + str(goal_x) + ', ' + str(goal_y) + ') ' +
'completed. Current gazebo odom pose is (' + str(gazebo_x) + ', ' + str(gazebo_y) + '). ' +
'Current continuous odom pose is (' + str(continuous_x) + ', ' + str(continuous_y) + '). ' +
'Current discrete odom pose is (' + str(discrete_x) + ', ' + str(discrete_y) + '). ')
except rospy.ROSException as e:
rospy.logwarn(self.namespace + ': caught exception while sending move_base a movement goal - ' + e.message)
def set_goal(self, goal_id):
if self.current_goal_status() != "FREE":
self.cancel_goal()
# invalid goal id
if goal_id >= len(self.waypoints):
self.goal_status = "ERROR"
rospy.logerr("Invalid goal id: " + str(goal_id))
return
self.current_goal_id = goal_id
self.goal_status = "WORKING"
goal = MoveBaseGoal()
goal.target_pose = self.waypoints[goal_id]
def done_cb(status, result):
self.goal_status = "FREE"
# wait for 1s
if not self.move_base.wait_for_server(rospy.Duration(1)):
self.goal_status = "ERROR"
self.cancel_goal()
return
self.move_base.send_goal(goal, done_cb=done_cb)
def begin_explore( self, radius, preempt_func = retfalse ):
rospy.logout( 'snaking_explore: setting radius' )
waypoints = self.setup_poses( radius ) # PoseStamped in self.frame
local_planner = rospy.get_param('/move_base_node/base_local_planner') # : dwa_local_planner/DWAPlannerROS
local_planner = local_planner[string.find(local_planner,'/')+1:]
obs_range = rospy.get_param('/move_base_node/global_costmap/obstacle_range')
move_tol = rospy.get_param('/move_base_node/'+local_planner+'/xy_goal_tolerance')
no_progress = rospy.get_param( rospy.get_name() + '/no_progress_move', 5)
unreached_time = rospy.get_param( rospy.get_name() + '/not_reached_move', 30)
# I'm not entirely sure which service to use. I do know that
# non-NavfnROS make_plan sometimes fails for an unknown
# reason... and thus NavfnROS/make_plan is more robust.
# srv_name = '/move_base_node/make_plan'
srv_name = '/move_base_node/NavfnROS/make_plan'
get_plan = rospy.ServiceProxy( srv_name, GetPlan )
# Clear costmap...? Do this here or in smach...?
if preempt_func(): # immediately exit if overall action preempted
return 'preempted'
for i,w in enumerate( waypoints ):
if preempt_func(): # immediately exit if overall action preempted
return 'preempted'
rospy.logout( 'snaking_explore: Seeking waypoint %d of %d' % (i+1,len(waypoints)))
# rospy.logout( 'snaking_explore: %2.2f %2.2f in frame %s' % (w.pose.position.x, w.pose.position.y, w.header.frame_id))
rim = self.robot_in_map()
w.header.stamp = rospy.Time(0) # transform it _now_
wim = self.ps_in_map( w ) # waypoint in map
if not rim or not wim: # if transforms failed
rospy.logout( 'snaking_explore: Bad transforms. Aborting explore' )
return 'aborted'
if self.range_to_waypoint( rim, wim ) < 0.9 * obs_range:
# We're nearby the waypoint, so we'll just trust that the planner
# has a good view of its surroundings to determine reachability.
req = GetPlanRequest()
req.tolerance = 0.1
req.start = rim
req.goal = wim
resp = get_plan( req )
found_plan = bool( resp.plan.poses != [] )
if not found_plan:
rospy.logout( 'snaking_explore: No plan to nearby waypoint. Proceeding to next waypoint' )
# Perhaps its worth pulling the waypoint closer until a path _is_ found?
continue
# We now know that a path exists. Send the waypoint to the client.
rospy.logout( 'snaking_explore: Near by with good plan. Calling move_base action.' )
else:
# Any nav plan at beyond obstacle range will require the
# nav stack to get a better vantage. Send the waypoint to
# the client.
rospy.logout( 'snaking_explore: Far away. Calling move_base action.' )
# If we made it this far, it's time to call move_base action.
# Cancel any previous goals
self.client.cancel_goals_at_and_before_time( rospy.Time.now() )
if os.environ.has_key('ROBOT') and os.environ['ROBOT'] == 'sim':
rospy.logout( 'running in sim: give the system a little time to respond' )
rospy.sleep( 1 )
# Send our new goal
rospy.logout( 'snaking_explore: sending movebase action goal.' )
move_goal = MoveBaseGoal()
move_goal.target_pose = wim
self.client.send_goal( move_goal )
if os.environ.has_key('ROBOT') and os.environ['ROBOT'] == 'sim':
rospy.logout( 'running in sim: give the system a little time to respond' )
rospy.sleep( 1 )
# We'll monitor the state ourselves, and allow ourselves
# the opportunity to cancel the goal for various reasons
# (eg. if we haven't moved for a while or if new plans
# aren't found after getting within obstacle distance)
# When this loop is broken, we'll go to the next goal. We
# don't need to cancel the goals here -- they will be
# cancelled before a new one is sent.
r = rospy.Rate( 10 )
xytt_old = None # Checks for movement
stime = rospy.Time.now().to_sec()
while True:
if self.client.simple_state == SimpleGoalState.DONE:
res = self.client.get_result()
rospy.logout('snaking_explore: Movebase actionlib completed with result. Proceeding to next waypoint')
break
rim = self.robot_in_map()
w.header.stamp = rospy.Time( 0 )
wim = self.ps_in_map( w )
# Proceed when close enough to goal (within two tolerance factors)
if rim and wim: # only do this check if transform succeeds
if self.range_to_waypoint( rim, wim ) < move_tol * 2.0:
rospy.logout( 'snaking_explore: Close enough to waypoint. Proceeding to next waypoint' )
break # We're near the waypoint, so let's must move on to next
# Proceed when there has been no movement (x,y, or theta) for X sec.
if rim: # only do this check if transform succeeds
xytt = self.xythetatime( rim )
if not xytt_old:
xytt_old = xytt
if np.abs( xytt[0] - xytt_old[0] ) > 0.05 or \
np.abs( xytt[1] - xytt_old[1] ) > 0.05 or \
np.abs( xytt[2] - xytt_old[2] ) > math.radians(10):
xytt_old = xytt
if xytt[3] - xytt_old[3] > no_progress:
rospy.logout( 'snaking_explore: No movement in %2.1f seconds. Proceeding to next waypoint' % no_progress )
break
if rospy.Time.now().to_sec() - stime > unreached_time:
rospy.logout( 'snaking_explore: Goal unreached in %2.1f seconds. Proceeding to next waypoint' % unreached_time )
break
r.sleep()
self.client.cancel_all_goals()
rospy.logout( 'snaking_explore: Finished with exploration.' )
return 'succeeded'
def execute(self, target, blocking=True):
component_name = "base"
ah = ActionHandle("move_base", component_name, target, blocking)
rospy.loginfo("Move <<%s>> to <<%s>>", component_name, target)
#get pose from parameter server
if type(target) is str:
if not rospy.has_param(self.ns_global_prefix + "/" + component_name + "/" + target):
rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...", self.ns_global_prefix + "/" + component_name + "/" + target)
ah.set_failed(2)
return ah
param = rospy.get_param(self.ns_global_prefix + "/" + component_name + "/" + target)
else:
param = target
#check pose
if not type(param) is list: # check outer list
rospy.logerr("no valid parameter for %s: not a list, aborting...", component_name)
#print "parameter is:", param
ah.set_failed(3)
return ah
else:
#print i,"type1 = ", type(i)
DOF = 3
if not len(param) == DOF: # check dimension
rospy.logerr("no valid parameter for %s: dimension should be %d and is %d, aborting...", component_name, DOF, len(param))
#print "parameter is:", param
ah.set_failed(3)
return ah
else:
for i in param:
#print i,"type2 = ", type(i)
if not ((type(i) is float) or (type(i) is int)):
#check type
#print type(i)
rospy.logerr("no valid parameter for %s: not a list of float or int, aborting...", component_name)
print "parameter is:", param
ah.set_failed(3)
return ah
else:
rospy.logdebug("accepted parameter %f for %s", i, component_name)
#convert to pose message
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = "/map"
pose.pose.position.x = param[0]
pose.pose.position.y = param[1]
pose.pose.position.z = 0.0
q = tf.transformations.quaternion_from_euler(0, 0, param[2])
pose.pose.orientation.x = q[0]
pose.pose.orientation.y = q[1]
pose.pose.orientation.z = q[2]
pose.pose.orientation.w = q[3]
action_server_name = "/move_base"
rospy.logdebug("calling %s action server", action_server_name)
client = actionlib.SimpleActionClient(action_server_name, MoveBaseAction)
#trying to connect to server
rospy.logdebug("waiting for %s action server to start", action_server_name)
if not client.wait_for_server(rospy.Duration(5)):
#error: server did not respond
rospy.logerr("%s action server not ready within timeout, aborting...", action_server_name)
ah.set_failed(4)
return ah
else:
rospy.logdebug("%s action server ready", action_server_name)
# sending goal
client_goal = MoveBaseGoal()
client_goal.target_pose = pose
print client_goal
client.send_goal(client_goal)
ah.set_client(client)
ah.wait_inside()
return ah
#!/usr/bin/env python
import rospy
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from actionlib import SimpleActionClient
from geometry_msgs.msg import PoseStamped
if __name__ == '__main__':
rospy.init_node('robocup_test_node')
goal = MoveBaseGoal()
pose = PoseStamped()
pose.header.frame_id = 'map'
pose.header.stamp = rospy.Time.now()
pose.pose.position.x = 1.0
pose.pose.position.y = 1.0
pose.pose.orientation.x = 0.0
pose.pose.orientation.y = 0.0
pose.pose.orientation.z = 0.0
pose.pose.orientation.z = 1.0
goal.target_pose = pose
move_base_client = SimpleActionClient('move_base', MoveBaseAction)
move_base_client.send_goal(goal)
move_base_client.wait_for_result(rospy.Duration(20))
if len(goal_list) == 0:
all_bad_cnt += 1
if all_bad_cnt > 2:
sounds.publish(Sound(0))
rospy.sleep(2.)
sounds.publish(Sound(1))
rospy.sleep(2.)
sys.exit()
break
goal = goal_list.pop(0)[1]
test_plan = do_astar(goal, pose, 0.0).plan.poses
print test_plan
goal_lock = False
dist = pose_dist(pose, goal)
print dist
#path = do_astar(pose, goal, 0.0).plan
go_pub.publish(goal)
pub_goal = MoveBaseGoal()
pub_goal.target_pose = goal
go_action.send_goal(pub_goal)
result = go_action.wait_for_result()
goal_blacklist.append(goal)
goal_list.pop(0)
if go_action.get_state() == 4:
do_circle = False
else:
do_circle = True
except Exception as exc:
print exc
def person_position_goal_cb(userdata, nav_goal):
move_base_goal = MoveBaseGoal()
move_base_goal.target_pose = userdata.person_pose
move_base_goal.target_pose.header.stamp = rospy.Time.now()
return move_base_goal
def main():
rospy.init_node('smach_example_state_machine')
# Create the top level SMACH state machine
sm_top = smach.StateMachine(outcomes=['done', 'exit'])
# Open the container
with sm_top:
goal1=MoveBaseGoal()
goal1.target_pose.header.frame_id = "dtw_robot1/map"
goal1.target_pose.pose.position.x = 2.7
goal1.target_pose.pose.position.y = 2.8
goal1.target_pose.pose.orientation.w = 1.0
smach.StateMachine.add('MOVE1', smach_ros.SimpleActionState('/dtw_robot1/move_base', MoveBaseAction, goal=goal1), transitions={'succeeded':'MOVE2', 'preempted':'MOVE1', 'aborted':'MOVE1'})
goal2=MoveBaseGoal()
goal2.target_pose.header.frame_id = "dtw_robot1/map"
goal2.target_pose.pose.position.x = 4.3
goal2.target_pose.pose.position.y = 3.8
goal2.target_pose.pose.orientation.w = 1.0
smach.StateMachine.add('MOVE2', smach_ros.SimpleActionState('/dtw_robot1/move_base', MoveBaseAction, goal=goal2), transitions={'succeeded':'TASK2', 'preempted':'exit', 'aborted':'TASK2'})
sm_sub = smach.StateMachine(outcomes=['done'])
with sm_sub:
#vel_pub = rospy.Publisher('/dtw_robot1/diff_drive_controller/cmd_vel', Twist, queue_size=10)
#vel = Twist()
#callback_lambda = lambda x: callback(x, vel, vel_pub)
#goal=MoveBaseGoal()
#goal.target_pose.header.frame_id = "dtw_robot1/map"
#goal.target_pose.pose.position.x = 4.5
#goal.target_pose.pose.orientation.w = 1.0
#smach.Concurrence.add('MOVE', smach_ros.SimpleActionState('/dtw_robot1/move_base', MoveBaseAction, goal=goal))
#vel_pub = rospy.Publisher('/dtw_robot1/diff_drive_controller/cmd_vel', Twist, queue_size=10)
#vel = Twist()
#callback_lambda = lambda x: callback(x, vel, vel_pub)
#while not rospy.is_shutdown():
# rospy.init_node('listener', anonymous=True)
# rospy.Subscriber('/darknet_ros/bounding_boxes', BoundingBoxes , callback_lambda)
# rospy.spin()
#smach.Concurrence.add('DETECTION', smach_ros.DetectionState("/dtw_robot1/diff_drive_controller/cmd_vel", Twist, callback))
smach.StateMachine.add('STATE1', State1(), transitions={'done':'STATE2', 'exit':'done'})
smach.StateMachine.add('STATE2', State2(), transitions={'done':'STATE1'})
smach.StateMachine.add('TASK2', sm_sub, transitions={'done':'MOVE3'})
goal3=MoveBaseGoal()
goal3.target_pose.header.frame_id = "dtw_robot1/map"
goal3.target_pose.pose.position.x = 4.5
goal3.target_pose.pose.position.y = 1.7
goal3.target_pose.pose.orientation.w = 1.0
smach.StateMachine.add('MOVE3', smach_ros.SimpleActionState('/dtw_robot1/move_base', MoveBaseAction, goal=goal3), transitions={'succeeded':'exit', 'preempted':'MOVE1', 'aborted':'MOVE2'})
# Execute SMACH plan
sis = smach_ros.IntrospectionServer('smach_server', sm_top, '/SM_ROOT')
sis.start()
outcome = sm_top.execute()
rospy.spin()
sis.stop()
