def main():
global distance_to_rover
global distance_to_qc
no_of_objects_found = 0
angle_rotated = 0
final_angle = 0
# Rotate to Detect Nearby Objects
for i in range(27):
rotate_bot()
print("Rotated 10 Degrees")
rospy.sleep(1)
angle_rotated += 10
#Find Objects if Available
result = find_obj()
print(result)
if result[3] < 400:
result[0] = -1
if result[0] != -1:
if (result[0] == 1):
print("Bowl Detected")
elif result[0] == 2:
print("Wheel Detected")
elif result[0] == 3:
print("Mars Rover Detected")
else:
print("Quadcopter Detected")
result_distance = find_distance(result[1] + result[4] / 2,
result[2] + result[3] / 2)
# print(result_distance)
if result[0] == 3 and distance_to_rover > result_distance:
distance_to_rover = result_distance
angle_rotated = 0
if result[0] == 4 and distance_to_qc > result_distance:
distance_to_qc = result_distance
final_angle = angle_rotated
distance_to_qc = distance_to_qc * 3
distance_to_rover = distance_to_rover * 3
# Pose Estimation
print(final_angle, distance_to_qc, distance_to_rover)
x, y = fsolve(equations, (3, 0))
y = y * -1
print(x, y)
theta = math.atan2(qc[0] - x, qc[1] - y)
theta = theta * -1 + math.pi / 2
print("Estimated")
print(y, x, theta)
print("Actual")
print(pose)
# Go to Destination
print("Given current pose, beginnning to path plan and go to target")
# Create an action client called "move_base" with action definition file "MoveBaseAction"
client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
# Waits until the action server has started up and started listening for goals.
client.wait_for_server()
# Creates a new goal with the MoveBaseGoal constructor
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = "odom"
goal.target_pose.pose.position.x = 0
goal.target_pose.pose.position.y = 0
goal.target_pose.pose.position.z = 0
# No rotation of the mobile base frame w.r.t. map frame
goal.target_pose.pose.orientation.x = 0
goal.target_pose.pose.orientation.y = 0
goal.target_pose.pose.orientation.z = -0.683466457574
goal.target_pose.pose.orientation.w = 0.729981918523
# Sends the goal to the action server.
client.send_goal(goal)
client.wait_for_result()
print("Reached")
#!/usr/bin/env python
import rospy
from smach import StateMachine # <1>
from smach_ros import SimpleActionState # <2>
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
waypoints = [['one', (2.1, 2.2), (0.0, 0.0, 0.0, 1.0)],
['two', (6.5, 4.43), (0.0, 0.0, -0.984047240305, 0.177907360295)]]
if __name__ == '__main__':
patrol = StateMachine('success')
with patrol:
for i, w in enumerate(waypoints):
goal_pose = MoveBaseGoal()
goal_pose.target_pose.header.frame_id = 'map'
goal_pose.target_pose.pose.position.x = w[1][0]
goal_pose.target_pose.pose.position.y = w[1][1]
goal_pose.target_pose.pose.position.z = 0.0
goal_pose.target_pose.pose.orientation.x = w[2][0]
goal_pose.target_pose.pose.orientation.y = w[2][1]
goal_pose.target_pose.pose.orientation.z = w[2][2]
goal_pose.target_pose.pose.orientation.w = w[2][3]
StateMachine.add(w[0],
SimpleActionState('move_base',
MoveBaseAction,
goal=goal_pose),
transitions={
'success':
waypoints[(i + 1) % len(waypoints)][0]
# is received from the action server
# it just prints a message indicating a new message has been received
def feedback_callback(feedback):
print('[Feedback] Going to Goal Pose...')
# initializes the action client node
rospy.init_node('move_base_action_client')
# create the connection to the action server
client = actionlib.SimpleActionClient('/move_base', MoveBaseAction)
# waits until the action server is up and running
client.wait_for_server()
goal1 = MoveBaseGoal()
goal1.target_pose.header.frame_id = 'map'
goal1.target_pose.pose.position.x = 0.536989629269
goal1.target_pose.pose.position.y = -3.84424805641
goal1.target_pose.pose.position.z = 0.0
goal1.target_pose.pose.orientation.x = 0.0
goal1.target_pose.pose.orientation.y = 0.0
goal1.target_pose.pose.orientation.z = -0.598102121021
goal1.target_pose.pose.orientation.w = 0.80141989795
goal2 = MoveBaseGoal()
goal2.target_pose.header.frame_id = 'map'
goal2.target_pose.pose.position.x = 1.98187232018
goal2.target_pose.pose.position.y = -3.01903486252
goal2.target_pose.pose.position.z = 0.0
goal2.target_pose.pose.orientation.x = 0.0
def __init__(self):
rospy.init_node('nav_test', anonymous=True)
rospy.on_shutdown(self.shutdown)
# How long in seconds should the robot pause at each location?
self.rest_time = rospy.get_param("~rest_time", 10)
# Are we running in the fake simulator?
self.fake_test = rospy.get_param("~fake_test", False)
# Goal state return values
goal_states = [
'PENDING', 'ACTIVE', 'PREEMPTED', 'SUCCEEDED', 'ABORTED',
'REJECTED', 'PREEMPTING', 'RECALLING', 'RECALLED', 'LOST'
]
# Set up the goal locations. Poses are defined in the map frame.
# An easy way to find the pose coordinates is to point-and-click
# Nav Goals in RViz when running in the simulator.
# Pose coordinates are then displayed in the terminal
# that was used to launch RViz.
locations = dict()
locations['hall_foyer'] = Pose(Point(0.643, 4.720, 0.000),
Quaternion(0.000, 0.000, 0.223, 0.975))
locations['hall_kitchen'] = Pose(
Point(-1.994, 4.382, 0.000), Quaternion(0.000, 0.000, -0.670,
0.743))
locations['hall_bedroom'] = Pose(
Point(-3.719, 4.401, 0.000), Quaternion(0.000, 0.000, 0.733,
0.680))
locations['living_room_1'] = Pose(
Point(0.720, 2.229, 0.000), Quaternion(0.000, 0.000, 0.786, 0.618))
locations['living_room_2'] = Pose(
Point(1.471, 1.007, 0.000), Quaternion(0.000, 0.000, 0.480, 0.877))
locations['dining_room_1'] = Pose(
Point(-0.861, -0.019, 0.000),
Quaternion(0.000, 0.000, 0.892, -0.451))
# Publisher to manually control the robot (e.g. to stop it)
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist)
# Subscribe to the move_base action server
self.move_base = actionlib.SimpleActionClient("move_base",
MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
# Wait 60 seconds for the action server to become available
self.move_base.wait_for_server(rospy.Duration(60))
rospy.loginfo("Connected to move base server")
# A variable to hold the initial pose of the robot to be set by
# the user in RViz
initial_pose = PoseWithCovarianceStamped()
# (LK: Private/ locally used/accessed only- no self.)
# Variables to keep track of success rate, running time,
# and distance traveled
n_locations = len(locations)
n_goals = 0
n_successes = 0
i = n_locations
distance_traveled = 0
start_time = rospy.Time.now()
running_time = 0
location = ""
last_location = ""
# Get the initial pose from the user
rospy.loginfo(
"*** Click the 2D Pose Estimate button in RViz to set the robot's initial pose..."
)
rospy.wait_for_message('initialpose', PoseWithCovarianceStamped)
self.last_location = Pose()
rospy.Subscriber('initialpose', PoseWithCovarianceStamped,
self.update_initial_pose)
# Make sure we have the initial pose
while initial_pose.header.stamp == "":
rospy.sleep(1)
rospy.loginfo("Starting navigation test")
# Begin the main loop and run through a sequence of locations
while not rospy.is_shutdown():
# If we've gone through the current sequence,
# start with a new random sequence
# only first loop of going round points
if i == n_locations:
i = 0
# (LK: create a list/name of locations randomly- [shuffle location])
sequence = sample(locations, n_locations)
# Skip over first location if it is the same as
# the last location
if sequence[0] == last_location:
i = 1
# Get the next location in the current sequence
location = sequence[i]
# Keep track of the distance traveled.
# Use updated initial pose if available.
if initial_pose.header.stamp == "":
distance = sqrt(
pow(
locations[location].position.x -
locations[last_location].position.x, 2) + pow(
locations[location].position.y -
locations[last_location].position.y, 2))
else:
# <LK> running only once on the startup setting initial pose
rospy.loginfo("Updating current pose.")
distance = sqrt(
pow(
locations[location].position.x -
initial_pose.pose.pose.position.x, 2) + pow(
locations[location].position.y -
initial_pose.pose.pose.position.y, 2))
initial_pose.header.stamp = ""
# Store the last location for distance calculations
last_location = location
# Increment the counters
# (LK: i = the next location)
i += 1
# (LK: counting how many have been visited)
n_goals += 1
# Set up the next goal location
# create goal data structure and filling information required!
self.goal = MoveBaseGoal()
self.goal.target_pose.pose = locations[location]
self.goal.target_pose.header.frame_id = 'map'
self.goal.target_pose.header.stamp = rospy.Time.now()
# Let the user know where the robot is going next
rospy.loginfo("Going to: " + str(location))
# Start the robot toward the next location- (LK: in the meantime get action update)
self.move_base.send_goal(self.goal)
# Allow 5 minutes to get there (300 secs)
finished_within_time = self.move_base.wait_for_result(
rospy.Duration(300))
# Check for success or failure
if not finished_within_time:
# (LK: feedback of cmd failure)
self.move_base.cancel_goal()
rospy.loginfo("Timed out achieving goal")
else:
state = self.move_base.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Goal succeeded!")
n_successes += 1
distance_traveled += distance
rospy.loginfo("State:" + str(state))
else:
rospy.loginfo("Goal failed with error code: " +
str(goal_states[state]))
# How long have we been running?
running_time = rospy.Time.now() - start_time
running_time = running_time.secs / 60.0
# Print a summary success/failure, distance traveled and time elapsed
rospy.loginfo("Success so far: " + str(n_successes) + "/" +
str(n_goals) + " = " +
str(100 * n_successes / n_goals) + "%")
rospy.loginfo("Running time: " + str(trunc(running_time, 1)) +
" min Distance: " +
str(trunc(distance_traveled, 1)) + " m")
rospy.sleep(self.rest_time)
def __init__(self):
rospy.init_node('position_nav_node', anonymous=True)
rospy.on_shutdown(self.shutdown)
# How long in seconds should the robot pause at each location?
self.rest_time = rospy.get_param("~rest_time", 10)
# Are we running in the fake simulator?
self.fake_test = rospy.get_param("~fake_test", False)
# Goal state return values
goal_states = ['PENDING', 'ACTIVE', 'PREEMPTED',
'SUCCEEDED', 'ABORTED', 'REJECTED',
'PREEMPTING', 'RECALLING', 'RECALLED',
'LOST']
# Set up the goal locations. Poses are defined in the map frame.
# An easy way to find the pose coordinates is to point-and-click
# Nav Goals in RViz when running in the simulator.
#
# Pose coordinates are then displayed in the terminal
# that was used to launch RViz.
locations = dict()
locations['one'] = Pose(Point(1.249, 3.261, 0.000), Quaternion(0.000, 0.000, 0.012, 0.999))
locations['two'] = Pose(Point(4.681, 3.066, 0.000), Quaternion(0.000, 0.000, -0.752, 0.659))
locations['three'] = Pose(Point(4.318, -1.294, 0.000), Quaternion(0.000, 0.000, 0.998,-0.053))
locations['four'] = Pose(Point(0.012, -1.138, 0.000), Quaternion(0.000, 0.000, 0.999, 0.039))
# Publisher to manually control the robot (e.g. to stop it)
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=10)
self.IOTnet_pub = rospy.Publisher('/IOT_cmd', IOTnet, queue_size=10)
# Subscribe to the move_base action server
self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
# Wait 60 seconds for the action server to become available
self.move_base.wait_for_server(rospy.Duration(60))
rospy.loginfo("Connected to move base server")
# A variable to hold the initial pose of the robot to be set by
# the user in RViz
initial_pose = PoseWithCovarianceStamped()
# Variables to keep track of success rate, running time,
# and distance traveled
n_locations = len(locations)
n_goals = 0
n_successes = 0
i = 0
distance_traveled = 0
start_time = rospy.Time.now()
running_time = 0
location = ""
last_location = ""
sequeue=['one', 'two', 'three', 'four']
# Get the initial pose from the user
rospy.loginfo("*** Click the 2D Pose Estimate button in RViz to set the robot's initial pose...")
rospy.wait_for_message('initialpose', PoseWithCovarianceStamped)
self.last_location = Pose()
rospy.Subscriber('initialpose', PoseWithCovarianceStamped, self.update_initial_pose)
# Make sure we have the initial pose
while initial_pose.header.stamp == "":
rospy.sleep(1)
rospy.loginfo("Starting position navigation ")
# Begin the main loop and run through a sequence of locations
while not rospy.is_shutdown():
# If we've gone through the all sequence, then exit
if i == n_locations:
rospy.logwarn("Now reach all destination, now exit...")
rospy.signal_shutdown('Quit')
break
# Get the next location in the current sequence
location = sequeue[i]
# Keep track of the distance traveled.
# Use updated initial pose if available.
if initial_pose.header.stamp == "":
distance = sqrt(pow(locations[location].position.x -
locations[last_location].position.x, 2) +
pow(locations[location].position.y -
locations[last_location].position.y, 2))
else:
rospy.loginfo("Updating current pose.")
distance = sqrt(pow(locations[location].position.x -
initial_pose.pose.pose.position.x, 2) +
pow(locations[location].position.y -
initial_pose.pose.pose.position.y, 2))
initial_pose.header.stamp = ""
# Store the last location for distance calculations
last_location = location
# Increment the counters
i += 1
n_goals += 1
# Set up the next goal location
self.goal = MoveBaseGoal()
self.goal.target_pose.pose = locations[location]
self.goal.target_pose.header.frame_id = 'map'
self.goal.target_pose.header.stamp = rospy.Time.now()
# Let the user know where the robot is going next
rospy.loginfo("Going to: " + str(location))
# Start the robot toward the next location
self.move_base.send_goal(self.goal) #move_base.send_goal()
# Allow 5 minutes to get there
finished_within_time = self.move_base.wait_for_result(rospy.Duration(300))
# Check for success or failure
if not finished_within_time:
self.move_base.cancel_goal() #move_base.cancle_goal()
rospy.loginfo("Timed out achieving goal")
else:
state = self.move_base.get_state() #move_base.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Goal succeeded!")
n_successes += 1
distance_traveled += distance
rospy.loginfo("State:" + str(state))
self.IOTnet_pub.publish(i)
rospy.sleep(1)
else:
rospy.loginfo("Goal failed with error code: " + str(goal_states[state]))
# How long have we been running?
running_time = rospy.Time.now() - start_time
running_time = running_time.secs / 60.0
# Print a summary success/failure, distance traveled and time elapsed
rospy.loginfo("Success so far: " + str(n_successes) + "/" +
str(n_goals) + " = " +
str(100 * n_successes/n_goals) + "%")
rospy.loginfo("Running time: " + str(trunc(running_time, 1)) +
" min Distance: " + str(trunc(distance_traveled, 1)) + " m")
rospy.sleep(self.rest_time)
def __init__(self):
rospy.init_node('nav_test', anonymous=False)
rospy.on_shutdown(self.shutdown)
# How big is the square we want the robot to navigate?
square_size = rospy.get_param("~square_size", 1.0) # meters
# Create a list to hold the target quaternions (orientations)
quaternions = list()
# First define the corner orientations as Euler angles
euler_angles = (pi/2, pi, 3*pi/2, 0)
# Then convert the angles to quaternions
for angle in euler_angles:
q_angle = quaternion_from_euler(0, 0, angle, axes='sxyz')
q = Quaternion(*q_angle)
quaternions.append(q)
# Create a list to hold the waypoint poses
waypoints = list()
# Append each of the four waypoints to the list. Each waypoint
# is a pose consisting of a position and orientation in the map frame.
waypoints.append(Pose(Point(square_size, 0.0, 0.0), quaternions[0]))
waypoints.append(Pose(Point(square_size, square_size, 0.0), quaternions[1]))
waypoints.append(Pose(Point(0.0, square_size, 0.0), quaternions[2]))
waypoints.append(Pose(Point(0.0, 0.0, 0.0), quaternions[3]))
# Initialize the visualization markers for RViz
self.init_markers()
# Set a visualization marker at each waypoint
for waypoint in waypoints:
p = Point()
p = waypoint.position
self.markers.points.append(p)
# Publisher to manually control the robot (e.g. to stop it)
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist)
# Subscribe to the move_base action server
self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
# Wait 60 seconds for the action server to become available
self.move_base.wait_for_server(rospy.Duration(60))
rospy.loginfo("Connected to move base server")
rospy.loginfo("Starting navigation test")
# Initialize a counter to track waypoints
i = 0
# Cycle through the four waypoints
while i < 4 and not rospy.is_shutdown():
# Update the marker display
self.marker_pub.publish(self.markers)
# Intialize the waypoint goal
goal = MoveBaseGoal()
# Use the map frame to define goal poses
goal.target_pose.header.frame_id = 'map'
# Set the time stamp to "now"
goal.target_pose.header.stamp = rospy.Time.now()
# Set the goal pose to the i-th waypoint
goal.target_pose.pose = waypoints[i]
# Start the robot moving toward the goal
self.move(goal)
i += 1
def execute(self, userdata):
global CURRENT_POSE, START, END_GOAL, isToTheLeft, MIDDLE_GOAL
if START and not rospy.is_shutdown():
if self.mode == 0:
quaternion = quaternion_from_euler(0, 0, self.yaw)
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = self.frame
goal.target_pose.pose.position.x = self.distance
goal.target_pose.pose.position.y = self.horizontal
goal.target_pose.pose.orientation.x = quaternion[0]
goal.target_pose.pose.orientation.y = quaternion[1]
goal.target_pose.pose.orientation.z = quaternion[2]
goal.target_pose.pose.orientation.w = quaternion[3]
self.move_base_client.send_goal_and_wait(goal)
elif self.mode == 1:
rospy.set_param(
"/move_base/DWAPlannerROS/yaw_goal_tolerance", 2*math.pi)
rospy.set_param(
"/move_base/DWAPlannerROS/latch_xy_goal_tolerance", True)
if isToTheLeft:
angle = -math.pi/2
else:
angle = math.pi/2
quaternion = quaternion_from_euler(0, 0, angle)
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = "base_link"
goal.target_pose.pose.position.x = abs(
CURRENT_POSE.position.y - END_GOAL.target_pose.pose.position.y) - 0.15 # TODO: adjust this value
goal.target_pose.pose.position.y = 0
goal.target_pose.pose.orientation.x = quaternion[0]
goal.target_pose.pose.orientation.y = quaternion[1]
goal.target_pose.pose.orientation.z = quaternion[2]
goal.target_pose.pose.orientation.w = quaternion[3]
self.move_base_client.send_goal_and_wait(goal)
rospy.set_param(
"/move_base/DWAPlannerROS/yaw_goal_tolerance", 0.3)
rospy.set_param(
"/move_base/DWAPlannerROS/latch_xy_goal_tolerance", False)
elif self.mode == 2:
rospy.set_param(
"/move_base/DWAPlannerROS/yaw_goal_tolerance", 2*math.pi)
rospy.set_param(
"/move_base/DWAPlannerROS/latch_xy_goal_tolerance", True)
quaternion = quaternion_from_euler(0, 0, 0)
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = "base_link"
goal.target_pose.pose.position.x = abs(
CURRENT_POSE.position.x - END_GOAL.target_pose.pose.position.x) - 0.3 # TODO: adjust this value
goal.target_pose.pose.position.y = 0 # TODO: adjust this value
goal.target_pose.pose.orientation.x = quaternion[0]
goal.target_pose.pose.orientation.y = quaternion[1]
goal.target_pose.pose.orientation.z = quaternion[2]
goal.target_pose.pose.orientation.w = quaternion[3]
self.move_base_client.send_goal_and_wait(goal)
rospy.set_param(
"/move_base/DWAPlannerROS/yaw_goal_tolerance", 0.3)
rospy.set_param(
"/move_base/DWAPlannerROS/latch_xy_goal_tolerance", False)
elif self.mode == -1:
self.move_base_client.send_goal_and_wait(MIDDLE_GOAL)
return "done"
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal, MoveBaseResult, MoveBaseActionResult
if __name__ == '__main__':
# Initialize a ROS Node
rospy.init_node('move_turtlebot')
# Create a SimpleActionClient for the move_base action server.
turtlebot_navigation_client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
# Wait until the move_base action server becomes available.
rospy.loginfo("Waiting for move_base action server to come up...")
turtlebot_navigation_client.wait_for_server()
rospy.loginfo("move_base action server is available...")
# Creates a goal to send to the action server.
turtlebot_robot1_goal = MoveBaseGoal()
# Construct the target pose for the turtlebot in the "map" frame.
turtlebot_robot1_goal.target_pose.header.stamp = rospy.Time.now()
turtlebot_robot1_goal.target_pose.header.frame_id = "map"
turtlebot_robot1_goal.target_pose.header.seq = 1
turtlebot_robot1_goal.target_pose.pose.position.x = <Add-first-goal-Position-X>
turtlebot_robot1_goal.target_pose.pose.position.y = <Add-first-goal-Position-Y>
turtlebot_robot1_goal.target_pose.pose.position.z = 0.0
turtlebot_robot1_goal.target_pose.pose.orientation.x = 0.0
turtlebot_robot1_goal.target_pose.pose.orientation.y = 0.0
turtlebot_robot1_goal.target_pose.pose.orientation.z = 0.0
turtlebot_robot1_goal.target_pose.pose.orientation.w = 1.0
# Send the goal to the action server.
try:
def done_cb(self, status, result):
'''
Keeps track of which goals failed.
Generates the next goal index randomly from waypoints range, with failed goal indices removed.
'''
if status == 2:
rospy.loginfo(
"Goal pose " + str(self.goal_cnt) +
" received a cancel request after it started executing, completed execution!"
)
if status == 3:
rospy.loginfo("Goal pose " + str(self.goal_cnt + 1) + " reached")
self.goal_cnt += 1
if self.goal_cnt < len(self.waypoints):
rospy.sleep(2.0)
next_goal = MoveBaseGoal()
next_goal.target_pose.header.frame_id = "map"
next_goal.target_pose.header.stamp = rospy.Time.now()
next_goal.target_pose.pose = self.waypoints[self.goal_cnt]
rospy.loginfo("Sending goal pose " + str(self.goal_cnt + 1) +
" to Action Server")
rospy.loginfo(str(self.waypoints[self.goal_cnt]))
self.client.send_goal(next_goal, self.done_cb, self.active_cb,
self.feedback_cb)
else:
print("\nRe-looping")
print("Waypoints reversed\n")
self.waypoints.reverse()
self.goal_cnt = 1 # waypoints have been reversed
self.skip_waypoint()
return
if status == 4:
rospy.loginfo("Goal pose " + str(self.goal_cnt) +
" was aborted by the Action Server")
self.failed_goal_index.append(self.goal_cnt)
print("Goal " + str(self.goal_cnt) +
" appended to Failed Goals Index List")
self.waypoints.remove(self.waypoints[self.goal_cnt])
print("Bad waypoint removed from waypoints list!")
#self.goal_cnt += 1
# dont need to increment here because the index was removed
self.skip_waypoint()
return
if status == 5:
rospy.loginfo("Goal pose " + str(self.goal_cnt) +
" has been rejected by the Action Server")
self.failed_goal_index.append(self.goal_cnt)
self.goal_cnt += 1
#self.check_goal()
self.skip_waypoint()
return
if status == 8:
rospy.loginfo(
"Goal pose " + str(self.goal_cnt) +
" received a cancel request before it started executing, successfully cancelled!"
)
def __init__(self):
rospy.init_node('lab_test', anonymous=True)
rospy.on_shutdown(self.shutdown)
# How long in seconds should the robot pause at each location? 10s default
self.rest_time = rospy.get_param("~rest_time", 10)
# Are we running in the fake simulator?
self.fake_test = rospy.get_param("~fake_test", False)
# Goal state return values
goal_states = [
'PENDING', 'ACTIVE', 'PREEMPTED', 'SUCCEEDED', 'ABORTED',
'REJECTED', 'PREEMPTING', 'RECALLING', 'RECALLED', 'LOST'
]
locations = dict()
#locations['computador'] = Pose(Point(1.288, 0.094, 0.000), Quaternion(0.000, 0.000, 0.707, 0.707))
for point in posiciones:
locations[point] = Pose(
Point(df.ix[point, 'x'], df.ix[point, 'y'], 0.000),
Quaternion(0.000, 0.000, 0.707, 0.707))
#locations[point]= [df.ix[point, 'x'], df.ix[point, 'y']]
# locations['computador'] = Pose(Point(1.288, 0.094, 0.000), Quaternion(0.000, 0.000, 0.707, 0.707))
# locations['izquierda_arriba'] = Pose(Point(1.428, 3.828, 0.000), Quaternion(0.000, 0.000, 0.707, 0.707))
# locations['medio_estantes'] = Pose(Point(-0.461, 2.063, 0.000), Quaternion(0.000, 0.000, 0.707, 0.707))
# locations['izquierda_abajo'] = Pose(Point(-2.528, 3.825, 0.000), Quaternion(0.000, 0.000, 0.707, 0.707))
# locations['puerta'] = Pose(Point(-2.531, -1.800, 0.000), Quaternion(0.000, 0.000, 0.707, 0.707))
# locations['estante_afuera'] = Pose(Point(-8.570, -2.846, 0.000), Quaternion(0.000, 0.000, 0.707, 0.707))
# Publisher to manually control the robot (e.g. to stop it, queue_size=5)
self.cmd_vel_pub = rospy.Publisher('cmd_vel_mux/input/teleop',
Twist,
queue_size=5)
# Subscribe to the move_base action server
self.move_base = actionlib.SimpleActionClient("move_base",
MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
# Wait 60 seconds for the action server to become available
self.move_base.wait_for_server(rospy.Duration(60))
rospy.loginfo("Connected to move base server")
# A variable to hold the initial pose of the robot to be set by
# the user in RViz
initial_pose = PoseWithCovarianceStamped()
# Variables to keep track of success rate, running time,
# and distance traveled
n_locations = len(posiciones)
n_goals = 0
n_successes = 0
i = n_locations
distance_traveled = 0
start_time = rospy.Time.now()
running_time = 0
location = ""
last_location = ""
# Get the initial pose from the user
rospy.loginfo(
"*** Click the 2D Pose Estimate button in RViz to set the robot's initial pose..."
)
rospy.wait_for_message('initialpose', PoseWithCovarianceStamped)
self.last_location = Pose()
rospy.Subscriber('initialpose', PoseWithCovarianceStamped,
self.update_initial_pose)
# Make sure we have the initial pose
while initial_pose.header.stamp == "":
rospy.sleep(1)
rospy.loginfo("Starting navigation")
# Begin the main loop and run through the sequence of locations
while not rospy.is_shutdown():
# If we've gone through the current sequence,
# start again with the sequence
if i == n_locations:
i = 0
sequence = posiciones
# Skip over first location if it is the same as
# the last location
if sequence[0] == last_location:
i = 1
# Get the next location in the current sequence
location = sequence[i]
# Keep track of the distance traveled.
# Use updated initial pose if available.
if initial_pose.header.stamp == "":
distance = sqrt(
pow(
locations[location].position.x -
locations[last_location].position.x, 2) + pow(
locations[location].position.y -
locations[last_location].position.y, 2))
else:
rospy.loginfo("Updating current pose.")
distance = sqrt(
pow(
locations[location].position.x -
initial_pose.pose.pose.position.x, 2) + pow(
locations[location].position.y -
initial_pose.pose.pose.position.y, 2))
initial_pose.header.stamp = ""
# Store the last location for distance calculations
last_location = location
# Set up the next goal location
self.goal = MoveBaseGoal()
self.goal.target_pose.pose = locations[location]
self.goal.target_pose.header.frame_id = 'map'
self.goal.target_pose.header.stamp = rospy.Time.now()
# Let the user know where the robot is going next
rospy.loginfo("Going to location: " + str(location))
# Start the robot toward the next location
self.move_base.send_goal(self.goal)
# Allow 5 minutes to get there (300 seconds)
finished_within_time = self.move_base.wait_for_result(
rospy.Duration(60))
# Check for success or failure
if not finished_within_time:
self.move_base.cancel_goal()
rospy.loginfo("Timed out achieving goal, restarting goal")
rate = 20
r = rospy.Rate(rate)
self.tf_listener = tf.TransformListener()
self.odom_frame = '/odom'
try:
self.tf_listener.waitForTransform(self.odom_frame,
'/base_footprint',
rospy.Time(),
rospy.Duration(1.0))
self.base_frame = '/base_footprint'
except (tf.Exception, tf.ConnectivityException,
tf.LookupException):
try:
self.tf_listener.waitForTransform(
self.odom_frame, '/base_link', rospy.Time(),
rospy.Duration(1.0))
self.base_frame = '/base_link'
except (tf.Exception, tf.ConnectivityException,
tf.LookupException):
rospy.loginfo(
"Cannot find transform between /odom and /base_link or /base_footprint"
)
rospy.signal_shutdown("tf Exception")
move_cmd = Twist()
goal_angle = 1800
angular_tolerance = 2.5 # Converts 2.5 degrees to radians and sets it as tolerance value
angular_speed = 0.5
(position, rotation) = self.get_odom()
move_cmd.angular.z = angular_speed
last_angle = rotation
turn_angle = 0
#
while abs(turn_angle + angular_tolerance) < abs(
goal_angle) and not rospy.is_shutdown():
self.cmd_vel_pub.publish(move_cmd)
r.sleep()
(position, rotation) = self.get_odom()
delta_angle = normalize_angle(rotation - last_angle)
turn_angle += delta_angle
rospy.loginfo(turn_angle)
last_angle = rotation
#self.move_base.send_goal(self.goal)
else:
# Increment the counters
state = self.move_base.get_state()
if state == GoalStatus.SUCCEEDED:
i += 1
n_goals += 1
rospy.loginfo("Goal succeeded!")
n_successes += 1
distance_traveled += distance
rospy.loginfo("State:" + str(state))
else:
rospy.loginfo("Goal failed with error code: " +
str(goal_states[state]))
self.move_base.cancel_goal()
rospy.loginfo("Timed out achieving goal, restarting goal")
rate = 20
r = rospy.Rate(rate)
self.tf_listener = tf.TransformListener()
self.odom_frame = '/odom'
try:
self.tf_listener.waitForTransform(
self.odom_frame, '/base_footprint', rospy.Time(),
rospy.Duration(1.0))
self.base_frame = '/base_footprint'
except (tf.Exception, tf.ConnectivityException,
tf.LookupException):
try:
self.tf_listener.waitForTransform(
self.odom_frame, '/base_link', rospy.Time(),
rospy.Duration(1.0))
self.base_frame = '/base_link'
except (tf.Exception, tf.ConnectivityException,
tf.LookupException):
rospy.loginfo(
"Cannot find transform between /odom and /base_link or /base_footprint"
)
rospy.signal_shutdown("tf Exception")
move_cmd = Twist()
goal_angle = 1800
angular_tolerance = 2.5 # Converts 2.5 degrees to radians and sets it as tolerance value
angular_speed = 0.5
(position, rotation) = self.get_odom()
move_cmd.angular.z = angular_speed
last_angle = rotation
turn_angle = 0
#
while abs(turn_angle + angular_tolerance) < abs(
goal_angle) and not rospy.is_shutdown():
self.cmd_vel_pub.publish(move_cmd)
r.sleep()
(position, rotation) = self.get_odom()
delta_angle = normalize_angle(rotation - last_angle)
turn_angle += delta_angle
rospy.loginfo(turn_angle)
last_angle = rotation
#self.move_base.send_goal(self.goal)
# How long have we been running?
running_time = rospy.Time.now() - start_time
running_time = running_time.secs / 60.0
# Print a summary success/failure, distance traveled and time elapsed
rospy.loginfo("Success so far: " + str(n_successes) + "/" +
str(n_goals) + " = " +
str(100 * n_successes / n_goals) + "%")
rospy.loginfo("Running time: " + str(trunc(running_time, 1)) +
" min Distance: " +
str(trunc(distance_traveled, 1)) + " m")
rospy.sleep(self.rest_time)
def do_gps_goal(lat, long, marker_only=False):
rospy.init_node('gps_goal')
# Check for degrees, minutes, seconds format and convert to decimal
if ',' in lat:
degrees, minutes, seconds = lat.split(',')
degrees, minutes, seconds = float(degrees), float(minutes), float(
seconds)
if lat[0] == '-': # check for negative sign
minutes = -minutes
seconds = -seconds
lat = degrees + minutes / 60 + seconds / 3600
if ',' in long:
degrees, minutes, seconds = long.split(',')
degrees, minutes, seconds = float(degrees), float(minutes), float(
seconds)
if long[0] == '-': # check for negative sign
minutes = -minutes
seconds = -seconds
long = degrees + minutes / 60 + seconds / 3600
goal_lat = float(lat)
goal_long = float(long)
rospy.loginfo('Given GPS goal: lat %s, long %s.' % (goal_lat, goal_long))
# Get the lat long coordinates of our map frame's origin which must be publshed on topic /local_xy_origin. We use this to calculate our goal within the map frame.
origin_topic = '/local_xy_origin'
rospy.loginfo('Listening for origin location on topic %s' % origin_topic)
origin_pose = rospy.wait_for_message(origin_topic, PoseStamped, timeout=10)
origin_lat = origin_pose.pose.position.y
origin_long = origin_pose.pose.position.x
rospy.loginfo('Received origin: lat %s, long %s.' %
(origin_lat, origin_long))
# Calculate distance and azimuth between GPS points
geod = Geodesic.WGS84 # define the WGS84 ellipsoid
g = geod.Inverse(
origin_lat, origin_long, goal_lat, goal_long
) # Compute several geodesic calculations between two GPS points
hypotenuse = distance = g['s12'] # access distance
rospy.loginfo(
"The distance from the origin to the goal is {:.3f} m.".format(
distance))
azimuth = g['azi1']
rospy.loginfo(
"The azimuth from the origin to the goal is {:.3f} degrees.".format(
azimuth))
# Convert polar (distance and azimuth) to x,y translation in meters (needed for ROS) by finding side lenghs of a right-angle triangle
x = adjacent = math.cos(azimuth) * hypotenuse
y = opposite = math.sin(azimuth) * hypotenuse
rospy.loginfo(
"The translation from the origin to the goal is (x,y) {:.3f}, {:.3f} m."
.format(x, y))
# TODO(Dan): Set target yaw pose
# import tf_conversions
# if yaw:
# euler_tuple = tf_conversions.transformations.euler_from_quaternion(quaternion)
# else:
# # Use azimuth as yaw
# Publish goal as point message for visualization purposes
marker_topic = 'point_to_marker' # publish GPS point in ROS coordinates for visualization
point_publisher = rospy.Publisher(marker_topic,
PointStamped,
queue_size=10)
rospy.sleep(1) # allow time for subscribers to join
gps_point = PointStamped()
gps_point.point.x = x
gps_point.point.y = y
gps_point.point.z = 0
point_publisher.publish(gps_point)
rospy.loginfo("Published point {:.3f}, {:.3f} on topic {}.".format(
x, y, marker_topic))
if marker_only:
return
# Create move_base goal
rospy.loginfo("Connecting to move_base...")
move_base = actionlib.SimpleActionClient('move_base', MoveBaseAction)
move_base.wait_for_server()
rospy.loginfo("Connected.")
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = 'map'
goal.target_pose.pose.position.x = x
goal.target_pose.pose.position.y = y
goal.target_pose.pose.position.z = 0.0
goal.target_pose.pose.orientation.x = 0.0
goal.target_pose.pose.orientation.y = 0.0
goal.target_pose.pose.orientation.z = 0.0
goal.target_pose.pose.orientation.w = 1
rospy.loginfo(
'Executing move_base goal to position (x,y) %s, %s.' %
(goal.target_pose.pose.position.x, goal.target_pose.pose.position.y))
rospy.loginfo(
"To cancel the goal: 'rostopic pub -1 /move_base/cancel actionlib_msgs/GoalID -- {}'"
)
# Send goal
move_base.send_goal(goal)
rospy.loginfo('Inital goal status: %s' %
GoalStatus.to_string(move_base.get_state()))
status = move_base.get_goal_status_text()
if status:
rospy.loginfo(status)
# Wait for goal result
move_base.wait_for_result()
rospy.loginfo('Final goal status: %s' %
GoalStatus.to_string(move_base.get_state()))
status = move_base.get_goal_status_text()
if status:
rospy.loginfo(status)
def move_callback(self,marker):
self.client.wait_for_server()
# rospy.loginfo('got server')
goal_pose = MoveBaseGoal()
goal_pose.target_pose.header.stamp = rospy.Time.now()
goal_pose.target_pose.header.frame_id = "base_footprint"
goal_pose.target_pose.pose.position.x = 0
goal_pose.target_pose.pose.position.y = 0
goal_pose.target_pose.pose.position.z = 0
goal_pose.target_pose.pose.orientation.x = 0
goal_pose.target_pose.pose.orientation.y = 0
goal_pose.target_pose.pose.orientation.z = 0
goal_pose.target_pose.pose.orientation.w = 1
waypoint_marker = Marker()
waypoint_marker.header.frame_id = "base_footprint"
waypoint_marker.header.stamp = rospy.get_rostime()
waypoint_marker.ns = "robot"
waypoint_marker.id = 1
waypoint_marker.type = 9
waypoint_marker.action = 0
waypoint_marker.pose.position.x = 0
waypoint_marker.pose.position.y = 0
waypoint_marker.pose.position.z = 0
waypoint_marker.pose.orientation.x = 0
waypoint_marker.pose.orientation.y = 0
waypoint_marker.pose.orientation.z = 0
waypoint_marker.scale.x = .25
waypoint_marker.scale.y = .25
waypoint_marker.scale.z = .25
waypoint_marker.color.r = 1.0
waypoint_marker.color.g = 0.0
waypoint_marker.color.b = 0.0
waypoint_marker.color.a = 1.0
waypoint_marker.lifetime = rospy.Duration(0)
if (self.joy_bool and self.position_marker.ns != "NONE"):
if (marker.pose.position.x >= (self.position_marker.pose.position.x+.1)):
print "greater"
# self.command.linear.x = .75
goal_pose.target_pose.pose.position.x = 2
waypoint_marker.pose.position.x = 2
waypoint_marker.pose.orientation.w = 1
self.client.send_goal(goal_pose)
rospy.loginfo('sent forward goal')
self.waypoint_viz_pub.publish(waypoint_marker)
self.client.wait_for_result()
# self.follower_twist.publish(self.command)
elif (marker.pose.position.x <= (self.position_marker.pose.position.x-.1)):
# self.command.linear.x = -.75
print "Less Than"
goal_pose.target_pose.pose.position.x = -2
waypoint_marker.pose.position.x = -2
waypoint_marker.pose.orientation.w = 1
self.client.send_goal(goal_pose)
rospy.loginfo('sent backwards goal')
self.waypoint_viz_pub.publish(waypoint_marker)
self.client.wait_for_result()
# self.follower_twist.publish(self.command)
else:
self.client.cancel_goal()
print "Canceled Goals"
# self.command.linear.x = 0
# self.follower_twist.publish(self.command)
rospy.loginfo('sent stay goal')
# print self.command.linear.x
else:
pass
def __init__(self):
rospy.init_node('exploring_maze_pro', anonymous=True)
rospy.on_shutdown(self.shutdown)
# 在每个目标位置暂停的时间 (单位：s)
self.rest_time = rospy.get_param("~rest_time", 0.5)
# 发布控制机器人的消息
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)
# 创建一个Subscriber，订阅/exploring_cmd
rospy.Subscriber("/exploring_cmd", Int8, self.cmdCallback)
# 订阅move_base服务器的消息
self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
# 60s等待时间限制
self.move_base.wait_for_server(rospy.Duration(60))
rospy.loginfo("Connected to move base server")
# 保存运行时间的变量
start_time = rospy.Time.now()
running_time = 0
rospy.loginfo("Starting exploring maze")
# 初始位置
start_location = Pose(Point(0, 0, 0), Quaternion(0.000, 0.000, 0.709016873598, 0.705191515089))
locations = []
locations.append(Pose(Point( 3.90563511848, 2.18432855606, 0.000), Quaternion(0.000, 0.000, 0.627458332802, 0.778650140048)))
locations.append(Pose(Point( 0.341322928667, 2.84021830559, 0.000), Quaternion(0.000, 0.000, -0.136402137071,0.990653550442)))
locations.append(Pose(Point( 0.684419214725, 7.0397400856, 0.000), Quaternion(0.000, 0.000,-0.018217150058, 0.999834053953)))
locations.append(Pose(Point( 3.97112774849, 7.0865688324, 0.000), Quaternion(0.000, 0.000,0.663261842633, 0.748387418459)))
locations.append(Pose(Point( 7.59590530396, 4.96546936035, 0.000), Quaternion(0.000, 0.000, 0.361617159112, 0.932326675707)))
locations.append(Pose(Point( 6.89283561707, 7.70138311386, 0.000), Quaternion(0.000, 0.000,0.0924645917946, 0.995715973189)))
locations.append(Pose(Point( 7.68732309341, 0.281919270754, 0.000), Quaternion(0.000, 0.000, -0.336762420405, 0.941589651708)))
# 命令初值
self.exploring_cmd = 0
location = 0
# 开始主循环，随机导航
while not rospy.is_shutdown():
# 设定下一个随机目标点
self.goal = MoveBaseGoal()
self.goal.target_pose.pose = start_location
self.goal.target_pose.header.frame_id = 'map'
self.goal.target_pose.header.stamp = rospy.Time.now()
if self.exploring_cmd is STATUS_EXPLORING:
self.goal.target_pose.pose = locations[location]
location = location + 1
if location is 7:
location = 0
elif self.exploring_cmd is STATUS_CLOSE_TARGET:
rospy.sleep(0.1)
continue
elif self.exploring_cmd is STATUS_GO_HOME:
self.goal.target_pose.pose.position.x = 0
self.goal.target_pose.pose.position.y = 0
#让用户知道下一个位置
rospy.loginfo("Going to:" + str(self.goal.target_pose.pose))
#向下一个位置出发
self.move_base.send_goal(self.goal)
#五分钟限制时间
finished_within_time = self.move_base.wait_for_result(rospy.Duration(300))
#查看是否成功到达
if not finished_within_time:
self.move_base.cancel_goal()
rospy.loginfo("Timed out achieving goal")
else:
state = self.move_base.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Goal successed!")
else:
rospy.loginfo("Goal failed!")
#运行所用的时间
running_time = rospy.Time.now() - start_time
running_time = running_time.secs / 60.0
#输出本次导航的所有信息
rospy.loginfo("Current time:" + str(trunc(running_time, 1))+ "min")
self.shutdown()
def ligarNavigation(area, seq, nome):
#send_goal
#aux = 0
#while(client.get_result() != 3 and aux != 3):
client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
# Waits until the action server has started up and started
# listening for goals.
goal = MoveBaseGoal()
q = transformations.quaternion_from_euler(0, 0, area[2])
quat = Quaternion(*q)
goal.target_pose.header.seq = seq
goal.target_pose.header.frame_id = "map"
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose.position.x = area[0]
goal.target_pose.pose.position.y = area[1]
goal.target_pose.pose.orientation = quat
#pub = rospy.Publisher("move_base_simple/goal", PoseStamped)
#pub.publish(goal.target_pose)
client.wait_for_server()
# Sends the goal to the action server.
client.send_goal(goal)
'''global vel_x
global vel_y
global ang_x
global ang_y
rospy.logwarn(client.get_state())
while(client.get_state() == 0 or client.get_state() == 1):
rospy.logwarn(client.get_state())
rospy.logwarn("dentro do while")
tempo1 = rospy.get_time()
tempo2 = rospy.get_time()
while(tempo2-tempo1 < 4 or vel_x != 0 or vel_y != 0 or ang_x != 0 or ang_y != 0):
tempo2 = rospy.get_time()
if(tempo2 - tempo1 >= 4):
twist = Twist()
twist.linear.x = 0.5
pub = rospy.Publisher('cmd_vel', Twist)
for i in range(0, 30):
pub.publish(twist)
twist.linear.x = 0
pub.publish(twist)
rospy.logwarn("passou o while")'''
# Waits for the server to finish performing the action.
client.wait_for_result()
#resultado = client.get_result()
#rospy.logwarn("status do navigation"+str(resultado.status))
#aux += 1
rospy.logwarn("Cheguei no pose " + str(area[0]) + " " + str(area[1]) +
"Area = " + nome)
'''if nome == "Pedidos":
class robot:
goal = MoveBaseGoal()
start = PoseStamped()
end = PoseStamped()
def __init__(self, name):
self.assigned_point = []
self.name = name
self.global_frame = rospy.get_param('~global_frame', 'map')
self.listener = tf.TransformListener()
rospy.sleep(2)
self.listener.waitForTransform(self.global_frame, name + '/base_link',
rospy.Time(0), rospy.Duration(10.0))
cond = 0
while cond == 0:
try:
self.listener.waitForTransform(self.global_frame,
name + '/base_link',
rospy.Time(0),
rospy.Duration(10.0))
(trans,
rot) = self.listener.lookupTransform(self.global_frame,
self.name + '/base_link',
rospy.Time(0))
cond = 1
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
cond == 0
self.position = Point()
self.position.x = trans[0]
self.position.y = trans[1]
self.position.z = 0.0
self.current_goal = self.position
self.client = actionlib.SimpleActionClient(self.name + '/move_base',
MoveBaseAction)
self.client.wait_for_server()
robot.goal.target_pose.header.frame_id = self.global_frame
robot.goal.target_pose.header.stamp = rospy.Time.now()
rospy.wait_for_service(self.name + '/move_base/NavfnROS/make_plan')
self.make_plan = rospy.ServiceProxy(
self.name + '/move_base/NavfnROS/make_plan', GetPlan)
robot.start.header.frame_id = self.global_frame
robot.end.header.frame_id = self.global_frame
def getPosition(self):
cond = 0
while cond == 0:
try:
(trans,
rot) = self.listener.lookupTransform(self.global_frame,
self.name + '/base_link',
rospy.Time(0))
cond = 1
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
cond == 0
self.position.x = trans[0]
self.position.y = trans[1]
self.position.z = 0.0
return self.position
def sendGoal(self, point):
robot.goal.target_pose.pose.position.x = point.x
robot.goal.target_pose.pose.position.y = point.y
robot.goal.target_pose.pose.orientation.w = 1.0
self.client.send_goal(robot.goal)
self.current_goal = point
def cancelGoal(self):
self.client.cancel_goal()
self.current_goal = self.getPosition()
def getState(self):
return self.client.get_state()
def makePlan(self, start, end):
robot.start.pose.position.x = start[0]
robot.start.pose.position.y = start[1]
robot.end.pose.position.x = end[0]
robot.end.pose.position.y = end[1]
#start=self.listener.transformPose(self.name+'/map', robot.start)
start = self.listener.transformPose('/map', robot.start)
end = self.listener.transformPose('/map', robot.end)
plan = self.make_plan(start=start, goal=end, tolerance=0.0)
return plan.plan.poses
def go_to_pose(self, pos):
if self.starting_pose:
rospy.loginfo(
str(self.robot_id) + ' - moving to starting position ' +
str(pos))
elif self.alone:
rospy.loginfo(
str(self.robot_id) + ' - moving alone to final position ' +
str(pos))
else:
rospy.loginfo(str(self.robot_id) + ' - moving to ' + str(pos))
success = False
old_pos = pos
already_found = False
already_tried = False
fixing_pose = False
fix = 1
loop_count = 0
fixing_attempts = 0
max_attempts = 3
radius = 1.5
self.timer = rospy.Time.now()
while not success:
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = '/map'
goal.target_pose.pose.position = Point(pos[0], pos[1], 0.000)
goal.target_pose.pose.orientation.w = 1
self.last_feedback_pose = None
self.last_motion_time = rospy.Time.now()
# Start moving
self.client_motion.send_goal(goal,
feedback_cb=self.feedback_motion_cb)
self.client_motion.wait_for_result()
state = self.client_motion.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo(str(self.robot_id) + ' - position reached')
success = True
self.myself['arrived_nominal_dest'] = True
if fixing_pose and pos not in self.fixed_wall_poses:
self.fixed_wall_poses.append(pos)
elif state == GoalStatus.PREEMPTED:
if loop_count == 0:
rospy.loginfo(
str(self.robot_id) + ' - preempted, using recovery')
if pos not in self.problematic_poses:
self.problematic_poses.append(pos)
else:
for pose in self.fixed_wall_poses:
pos = list(pos)
if (pose[0] - pos[0])**2 + (
pose[1] - pos[1]
)**2 <= radius**2: #if a pose is in a circle centered in pos
rospy.loginfo(
str(self.robot_id) +
' - moving to the fixed position already found: '
+ str(pose))
pos[0] = pose[0]
pos[1] = pose[1]
already_found = True
pos = tuple(pos)
if loop_count >= 4:
if not already_found:
fixing_pose = True
if not already_tried:
already_tried = True
rospy.loginfo(
str(self.robot_id) +
' - preempted, trying to fix the goal after too many recoveries'
)
if pos in self.fixed_wall_poses:
self.fixed_wall_poses.remove(
pos
) # removing a fixed position that is not working anymore
pos = self.fix_pose(old_pos, fix)
if pos:
rospy.loginfo(
str(self.robot_id) +
' - moving to new fixed goal ' + str(pos))
loop_count = 1
fixing_attempts += 1
if fixing_attempts == max_attempts: #increasing the radius of fixing area
fix += 0.5
fixing_attempts = 0
else:
break
else:
if loop_count == 7:
already_found = False
loop_count = 4
self.clear_costmap_service()
self.motion_recovery()
self.clear_costmap_service()
loop_count += 1
if self.myself['time_expired']:
success = True
else:
success = self.check_time_expired()
elif state == GoalStatus.ABORTED:
rospy.logerr(
str(self.robot_id) +
" - motion aborted by the server! Trying to recover")
self.clear_costmap_service()
self.motion_recovery()
self.clear_costmap_service()
success = False
else:
rospy.loginfo(str(self.robot_id) + ' - state: ' + str(state))
break
def manual_cb(self, target_pose):
goal = MoveBaseGoal(target_pose=target_pose)
self.client.send_goal(goal)
def __init__(self):
rospy.init_node('Pi_Smach', anonymous=False)
rospy.on_shutdown(self.shutdown)
setup_environment(self)
self.last_nav_state = None
self.recharging = False
nav_states = list()
for waypoint in self.waypoints:
nav_goal = MoveBaseGoal()
nav_goal.target_pose.header.frame_id = 'map'
nav_goal.target_pose.pose = waypoint
move_base_state = SimpleActionState(
'move_base',
MoveBaseAction,
goal=nav_goal,
exec_timeout=rospy.Duration(60.0),
server_wait_timeout=rospy.Duration(10.0))
nav_states.append(move_base_state)
nav_goal = MoveBaseGoal()
nav_goal.target_pose.header.frame_id = 'map'
nav_goal.target_pose.pose = self.docking_station_pose
nav_docking_station = SimpleActionState(
'move_base',
MoveBaseAction,
goal=nav_goal,
exec_timeout=rospy.Duration(60.0),
server_wait_timeout=rospy.Duration(10.0))
self.sm_nav = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
self.sm_nav.userdata.X_input = 0
self.sm_nav.userdata.Y_input = 0
with self.sm_nav:
StateMachine.add('NAV_STATE_0',
nav_states[0],
transitions={
'succeeded': 'NAV_STATE_1',
'aborted': 'NAV_STATE_1'
})
StateMachine.add('NAV_STATE_1',
nav_states[1],
transitions={
'succeeded': 'INPUTXY',
'aborted': 'INPUTXY'
})
StateMachine.add('INPUTXY',
InputXY(),
transitions={'succeeded': 'GOCHOSEWAY'},
remapping={
'X_output': 'X_input',
'Y_output': 'Y_input'
})
StateMachine.add('GOCHOSEWAY',
ChoseWay(),
transitions={
'succeeded': 'MOVE_ARM',
'aborted': 'MOVE_ARM'
},
remapping={
'X': 'X_input',
'Y': 'Y_input'
})
StateMachine.add('MOVE_ARM',
Move_Arm(),
transitions={'succeeded': 'NAV_STATE_2'})
StateMachine.add('NAV_STATE_2',
nav_states[2],
transitions={
'succeeded': '',
'aborted': ''
})
self.sm_nav.register_transition_cb(self.nav_transition_cb, cb_args=[])
self.sm_recharge = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
with self.sm_recharge:
StateMachine.add('NAV_DOCKING_STATION',
nav_docking_station,
transitions={'succeeded': 'RECHARGE_BATTERY'})
StateMachine.add('RECHARGE_BATTERY',
ServiceState(
'battery_simulator/set_battery_level',
SetBatteryLevel,
100,
response_cb=self.recharge_cb),
transitions={'succeeded': ''})
self.nav_patrol = Concurrence(
outcomes=['succeeded', 'recharge', 'stop'],
default_outcome='succeeded',
child_termination_cb=self.concurrence_child_termination_cb,
outcome_cb=self.concurrence_outcome_cb)
with self.nav_patrol:
Concurrence.add('SM_NAV', self.sm_nav)
Concurrence.add(
'MONITOR_BATTERY',
MonitorState("battery_level", Float32, self.battery_cb))
self.sm_top = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
with self.sm_top:
StateMachine.add('PATROL',
self.nav_patrol,
transitions={
'succeeded': 'PATROL',
'recharge': 'RECHARGE',
'stop': 'STOP'
})
StateMachine.add('RECHARGE',
self.sm_recharge,
transitions={'succeeded': 'PATROL'})
StateMachine.add('STOP', Stop(), transitions={'succeeded': ''})
intro_server = IntrospectionServer('patrol', self.sm_top, '/SM_ROOT')
intro_server.start()
sm_outcome = self.sm_top.execute()
rospy.loginfo('State Machine Outcome: ' + str(sm_outcome))
intro_server.stop()
def execute(self, userdata):
global CURRENT_STATE, TAGS_FOUND, TAG_POSE, MIDDLE_POSE, CURRENT_POSE, boxToTheLeft, MIDDLE_GOAL, BOX_MARK
CURRENT_STATE = "turn"
self.count = 0
self.marker_detected = False
while not self.marker_detected:
msg = Twist()
msg.linear.x = 0.0
msg.angular.z = -0.4
self.cmd_pub.publish(msg)
self.rate.sleep()
userdata.current_marker = TAGS_FOUND[-1]
if self.mode == 1:
MIDDLE_POSE = TAG_POSE
BOX_MARK = TAGS_FOUND[-1]
yaw = euler_from_quaternion([CURRENT_POSE.orientation.x, CURRENT_POSE.orientation.y,
CURRENT_POSE.orientation.z, CURRENT_POSE.orientation.w])[2]
if -180 < yaw < 0:
boxToTheLeft = True
elif 0 < yaw < 180:
boxToTheLeft = False
if boxToTheLeft:
delta_x = -2
else:
delta_x = 2
middle_point = PointStamped()
middle_point.header.frame_id = "ar_marker_" + \
str(TAGS_FOUND[-1])
middle_point.header.stamp = rospy.Time(0)
middle_point.point.x = delta_x
self.listener.waitForTransform(
"odom", middle_point.header.frame_id, rospy.Time(0), rospy.Duration(4))
middle_point_transformed = self.listener.transformPoint(
"odom", middle_point)
quaternion = quaternion_from_euler(0, 0, -math.pi/2)
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = "odom"
goal.target_pose.pose.position.x = middle_point_transformed.point.x
goal.target_pose.pose.position.y = middle_point_transformed.point.y
goal.target_pose.pose.position.z = middle_point_transformed.point.z
goal.target_pose.pose.orientation.x = quaternion[0]
goal.target_pose.pose.orientation.y = quaternion[1]
goal.target_pose.pose.orientation.z = quaternion[2]
goal.target_pose.pose.orientation.w = quaternion[3]
MIDDLE_GOAL = goal
self.marker_detected = False
# CURRENT_STATE = None
self.cmd_pub.publish(Twist())
return "find"
# rospy.loginfo('Waiting for LaneServer to start up...')
# rospy.wait_for_service('lane_srv')
# lane_srv = rospy.ServiceProxy('lane_srv', mission_to_lane)
# rospy.loginfo('Mission Connected to LaneServer')
# Action Client for PIDs
locomotion_client = actionlib.SimpleActionClient(
'LocomotionServer', bbauv_msgs.msg.ControllerAction)
locomotion_client.wait_for_server()
rospy.loginfo("Mission connected to LocomotionServer")
# Action Client for Move Base
# movebase_client = actionlib.SimpleActionClient("move_base", MoveBaseAction)
# movebase_client.wait_for_server()
rospy.loginfo("Mission connected to MovebaseServer")
movebaseGoal = MoveBaseGoal()
sm_mission = smach.StateMachine(
outcomes=['mission_complete', 'mission_failed'])
with sm_mission:
smach.StateMachine.add('COUNTDOWN',
Countdown(),
transitions={'succeeded': 'START'})
smach.StateMachine.add('START',
Start(),
transitions={'start_complete': 'GATE'})
smach.StateMachine.add('GATE',
Gate(),
transitions={
'gate_complete': 'PARK',
def execute(self, userdata):
global CURRENT_POSE
global CURRENT_STATE, START_POSE, END_GOAL, MARKER_POSE, PARK_MARK
CURRENT_STATE = "move_closer"
self.tag_pose_base = None
self.current_marker = userdata.current_marker
max_angular_speed = 0.8
min_angular_speed = 0.0
max_linear_speed = 0.8
min_linear_speed = 0.0
while True:
if self.tag_pose_base is not None and self.tag_pose_base.position.x < self.how_close:
break
elif self.tag_pose_base is not None and self.tag_pose_base.position.x > self.how_close:
move_cmd = Twist()
if self.tag_pose_base.position.x > self.how_close+0.1: # goal too far
move_cmd.linear.x += 0.1
elif self.tag_pose_base.position.x > self.how_close: # goal too close
move_cmd.linear.x -= 0.1
else:
move_cmd.linear.x = 0
if self.tag_pose_base.position.y < 1e-3: # goal to the left
move_cmd.angular.z -= 0.1
elif self.tag_pose_base.position.y > -1e-3: # goal to the right
move_cmd.angular.z += 0.1
else:
move_cmd.angular.z = 0
move_cmd.linear.x = math.copysign(max(min_linear_speed, min(
abs(move_cmd.linear.x), max_linear_speed)), move_cmd.linear.x)
move_cmd.angular.z = math.copysign(max(min_angular_speed, min(
abs(move_cmd.angular.z), max_angular_speed)), move_cmd.angular.z)
move_cmd.linear.x = abs(move_cmd.linear.x)
self.vel_pub.publish(move_cmd)
self.rate.sleep()
print("marker!!!:", self.current_marker)
pose = PointStamped()
pose.header.frame_id = "ar_marker_" + str(self.current_marker)
pose.header.stamp = rospy.Time(0)
pose.point.z = self.distance_from_marker
# TODO: Change to -0.1?????
pose.point.x = -0.1
self.listener.waitForTransform(
"odom", pose.header.frame_id, rospy.Time(0), rospy.Duration(4))
pose_transformed = self.listener.transformPoint("odom", pose)
if self.ACAP:
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = "odom"
goal.target_pose.pose.position.x = pose_transformed.point.x
goal.target_pose.pose.position.y = pose_transformed.point.y
goal.target_pose.pose.orientation = START_POSE.orientation
END_GOAL = goal
PARK_MARK = self.current_marker
else:
MARKER_POSE = self.tag_pose_base
return "close_enough"
def pick(self, data):
""" Executes picking action. """
if (data.action == 'pick'):
self.command_id = data.command_id
self.action = data.action # to be removed after msg modification
self.cart_id = data.cart_id
#self.reconf_client.update_configuration({"cart_id": self.cart_id}) # dynamic parameter to share cart_id in between clients at runtime
self.cart_id_pub.publish(self.cart_id)
home_flag = rospy.get_param('/' + ROBOT_ID +
'/dynamic_reconf_server/home')
undock_flag = rospy.get_param('/' + ROBOT_ID +
'/dynamic_reconf_server/undock')
if ((home_flag == True) or (undock_flag == True)):
#print('calculating docking position for cart_id: {}'.format(self.cart_id)) ###
dock_pose = self.calc_dock_position(self.cart_id)
#rospy.loginfo('Dock Pose coordinates: {}'.format(dock_pose))
if (dock_pose == None):
#rospy.logerr('[ {} ]: Cart Topic Not Found!'.format(rospy.get_name()))
return
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = "vicon_world" # Always send goals in reference to vicon_world when using ros_mocap package
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose.position.x = dock_pose.position.x
goal.target_pose.pose.position.y = dock_pose.position.y
goal.target_pose.pose.orientation.x = dock_pose.orientation.x
goal.target_pose.pose.orientation.y = dock_pose.orientation.y
goal.target_pose.pose.orientation.z = dock_pose.orientation.z
goal.target_pose.pose.orientation.w = dock_pose.orientation.w
#rospy.loginfo('Pick goal coordinates: {}'.format(goal))
try:
rospy.wait_for_service(
'/' + ROBOT_ID + '/move_base/clear_costmaps'
) # clear cost maps before sending goal to remove false positive obstacles
reset_costmaps = rospy.ServiceProxy(
'/' + ROBOT_ID + '/move_base/clear_costmaps', Empty)
reset_costmaps()
rospy.loginfo(
'[ {} ]: Costmaps Cleared Successfully'.format(
rospy.get_name()))
except:
rospy.logwarn(
'[ {} ]: Costmaps Clearing Service Call Failed!'.
format(rospy.get_name()))
rospy.sleep(0.5)
#self.act_client.send_goal_and_wait(goal) # blocking
rospy.loginfo('[ {} ]: Sending Goal to Action Server'.format(
rospy.get_name()))
self.act_client.send_goal(
goal
) # non-blocking - Also alternative goal pursuit is also possible in this mode
self.status_flag = True
else:
#self.act_client.cancel_goal()
#self.reconf_client.update_configuration({'pick': False})
rospy.logerr(
'[ {} ]: Action Rejected! - Invalid Pick Action'.format(
rospy.get_name()))
return
else:
if (data.action == 'cancelCurrent'):
self.act_client.cancel_goal()
self.reconf_client.update_configuration({'pick': False})
rospy.logwarn('Cancelling Current Goal')
if (data.action == 'cancelAll'):
self.act_client.cancel_all_goals()
self.reconf_client.update_configuration({'pick': False})
rospy.logwarn('cancelling All Goals')
if (data.action == 'cancelAtAndBefore'):
self.act_client.cancel_goals_at_and_before_time(
data.cancellation_stamp)
self.reconf_client.update_configuration({'pick': False})
s = 'Cancelling all Goals at and before {}'.format(
data.cancellation_stamp)
rospy.logwarn(s)
#self.act_client.stop_tracking_goal()
#self.status_flag = False
return
import sys, rospy, tf, actionlib, moveit_commander
from geometry_msgs.msg import *
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from tf.transformations import quaternion_from_euler
from control_msgs.msg import (GripperCommandAction, GripperCommandGoal)
if __name__ == '__main__':
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('deliver_to_counter')
args = rospy.myargv(argv=sys.argv)
gripper = actionlib.SimpleActionClient("gripper_controller/gripper_action",
GripperCommandAction)
gripper.wait_for_server()
move_base = actionlib.SimpleActionClient('move_base', MoveBaseAction)
move_base.wait_for_server()
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = 'map'
goal.target_pose.pose.position.x = 4
orient = Quaternion(*quaternion_from_euler(0, 0, 0))
goal.target_pose.pose.orientation = orient
move_base.send_goal(goal)
move_base.wait_for_result()
arm = moveit_commander.MoveGroupCommander("arm")
arm.allow_replanning(True)
p = Pose()
p.position.x = 0.9
p.position.z = 0.95
p.orientation = Quaternion(*quaternion_from_euler(0, 0.5, 0))
arm.set_pose_target(p)
arm.go(True)
goal_pts[-1][1] += 0.05
elif(obj[2]==0):
goal_pts[-1][2] += 0.05
elif(obj[1]==6 and obj[3]==pi/2):
goal_pts[-1][1] -= 0.05
elif(obj[2]==6 and obj[3]==0):
goal_pts[-1][2] -= 0.05
print goal_pts
rospy.init_node('Goal_Point')
# stuff related to goal_point() function
move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
move_base.wait_for_server(rospy.Duration(60))
goal_pt = MoveBaseGoal()
goal_pt.target_pose.header.frame_id = '/map'
# stuff related to path_length() function
rospy.wait_for_service('/move_base/GlobalPlanner/make_plan')
client = rospy.ServiceProxy('/move_base/GlobalPlanner/make_plan', GetPlan)
start = PoseStamped()
start.header.frame_id = "/map"
goal = PoseStamped()
goal.header.frame_id = "/map"
# stuff related to robot_pose() function
listener = tf.TransformListener()
odom_pose = PoseStamped()
odom_pose.header.frame_id = "/odom"
robot_x = 0.0
def __init__(self):
rospy.init_node('nav_test', anonymous=True)
rospy.on_shutdown(self.shutdown)
# How long in seconds should the robot pause at each location?
self.rest_time = rospy.get_param("~rest_time", 10)
# Are we running in the fake simulator?
self.fake_test = rospy.get_param("~fake_test", False)
# Goal state return values
goal_states = [
'PENDING', 'ACTIVE', 'PREEMPTED', 'SUCCEEDED', 'ABORTED',
'REJECTED', 'PREEMPTING', 'RECALLING', 'RECALLED', 'LOST'
]
# Set up the goal locations. Poses are defined in the map frame.
# An easy way to find the pose coordinates is to point-and-click
# Nav Goals in RViz when running in the simulator.
# Pose coordinates are then displayed in the terminal
# that was used to launch RViz.
locations = dict()
locations['O'] = Pose(Point(-0.012, 0.552, 0.000),
Quaternion(0.000, 0.000, 0.000, 1.000))
locations['A'] = Pose(Point(9.319, 1.399, 0.000),
Quaternion(0.000, 0.000, 1.000, -0.028))
locations['B'] = Pose(Point(8.016, -1.552, 0.000),
Quaternion(0.000, 0.000, 1.000, -0.025))
locations['C'] = Pose(Point(3.474, -3.718, 0.000),
Quaternion(0.000, 0.000, 0.909, 0.418))
locations['D'] = Pose(Point(-3.798, -5.547, 0.000),
Quaternion(0.000, 0.000, 0.585, 0.811))
locations['E'] = Pose(Point(-7.230, -1.971, 0.000),
Quaternion(0.000, 0.000, 0.305, 0.952))
locations['F'] = Pose(Point(-7.843, 2.171, 0.000),
Quaternion(0.000, 0.000, 0.000, 1.000))
locations['G'] = Pose(Point(-6.984, 5.932, 0.000),
Quaternion(0.000, 0.000, 0.556, 0.831))
locations['H'] = Pose(Point(-4.430, 8.280, 0.000),
Quaternion(0.000, 0.000, -0.561, 0.828))
locations['I'] = Pose(Point(-1.257, 9.187, 0.000),
Quaternion(0.000, 0.000, -0.707, 0.707))
locations['J'] = Pose(Point(1.840, 8.647, 0.000),
Quaternion(0.000, 0.000, -0.745, 0.667))
locations['K'] = Pose(Point(5.272, 7.514, 0.000),
Quaternion(0.000, 0.000, 0.870, -0.493))
locations['L'] = Pose(Point(8.850, 5.731, 0.000),
Quaternion(0.000, 0.000, 0.957, -0.290))
# Publisher to manually control the robot (e.g. to stop it, queue_size=5)
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)
# Subscribe to the move_base action server
self.move_base = actionlib.SimpleActionClient("move_base",
MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
# Wait 60 seconds for the action server to become available
self.move_base.wait_for_server(rospy.Duration(60))
rospy.loginfo("Connected to move base server")
# A variable to hold the initial pose of the robot to be set by
# the user in RViz
initial_pose = PoseWithCovarianceStamped()
# Variables to keep track of success rate, running time,
# and distance traveled
n_locations = len(locations)
n_goals = 0
n_successes = 0
i = n_locations
distance_traveled = 0
start_time = rospy.Time.now()
running_time = 0
location = ""
last_location = ""
# Get the initial pose from the user
rospy.loginfo(
"*** Click the 2D Pose Estimate button in RViz to set the robot's initial pose..."
)
rospy.wait_for_message('initialpose', PoseWithCovarianceStamped)
self.last_location = Pose()
rospy.Subscriber('initialpose', PoseWithCovarianceStamped,
self.update_initial_pose)
# Make sure we have the initial pose
while initial_pose.header.stamp == "":
rospy.sleep(1)
rospy.loginfo("Starting navigation test")
# Begin the main loop and run through a sequence of locations
while not rospy.is_shutdown():
# If we've gone through the current sequence,
# start with a new random sequence
if i == n_locations:
i = 0
sequence = sample(locations, n_locations)
# Skip over first location if it is the same as
# the last location
if sequence[0] == last_location:
i = 1
# Get the next location in the current sequence
location = sequence[i]
# Keep track of the distance traveled.
# Use updated initial pose if available.
if initial_pose.header.stamp == "":
distance = sqrt(
pow(
locations[location].position.x -
locations[last_location].position.x, 2) + pow(
locations[location].position.y -
locations[last_location].position.y, 2))
else:
rospy.loginfo("Updating current pose.")
distance = sqrt(
pow(
locations[location].position.x -
initial_pose.pose.pose.position.x, 2) + pow(
locations[location].position.y -
initial_pose.pose.pose.position.y, 2))
initial_pose.header.stamp = ""
# Store the last location for distance calculations
last_location = location
# Increment the counters
i += 1
n_goals += 1
# Set up the next goal location
self.goal = MoveBaseGoal()
self.goal.target_pose.pose = locations[location]
self.goal.target_pose.header.frame_id = 'map'
self.goal.target_pose.header.stamp = rospy.Time.now()
# Let the user know where the robot is going next
rospy.loginfo("Going to: " + str(location))
# Start the robot toward the next location
self.move_base.send_goal(self.goal)
# Allow 5 minutes to get there
finished_within_time = self.move_base.wait_for_result(
rospy.Duration(300))
# Check for success or failure
if not finished_within_time:
self.move_base.cancel_goal()
rospy.loginfo("Timed out achieving goal")
else:
state = self.move_base.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Goal succeeded!")
n_successes += 1
distance_traveled += distance
rospy.loginfo("State:" + str(state))
else:
rospy.loginfo("Goal failed with error code: " +
str(goal_states[state]))
# How long have we been running?
running_time = rospy.Time.now() - start_time
running_time = running_time.secs / 60.0
# Print a summary success/failure, distance traveled and time elapsed
rospy.loginfo("Success so far: " + str(n_successes) + "/" +
str(n_goals) + " = " +
str(100 * n_successes / n_goals) + "%")
rospy.loginfo("Running time: " + str(trunc(running_time, 1)) +
" min Distance: " +
str(trunc(distance_traveled, 1)) + " m")
rospy.sleep(self.rest_time)
def _send_dummy_goal(self):
pose = GM.PoseStamped()
pose.header.frame_id = "odom"
pose.pose.orientation.w = 1.0
self.al_client.send_goal(MoveBaseGoal(pose),
done_cb=self._move_base_cb)
def movebase_client(inp):
client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
client.wait_for_server()
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = "map"
if inp == 0:
print("Point 1: " + str(final_route[1]))
goal.target_pose.pose.position.x = final_route[1][0]
goal.target_pose.pose.position.y = final_route[1][1]
goal.target_pose.pose.position.z = 0.0
goal.target_pose.pose.orientation.x = 0.0
goal.target_pose.pose.orientation.y = 0.0
goal.target_pose.pose.orientation.z = 0.0
goal.target_pose.pose.orientation.w = 1.0
elif inp == 1:
print("Point 2: " + str(final_route[2]))
goal.target_pose.pose.position.x = final_route[2][0]
goal.target_pose.pose.position.y = final_route[2][1]
goal.target_pose.pose.position.z = 0.0
goal.target_pose.pose.orientation.x = 0.0
goal.target_pose.pose.orientation.y = 0.0
goal.target_pose.pose.orientation.z = 0.0
goal.target_pose.pose.orientation.w = 1.0
elif inp == 2:
print("Point 3: " + str(final_route[3]))
goal.target_pose.pose.position.x = final_route[3][0]
goal.target_pose.pose.position.y = final_route[3][1]
goal.target_pose.pose.position.z = 0.0
goal.target_pose.pose.orientation.x = 0.0
goal.target_pose.pose.orientation.y = 0.0
goal.target_pose.pose.orientation.z = 0.0
goal.target_pose.pose.orientation.w = 1.0
elif inp == 3:
print("Point 4: " + str(final_route[4]))
goal.target_pose.pose.position.x = final_route[4][0]
goal.target_pose.pose.position.y = final_route[4][1]
goal.target_pose.pose.position.z = 0.0
goal.target_pose.pose.orientation.x = 0.0
goal.target_pose.pose.orientation.y = 0.0
goal.target_pose.pose.orientation.z = 0.0
goal.target_pose.pose.orientation.w = 1.0
else:
print("Point 5: " + str(final_route[5]))
goal.target_pose.pose.position.x = final_route[5][0]
goal.target_pose.pose.position.y = final_route[5][1]
goal.target_pose.pose.position.z = 0.0
goal.target_pose.pose.orientation.x = 0.0
goal.target_pose.pose.orientation.y = 0.0
goal.target_pose.pose.orientation.z = 0.0
goal.target_pose.pose.orientation.w = 1.0
client.send_goal(goal)
bekle = client.wait_for_result()
if not bekle:
rospy.signal_shutdown("No Action Servicel!")
else:
return client.get_result()
def __init__(self):
rospy.on_shutdown(self.cleanup)
# Subscribe to the move_base action server
self.move_base = actionlib.SimpleActionClient("move_base",
MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
# Wait for the action server to become available
self.move_base.wait_for_server(rospy.Duration(120))
rospy.loginfo("Connected to move base server")
# A variable to hold the initial pose of the robot to be set by the user in RViz
initial_pose = PoseWithCovarianceStamped()
rospy.Subscriber('initialpose', PoseWithCovarianceStamped,
self.update_initial_pose)
# Get the initial pose from the user
rospy.loginfo(
"*** Click the 2D Pose Estimate button in RViz to set the robot's initial pose..."
)
rospy.wait_for_message('initialpose', PoseWithCovarianceStamped)
# Make sure we have the initial pose
while initial_pose.header.stamp == "":
rospy.sleep(1)
rospy.loginfo("Ready to go")
rospy.sleep(1)
locations = dict()
# Location A
A_x = 1.0
A_y = 1.0
A_theta = 1.5708
quaternion = quaternion_from_euler(0.0, 0.0, A_theta)
locations['A'] = Pose(
Point(A_x, A_y, 0.000),
Quaternion(quaternion[0], quaternion[1], quaternion[2],
quaternion[3]))
self.goal = MoveBaseGoal()
rospy.loginfo("Starting navigation test")
while not rospy.is_shutdown():
self.goal.target_pose.header.frame_id = 'map'
self.goal.target_pose.header.stamp = rospy.Time.now()
# Robot will go to point A
if start == 1:
rospy.loginfo("Going to point A")
rospy.sleep(2)
self.goal.target_pose.pose = locations['A']
self.move_base.send_goal(self.goal)
waiting = self.move_base.wait_for_result(rospy.Duration(300))
if waiting == 1:
rospy.loginfo("Reached point A")
rospy.sleep(2)
rospy.loginfo("Ready to go back")
rospy.sleep(2)
global start
start = 0
# After reached point A, robot will go back to initial position
elif start == 0:
rospy.loginfo("Going back home")
rospy.sleep(2)
self.goal.target_pose.pose = self.origin
self.move_base.send_goal(self.goal)
waiting = self.move_base.wait_for_result(rospy.Duration(300))
if waiting == 1:
rospy.loginfo("Reached home")
rospy.sleep(2)
global start
start = 2
rospy.Rate(5).sleep()
def execute_move_to_cb(self, goal):
# helper variables
#self._move_base_as_recfg_client.update_configuration({"yaw_goal_tolerance": 0.5, "xy_goal_tolerance": 0.5})
if goal.target_pose.header.frame_id != MAP_FRAME:
goal.target_pose = self.tf_listener.transformPose(
MAP_FRAME, goal.target_pose)
goal_2d = MoveBaseGoal()
goal_2d.target_pose.header = goal.target_pose.header
goal_2d.target_pose.pose.position.x = goal.target_pose.pose.position.x
goal_2d.target_pose.pose.position.y = goal.target_pose.pose.position.y
roll, pitch, yaw = t.euler_from_quaternion(
(goal.target_pose.pose.orientation.x,
goal.target_pose.pose.orientation.y,
goal.target_pose.pose.orientation.z,
goal.target_pose.pose.orientation.w))
x2d, y2d, z2d, w2d = t.quaternion_from_euler(0, 0, yaw)
rospy.loginfo("goal : {}".format(goal_2d.target_pose.pose.position))
rospy.loginfo("roll, pitch, yaw : {}\tquaternion2d : {}".format(
(roll, pitch, yaw), (x2d, y2d, z2d, w2d)))
goal_2d.target_pose.pose.orientation.x = x2d
goal_2d.target_pose.pose.orientation.y = y2d
goal_2d.target_pose.pose.orientation.z = z2d
goal_2d.target_pose.pose.orientation.w = w2d
# resp = self.toggle_human_monitor(False)
# if not resp.success:
# rospy.logwarn(NODE_NAME + " could not stop human monitoring, returning failure.")
# self.as_move_to.set_aborted(text="Could not stop monitoring")
# return
#begin_nav_fact = rospy.ServiceProxy(START_FACT_SRV_NAME, StartFact)
#resp = begin_nav_fact("base", "isNavigating(robot)", rospy.Time.now().to_sec(), False)
#if resp.success:
# self.move_to_fact_id = resp.fact_id
self.running = True
# rospy.info(self.move_base_as.status_list)
self.move_base_as.send_goal(goal_2d,
done_cb=self.done_move_to_cb,
feedback_cb=self.feedback_move_to_cb)
# self.go_to_stable_posture(["HipPitch","HipRoll","KneePitch"], [-0.071,0,-0.01], 0.1)
#r_pose, _ = self.tf_listener.lookupTransform(MAP_FRAME, FOOTPRINT_FRAME, rospy.Time(0))
#last_distance = math.hypot(goal_2d.target_pose.pose.position.x - r_pose[0],
# goal_2d.target_pose.pose.position.y - r_pose[1])
#last_decrease_time = rospy.Time.now()
while self.running:
if self.as_move_to.is_preempt_requested():
self.as_move_to.set_preempted()
self.move_base_as.cancel_all_goals()
self.running = False
return
#r_pose, _ = self.tf_listener.lookupTransform(MAP_FRAME, FOOTPRINT_FRAME, rospy.Time(0))
#d = math.hypot(goal_2d.target_pose.pose.position.x - r_pose[0],
# goal_2d.target_pose.pose.position.y - r_pose[1])
#rospy.loginfo_throttle(1, "[mummer_navigation] distance : {}, old_distance : {}".format(d, last_distance))
#if d < last_distance:
# last_decrease_time = rospy.Time.now()
# last_distance = d
#elif rospy.Time.now() - last_decrease_time >= rospy.Duration(900000):
# self.move_base_as.cancel_all_goals()
# self.need_final_rotation = True
# break
if self._feedback_move_to is not None:
self.as_move_to.publish_feedback(self._feedback_move_to)
self._feedback_rate.sleep()
def __init__(self):
# init node
rospy.init_node('pool_patrol', anonymous=False)
# Set the shutdown fuction (stop the robot)
rospy.on_shutdown(self.shutdown)
# Initilalize the mission parameters and variables
setup_task_environment(self)
# A variable to hold the last/current mission goal/state
self.last_mission_state = None
# A flag to indicate whether or not we are recharging
self.recharging = False
# Turn the target locations into SMACH MoveBase and LosPathFollowing action states
nav_terminal_states = {}
nav_transit_states = {}
# DP controller
for target in self.pool_locations.iterkeys():
nav_goal = MoveBaseGoal()
nav_goal.target_pose.header.frame_id = 'odom'
nav_goal.target_pose.pose = self.pool_locations[target]
move_base_state = SimpleActionState(
'move_base',
MoveBaseAction,
goal=nav_goal,
result_cb=self.nav_result_cb,
exec_timeout=self.nav_timeout,
server_wait_timeout=rospy.Duration(10.0))
nav_terminal_states[target] = move_base_state
# Path following
for target in self.pool_locations.iterkeys():
nav_goal = LosPathFollowingGoal()
#nav_goal.prev_waypoint = navigation.vehicle_pose.position
nav_goal.next_waypoint = self.pool_locations[target].position
nav_goal.forward_speed.linear.x = self.transit_speed
nav_goal.desired_depth.z = self.search_depth
nav_goal.sphereOfAcceptance = self.search_area_size
los_path_state = SimpleActionState(
'los_path',
LosPathFollowingAction,
goal=nav_goal,
result_cb=self.nav_result_cb,
exec_timeout=self.nav_timeout,
server_wait_timeout=rospy.Duration(10.0))
nav_transit_states[target] = los_path_state
""" Create individual state machines for assigning tasks to each target zone """
# Create a state machine container for the orienting towards the gate subtask(s)
self.sm_gate_tasks = StateMachine(outcomes=[
'found', 'unseen', 'missed', 'passed', 'aborted', 'preempted'
])
# Then add the subtask(s)
with self.sm_gate_tasks:
# if gate is found, pass pixel info onto TrackTarget. If gate is not found, look again
StateMachine.add('SCANNING_OBJECTS',
SearchForTarget('gate'),
transitions={
'found': 'CAMERA_CENTERING',
'unseen': 'BROADEN_SEARCH',
'passed': '',
'missed': ''
},
remapping={
'px_output': 'object_px',
'fx_output': 'object_fx',
'search_output': 'object_search',
'search_confidence_output':
'object_confidence'
})
StateMachine.add('CAMERA_CENTERING',
TrackTarget('gate',
self.pool_locations['gate'].position),
transitions={'succeeded': 'SCANNING_OBJECTS'},
remapping={
'px_input': 'object_px',
'fx_input': 'object_fx',
'search_input': 'object_search',
'search_confidence_input': 'object_confidence'
})
StateMachine.add('BROADEN_SEARCH',
TrackTarget('gate',
self.pool_locations['gate'].position),
transitions={'succeeded': 'SCANNING_OBJECTS'},
remapping={
'px_input': 'object_px',
'fx_input': 'object_fx',
'search_input': 'object_search',
'search_confidence_input': 'object_confidence'
})
# Create a state machine container for returning to dock
self.sm_docking = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
# Add states to container
with self.sm_docking:
StateMachine.add('RETURN_TO_DOCK',
nav_transit_states['docking'],
transitions={
'succeeded': 'DOCKING_SECTOR',
'aborted': '',
'preempted': 'RETURN_TO_DOCK'
})
StateMachine.add('DOCKING_SECTOR',
ControlMode(POSE_HEADING_HOLD),
transitions={
'succeeded': 'DOCKING_PROCEEDURE',
'aborted': '',
'preempted': ''
})
StateMachine.add('DOCKING_PROCEEDURE',
nav_terminal_states['docking'],
transitions={
'succeeded': '',
'aborted': '',
'preempted': ''
})
""" Assemble a Hierarchical State Machine """
# Initialize the HSM
self.sm_mission = StateMachine(outcomes=[
'succeeded', 'aborted', 'preempted', 'passed', 'missed', 'unseen',
'found'
])
# Build the HSM from nav states and target states
with self.sm_mission:
""" Navigate to GATE in TERMINAL mode """
StateMachine.add('TRANSIT_TO_GATE',
nav_transit_states['gate'],
transitions={
'succeeded': 'GATE_SEARCH',
'aborted': 'DOCKING',
'preempted': 'TRANSIT_TO_GATE'
})
""" When in GATE sector"""
StateMachine.add('GATE_SEARCH',
self.sm_gate_tasks,
transitions={
'passed': 'GATE_PASSED',
'missed': 'TRANSIT_TO_GATE',
'aborted': 'DOCKING'
})
""" Transiting to gate """
StateMachine.add('GATE_PASSED',
ControlMode(OPEN_LOOP),
transitions={
'succeeded': 'TRANSIT_TO_POLE',
'aborted': 'DOCKING',
'preempted': 'TRANSIT_TO_GATE'
})
StateMachine.add('TRANSIT_TO_POLE',
nav_transit_states['pole'],
transitions={
'succeeded': '',
'aborted': '',
'preempted': ''
})
""" When in POLE sector"""
#StateMachine.add('POLE_PASSING_TASK', sm_pole_tasks, transitions={'passed':'POLE_PASSING_TASK','missed':'RETURN_TO_DOCK','aborted':'RETURN_TO_DOCK'})
""" When aborted, return to docking """
StateMachine.add('DOCKING',
self.sm_docking,
transitions={'succeeded': ''})
# Register a callback function to fire on state transitions within the sm_mission state machine
self.sm_mission.register_transition_cb(self.mission_transition_cb,
cb_args=[])
# Initialize the recharge state machine
self.sm_recharge = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
# Add states to the container
with self.sm_recharge:
""" To be able to dock we have to be in the recharging area """
StateMachine.add('DOCKING',
self.sm_docking,
transitions={'succeeded': 'RECHARGE_BATTERY'})
""" Batteru is recharged by using the set_battery_level service to 100 percent battery level """
StateMachine.add('RECHARGE_BATTERY',
ServiceState(
'battery_simulator/set_battery_level',
SetBatteryLevel,
100,
response_cb=self.recharge_cb),
transitions={'succeeded': 'RECHARGE_FINISHED'})
""" when recharge is finished, we have to set the DP controller in open loop so we can use the transit controller """
StateMachine.add('RECHARGE_FINISHED',
ControlMode(OPEN_LOOP),
transitions={'succeeded': ''})
# Create the sm_concurrence state machine using a concurrence container
self.sm_concurrence = Concurrence(
outcomes=['succeeded', 'recharge', 'stop'],
default_outcome='succeeded',
child_termination_cb=self.concurrence_child_termination_cb,
outcome_cb=self.concurrence_outcome_cb)
# Add the sm_mission and a battery MonitorState to the sm_concurrence container
with self.sm_concurrence:
Concurrence.add('SM_MISSION', self.sm_mission)
Concurrence.add(
'MONITOR_BATTERY',
MonitorState("/manta/battery_level", Float32, self.battery_cb))
# Create the top level state machine
self.sm_top = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
# Add sm_concurrence, sm_recharge and a Stop() objective to sm_top
with self.sm_top:
StateMachine.add('PATROL',
self.sm_concurrence,
transitions={
'succeeded': '',
'recharge': 'RECHARGE',
'stop': 'STOP'
})
StateMachine.add('RECHARGE',
self.sm_recharge,
transitions={'succeeded': 'PATROL'})
StateMachine.add('STOP', Stop(), transitions={'succeeded': ''})
# Create and start the SMACH Introspection server
intro_server = IntrospectionServer(str(rospy.get_name()), self.sm_top,
'/SM_ROOT')
intro_server.start()
# Execute the state machine
hsm_outcome = self.sm_top.execute()
intro_server.stop()
