def __init__(self):
rospy.init_node('lane_detection')
hz = 30
self.rate = rospy.Rate(hz)
self.image_sub = rospy.Subscriber('camera_feed', Image,
self.imageCallback)
self.display = rospy.get_param('~lane_display')
self.ready = False #TODO: Implement some kind of check that the camera is workin
self.img = 0
self.msgread = {'image': False}
self.bridge = CvBridge()
self.num_col = 21
self.num_row = 10
self.num_ele = self.num_col * self.num_row
self.start_pos = [self.num_row - 1, (self.num_col - 1) / 2]
self.node_thresh = 0.5 #This is the threshold for driveable and not driveable nodes. Is between 0 and 1
println('Lane detection running')
def callback(msg_data):
global imu_data, velocity_x, velocity_y, velocity_z, velocity_magnitude, velocity_pub, distance_pub
imu_data = msg_data
println("Data {}".format(msg_data))
if msg_data.type == 'imu':
velocity_x, velocity_y, velocity_z, velocity_magnitude = calculate_velocity(msg_data)
distance_x, distance_y, distance_z, distance_magnitude = calculate_distance(msg_data)
velocity_pub.send(velocity_x, velocity_y, velocity_z, velocity_magnitude)
distance_pub.send(distance_x, distance_y, distance_z, distance_magnitude)
println("Velocity: x: " + str(velocity_x) + ', y: ' + str(velocity_y) + ', z: ' + str(velocity_z) + ', mag: ' + str(velocity_magnitude))
println("Distance: x: " + str(distance_x) + ', y: ' + str(distance_y) + ', z: ' + str(distance_z) + ', mag: ' + str(distance_magnitude))
def callback(msg_data):
global gyroscope_data
gyroscope_data = msg_data
println("Data {}".format(msg_data))
#!/usr/bin/env python
'''
A test script to act as a publisher from the ros_api.
Last Updated: 31 Jan 2019
Author: Isaac
'''
import std_msgs.msg as std_msgs
import custom_msgs.msg as msgs
import ros_api as ros
from ros_api import println, is_running, sleep
if __name__ == '__main__':
println('Starting test pkg')
ros.init_node('pub_node')
pub1 = ros.Publisher('test_topic', msgs.coord)
pub2 = ros.Publisher('another_topic', msgs.coord)
while is_running():
# pub1.send(32,10,500)
# pub1.send(x=10,y=32,z=40)
m = ros.json_to_msg('{"x":5,"y":2,"z":14}', msgs.coord)
pub1.send(m)
pub2.send(32, 102, 1)
sleep(1)
def custom_callback(data):
#println('Recieved custom: {}'.format(data))
println(ros.msg_to_dict(data))
#!/usr/bin/env python
'''
A test script to recieve data using ros_api
Last Updated: 31 Jan 2019
Author: Isaac
'''
import std_msgs.msg as std_msgs
import custom_msgs.msg as msgs
import ros_api as ros
from ros_api import println, spin
def custom_callback(data):
#println('Recieved custom: {}'.format(data))
println(ros.msg_to_dict(data))
if __name__ == '__main__':
println('Starting test revieve')
ros.init_node('test')
sub1 = ros.Subscriber('test_topic', msgs.coord, call=custom_callback)
sub2 = ros.Subscriber('another_topic', msgs.coord)
spin()
println('Node finished with no errors')
img = self.img
# println(np.max(img),np.min(img))
if self.ready and self.display:
node_scores, processed_img = self.GetDriveableNodes(img)
list_of_nodes, list_of_edges = self.buildAlgorithmStructures(
node_scores)
end_pos = [0, (self.num_col - 1) / 2]
path = pathfinding.search(list_of_nodes, list_of_edges,
self.start_pos, end_pos, 'A*')
path_img = self.plot_path(processed_img, path)
# rospy.loginfo(path)
if self.display:
cv2.imshow("input", path_img)
else:
pass
else:
pass
key = cv2.waitKey(10)
if key == 27:
break
if __name__ == '__main__':
println('Starting lane detection node')
lane_detector = LaneDetector()
while is_running():
rospy.sleep(1)
lane_detector.execute()
println('Node finished with no errors')
def callback(msg_data):
global compass_data
compass_data = msg_data
println("Data {}".format(msg_data))
