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 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 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)
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
break rospy.loginfo("Distance is: %s", distance) rospy.sleep(0.1) rospy.loginfo("Stop wall follower") wall.stop() wall_session = False session_state_msg = False start_session_pub.publish(session_state_msg) else: rospy.loginfo("Start navi") success = navigator.goto(obj['position'], obj['quaternion']) rospy.loginfo("Stop navi") if not success: rospy.loginfo("Failed to reach %s pose", name[:-4]) continue rospy.loginfo("Reached %s pose", name[:-4]) # Take a photo from kinnect if camera.take_picture(name): rospy.loginfo("Saved image " + name) else: rospy.loginfo("No images received") rospy.sleep(1)
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.loginfo("Reached %s pose", name[:-4]) # Take a photo if camera.take_picture(name): rospy.loginfo("Saved image " + name) else: rospy.loginfo("No images received") rospy.sleep(1) except rospy.ROSInterruptException: rospy.loginfo("Ctrl-C caught. Quitting")
start_session_pub.publish(session_state_msg) else: rospy.loginfo("Start navi") q = quaternion_from_euler(0, 0, math.radians(obj['yaw'])) q_dict = dict() q_dict["r1"] = q[0] q_dict["r2"] = q[1] q_dict["r3"] = q[2] q_dict["r4"] = q[3] success = navigator.goto(obj['position'], q_dict) rospy.loginfo("Stop navi") if not success: rospy.loginfo("Failed to reach %s pose", name[:-4]) continue rospy.loginfo("Reached %s pose", name[:-4]) # Take a photo from kinnect #if camera.take_picture(name): #rospy.loginfo("Saved image " + name) #else: #rospy.loginfo("No images received") rospy.sleep(1)
# 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: rospy.loginfo("Failed to reach %s pose", name[:-4]) continue rospy.loginfo("Reached %s pose", name[:-4]) # after reaching position, take pic and save img_path = rospy.get_param("img_path") print(img_path + name) if camera.take_picture(img_path + name): rospy.loginfo("Saved image " + name) else: rospy.loginfo("No images received") #load the saved image and identify, could do this in the photo node # or just send this to another node and do the identification else:
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)