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)