def move(distance, angular, speed, delta_y, rotation):
if not SETUP_DONE: setup()
rospy.sleep(1)
# create a Twist message, fill it in to drive forward
twist = Twist()
if speed != 0:
move_base = publisher.move_base_action_client()
# go the full distance
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = 'base_link'
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose.position.x = distance
goal.target_pose.pose.orientation.w = 1
move_base.send_goal(goal)
success = move_base.wait_for_result()
# for i in range(int(distance*2)):
# 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.5 #3 meters
# goal.target_pose.pose.orientation.w = 1.0 #go forward
# move_base.send_goal(goal)
# success = move_base.wait_for_result(rospy.Duration(10))
# if not success:
# move_base.cancel_goal()
#rospy.loginfo("The base failed to move forward 3 meters for some reason")
# break
# else:
# We made it!
# state = move_base.get_state()
# if state == GoalStatus.SUCCEEDED:
# continue
publisher.close_move_base_action_client()
# create a twist message, fill it in to turn
else:
move_base = publisher.move_base_action_client()
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
yaw = rotation * math.pi/180;
q = tf.transformations.quaternion_from_euler(0,0,yaw)
print "x = %.2f" % q[0], ", y= %.2f" % q[1],", z= %.2f"% q[2], ", w= %.2f" %q[3]
goal.target_pose.pose.orientation.x = q[0]
goal.target_pose.pose.orientation.y = q[1]
goal.target_pose.pose.orientation.z = q[2]
goal.target_pose.pose.orientation.w = q[3]
move_base.send_goal(goal)
success = move_base.wait_for_result()
publisher.close_move_base_action_client()
def move(x, y, v, action):
print "MOVING TO x:" + str(x) + " y:" + str(y)
status = True
msg = "Successfully executed the vertex"
#setVelocity(v, 'LINEAR');
if action == "Absolute":
frameType = "map"
else:
frameType = 'base_link'
move_base = publisher.move_base_action_client()
print "moving ->" + str(frameType)
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = frameType
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose.position.x = x
if frameType == 'map':
goal.target_pose.pose.position.y = y #3 meters
goal.target_pose.pose.orientation.w = 1.0 #go forward
move_base.send_goal(goal)
success = move_base.wait_for_result()
if not success:
move_base.cancel_goal()
rospy.logerr("The base failed to move forward")
msg = "The base failed to move forward"
status = False
else:
rospy.loginfo("Successfully executed the vertex")
publisher.close_move_base_action_client()
return status, msg
def moveTo(x, y):
move_base_client = publisher.move_base_action_client (); #actionlib.SimpleActionClient("move_base", MoveBaseAction)
rospy.loginfo ("Go to (%s, %s) pose", x, y)
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = 'map'
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose = Pose(Point(x,y, 0.000),
Quaternion(quaternion['r1'], quaternion['r2'], quaternion['r3'], quaternion['r4']))
move_base_client.send_goal(goal)
result = move_base_client.wait_for_result()
state = move_base_client.get_state()
success = False
msg = ""
if result and state == GoalStatus.SUCCEEDED:
success = True
if success:
rospy.loginfo("Reached the destination")
msg = "Reached the destination"
else:
rospy.loginfo("Unable to reach destination")
msg = "Unable to reach destination"
# Sleep to give the last log messages time to be sent
rospy.sleep(1)
publisher.close_move_base_action_client()
return success,msg;
def move(x,y,v,action):
print "MOVING TO x:" + str(x) + " y:" + str(y)
status=True
msg = "Successfully executed the vertex"
#setVelocity(v, 'LINEAR');
if action == "Absolute":
frameType = "map"
else:
frameType = 'base_link'
move_base = publisher.move_base_action_client ()
print "moving ->" + str(frameType)
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = frameType
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose.position.x = x
if frameType == 'map':
goal.target_pose.pose.position.y = y #3 meters
goal.target_pose.pose.orientation.w = 1.0 #go forward
move_base.send_goal(goal)
success = move_base.wait_for_result()
if not success:
move_base.cancel_goal()
rospy.logerr("The base failed to move forward")
msg = "The base failed to move forward"
status = False
else:
rospy.loginfo("Successfully executed the vertex")
publisher.close_move_base_action_client()
return status, msg
def moveAllAtOnce(distance, angular, speed, delta_y, rotation):
if not SETUP_DONE: setup()
rospy.sleep(1)
# create a Twist message, fill it in to drive forward
twist = Twist()
if speed != 0:
move_base = publisher.move_base_action_client()
# go the full distance
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = 'base_link'
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose.position.x = distance
goal.target_pose.pose.orientation.w = 1
move_base.send_goal(goal)
success = move_base.wait_for_result()
# for i in range(int(distance*2)):
# 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.5 #3 meters
# goal.target_pose.pose.orientation.w = 1.0 #go forward
# move_base.send_goal(goal)
# success = move_base.wait_for_result(rospy.Duration(10))
# if not success:
# move_base.cancel_goal()
#rospy.loginfo("The base failed to move forward 3 meters for some reason")
# break
# else:
# We made it!
# state = move_base.get_state()
# if state == GoalStatus.SUCCEEDED:
# continue
publisher.close_move_base_action_client()
return success, ""
# create a twist message, fill it in to turn
else:
twist.angular.z = radians(45)*rotation #0.785398*2*rotation # 90 deg/s
for i in range(0,int(10*angular)):
cmd_vel.publish(twist)
rospy.sleep(0.2)
return True, ""
def move(x, y, v, action, w=1.5):
print "MOVING TO x:" + str(x) + " y:" + str(y)
status = True
msg = "Successfully executed the vertex"
setVelocity(v, 'LINEAR')
if action == "Absolute":
frameType = "map"
else:
frameType = 'base_link'
move_base = publisher.move_base_action_client()
print "moving ->" + str(frameType)
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = frameType
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose.position.x = x
if frameType == 'map':
goal.target_pose.pose.position.y = y #3 meters
q = t.quaternion_from_euler(0, 0, w)
goal.target_pose.pose.orientation.x = q[0]
goal.target_pose.pose.orientation.y = q[1]
goal.target_pose.pose.orientation.z = q[2]
goal.target_pose.pose.orientation.w = q[3]
move_base.send_goal(goal)
success = move_base.wait_for_result()
if not success:
move_base.cancel_goal()
rospy.logerr("The base failed to move forward")
msg = "The base failed to move forward"
status = False
else:
succeeded = move_base.get_state() == GoalStatus.SUCCEEDED
if not succeeded:
rospy.logerr("The base failed to move, status=%s" % succeeded)
msg = "Move %s failed" % action
status = False
else:
rospy.loginfo("Successfully executed the vertex")
publisher.close_move_base_action_client()
return status, msg
def move(distance, angular, speed, delta_y, rotation):
return moveAllAtOnce(distance, angular, speed, delta_y, rotation)
if not SETUP_DONE: setup()
cmd_vel = rospy.Publisher("cmd_vel_mux/input/navi", Twist, queue_size=10)
rospy.sleep(1)
# create a Twist message, fill it in to drive forward
twist = Twist()
if speed != 0:
move_base = publisher.move_base_action_client()
#actionlib.SimpleActionClient("move_base", MoveBaseAction)
#move_base.wait_for_server(rospy.Duration(10))
for i in range(int(distance * 2)):
rospy.loginfo("Doing iteration " + str(i) + " of " +
str(distance * 2))
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.5 #3 meters
goal.target_pose.pose.orientation.w = 1.0 #go forward
move_base.send_goal(goal)
success = move_base.wait_for_result(rospy.Duration(30))
if not success:
move_base.cancel_goal()
rospy.loginfo(
"The base failed to move forward .5 meters for some reason"
)
break
else:
# We made it!
state = move_base.get_state()
if state == GoalStatus.SUCCEEDED:
continue
publisher.close_move_base_action_client()
# create a twist message, fill it in to turn
else:
twist.angular.z = radians(
45) * rotation #0.785398*2*rotation # 90 deg/s
for i in range(0, int(10 * angular)):
cmd_vel.publish(twist)
rospy.sleep(0.2)
def move(distance, angular, speed, delta_y, rotation):
return moveAllAtOnce(distance, angular, speed, delta_y, rotation)
if not SETUP_DONE: setup()
cmd_vel = rospy.Publisher("cmd_vel_mux/input/navi", Twist, queue_size=10)
rospy.sleep(1)
# create a Twist message, fill it in to drive forward
twist = Twist()
if speed != 0:
move_base = publisher.move_base_action_client (); #actionlib.SimpleActionClient("move_base", MoveBaseAction)
#move_base.wait_for_server(rospy.Duration(10))
for i in range(int(distance*2)):
rospy.loginfo("Doing iteration " + str(i) + " of " + str (distance*2))
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.5 #3 meters
goal.target_pose.pose.orientation.w = 1.0 #go forward
move_base.send_goal(goal)
success = move_base.wait_for_result(rospy.Duration(30))
if not success:
move_base.cancel_goal()
rospy.loginfo("The base failed to move forward .5 meters for some reason")
break
else:
# We made it!
state = move_base.get_state()
if state == GoalStatus.SUCCEEDED:
continue
publisher.close_move_base_action_client()
# create a twist message, fill it in to turn
else:
twist.angular.z = radians(45)*rotation #0.785398*2*rotation # 90 deg/s
for i in range(0,int(10*angular)):
cmd_vel.publish(twist)
rospy.sleep(0.2)
def cancel():
publisher.move_base_action_client().cancel_goal()
def preempt_cb(self):
rospy.loginfo('Server preempted')
self._canceled.cancel()
publisher.move_base_action_client().cancel_all_goals()
def cancel_instruction(): move_base = publisher.move_base_action_client() move_base.cancel_all_goals()