def keyPressEvent(self, event:QKeyEvent):
if event.isAutoRepeat():
return
key = event.key()
if key == Qt.Key_G:
img = ObstacleMap(self.obstacles, GRAPH_TABLE_RATIO, self.robot.radius)
print("dumping")
img.dump_obstacle_grid_to_file("graph.txt")
elif key == Qt.Key_Ampersand:
self.ivy.send_action(1)
elif key == Qt.Key_Eacute:
self.ivy.send_action(2)
elif key == Qt.Key_QuoteDbl:
self.ivy.send_action(3)
elif key == Qt.Key_Apostrophe:
self.ivy.send_action(4)
elif key == Qt.Key_ParenLeft:
self.ivy.send_action(5)
elif key == Qt.Key_Minus:
self.ivy.send_action(6)
elif key == Qt.Key_Egrave:
self.ivy.send_action(7)
elif key == Qt.Key_Underscore:
self.ivy.send_action(8)
elif key == Qt.Key_Ccedilla:
self.ivy.send_action(9)
elif key == Qt.Key_Agrave:
self.ivy.send_action(10)
elif key == Qt.Key_ParenRight:
self.ivy.send_action(11)
elif key == Qt.Key_Equal:
self.ivy.send_action(12)
elif key == Qt.Key_Z:
self.robot_speed_command[1] = -1
self.ivy.send_speed_direction(self.robot_speed_command)
elif key == Qt.Key_Q:
self.robot_speed_command[0] = 1
self.ivy.send_speed_direction(self.robot_speed_command)
elif key == Qt.Key_S:
self.robot_speed_command[1] = 1
self.ivy.send_speed_direction(self.robot_speed_command)
elif key == Qt.Key_D:
self.robot_speed_command[0] = -1
self.ivy.send_speed_direction(self.robot_speed_command)
elif key == Qt.Key_A:
self.robot_speed_command[2] = 1
self.ivy.send_speed_direction(self.robot_speed_command)
elif key == Qt.Key_E:
self.robot_speed_command[2] = -1
self.ivy.send_speed_direction(self.robot_speed_command)
def __init__(self, level, bounding_box):
# type: (MCLevel, TransformBox) -> Maps
self.__width = bounding_box.size.x
self.__length = bounding_box.size.z
self.box = bounding_box
self.obstacle_map = ObstacleMap(self.__width, self.__length, self)
if level is not None:
self.height_map = compute_height_map(level, bounding_box)
else:
xmin, xmax = bounding_box.minx, bounding_box.maxx
zmin, zmax = bounding_box.minz, bounding_box.maxz
self.height_map = array([[0 for _ in range(zmin, zmax)]
for _ in range(xmin, xmax)])
self.road_network = RoadNetwork(self.__width,
self.__length,
mc_map=self)
self.fluid_map = FluidMap(self, level)
def __init__(self, robot):
#initliase the node
rospy.init_node('shared_control')
#set up obstacle map
self.robot = robot
self.obstacle_map = ObstacleMap(robot)
#set up locks
self.obstacle_map_lock = Lock()
self.js_lock = Lock()
self.laser_lock = Lock()
self.sonar_lock = Lock()
self.odom_lock = Lock()
self.polar_range_hist_lock = Lock()
#set up internal variables
self.curr_cmd = [0.0, 0.0] #0 linear, 0 angular
self.curr_vel = [0.0, 0.0] #not moving initially
self.sonar_readings = []
self.laser_readings = []
self.angles = []
self.cosangles = []
self.sinangles = []
#basic safeguarding
self.max_vel = 0.5
self.max_turn_vel = 0.5
#Forward simulation parameters
self.delay_time = 0.1 #max delay before command is issued
self.time_applied = 0.2 #how long the command is applied
self.time_decc = 0.5 #how long to compute deceleration
self.sim_interval = 0.1 #simulation interval
#VFH parameters
self.prh_smooth_window = 2
self.prh_resolution = 2.0 / 180.0 * np.pi
self.polar_range_hist = [0] * int(2.0 * np.pi / self.prh_resolution)
self.raw_polar_range_hist = [0] * int(
2.0 * np.pi / self.prh_resolution)
self.polar_range_hist_lock = Lock()
self.max_considered_dist = 2.0
self.closeness_weight = 0.5
self.free_zone_weight = 0.5
self.turning_coeff = 2.0
#set up subscribers
rospy.Subscriber('js_cmd_vel', Twist, self.cmdVelCallback)
rospy.Subscriber('base_scan', LaserScan, self.laserCallback)
rospy.Subscriber('sonar_pc', PointCloud, self.sonarCallback)
rospy.Subscriber('odom', Odometry, self.odomCallback)
#set up publishers
self.cmd_pub = rospy.Publisher('cmd_vel', Twist)
self.obs_pub = rospy.Publisher('obstacle', Marker)
self.polar_hist_pub = rospy.Publisher('polar_histogram', LaserScan)
self.zone_score_pub = rospy.Publisher('zone_scores', LaserScan)
self.projection_pub = rospy.Publisher('projection', PoseArray)
self.time_taken_pub = rospy.Publisher('sc_time_taken', Float32)
return
def build_graph(path_to_map, robot_radius):
obst_map = ObstacleMap(robot_radius)
obst_map.construct_obstacle_map(path_to_map)
graph = Graph(obst_map.obstacles, obst_map.map_dimensions, robot_radius)
graph.build_visibility_graph()
return graph
