def verticalWallScenarioEasy():
# Create the occupancy grid
occupancyGrid = OccupancyGrid(21, 21, 1)
for y in xrange(1, 19):
occupancyGrid.setCell(11, y, 1)
start = (2, 18)
goal = (20, 0)
return start, goal, occupancyGrid
def horizontalWallScenario():
# Create the occupancy grid
occupancyGrid = OccupancyGrid(21, 21, 1)
for x in xrange(2, 20):
occupancyGrid.setCell(x, 11, 1)
start = (2, 2)
goal = (20, 20)
return start, goal, occupancyGrid
def test_map_loading():
# create an occupancy grid
grid_map = OccupancyGrid("../../resources/occ_maps/test_map.png", 0.01)
# create a plot axis
fig, ax = plt.subplots()
ax.set_aspect('equal')
ax.set_xlim(0, 7.5)
ax.set_ylim(0, 6)
# visualize map
grid_map.visualize(ax)
plt.show()
def emptyGridScenario():
occupancyGrid = OccupancyGrid(21, 21, 1)
start = (5, 10)
goal = (15, 10)
return start, goal, occupancyGrid
def __init__(self, parent=None):
super(App, self).__init__(parent)
#### Create Gui Elements ###########
self.mainbox = QtGui.QWidget()
self.setCentralWidget(self.mainbox)
self.mainbox.setLayout(QtGui.QVBoxLayout())
self.canvas = pg.GraphicsLayoutWidget()
self.mainbox.layout().addWidget(self.canvas)
self.label = QtGui.QLabel()
self.mainbox.layout().addWidget(self.label)
self.view = self.canvas.addViewBox()
self.view.setAspectLocked(True)
self.view.setRange(QtCore.QRectF(0, 0, 100, 100))
#### Set Data #####################
self.idx = 10
self.x = np.linspace(0, 50., num=100)
self.X, self.Y = np.meshgrid(self.x, self.x)
# image plot
self.img = pg.ImageItem(border='w')
self.turtlebot = TurtleBotItem(self.X[:, self.idx], 1.5)
self.view.addItem(self.img)
self.view.addItem(self.turtlebot)
self.counter = 0
self.fps = 0.
self.lastupdate = time.time()
# Variables used for creating and visualizing the map
self.grid = OccupancyGrid()
self.data = self.grid.map * 255
#### Start #####################
self._update()
def __init__(self, name, coord, dims):
self.time = 0
self.pos = coord
self.name = name
self.limits = dims
self.obs = None
self.S = OccupancyGrid(False, dims[0], dims[1])
self.D = OccupancyGrid(False, dims[0], dims[1])
self.T = OccupancyGrid(length=dims[0], width=dims[1])
self.C = 80.0
def __init__(self):
""" Intitialize map reading node. """
rospy.wait_for_service('static_map')
try:
static_map = rospy.ServiceProxy('static_map', GetMap)
resp = static_map()
map = resp.map
if map is not None:
print "========================="
print("MAP TYPE:,", type(map))
print "========================="
else:
print "no map :("
self.grid_map = OccupancyGrid(map)
self.info = map.info
except rospy.ServiceException, e:
print "Service call failed: %s" % e
class App(QtGui.QMainWindow):
def __init__(self, parent=None):
super(App, self).__init__(parent)
#### Create Gui Elements ###########
self.mainbox = QtGui.QWidget()
self.setCentralWidget(self.mainbox)
self.mainbox.setLayout(QtGui.QVBoxLayout())
self.canvas = pg.GraphicsLayoutWidget()
self.mainbox.layout().addWidget(self.canvas)
self.label = QtGui.QLabel()
self.mainbox.layout().addWidget(self.label)
self.view = self.canvas.addViewBox()
self.view.setAspectLocked(True)
self.view.setRange(QtCore.QRectF(0, 0, 100, 100))
#### Set Data #####################
self.idx = 10
self.x = np.linspace(0, 50., num=100)
self.X, self.Y = np.meshgrid(self.x, self.x)
# image plot
self.img = pg.ImageItem(border='w')
self.turtlebot = TurtleBotItem(self.X[:, self.idx], 1.5)
self.view.addItem(self.img)
self.view.addItem(self.turtlebot)
self.counter = 0
self.fps = 0.
self.lastupdate = time.time()
# Variables used for creating and visualizing the map
self.grid = OccupancyGrid()
self.data = self.grid.map * 255
#### Start #####################
self._update()
def _update(self):
if self.counter < params.x.shape[1]:
self.grid.updateMap2(params.x[:, self.counter],
params.z[:, :, self.counter])
self.turtlebot.setPose(params.x[:, self.counter])
self.counter += 1
self.data = self.grid.map * 255.0
self.img.setImage(self.data)
now = time.time()
dt = (now - self.lastupdate)
if dt <= 0:
dt = 0.000000000001
fps2 = 1.0 / dt
self.lastupdate = now
self.fps = self.fps * 0.9 + fps2 * 0.1
tx = 'Mean Frame Rate: {fps:.3f} FPS'.format(fps=self.fps)
self.label.setText(tx)
QtCore.QTimer.singleShot(1, self._update)
arg_parser.add_argument('--start_y', type=int, default=0)
arg_parser.add_argument('--robot_radius', type=float, default=0.8)
arg_parser.add_argument('--robot_yaw', type=float, default=0)
arg_parser.add_argument('--robot_sensor_range', type=float, default=7)
arg_parser.add_argument('--robot_sensor_view_angle',
type=float,
default=60)
arg_parser.add_argument('--wall_obseration_angle', type=float, default=70)
arg_parser.add_argument('--output', type=str, default="waypoints.txt")
arg_parser.add_argument('--show_plot', type=bool, default=True)
args = arg_parser.parse_args()
# Transform the provided 3D point cloud to a 2D grid representation.
pcl_3d = PointCloud3D(read_point_cloud_log("pcl/sas_pc_0.bin", 3, False))
pcl_2d = pcl_3d.to_2D(args.z_min, args.z_max)
map_grid = OccupancyGrid(pcl_2d, args.grid_resolution)
# Create a robot within the grid world.
robot = Robot(args.start_x, args.start_y, args.robot_radius,
args.robot_yaw, args.robot_sensor_range,
args.robot_sensor_view_angle)
# Plan a full coverage path for the robot to explore the gridworld.
planner = FrontierPlanner(map_grid, robot, args.wall_obseration_angle,
args.show_plot)
waypoints = planner.get_full_coverage_path()
# Write the planned waypoints out to a file for later use.
with open(args.output, "w") as f:
for p in waypoints:
f.write(",".join([str(v) for v in p]))
r1 = x
r2 = r1 + 3
c1 = y
c2 = y + 3
return env1[r1:r2, c1:c2]
if __name__ == "__main__":
"""
2 robot test in 10X10 world
"""
# Load world
with open('./worlds/10X10.json') as file:
maze = json.load(file)
# Initialize maze
dim = [maze['dim1'], maze['dim2']]
world = OccupancyGrid(True, dim[0], dim[1])
# Initialize robots
robots = []
for i in range(maze['n']):
name = i + 3
robots.append(Robot(name, maze['r' + str(i)], dim))
world.update_robot_position(name, maze['r' + str(i)], maze['r' + str(i)])
# Initialize static objects
for obj in maze['static']:
world.add_static_object(obj[0], obj[1])
# Initialize Dynamic Objects
for obj in maze['dynamic']:
print(obj)
world.add_dynamic_object(obj[0][0], obj[0][1], obj[1][0], obj[1][1])
# Update poses
for x in range(10):
def main():
# Instantiate obstacle manager
try:
s = rospy.Service('create_obstacle', obstacle_create_srv, handle_create_request)
rospy.init_node('runtime', anonymous=True)
tf_buffer = tf2_ros.Buffer()
tf_listener = tf2_ros.TransformListener(tf_buffer)
time.sleep(1)
cf_tf_pos = tf_buffer.lookup_transform("world", "cf", rospy.Time(0))
cf_pos = np.array([cf_tf_pos.transform.translation.x, cf_tf_pos.transform.translation.y]) # cf [x y] position from motive
# subprocess.call(["roslaunch", "crazyflie_examples","hw_hover.launch", "takeoff_hover_x:=" + str(cf_pos[0]), "takeoff_hover_y:=" + str(cf_pos[1]), "takeoff_hover_z:=1.5"])
except (rospy.ROSInterruptException, tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
rospy.loginfo("")
# rospy.logerr("No takeoff position info, using defaults...")
# subprocess.call(["roslaunch", "crazyflie_examples","hw_hover.launch"])
# Build occupancy grid
r = rospy.Rate(10)
occupancy_grid = OccupancyGrid()
occupancy_grid.Initialize()
turtlebot_tracker = TrackTurtlebot()
turtlebot_tracker.Initialize()
track_flag = False
# position_helper = PositionHelper()
prev_cf_pos = [0,0]
mydata = raw_input("Press any key to start: ")
rospy.wait_for_service('/takeoff')
subprocess.call(["rosservice", "call","/takeoff"])
returnGrid = np.array([0,0])
time.sleep(3)
while not rospy.is_shutdown():
try:
occupancy_grid.Update(obstacle_manager.getObstacles())
except rospy.ROSInterruptException:
rospy.loginfo("")
# Get current crazyflie grid pos to increase cost of returning to same grid
try:
mod_occupied_grid = occupancy_grid.getOccupancy()
# mod_occupied_grid[returnGrid[0]][returnGrid[1]] = 1
occupancy_grid.Visualize()
except rospy.ROSInterruptException:
rospy.loginfo("")
# Track turtlebot
try:
track_flag, returnGrid = turtlebot_tracker.Track(occupancy_grid, obstacle_manager, mod_occupied_grid)
except rospy.ROSInterruptException:
rospy.loginfo("")
if (track_flag == True):
break
r.sleep()
rospy.spin()
import json
import numpy as np
from robot import Robot
from occupancy_grid import OccupancyGrid
with open('.\\worlds\\10X10.json') as file:
maze = json.load(file)
# Initialize maze
dim = [maze['dim1'], maze['dim2']]
world = OccupancyGrid(True, dim[0], dim[1])
# Initialize robots
robots = []
for i in range(maze['n']):
name = i + 3
robots.append(Robot(name, maze['r' + str(i)], dim))
world.update_robot_position(name, maze['r' + str(i)],
maze['r' + str(i)])
# Initialize static objects
for i in maze['static']:
world.add_static_object(i[0], i[1])
world.show()
class Robot(object):
def __init__(self, name, coord, dims):
self.time = 0
self.pos = coord
self.name = name
self.limits = dims
self.obs = None
self.S = OccupancyGrid(False, dims[0], dims[1])
self.D = OccupancyGrid(False, dims[0], dims[1])
self.T = OccupancyGrid(length=dims[0], width=dims[1])
self.C = 80.0
def move_right(self):
## check if no static object
self.pos[1] = self.pos[1] + 1 if self.pos[
1] < self.limits[1] - 1 else self.pos[1]
return self.pos
def move_left(self):
self.pos[1] = self.pos[1] - 1 if self.pos[1] > 0 else self.pos[1]
return self.pos
def move_up(self):
self.pos[0] = self.pos[0] - 1 if self.pos[0] > 0 else self.pos[0]
return self.pos
def move_down(self):
self.pos[0] = self.pos[0] + 1 if self.pos[
0] < self.limits[0] - 1 else self.pos[0]
return self.pos
def setObs(self, observation):
self.obs = observation #3*3
list_global = {}
for i in range(3):
for j in range(3):
if observation[i][j] == -1:
continue
observation[i][
j] = 0 if observation[i][j] >= 3 else observation[i][j]
gx = self.pos[0]
gy = self.pos[1]
lx, ly = 1, 1
nx = gx - (lx - i)
ny = gy - (ly - j)
list_global[(nx, ny)] = observation[i][j]
for pose, obs in list_global.items():
# print(pose, obs)
# print(self.get_static_inv_sensor_model(pose, obs))
self.update_static_grid(
self.get_static_inv_sensor_model(pose, obs), pose)
self.update_dynamic_grid(
self.get_dynamic_inv_sensor_model(pose, obs), pose)
self.T.update(self.time, pose)
self.update_time()
def update_time(self):
observed = np.array(self.T.get_arr() > 0, np.int)
delT = np.full((self.limits[0], self.limits[1]),
self.time) * observed - self.T.get_arr()
delT = delT / self.C
self.T.mat = self.T.mat - delT
def merge(self, robot_S, robot_D, robot_T):
for i in range(self.limits[0]):
for j in range(self.limits[1]):
if robot_T[i, j] > self.T.get_arr()[i, j]:
self.S.update(robot_S[i, j], (i, j))
self.D.update(robot_D[i, j], (i, j))
self.T.update(robot_T[i, j], (i, j))
def get_position(self):
return self.pos
def get_static_inv_sensor_model(self, pose, obs):
"""
:param s_prob_val: The previous occupancy probability value at the (x,y) position
:param obs: The observation at a particular (x,y) location. 0: Free | 1: Occupied
:return: The inverse sensor model
"""
# print(self.S.get_arr())
s_prob_val = self.S.get_arr()[pose]
free_threshold = 0.45 # Probability below which position is certainly free
occupied_threshold = 0.55 # Probability above which position is certainly occupied
inv_sensor_low = 0.05
inv_sensor_high = 0.85
if s_prob_val <= free_threshold:
if obs:
return 0.01
else:
return inv_sensor_low # Free, Free and Free, Occupied
elif s_prob_val >= occupied_threshold:
if obs:
return inv_sensor_high # Occupied, Occupied
else:
return inv_sensor_low # Occupied, Free
else: # If unknown
if obs:
return inv_sensor_high # Unknown, Occupied
else:
return inv_sensor_low # Unknown, Free
def get_dynamic_inv_sensor_model(self, pose, obs):
"""
:param s_prob_val: The previous occupancy probability value at the (x,y) position
:param obs: The observation at a particular (x,y) location. 0: Free | 1: Occupied
:return: The inverse sensor model
"""
s_prob_val = self.S.get_arr()[pose]
free_threshold = 0.1 # Probability below which position is certainly free
occupied_threshold = 0.9 # Probability above which position is certainly occupied
inv_sensor_low = 0.05
inv_sensor_high = 0.99
if s_prob_val <= free_threshold:
if obs:
return inv_sensor_high # Free, Occupied
else:
return inv_sensor_low # Free, Free
elif s_prob_val >= occupied_threshold:
if obs:
return inv_sensor_low # Occupied, Free and Occupied, Occupied
else:
return 0.2
else: # If unknown
return inv_sensor_low # Unknown, Free and Unknown, Occupied
def update_static_grid(self, inv_sensor_model, pose):
prev_S = self.S.get_arr()[pose]
S_update = 0
for x, y in np.ndindex(self.S.get_arr().shape):
c = np.exp(
np.log(inv_sensor_model / (1 - inv_sensor_model)) +
np.log(prev_S / (1.00001 - prev_S)))
S_update = c / (1 + c)
self.S.update(S_update, pose)
def update_dynamic_grid(self, inv_sensor_model, pose):
prev_D = self.D.get_arr()[pose]
D_update = 0
for x, y in np.ndindex(self.D.get_arr().shape):
c = np.exp(
np.log(inv_sensor_model / (1 - inv_sensor_model)) +
np.log(prev_D / (1.00001 - prev_D)))
D_update = c / (1 + c)
self.D.update(D_update, pose)
def step(self, action):
self.time += 1
if action == "r":
return self.move_right()
elif action == "o":
return self.get_position()
elif action == "l":
return self.move_left()
elif action == "u":
return self.move_up()
elif action == "d":
return self.move_down()
def __init__(self):
# Build list of turn primitives.
assert self.NUM_TURN_DIVISIONS % 2 == 1
turn_angles = np.linspace(-self.MAX_TURN, self.MAX_TURN,
self.NUM_TURN_DIVISIONS)
np.testing.assert_almost_equal(
turn_angles[self.NUM_TURN_DIVISIONS / 2], 0.0)
turn_angles = sorted(turn_angles, key=lambda x: math.fabs(x))
rospy.loginfo("Making turn angles: {}".format(turn_angles))
self.primitives = [Turn_Primitive(angle) for angle in turn_angles]
self.primitives.extend([
Turn_Primitive(a * self.MAX_TURN_POSSIBLE,
total_time=self.MAX_TURN_TOTAL_TIME)
for a in [1, -1]
])
# Setup visualization publishers - latch them for easier debugging.
self.display_tree = []
self.display_path = []
graph_viz_topic = utils.getParamOrFail(
"/obstacle_avoidance/graph_viz_topic")
path_viz_topic = utils.getParamOrFail(
"/obstacle_avoidance/path_viz_topic")
goal_line_viz_topic = utils.getParamOrFail(
"/obstacle_avoidance/goal_line_viz_topic")
self.graph_pub = rospy.Publisher(graph_viz_topic,
Marker,
queue_size=10,
latch=True)
self.path_pub = rospy.Publisher(path_viz_topic,
Marker,
queue_size=10,
latch=True)
self.goal_line_pub = rospy.Publisher(goal_line_viz_topic,
Marker,
queue_size=3,
latch=True)
# Data structure and publisher for the waypoints (PolygonStamped).
self.line_trajectory = utils.LineTrajectory(
viz_namespace='visualization')
waypoint_topic = utils.getParamOrFail(
"/obstacle_avoidance/trajectory_topic")
self.waypoint_pub = rospy.Publisher(waypoint_topic,
PolygonStamped,
queue_size=10,
latch=True)
# Listen on /clicked_point for goal points.
# This will execute a plan from the current pose to specified goal.
self.clicked_point_sub = rospy.Subscriber("/clicked_point",
PointStamped,
self.clicked_point,
queue_size=10)
# Stores a list of points for collision checking.
# self.trajectory_plc = plc.Point2fList()
self.trajectory_list = []
self.current_trajectory_cost = None
# Listen on /initialpose for start and goal poses.
self.clicked_pose_sub = rospy.Subscriber("/initialpose",
PoseWithCovarianceStamped,
self.clicked_pose,
queue_size=10)
self.pose_update_sub = rospy.Subscriber(
utils.getParamOrFail("obstacle_avoidance/localization_topic"),
PoseStamped,
self.localization_cb,
queue_size=1)
# Setup occupancy grid.
# self.local_grid_sub = rospy.Subscriber(
# utils.getParamOrFail("obstacle_avoidance/local_map_topic"), OGMSG, self.local_map_cb, queue_size=5
# )
self.occupancy_grid_sub = rospy.Subscriber(
utils.getParamOrFail("obstacle_avoidance/dilated_map_topic"),
OGMSG,
self.occupancy_grid_cb,
queue_size=1)
self.occupancy_grid_msg = self.get_static_map(
) # Blocks until static map received.
# self.occupancy_grid = plc.OccupancyGrid(self.occupancy_grid_msg)
self.occupancy_grid = OccupancyGrid(self.occupancy_grid_msg)
self.occupancy_grid.dilateOccupancyGrid(0.2, True)
# Store state: (x, y, theta).
self.previous_pose = None
self.current_pose = None
self.goal_point = None # Set by the clicked point callback.
rospy.loginfo("Initialized Primitive Planner node!")
class Primitive_Planner:
# Primitive_Planner static params.
MAX_TURN = utils.getParamOrFail(
"/obstacle_avoidance/primitive_planner/max_turn_to_divide")
NUM_TURN_DIVISIONS = utils.getParamOrFail(
"/obstacle_avoidance/primitive_planner/num_turn_divisions")
TURNING_RADIUS = utils.getParamOrFail(
"/obstacle_avoidance/primitive_planner/turning_radius")
SAMPLING_RATE = utils.getParamOrFail(
"/obstacle_avoidance/primitive_planner/sampling_rate")
MAX_TURN_POSSIBLE = utils.getParamOrFail(
"/obstacle_avoidance/primitive_planner/max_turn_possible")
MAX_TURN_TOTAL_TIME = utils.getParamOrFail(
"/obstacle_avoidance/primitive_planner/max_turn_total_time")
def __init__(self):
# Build list of turn primitives.
assert self.NUM_TURN_DIVISIONS % 2 == 1
turn_angles = np.linspace(-self.MAX_TURN, self.MAX_TURN,
self.NUM_TURN_DIVISIONS)
np.testing.assert_almost_equal(
turn_angles[self.NUM_TURN_DIVISIONS / 2], 0.0)
turn_angles = sorted(turn_angles, key=lambda x: math.fabs(x))
rospy.loginfo("Making turn angles: {}".format(turn_angles))
self.primitives = [Turn_Primitive(angle) for angle in turn_angles]
self.primitives.extend([
Turn_Primitive(a * self.MAX_TURN_POSSIBLE,
total_time=self.MAX_TURN_TOTAL_TIME)
for a in [1, -1]
])
# Setup visualization publishers - latch them for easier debugging.
self.display_tree = []
self.display_path = []
graph_viz_topic = utils.getParamOrFail(
"/obstacle_avoidance/graph_viz_topic")
path_viz_topic = utils.getParamOrFail(
"/obstacle_avoidance/path_viz_topic")
goal_line_viz_topic = utils.getParamOrFail(
"/obstacle_avoidance/goal_line_viz_topic")
self.graph_pub = rospy.Publisher(graph_viz_topic,
Marker,
queue_size=10,
latch=True)
self.path_pub = rospy.Publisher(path_viz_topic,
Marker,
queue_size=10,
latch=True)
self.goal_line_pub = rospy.Publisher(goal_line_viz_topic,
Marker,
queue_size=3,
latch=True)
# Data structure and publisher for the waypoints (PolygonStamped).
self.line_trajectory = utils.LineTrajectory(
viz_namespace='visualization')
waypoint_topic = utils.getParamOrFail(
"/obstacle_avoidance/trajectory_topic")
self.waypoint_pub = rospy.Publisher(waypoint_topic,
PolygonStamped,
queue_size=10,
latch=True)
# Listen on /clicked_point for goal points.
# This will execute a plan from the current pose to specified goal.
self.clicked_point_sub = rospy.Subscriber("/clicked_point",
PointStamped,
self.clicked_point,
queue_size=10)
# Stores a list of points for collision checking.
# self.trajectory_plc = plc.Point2fList()
self.trajectory_list = []
self.current_trajectory_cost = None
# Listen on /initialpose for start and goal poses.
self.clicked_pose_sub = rospy.Subscriber("/initialpose",
PoseWithCovarianceStamped,
self.clicked_pose,
queue_size=10)
self.pose_update_sub = rospy.Subscriber(
utils.getParamOrFail("obstacle_avoidance/localization_topic"),
PoseStamped,
self.localization_cb,
queue_size=1)
# Setup occupancy grid.
# self.local_grid_sub = rospy.Subscriber(
# utils.getParamOrFail("obstacle_avoidance/local_map_topic"), OGMSG, self.local_map_cb, queue_size=5
# )
self.occupancy_grid_sub = rospy.Subscriber(
utils.getParamOrFail("obstacle_avoidance/dilated_map_topic"),
OGMSG,
self.occupancy_grid_cb,
queue_size=1)
self.occupancy_grid_msg = self.get_static_map(
) # Blocks until static map received.
# self.occupancy_grid = plc.OccupancyGrid(self.occupancy_grid_msg)
self.occupancy_grid = OccupancyGrid(self.occupancy_grid_msg)
self.occupancy_grid.dilateOccupancyGrid(0.2, True)
# Store state: (x, y, theta).
self.previous_pose = None
self.current_pose = None
self.goal_point = None # Set by the clicked point callback.
rospy.loginfo("Initialized Primitive Planner node!")
def localization_cb(self, msg):
"""
Called every time a new pose estimate is received.
"""
self.current_pose = (float(msg.pose.position.x),
float(msg.pose.position.y),
utils.quaternion_to_angle(msg.pose.orientation))
def update_path(self, path_pt_1, path_pt_2):
"""
Send a new path of waypoints to the controller (as PolygonStamped), and
publish a visualization of the path.
"""
rospy.loginfo('Updating path!')
# Reset the trajectories for publishing and collision checking.
self.line_trajectory.clear()
# self.trajectory_plc = plc.Point2fList()
self.trajectory_list
for node in path_pt_1:
for x, y, theta in node.poses:
pt = plc.Point2f(x, y)
self.line_trajectory.addPoint(pt)
# self.trajectory_plc.append(pt)
self.trajectory_list.append((x, y))
for x, y, theta in path_pt_2:
pt = plc.Point2f(x, y)
self.line_trajectory.addPoint(pt)
# self.trajectory_plc.append(pt)
self.trajectory_list.append((x, y))
# Send new path to controller.
self.waypoint_pub.publish(self.line_trajectory.toPolygon())
# Visualize the controller's new path.
color = [0.2, 0.3, 1.0]
marker = Marker()
marker.header = utils.make_header("/map")
marker.id = 106
marker.type = 5 # Line list.
marker.lifetime = rospy.Duration(0) # Stay forever.
marker.action = 0 # Add/update.
marker.scale.x = 0.1
marker.color.r = color[0]
marker.color.g = color[1]
marker.color.b = color[2]
marker.color.a = 0.5
for ii in range(len(self.line_trajectory.points) - 1):
marker.points.append(
Point32(self.line_trajectory.points[ii][0],
self.line_trajectory.points[ii][1], 0.0))
marker.points.append(
Point32(self.line_trajectory.points[ii + 1][0],
self.line_trajectory.points[ii + 1][1], 0.0))
self.path_pub.publish(marker)
def occupancy_grid_cb(self, msg, debug=False): #local_map_cb(self, msg):
"""
Called every time a new local map is received. Updates the occupancy grid and replans
if the current path results in a collision.
"""
# # Need a pose to transform local map into world map frame.
# if (self.current_pose is not None):
# self.occupancy_grid.dynamicUpdate(msg, self.current_pose[0], self.current_pose[1], self.current_pose[2])
# time.sleep(0.2)
self.occupancy_grid_msg = msg
# self.occupancy_grid = plc.OccupancyGrid(self.occupancy_grid_msg)
self.occupancy_grid.setGridData(self.occupancy_grid_msg.data)
if debug:
pub = rospy.Publisher('/debugdebug', OGMSG, queue_size=5)
pub.publish(self.occupancy_grid.getOccupancyGridMsg())
def main(self):
"""
Replans at a given frequency.
"""
target_rate = 20
target_time = 1.0 / target_rate
starttime = time.time()
while not rospy.is_shutdown():
# Replan at every map update (after goal set) to see if a better path can be found.
if (self.goal_point is not None):
starttime = time.time()
success, path_pt_1, path_pt_2, path_cost = self.plan_path(
self.current_pose, self.goal_point, visualize=True)
rospy.loginfo('Planned for %f sec' % (time.time() - starttime))
rospy.loginfo('Success: %d', success)
# Publish the new path if (1) none has been found yet, (2) it improves cost
# or (3) the current path causes a collision. Don't plan unless a goal point has
# been clicked.
if (self.goal_point is not None) and success:
shouldUpdatePath = (
(self.current_trajectory_cost is None)
or ((self.current_trajectory_cost - path_cost) > 1.0) or
# self.occupancy_grid.pathOccupiedVectPoint2f(self.trajectory_plc)
self.occupancy_grid.pathOccupied(self.trajectory_list))
isNone = (self.current_trajectory_cost is None)
pathDiff = float('-inf') if isNone else (
self.current_trajectory_cost - path_cost)
isShorter = (pathDiff > 5.0)
# isOccupied = self.occupancy_grid.pathOccupiedVectPoint2f(self.trajectory_plc)
isOccupied = self.occupancy_grid.pathOccupied(
self.trajectory_list)
print("isNone:", isNone, "pathDiff:", pathDiff, "isShorter:",
isShorter, "isOccupied:", isOccupied)
if shouldUpdatePath:
rospy.loginfo('Sending a new path to the controller!')
self.current_trajectory_cost = path_cost
self.update_path(path_pt_1, path_pt_2)
sleeptime = target_time - (time.time() - starttime)
time.sleep(0 if sleeptime < 0 else
sleeptime) # Sleep long enough to hit target rate.
def get_static_map(self):
"""
Blocks until a map is received on the ~static_map service.
Returns: a ROS Occupancy Grid message.
"""
map_service_name = rospy.get_param("~static_map", "static_map")
rospy.loginfo("Getting map from service: %s.", map_service_name)
rospy.wait_for_service(map_service_name)
map_msg = rospy.ServiceProxy(map_service_name, GetMap)().map
return map_msg
def clicked_pose(self, msg):
self.previous_pose = self.current_pose
self.current_pose = (msg.pose.pose.position.x,
msg.pose.pose.position.y,
utils.quaternion_to_angle(
msg.pose.pose.orientation))
rospy.loginfo('Current pose: %f %f %f' % self.current_pose)
def clicked_point(self, msg):
"""
Callback for /clicked_point from RViz. This will update the goal to be
the (x,y) location of the click.
"""
self.goal_point = (msg.point.x, msg.point.y, 0.0)
self.current_trajectory_cost = None # Mark that no trajectory has been found yet!
goalVect = np.array([
self.goal_point[0] - self.current_pose[0],
self.goal_point[1] - self.current_pose[1]
])
goalVect = goalVect / utils.norm(goalVect[0],
goalVect[1]) # Unit vector.
normal = (-1 * goalVect[1], goalVect[0])
scale = 100
endpoints = [(self.goal_point[0] + scale * normal[0],
self.goal_point[1] + scale * normal[1]),
(self.goal_point[0] - scale * normal[0],
self.goal_point[1] - scale * normal[1])]
marker = Marker()
marker.header = utils.make_header("/map")
marker.id = 104
marker.type = 5 # Line List.
marker.lifetime = rospy.Duration(0)
marker.scale.x = 0.1
marker.action = 0 # Add/update.
marker.color.g = 1.0
marker.color.a = 1.0
marker.points = [Point(pt[0], pt[1], 0) for pt in endpoints]
self.goal_line_pub.publish(marker)
rospy.loginfo('Updated goal point: %f %f %f' % self.goal_point)
def update_occupancy_grid(self, grid):
self.occupancy_grid = grid
def plan_path(self, start_pose, goal_x, visualize=True):
start_x, start_y, start_theta = start_pose
root = AnyNode(start_pose=None,
poses=[],
end_pose=start_pose,
cost=0.,
discovered=False)
success, (tree_leaf, dubins_path), cost = self.BFS(root,
self.goal_point,
visualize=visualize)
if success:
return success, tree_leaf.path, dubins_path, cost
else:
print 'Planning failed'
return False, None, None, None
def generate_and_check_dubins_curve(self, start, goal):
# shortest_path returns a tuple of sampled poses and cumulative distances
path, dists = dubins.shortest_path(
start, goal, self.TURNING_RADIUS).sample_many(self.SAMPLING_RATE)
total_dist = dists[-1]
# path_plc = plc.Point2fList()
# for x, y, theta in path:
# path_plc.append(plc.Point2f(x, y))
# path_is_occupied = self.occupancy_grid.pathOccupiedVectPoint2f(path_plc)
path_is_occupied = self.occupancy_grid.pathOccupied(path)
if path_is_occupied:
return None, None
return path, total_dist
def generate_children(self, node, visualize):
children = []
for i, primitive in enumerate(self.primitives):
new_pose, new_poses = primitive.apply(node.end_pose,
self.occupancy_grid)
if new_pose is not None: # If this connection does not collide.
children.append(
AnyNode(start_pose=node.end_pose,
poses=new_poses,
end_pose=new_pose,
primitive=primitive,
primitive_num=i,
cost=node.cost + primitive.TOTAL_TIME,
discovered=False,
parent=node))
if visualize:
for p1, p2 in zip(new_poses, new_poses[1:]):
self.display_tree.extend([p1, p2])
# TODO maybe choose children ordering according to a heuristic
if visualize:
self.visualize()
return children
def goal_line_check(self, test_pose, start_pt, goal_pt):
"""
Tests whether the test pose is past the goal line, which is defined as the
line passing through goal_pt that is perpendicular to the vector from start
to goal.
"""
goalVect = (goal_pt[0] - start_pt[0], goal_pt[1] - start_pt[1])
testVect = (test_pose[0] - start_pt[0], test_pose[1] - start_pt[1])
progVect = (testVect[0] - goalVect[0], testVect[1] - goalVect[1])
progress = progVect[0] * goalVect[0] + progVect[1] * goalVect[1]
return (progress > 0 or utils.distance(test_pose[0:2], goal_pt) < 0.1)
def BFS(self, root, goal_pt, visualize=False, with_dubins=False):
self.display_tree = []
# Store nodes in a min heap.
heap = [(root.end_pose[0], root)]
starttime = time.time()
timeout = 1.0
while len(heap) != 0 and (time.time() - starttime) < timeout:
# Gets the node with LOWEST cost.
_, node = heapq.heappop(heap)
if not node.discovered: # This check is probably unnecessary.
# If feasible path exists from this point onwards, returns goal = (goal_x, node.end_pose[1], 0.).
if with_dubins:
dubins_path, path_length = self.generate_and_check_dubins_curve(
node.end_pose, goal_pt)
if dubins_path is not None:
if visualize:
for p1, p2 in zip(dubins_path, dubins_path[1:]):
self.display_tree.extend([p1, p2])
self.visualize() # Show the tree.
return True, (node,
dubins_path), path_length + node.cost
# Alternatively, if the pose has passed the goal line, return success.
if self.goal_line_check(node.end_pose, self.current_pose,
goal_pt):
return True, (node, []), node.cost
node.discovered = True
for child in self.generate_children(node, visualize=visualize):
dist_to_goal = utils.distance(
child.end_pose,
goal_pt) # Cost = euclidean distance to goal.
heapq.heappush(heap, (dist_to_goal, child))
if (time.time() - starttime) >= timeout:
print 'Timed out'
_, node = heapq.heappop(heap)
return True, (node, []), node.cost
# Ran out nodes to try.
print 'Ran out of nodes'
return False, (None, None), None
def DFS(self, root, goal):
# self.visualize()
print "DFS level"
root.discovered = True
# if feasible path exists from this point onwards, returns
dubins_path, path_lengths = self.generate_and_check_dubins_curve(
root.end_pose, goal)
if dubins_path is not None:
print "Path to goal found"
for p1, p2 in zip(dubins_path, dubins_path[1:]):
self.display_tree.extend([p1, p2])
self.visualize()
return True, (root, dubins_path)
for child in self.generate_children(root):
if not child.discovered:
done, answer = self.DFS(child, goal)
if done:
return answer
return False, (None, None)
def visualize(self, duration=0.0, should_publish=True):
"""
Display the tree.
"""
red, green = (1., 0., 0.), (0., 1., 0.)
for (pub, linelist, color) in [(self.graph_pub, self.display_tree, red)
]:
# (self.path_pub, self.display_path, green)]:
marker = Marker()
marker.header = utils.make_header("/map")
marker.ns = 'visualization' + "/trajectory"
marker.id = 101
marker.type = 5 # Line list.
marker.lifetime = rospy.Duration.from_sec(duration)
if should_publish:
marker.action = 0
marker.scale.x = 0.1
marker.scale.y = 0.1
marker.scale.z = 0.1
marker.color.r = color[0] #1.0
marker.color.g = color[1] #0.0
marker.color.b = color[2] #0.0
marker.color.a = 0.5
for p in linelist:
pt = Point32(p[0], p[1], 0)
marker.points.append(pt)
else:
marker.action = 2
pub.publish(marker)
def controller(self):
try:
#TODO: get correct topic to publish to
pub = rospy.Publisher("/cmd_vel_mux/input/navi", Twist, queue_size=10)
tf_buffer = tf2_ros.Buffer()
tf_listener = tf2_ros.TransformListener(tf_buffer)
time.sleep(1)
vel_msg = Twist()
except (rospy.ROSInterruptException, tf2_ros.LookupException,
tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
rospy.loginfo("")
r = rospy.Rate(20)
occupancy_grid = OccupancyGrid()
occupancy_grid.Initialize()
grids_traversed = []
rot_angle = 0.5 * math.pi
while not rospy.is_shutdown():
try:
tb_tf_pos = tf_buffer.lookup_transform("world", "tb",
rospy.Time(0))
tb_pos = np.array([
tb_tf_pos.transform.translation.x,
tb_tf_pos.transform.translation.y
])
tb_grid = occupancy_grid.getGrid(
tb_pos) # tb grid number [x y] from occupancy_grid
tb_grid = tb_grid.tolist()
except (rospy.ROSInterruptException, tf2_ros.LookupException,
tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
rospy.loginfo("")
except Exception as e:
print(e)
if len(grids_traversed) == 5:
current_angle = 0
vel_msg.linear.x = 0
vel_msg.linear.y = 0
vel_msg.linear.z = 0
vel_msg.angular.x = 0
vel_msg.angular.y = 0
vel_msg.angular.z = 1
t0 = rospy.Time.now().to_sec()
while (current_angle < rot_angle):
pub.publish(vel_msg)
t1 = rospy.Time.now().to_sec()
current_angle = t1 - t0
vel_msg.angular.z = 0 #stop rotation
pub.publish(vel_msg)
del grids_traversed[:]
elif tb_grid in grids_traversed:
vel_msg.linear.x = 0.5
vel_msg.linear.y = 0
vel_msg.linear.z = 0
vel_msg.angular.x = 0
vel_msg.angular.y = 0
vel_msg.angular.z = 0
pub.publish(vel_msg)
else:
grids_traversed.append(tb_grid)
vel_msg.linear.x = 0.5
vel_msg.linear.y = 0
vel_msg.linear.z = 0
vel_msg.angular.x = 0
vel_msg.angular.y = 0
vel_msg.angular.z = 0
pub.publish(vel_msg)