def __init__(self, name):
self.name = name
# Navigator to send goals on map to turtlebot
self.navigator = GoToPose()
# Array of position objects. Each position is 2 long python dictionary with 'x' and 'y' keys with positions
self.positions = []
# TODO: Calculate quaternion based on previous point
self.quaternion = {'r1': 0.000, 'r2': 0.000, 'r3': 0.000, 'r4': 1.000}
# Publisher for points on map
self.publisher = rospy.Publisher(
"/visualization_marker", Marker, queue_size=10)
# Marker ids have to be different to show
self.marker_id_count = 0
self.database = Database()
# Create table in database for our map
self.database.create_table(self.name)
# Default map size in pixes can be found in rviz map launch file
self.map_size = 160
# Default map resolution in meters/pixel;
self.map_resolution = 0.05
# Between points on map to take photo in meters
self.photo_spacing = 0.5
def move_robot_to_place(self, position):
'''
this func move the robot into place. check the place coordinate and the make him walk there
'''
#TODO - while (robot not in place)
'''taken from: https://github.com/markwsilliman/turtlebot/blob/master/follow_the_route.py'''
try:
# Initialize
#rospy.init_node('follow_route', anonymous=False)
navigator = GoToPose()
# Navigation
rospy.loginfo("Go to %s pose", name[:-4])
success = navigator.goto(place['position'], place['quaternion'])
if success == False:
rospy.loginfo("Failed to reach %s pose", name[:-4])
return False
rospy.loginfo("Reached %s pose", position['x'], position['y'])
return True
#rospy.sleep(1)
except rospy.ROSInterruptException:
rospy.loginfo("Ctrl-C caught. Quitting")
return True
def __init__(self, name):
self.name = name
# Navigator to send goals on map to turtlebot
self.navigator = GoToPose()
# Array of position objects. Each position is 2 long python dictionary with 'x' and 'y' keys with positions
self.positions = []
# Array of quaternions
self.orientations = []
# TODO: Calculate quaternion based on previous point
# x y z w
self.quaternion = {'x': 0.000, 'y': 0.000, 'z': 0.000, 'w': 1.000}
# Publisher for points on map
self.publisher = rospy.Publisher("/visualization_marker",
Marker,
queue_size=10)
# Marker ids have to be different to show
self.marker_id_count = 0
self.database = Database()
# Create table in database for our map
self.database.create_table(self.name)
# Default map size in pixes can be found in rviz map launch file
self.map_size = StartRVIZ.get_map_size()
# Default map resolution in meters/pixel;
self.map_resolution = StartRVIZ.get_map_resolution()
# Between points on map to take photo in meters
self.photo_spacing = StartRVIZ.get_photo_spacing()
# Save locatio for map
self.map_path = "/home/darebalogun/Desktop/Turtlebot/turtlebot/frontend-webapp/maps/" + self.name
self.template_path = "/home/darebalogun/Desktop/Turtlebot/turtlebot/frontend-webapp/templates/" + \
self.name + "_template"
def __init__(self):
rospy.init_node('car_detection', anonymous=True)
self.navigator = GoToPose()
self.camera = TakePhoto()
self.rate_command_navigation = rospy.Rate(1. / 2.)
self.pose_subscriber = rospy.Subscriber\
('/amcl_pose', PoseWithCovarianceStamped, self.__update_pose__)
self.rate = rospy.Rate(5)
self.pose = Pose()
# Allow up to one second to connection
rospy.sleep(1)
def callback(self, data):
print "callback"
#Print the contents of the message to the console
print "Message:", data.x, data.y, "Received at:", rospy.get_time()
success = False
navigator = GoToPose()
success = navigator.goto(data.x, data.y)
if (success):
self.pub.publish(String("success"))
print("success")
else:
self.pub.publish(String("failure"))
print("failure")
# if success:
# rospy.loginfo("Hooray, reached the desired pose")
# else:
# rospy.loginfo("The base failed to reach the desired pose")
# Sleep to give the last log messages time to be sent
rospy.sleep(1)
def patrol(self):
if not self.num_spots:
return
navigator = GoToPose()
attempts = 0
for i in xrange(self.num_spots):
curr_spot = get_spot(i)
name = curr_spot['filename']
success = navigator.goto(curr_spot['position'],
curr_spot['quaternion'])
while not success and attempts < 10:
rospy.loginfo("%i attempt failed to reach %s pose", attempts,
name[:-4])
noised_pos = curr_spot['position']
noised_pos['x'] += random.choice(sys.float_info.epsilon,
-sys.float_info.epsilon)
noised_pos['y'] += random.choice(sys.float_info.epsilon,
-sys.float_info.epsilon)
success = navigator.goto(noised_pos, curr_spot['quaternion'])
attempts += 1
if not success:
return
def __init__(self):
rospy.init_node('car_detection', anonymous=True)
self.navigator = GoToPose()
self.camera = TakePhoto()
self.car_detection_subscriber = rospy.Subscriber(
'/camera/rgb/image_raw', Image, self.car_detection)
self.rate_car_detection = rospy.Rate(
1. / 120.) #1/120 Hz, very slow for debugging
self.rate_command_navigation = rospy.Rate(1. / 2.)
self.pose_subscriber = rospy.Subscriber\
('/amcl_pose', PoseWithCovarianceStamped, self.__update_pose__)
self.rate = rospy.Rate(5)
self.pose = Pose()
# Allow up to one second to connection
rospy.sleep(1)
try:
rospy.init_node('follow_route', anonymous=False)
rospy.Subscriber('/amcl_pose', PoseWithCovarianceStamped, amcl_pose_callback)
rospy.Subscriber("/odom", Odometry, odometry_callback)
start_session_pub = rospy.Publisher('/photo/start_session', Bool, queue_size=10)
session_state_msg = Bool()
session_num_pub = rospy.Publisher('/photo/session_num', UInt32, queue_size=10)
session_num_msg = UInt32()
rack_id_pub = rospy.Publisher('/photo/rack_id', UInt32, queue_size=10)
rack_id_msg = UInt32()
navigator = GoToPose()
camera = TakePhoto()
wall = WallFollow(LEFT)
wall.stop()
for obj in dataMap:
if rospy.is_shutdown():
break
name = obj['filename']
session_num_msg = obj['session']
rack_id_msg = obj['rackid']
rospy.loginfo("Go to %s pose of rack %s", name[:-4], rack_id_msg)
import rospy
import yaml
from take_photo import TakePhoto
from go_to_specific_point_on_map import GoToPose
if __name__ == '__main__':
# Read information from yaml file
with open("route.yaml", 'r') as stream:
dataMap = yaml.load(stream)
try:
# Initialize
rospy.init_node('follow_route', anonymous=False)
navigator = GoToPose()
camera = TakePhoto()
for obj in dataMap:
if rospy.is_shutdown():
break
name = obj['filename']
# Navigation
rospy.loginfo("Go to %s pose", name[:-4])
success = navigator.goto(obj['position'], obj['quaternion'])
if not success:
rospy.loginfo("Failed to reach %s pose", name[:-4])
continue
rospy.Subscriber("/odom_raw", Odometry, odometry_callback)
start_session_pub = rospy.Publisher('/photo/start_session',
Bool,
queue_size=10)
session_state_msg = Bool()
session_num_pub = rospy.Publisher('/photo/session_num',
UInt32,
queue_size=10)
session_num_msg = UInt32()
rack_id_pub = rospy.Publisher('/photo/rack_id', UInt32, queue_size=10)
rack_id_msg = UInt32()
navigator = GoToPose()
#camera = TakePhoto()
wall = WallFollow(LEFT)
wall.stop()
for obj in dataMap:
if rospy.is_shutdown():
break
name = obj['filename']
session_num_msg = obj['session']
rack_id_msg = obj['rackid']
rospy.loginfo("Go to %s pose of rack %s", name[:-4], rack_id_msg)
pass
print("publie")
pub_goal.publish(p_map)
#success = navigator.goto(pos, quat)
### Main
if __name__ == '__main__':
global detect
global time_last_detection
global navigator
global i
navigator = GoToPose()
detect = False
follow = False
time_last_detection = rospy.Time(0)
model = PinholeCameraModel()
model.fromCameraInfo(cam_info)
id_check = ID_Check()
rospy.init_node('navigate')
pub_goal = rospy.Publisher('move_base_simple/goal',
PoseStamped,
queue_size=10)
tf_listener = tf.TransformListener()
cmd_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
rate = rospy.Rate(3)
import rospy
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
import actionlib
from actionlib_msgs.msg import *
from geometry_msgs.msg import Pose, Point, Quaternion
from go_to_specific_point_on_map import GoToPose
if __name__ == '__main__':
try:
navigator = GoToPose()
position = {'x': 11.8, 'y': -10.5}
quaternion = {'r1': 0.000, 'r2': 0.000, 'r3': 0.000, 'r4': 1.000}
rospy.loginfo("Go to (%s, %s) pose", position['x'], position['y'])
success = navigator.goto(position, quaternion)
if success:
rospy.loginfo("Hooray, reached the desired pose")
else:
rospy.loginfo("The base failed to reach the desired pose")
# Sleep to give the last log messages time to be sent
rospy.sleep(1)
except rospy.ROSInterruptException:
rospy.loginfo("Ctrl-C caught. Quitting")
class MapPoints:
'''
Class represents set of points of interest through which the turtlebot will travel.
The turtlebot will also take a 360 photo at each photo and save images to database
'''
def __init__(self, name):
self.name = name
# Navigator to send goals on map to turtlebot
self.navigator = GoToPose()
# Array of position objects. Each position is 2 long python dictionary with 'x' and 'y' keys with positions
self.positions = []
# TODO: Calculate quaternion based on previous point
self.quaternion = {'r1': 0.000, 'r2': 0.000, 'r3': 0.000, 'r4': 1.000}
# Publisher for points on map
self.publisher = rospy.Publisher(
"/visualization_marker", Marker, queue_size=10)
# Marker ids have to be different to show
self.marker_id_count = 0
self.database = Database()
# Create table in database for our map
self.database.create_table(self.name)
# Default map size in pixes can be found in rviz map launch file
self.map_size = 160
# Default map resolution in meters/pixel;
self.map_resolution = 0.05
# Between points on map to take photo in meters
self.photo_spacing = 0.5
def callback(self, data):
"""
callback function is called whenever there a publish on the /clicked_point topic i.e whenever a published point
is added in rviz.
Parameters
----------
data :
Information on which publish triggered the callback. In this case the data on the clicked point
"""
rospy.loginfo("Point : " + str(data.point.x) + ' ' + str(data.point.y))
# Interpolate between points of interest
while True:
# TODO get starting point of robot here instead of (0,0)
if not self.positions:
a = (0, 0)
else:
a = (self.positions[-1]['x'], self.positions[-1]['y'])
b = (data.point.x, data.point.y)
midx, midy = splitpoints.get_split_point(a, b, self.photo_spacing)
if not (a[0] <= midx <= b[0] or b[0] <= midx <= a[0]):
position = {'x': data.point.x, 'y': data.point.y}
self.positions.append(position)
self.add_marker(position)
break
position = {'x': midx, 'y': midy}
self.positions.append(position)
self.add_marker(position)
def listener(self):
"""
Start listening for published points on the map
"""
rospy.loginfo(
"Please click points of interest on map in order! Press Enter when done!")
# Create separate thread to listen for done
check_done = Thread(target=self.check_for_done, args=())
check_done.daemon = True
check_done.start()
# Subscribe to the /clicked_point topic
rospy.Subscriber("/clicked_point", PointStamped, self.callback)
rospy.spin()
def check_for_done(self):
"""
Wait for the user to press enter to indicate done then process map and save info in database
"""
usr_input = raw_input()
rospy.loginfo("Saving Map...")
os.system(
"gnome-terminal -x rosrun map_server map_saver -f /home/darebalogun/Desktop/Turtlebot/turtlebot/frontend-webapp/maps/" + self.name)
rospy.sleep(3)
# Convert to png and save as png and save to db
Image.open("../frontend-webapp/maps/" + self.name +
".pgm").save("../frontend-webapp/maps/" + self.name + ".png")
self.database.add_map(self.name, "maps/" + self.name + ".png")
# Launch self navigation node
os.system("gnome-terminal -x roslaunch turtlebot3_navigation turtlebot3_navigation.launch map_file:=/home/darebalogun/Desktop/Turtlebot/turtlebot/frontend-webapp/maps/" + self.name + ".yaml")
rospy.loginfo("Estimate Initial Pose! Press Enter When Done")
rospy.sleep(3)
# Publish markers on navigation node map
self.add_marker_array()
# Wait for user to press enter when done then navigate
usr_input = raw_input()
self.perform_navigation()
def perform_navigation(self):
"""
Autonomous navigation to every position saved in self.positions
"""
for position in self.positions:
success = self.navigator.goto(position, self.quaternion)
if success:
rospy.loginfo("Hooray, reached the desired pose")
else:
rospy.loginfo("The base failed to reach the desired pose")
# TODO Add image capture, processing and storage code here
rospy.sleep(1)
def add_marker(self, position):
"""
Adds marker to the plot, and to the database
Parameters
----------
position : dict
'x' : x coordinate
'y' : y coordinate
"""
marker = Marker()
marker.header.frame_id = "map"
marker.header.stamp = rospy.Time.now()
marker.type = marker.SPHERE
marker.action = marker.ADD
marker.scale.x = 0.1
marker.scale.y = 0.1
marker.scale.z = 0.1
marker.color.a = 1.0
marker.color.r = 1.0
marker.pose.orientation.w = 1.0
marker.pose.position.x = position['x']
marker.pose.position.y = position['y']
marker.id = self.marker_id_count
self.marker_id_count += 1
self.publisher.publish(marker)
# ROS coordinates have origin in centre of map, so we need to map to a coordinate system with origin in top left
coordx = position['x']
coordy = position['y']
mappedx = position['x']/self.map_resolution + self.map_size/2
mappedy = -1*position['y']/self.map_resolution + self.map_size/2
# Add both sets of coordinates to database with image path
# TODO get image path from camera capture module
self.database.add_coordinate(
self.name, coordx, coordy, "images/360.jpg", mappedx, mappedy)
def add_marker_array(self):
"""
Add all markers
"""
for position in self.positions:
self.add_marker(position)
class MapPoints:
'''
Class represents set of points of interest through which the turtlebot will travel.
The turtlebot will also take a 360 photo at each photo and save images to database
'''
def __init__(self, name):
self.name = name
# Navigator to send goals on map to turtlebot
self.navigator = GoToPose()
# Array of position objects. Each position is 2 long python dictionary with 'x' and 'y' keys with positions
self.positions = []
# Array of quaternions
self.orientations = []
# TODO: Calculate quaternion based on previous point
# x y z w
self.quaternion = {'x': 0.000, 'y': 0.000, 'z': 0.000, 'w': 1.000}
# Publisher for points on map
self.publisher = rospy.Publisher("/visualization_marker",
Marker,
queue_size=10)
# Marker ids have to be different to show
self.marker_id_count = 0
self.database = Database()
# Create table in database for our map
self.database.create_table(self.name)
# Default map size in pixes can be found in rviz map launch file
self.map_size = StartRVIZ.get_map_size()
# Default map resolution in meters/pixel;
self.map_resolution = StartRVIZ.get_map_resolution()
# Between points on map to take photo in meters
self.photo_spacing = StartRVIZ.get_photo_spacing()
# Save locatio for map
self.map_path = "/home/darebalogun/Desktop/Turtlebot/turtlebot/frontend-webapp/maps/" + self.name
self.template_path = "/home/darebalogun/Desktop/Turtlebot/turtlebot/frontend-webapp/templates/" + \
self.name + "_template"
def get_location(self, data):
self.x = data.pose.pose.position.x
self.y = data.pose.pose.position.y
self.orientation = data.pose.pose.orientation
def callback(self, data):
"""
callback function is called whenever there a publish on the /clicked_point topic i.e whenever a published point
is added in rviz.
Parameters
----------
data :
Information on which publish triggered the callback. In this case the data on the clicked point
"""
#rospy.loginfo("Point : " + str(data.point.x) + ' ' + str(data.point.y))
'''
# Interpolate between points of interest
while True:
# TODO get starting point of robot here instead of (0,0)
if not self.positions:
a = (self.x,
self.y)
pose = {'x': self.orientation.x, 'y': self.orientation.y,
'z': self.orientation.z, 'w': self.orientation.w}
else:
a = (self.positions[-1]['x'], self.positions[-1]['y'])
pose = self.orientations[-1]
b = (data.point.x, data.point.y)
midx, midy = splitpoints.get_split_point(a, b, self.photo_spacing)
if not (a[0] <= midx <= b[0] or b[0] <= midx <= a[0]):'''
if not self.positions:
a = (0, 0)
else:
a = (self.positions[-1]['x'], self.positions[-1]['y'])
position = {'x': data.point.x, 'y': data.point.y}
self.positions.append(position)
self.add_marker(position)
angle = -1 * \
anglebtwnpoints.getangle(a, (position['x'], position['y']))
rotation = pyquaternion.Quaternion(axis=[0.0, 0.0, 1.0], radians=angle)
new_pose = {
'x': rotation.elements[1],
'y': rotation.elements[2],
'z': rotation.elements[3],
'w': rotation.elements[0]
}
# print(angle)
self.orientations.append(new_pose)
'''
break
position = {'x': midx, 'y': midy}
self.positions.append(position)
self.add_marker(position)
angle = -1 * \
anglebtwnpoints.getangle(a, (position['x'], position['y']))
rotation = pyquaternion.Quaternion(
axis=[0.0, 0.0, 1.0], radians=angle)
new_pose = {'x': rotation.elements[1], 'y': rotation.elements[2],
'z': rotation.elements[3], 'w': rotation.elements[0]}
print(angle)
self.orientations.append(new_pose)
'''
def listener(self):
"""
Start listening for published points on the map
"""
# Create separate thread to listen for done
# check_done = Thread(target=self.check_for_done, args=())
# check_done.daemon = True
# check_done.start()
# Subscribe to the /clicked_point topic
rospy.Subscriber("/clicked_point", PointStamped, self.callback)
rospy.Subscriber("/odom", Odometry, self.get_location)
rospy.spin()
def check_for_done(self):
"""
Wait for the user to press enter to indicate done then process map and save info in database
"""
rospy.loginfo("Saving Map...")
os.system("gnome-terminal -x rosrun map_server map_saver -f " +
self.map_path)
rospy.sleep(3)
# Save all points to db
self.save_all_points_db()
# Convert to png and save as png and save to db
Image.open("../../frontend-webapp/maps/" + self.name +
".pgm").save("../../frontend-webapp/maps/" + self.name +
".png")
self.database.add_map(self.name, "maps/" + self.name + ".png")
rospy.loginfo("Map Creation phase complete!")
def perform_localization(self):
# Get template
self.get_template()
rospy.loginfo("Performing localization...")
rospy.sleep(5)
max_score, max_score_loc, rot_angle = localization.localize(
self.map_path + ".pgm", self.template_path + ".pgm")
rospy.loginfo("max_score: " + str(max_score))
rospy.loginfo("location: " + str(max_score_loc))
rospy.loginfo("angle: " + str(rot_angle))
angle_rad = rot_angle * math.pi / 180
rotation = pyquaternion.Quaternion(axis=[0.0, 0.0, 1.0],
radians=angle_rad)
quat = Quaternion(rotation[1], rotation[2], rotation[3], rotation[0])
# Launch self navigation node
os.system(
"gnome-terminal -x roslaunch turtlebot3_navigation turtlebot3_navigation.launch map_file:=/home/darebalogun/Desktop/Turtlebot/turtlebot/frontend-webapp/maps/"
+ self.name + ".yaml")
rospy.loginfo("Estimating Initial Pose...")
rospy.sleep(3)
# Publish location from localization back to rviz /initialpose
pose_publisher = rospy.Publisher("/initialpose",
PoseWithCovarianceStamped,
queue_size=10)
poseWCS = PoseWithCovarianceStamped()
poseWCS.header.frame_id = "map"
poseWCS.header.stamp = rospy.Time.now()
poseWCS.pose.pose.position.x = (
max_score_loc[0] - self.map_size / 2) * self.map_resolution
poseWCS.pose.pose.position.y = -1 * \
(max_score_loc[1] - self.map_size/2) * self.map_resolution
poseWCS.pose.pose.position.z = 0
poseWCS.pose.pose.orientation = quat
poseWCS.pose.covariance = [
0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.25, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.06853891945200942
]
rate = rospy.Rate(1)
pose_publisher.publish(poseWCS)
rate.sleep()
pose_publisher.publish(poseWCS)
rate.sleep()
pose_publisher.publish(poseWCS)
# Publish markers on navigation node map
self.add_marker_array()
def perform_navigation(self):
"""
Autonomous navigation to every position saved in self.positions
"""
x = 0
for position in self.positions:
# TODO update quaternions
success = self.navigator.goto(position, self.orientations[x])
if success:
rospy.loginfo("Hooray, reached the desired pose")
else:
rospy.loginfo("The base failed to reach the desired pose")
# TODO Add image capture, processing and storage code here
message_client.send("capture", self.name)
# rospy.sleep(1)
x = x + 1
# TODO Add code to rename and transfer data
message_client.send("save_all_images", self.name)
def add_marker(self, position):
"""
Adds marker to the plot, and to the database
Parameters
----------
position : dict
'x' : x coordinate
'y' : y coordinate
"""
marker = Marker()
marker.header.frame_id = "map"
marker.header.stamp = rospy.Time.now()
marker.type = marker.SPHERE
marker.action = marker.ADD
marker.scale.x = 0.1
marker.scale.y = 0.1
marker.scale.z = 0.1
marker.color.a = 1.0
marker.color.r = 1.0
marker.pose.orientation.w = 1.0
marker.pose.position.x = position['x']
marker.pose.position.y = position['y']
marker.id = self.marker_id_count
self.marker_id_count += 1
self.publisher.publish(marker)
def save_point(self, position, index):
"""
Save point to database
"""
# ROS coordinates have origin in centre of map, so we need to map to a coordinate system with origin in top left
coordx = position['x']
coordy = position['y']
mappedx = position['x'] / self.map_resolution + self.map_size / 2
mappedy = -1 * position['y'] / self.map_resolution + self.map_size / 2
# Add both sets of coordinates to database with image path
# TODO get image path from camera capture module
self.database.add_coordinate(self.name, coordx, coordy,
"images/image" + str(index + 1) + ".jpg",
mappedx, mappedy)
def add_marker_array(self):
"""
Add all markers
"""
for position in self.positions:
self.add_marker(position)
def save_all_points_db(self):
"""
Save all points to db
"""
for index, position in enumerate(self.positions):
self.save_point(position, index)
rospy.loginfo("Points saved to database!")
def get_template(self):
"""
Get image of immediate surroundings
"""
rospy.loginfo("Getting template...")
StartRVIZ.start_rviz()
time.sleep(8)
rospy.loginfo("Saving Template...")
os.system("gnome-terminal -x rosrun map_server map_saver -f " +
self.template_path)
import rospy
import yaml
from go_to_specific_point_on_map import GoToPose
if __name__ == '__main__':
# Read information from yaml file
try:
routedatafile = rospy.get_param('/smoke/nav/route')
except KeyError:
routedatafile = './../config/jun05.yaml'
with open(routedatafile, 'r') as stream:
dataMap = yaml.load(stream)
rospy.init_node('follow_route', anonymous=False)
navigator = GoToPose()
pose_ind = 0
while (not rospy.is_shutdown()):
name = dataMap[pose_ind]['filename']
# Navigation
rospy.loginfo("Go to %s pose", name[:-4])
success = navigator.goto(dataMap[pose_ind]['position'],
dataMap[pose_ind]['quaternion'])
if not success:
rospy.loginfo("Failed to reach %s pose", name[:-4])
continue
rospy.loginfo("Reached %s pose", name[:-4])
rospy.sleep(1)
pose_ind += 1
if (pose_ind == len(dataMap)):
pose_ind = 0
if __name__ == '__main__':
import sys
if len(sys.argv) < 2:
print("<Usage> python main.py path_to_input_points.yaml")
exit(1)
INPUT_POINTS_PATH = sys.argv[1]
room_1 = RoomOne(INPUT_POINTS_PATH)
room_2 = RoomTwo(INPUT_POINTS_PATH)
try:
rospy.init_node('move_to_point', anonymous=True)
navigator = GoToPose()
room = room_1
if moveBasedOnRedOrGreen("entrance", room):
cI = colourIdentifier()
while True:
if cI.red_circle_detected:
rospy.loginfo("RED")
red = True
room = room_2
cI = None
if moveBasedOnRedOrGreen("entrance", room):
cI2 = colourIdentifier()
if cI2.green_circle_detected:
rospy.loginfo("GREEN")
if __name__ == '__main__':
# Read route information from yaml file
route = rospy.get_param("route")
print(route)
with open(route, 'r') as stream:
dataMap = yaml.load(stream)
#print('dataMap size: ', len(dataMap))
state = 0
manual_control = False
try:
# Initialize
rospy.init_node('follow_route', anonymous=False)
navigator = GoToPose()
camera = TakePhoto()
rospy.Subscriber("manual_pose", Float32MultiArray, callback, navigator)
while (not rospy.is_shutdown()):
print('testpoint C, manual_control: ', manual_control)
if (not manual_control):
# go to next position, length of dataMap is the number of state
state = (state + 1) % len(dataMap)
state_data = dataMap[state]
name = state_data['filename']
rospy.loginfo("Go to %s pose", name[:-4])
success = navigator.goto(state_data['position'],
state_data['quaternion'])
if not success:
