def __init__(self, ref_costmap, update_width, update_height, d2, d1, w, blind_offset): # Robot shape: This is a rectangle, but we can use other stuff self.d2 = d2 # robot center to fork edge distance self.d1 = d1 # robot center to front laser distance self.w = w # truck width self.blind_offset = blind_offset # extra margin self.robotVerts = [ (self.d1 + self.blind_offset, self.w / 2 + self.blind_offset), (-self.d2 - self.blind_offset, self.w / 2 + self.blind_offset), (-self.d2 - self.blind_offset, -self.w / 2 - self.blind_offset), (self.d1 + self.blind_offset, -self.w / 2 - self.blind_offset) ] self.current_occ_grid = ref_costmap self.current_occ_grid.header.seq = -1 # these are for easiness... self.resolution = ref_costmap.info.resolution self.height = ref_costmap.info.height self.width = ref_costmap.info.width self.ox = self.current_occ_grid.info.origin.position.x self.oy = self.current_occ_grid.info.origin.position.y #MFC: layered costmap kind of assumes same position and orientation in all static maps... so DON'T use rotations! # self.oa = self.get_rotation(self.current_occ_grid.info.origin.orientation) # MFC: odd stuff happens if you use different static map sizes... # self.height = int(20.0 / self.resolution) #y # self.width = int(26.0 / self.resolution) #x # self.current_occ_grid.info.height = self.height # self.current_occ_grid.info.width = self.width # Translate to be centered under costmap_frame_id #MFC: layered costmap kind of assumes same position and orientation in all static maps... so DON'T use rotations! # self.current_occ_grid.info.origin = Pose() # self.current_occ_grid.info.origin.position.x = -(self.width/2.0) * self.resolution # self.current_occ_grid.info.origin.position.y = -(self.height/2.0) * self.resolution # clear data self.current_occ_grid.data = np.zeros(self.width * self.height) self.lock = Lock() self.local_human_pose = None self.local_robot_pose = None self.situation = None self.angle_bounds = np.array([0, 2.0 * np.pi]) # Updates will be our way of adding new data self.update = OccupancyGridUpdate() self.update.header = self.current_occ_grid.header self.update.header.seq = -1 #sergi self.update.width = self.width self.update.height = self.height self.update.x = 0 self.update.y = 0 self.update.data = np.zeros(self.update.width * self.update.height)
def costmap_updates_callback(self,msg): with self.lock: self.costmap_update = msg self.path_costmap_update = OccupancyGridUpdate() self.path_costmap_update.header.frame_id = msg.header.frame_id self.path_costmap_update.header.seq = msg.header.seq self.path_costmap_update.header.stamp = rospy.Time.now() self.path_costmap_update.width = msg.width self.path_costmap_update.height = msg.height self.path_costmap_update.x = msg.x self.path_costmap_update.y = msg.y self.path_costmap_update.data = np.zeros(self.path_costmap_update.width * self.path_costmap_update.height)
def __init__(self): self.data = GetMapResponse() a = OccupancyGridUpdate() rospy.Subscriber("/move_base/global_costmap/costmap", OccupancyGrid, self.callback) self.waitFlag = False while not self.waitFlag: pass self.waitFlag = False rospy.Subscriber("/move_base/global_costmap/costmap_update", OccupancyGridUpdate, self.callback2) s = rospy.Service('/map_service', GetMap, self.handleMapService) rospy.loginfo("Costmap service have been ready!") rospy.spin()
def map_update_to_message(self, update): ''' :type update: OccupancyGridUpdate ''' update_ros = OccupancyGridUpdateROS() # Fill in the header update_ros.header.stamp = rospy.Time.now() update_ros.header.frame_id = update.header.frame_id update_ros.x = update.x update_ros.y = update.y update_ros.width = update.width update_ros.height = update.height update_ros.data = update.data return update_ros
import rospy import cv2 import numpy as np import matplotlib.path as mpltPath from geometry_msgs.msg import PoseStamped, Quaternion, PointStamped, Point from nav_msgs.msg import OccupancyGrid from map_msgs.msg import OccupancyGridUpdate from move_base_msgs.msg import MoveBaseActionResult, MoveBaseActionFeedback # from matplotlib import pyplot as plt from scipy.interpolate import interp1d from tf.transformations import quaternion_from_euler from visualization_msgs.msg import Marker cost_update = OccupancyGridUpdate() result = MoveBaseActionResult() newPose = PoseStamped() q = Quaternion() class jackal_explore: def __init__(self): self.costmap = OccupancyGrid() self.flagm = 0 self.flagg = 0 self.init = 0 self.newcost = [] self.grid = OccupancyGrid() self.feedback = MoveBaseActionFeedback() self.prevpnt = -1
str((closest_x, closest_y, cost))) rospy.loginfo("radius 40 (would be 2m at 0.05 resolution)") rospy.loginfo("Getting closest x y over (look point: " + str((0, 0))) closest_x, closest_y, cost = ogm.get_closest_cell_over_cost(0, 0, 253, 5) rospy.loginfo("closest x y cost over: " + str((closest_x, closest_y, cost))) # Repeat again but with updateable occupancy grid rospy.loginfo("\n\n\n\nRepeating map test but subscribed to updates") ogm = OccupancyGridManager('/map', subscribe_to_updates=True) # Give a moment to initialize rospy.sleep(1.0) # simulate an update with adding all of it again ogu = OccupancyGridUpdate() ogu.header = map_msg.header ogu.header.stamp = rospy.Time.now() ogu.x = ogu.y = 0 ogu.width = map_msg.info.width ogu.height = map_msg.info.height ogu.data = map_msg.data update_pub.publish(ogu) # Wait a moment for the update to arrive rospy.sleep(1.0) # Check bounds are correct (e.g. x is x and y is y) # Note that X is height, and Y is width, both starting # from the bottom left corner print("map info: ")
cost = ogm.get_cost_from_costmap_x_y(x, y) print("x: {}, y: {} is in gridmap (cost: {})".format( x, y, cost)) else: print("x: {}, y: {} is NOT in gridmap".format(x, y)) if ogm.is_in_gridmap(ogm.width - 1, ogm.height - 1): print("ogm.width - 1: {}, ogm.height - 1: {} is in gridmap.".format( ogm.width - 1, ogm.height - 1)) else: print( "ogm.width - 1: {}, ogm.height - 1: {} is NOT in gridmap.".format( ogm.width - 1, ogm.height - 1)) # simulate an update with adding all of it again ogu = OccupancyGridUpdate() ogu.header = occgrid.header ogu.header.stamp = rospy.Time.now() ogu.x = ogu.y = 0 ogu.width = occgrid.info.width ogu.height = occgrid.info.height ogu.data = occgrid.data ogm = OccupancyGridManager('/map', subscribe_to_updates=True) # Give a moment to the update subscriber to initialize rospy.sleep(1.0) update_pub.publish(ogu) # Wait a moment for the update to arrive rospy.sleep(1.0)