diff --git a/launch/downlidar.launch b/launch/downlidar.launch new file mode 100644 index 0000000..209a450 --- /dev/null +++ b/launch/downlidar.launch @@ -0,0 +1,5 @@ + + + + + \ No newline at end of file diff --git a/launch/frontlidar.launch b/launch/frontlidar.launch new file mode 100644 index 0000000..46d6ee9 --- /dev/null +++ b/launch/frontlidar.launch @@ -0,0 +1,5 @@ + + + + + \ No newline at end of file diff --git a/launch/lidar.launch b/launch/lidar.launch index 6debf74..27ba475 100644 --- a/launch/lidar.launch +++ b/launch/lidar.launch @@ -1,5 +1,4 @@ - - - - \ No newline at end of file + + + diff --git a/launch/lidarvis.launch b/launch/lidarvis.launch new file mode 100644 index 0000000..629e641 --- /dev/null +++ b/launch/lidarvis.launch @@ -0,0 +1,9 @@ + + + + + + + + diff --git a/launch/localization.launch b/launch/localization.launch index aea7212..346a8ad 100644 --- a/launch/localization.launch +++ b/launch/localization.launch @@ -1,13 +1,12 @@ - - - - - - - - - - - + + + + + + + + + + diff --git a/rviz/lidarvis.rviz b/rviz/lidarvis.rviz new file mode 100644 index 0000000..09fed7b --- /dev/null +++ b/rviz/lidarvis.rviz @@ -0,0 +1,137 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + Splitter Ratio: 0.5 + Tree Height: 712 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679016 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.0299999993 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /LidarFront_pt + Name: Marker + Namespaces: + "": true + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /TractorLine_pt + Name: Marker + Namespaces: + "": true + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /LidarLineDown_pt + Name: Marker + Namespaces: + {} + Queue Size: 100 + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: laser + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 10 + Enable Stereo Rendering: + Stereo Eye Separation: 0.0599999987 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.0500000007 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.00999999978 + Pitch: 0.630398154 + Target Frame: + Value: Orbit (rviz) + Yaw: 3.14539981 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 993 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000016a00000357fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000357000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000357fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000357000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000004fb0000035700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1920 + X: 0 + Y: 24 diff --git a/scripts/LidarCodeAdvanced.py b/scripts/LidarCodeAdvanced.py new file mode 100755 index 0000000..5f2e2f0 --- /dev/null +++ b/scripts/LidarCodeAdvanced.py @@ -0,0 +1,250 @@ +#!/usr/bin/env python +###################################################################### + # KUBO Safety Behavior Node (Teensy 3.5) + # @file LidarCodeAdvanced.py + # @author: Nathan Estill + # @email: nathan.estill@students.olin.edu + # @version: 2.0 + # + # Sensing with the Lidar, detecting obstacles, + # publishing True to estop if an obstacle is seen + ###################################################################### +import rospy +from math import * +from std_msgs.msg import Bool +from sensor_msgs.msg import LaserScan +from visualization_msgs.msg import Marker +from geometry_msgs.msg import Twist, Vector3, Point, Quaternion +from std_msgs.msg import ColorRGBA, Header +from ackermann_msgs.msg import AckermannDrive + + +class ObstacleDetection(): + + def __init__(self): + # Variables explained in Github Wiki: https://github.com/olinrobotics/gravl/wiki/Vehicle-Safety + rospy.init_node('ObstacleDetection', anonymous=True) + self.hasSensedFront = False # If there is an obstacle sensed + self.hasSensedDown = False + self.pubEstop = rospy.Publisher('/softestop', Bool, queue_size=10) # Publisher that publishes to the estop + self.frontlaserSub = rospy.Subscriber('/front/scan',LaserScan,self.frontLidarCB) # Subscribes to the front LIDAR, calls the callback function + self.downLaserSub = rospy.Subscriber('/down/scan',LaserScan,self.downLidarCB) # Subscribes to the down Lidar, calls the cb function + self.ackermannSub = rospy.Subscriber('/cmd_vel',AckermannDrive,self.ackermannCB) # Subscribes to the tractors velocity commands + self.frontData = None + self.downData = None + self.ackermannData = None + self.DownToGround = 1.2 # Distance from down lidar to the ground + self.senseRange = float(rospy.get_param('~senseRange','5.0')) + self.mode = rospy.get_param('~mode',"circle") + self.widthTractor = 1.25 # Horizontal length of the tractor + self.fbWheelDist = 1.524 # Measures the distance from the front wheels to the back wheels + self.angleOfIncline = 0.194724 # ?Empirically Discovered. Fix if necessary + self.threshold = 5 # How many points must be seen to trigger a stop? + self.update_rate = rospy.Rate(5) # Sets the update rate to 5 + self.vis_pubFront = rospy.Publisher('/LidarFront_pt', Marker, queue_size=2) + self.vis_pubDown = rospy.Publisher('/LidarLineDown_pt', Marker, queue_size=2) + self.vis_pubLines = rospy.Publisher('/TractorLine_pt',Marker,queue_size=2) + + + def frontLidarCB(self,data): + # Collects data from the Front Hokuyo Lidar and stores it + self.frontData = data + + def downLidarCB(self,data): + # Collects data from the Down Hokuyo Lidar and stores it + self.downData = data + + def ackermannCB(self,data): + # Collects data from the Wheels and stores it + self.ackermannData = data + + def convertToXDistAndYDistFront(self): + self.totalDistFront = [] # Initializing arrays + self.xDistFront = [] # Distance from front of tractor + self.yDistFront = [] # Distance from center of tractor + self.zDistFront = [] # Distance from the tractor to the point, (usually the ground) + dataPoints = len(self.frontData.ranges) + angleSweep = 190.0 + for i in range(len(self.frontData.ranges)): # Puts the tuple of data into x and y Distances + self.totalDistFront.append(self.frontData.ranges[i]) + angleRad = radians((i - dataPoints / 2) * (angleSweep / dataPoints)) + self.xDistFront.append(abs(cos(self.angleOfIncline) * cos(angleRad) * self.totalDistFront[i])) # Computes the distance from the object + self.yDistFront.append(sin(angleRad) * self.totalDistFront[i]) # Computes the distance parallel to tractor + self.zDistFront.append(sin(self.angleOfIncline) * cos(angleRad) * self.totalDistFront[i]) + + def getNumberOfObstaclesFront(self,visualize): + # Calculates the number of points that pose a threat to the tractor + if self.ackermannData == None: # If the wheel data cannot be found, assume the tractor is pointing forward + wheelAngle = 0 + else: + wheelAngle = self.ackermannData.steering_angle + self.obstaclePointsFront = 0 # Counts how many points are not the ground + self.triggerPointsFront = 0 # Counts number of points breaking threshold + if self.mode == "line" or wheelAngle == 0: # If the mode is set to check a straight line in front of the tractor + for i in range(len(self.totalDistFront)): # Sweep through the distances + if(sqrt(self.xDistFront[i]**2 + self.yDistFront[i]**2) < self.senseRange): # Is there an object that is not the ground? + self.obstaclePointsFront += 1 # Add a point into the number of obstacle points + if((sin(wheelAngle) * self.xDistFront[i] - self.widthTractor / 2.0) < self.yDistFront[i] < sin(wheelAngle) * self.xDistFront[i] + self.widthTractor / 2.0): #Will the obstacle hit the tractor? + self.triggerPointsFront += 1 # Add a point the the number of triggers + elif self.mode == "circle": # If the mode is set to check a curved path in front of the tractor + r = self.fbWheelDist / sin(wheelAngle) # sets the radius of the path of the tractor. Equation is a / sin(theta) + if(wheelAngle > 0): # if the wheel angle is to the left, the inner radius is the left one, elsewise, the right radius is inner + rInner = self.fbWheelDist / sin(wheelAngle) - self.widthTractor / 2 # radius slightly different from the one above + rOuter = self.fbWheelDist / sin(wheelAngle) + self.widthTractor / 2 + elif(wheelAngle < 0): + rInner = self.fbWheelDist / sin(wheelAngle) + self.widthTractor / 2 + rOuter = self.fbWheelDist / sin(wheelAngle) - self.widthTractor / 2 + for i in range(len(self.totalDistFront)): + if sqrt(self.xDistFront[i]**2 + self.yDistFront[i]**2) < self.senseRange: # checks if the point is within the sense range + self.obstaclePointsFront += 1 + if (rInner**2 < self.xDistFront[i]**2 + (self.yDistFront[i]+r)**2 < rOuter**2): # checks if the point is in the path of the tractor + self.triggerPointsFront += 1 + + if visualize: + lidarPoints = Marker() # Marker to visualize used lidar pts + lidarPoints.header.frame_id = "laser" # Publishes it to the laser link, idk if it should be changed + lidarPoints.header.stamp = rospy.Time.now() + lidarPoints.type = 8 # makes it a list of points + lidarPoints.scale = Vector3(0.02,0.02,0.02) # scale is about 2 cm + lidarPoints.color = ColorRGBA(0,0,1,1) # Color is blue + for i in range(len(self.totalDistFront)): + lidarPoints.points.append(Point(self.xDistFront[i],self.yDistFront[i],-self.zDistFront[i])) + self.vis_pubFront.publish(lidarPoints) + + def sendMessagesFront(self): + # If the number of trigger points is greater than the threshold, send a singal message to the tractor + if(self.triggerPointsFront > self.threshold and not self.hasSensedFront): # if there is an obstacle that will hit the tractor + # stop the tractor + self.pubEstop.publish(True) + self.hasSensedFront = True + if(self.triggerPointsFront <= self.threshold and self.hasSensedFront): + # don't stop the tractor + self.pubEstop.publish(False) + self.hasSensedFront = False + + def convertToYDistAndZDistDown(self): + self.totalDistDown = [] # Initializing arrays + self.zDistDown = [] # Distance from front of tractor + self.yDistDown = [] # Distance from center of tractor + dataPoints = len(self.downData.ranges) + angleSweep = 270.0 + for i in range(len(self.downData.ranges)): # Puts the tuple of data into x and y Distances + self.totalDistDown.append(self.downData.ranges[i]) + angleRad = radians((i - dataPoints / 2) * (angleSweep / dataPoints)) + self.zDistDown.append(cos(angleRad) * self.totalDistDown[i]) # Computes the distance from the object + self.yDistDown.append(sin(angleRad) * self.totalDistDown[i]) # Computes the distance parallel to tractor + + def getNumberOfObstaclesDown(self,visualize): + # Calculates the number of points that pose a threat to the tractor + self.obstaclePointsDown = 0 # Counts how many points are not the ground + self.triggerPointsDown = 0 # Counts number of points breaking threshold + for i in range(len(self.totalDistDown)): # Sweep through the distances + if(self.zDistDown[i] < self.DownToGround and self.zDistDown[i] > 0.1): # Is there an object that is not the ground? + self.obstaclePointsDown += 1 # Add a point into the number of obstacle points + if(abs(self.yDistDown[i]) < (self.widthTractor / 2.0)): #Will the obstacle hit the tractor? + self.triggerPointsDown += 1 # Add a point the the number of triggers + if visualize: + lidarPoints = Marker() # Marker to visualize used lidar pts + lidarPoints.header.frame_id = "laser" # Publishes it to the laser link, idk if it should be changed + lidarPoints.header.stamp = rospy.Time.now() + lidarPoints.type = 8 # makes it a list of points + lidarPoints.scale = Vector3(0.02,0.02,0.02) # scale is about 2 cm + lidarPoints.color = ColorRGBA(0,0,1,1) # Color is Boolue + for i in range(len(self.totalDistDown)): + lidarPoints.points.append(Point(0,self.yDistDown[i],-self.zDistDown[i])) + self.vis_pubDown.publish(lidarPoints) + + def sendMessagesDown(self): + # If the number of trigger points is greater than the threshold, send a singal message to the tractor + if(self.triggerPointsDown > self.threshold and not self.hasSensedDown): # if there is an obstacle that will hit the tractor + # stop the tractor + self.pubEstop.publish(True) + self.hasSensedDown = True + if(self.triggerPointsDown <= self.threshold and self.hasSensedDown): + # don't stop the tractor + self.pubEstop.publish(False) + self.hasSensedDown = False + + def publishTractorLines(self): + # publishes the path of the tractor going through space, purely for visualization. + tractorLines = Marker() # marks the path that the tractor will take + tractorLines.header.frame_id = "laser" + tractorLines.header.stamp = rospy.Time.now() + tractorLines.scale = Vector3(0.02, 0.01, 0.01) + tractorLines.color = ColorRGBA(1,1,1,1) # white + if self.mode == "line": # checks the mode the tractor is in + tractorLines.type = 5 # makes the markers a line that is on every 2 points, (line from 0-1, 2-3, 4-5, etc.) + if self.ackermannData == None: + wheelAngle = 0 # if teh tractor isn't getting data, it will make a path straight forward + else: + wheelAngle = self.ackermannData.steering_angle + tractorLines.points.append(Point(self.senseRange * cos(wheelAngle),self.widthTractor / 2 + sin(wheelAngle) * self.senseRange,0)) # makes a straight line in the direction of the wheels + tractorLines.points.append(Point(0,self.widthTractor / 2,0)) + tractorLines.points.append(Point(0,-self.widthTractor / 2,0)) + tractorLines.points.append(Point(self.senseRange * cos(wheelAngle),-self.widthTractor / 2 + sin(wheelAngle) * self.senseRange,0)) + self.vis_pubLines.publish(tractorLines) + elif self.mode == "circle": + if self.ackermannData == None or self.ackermannData.steering_angle == 0.0: # does the same as above + tractorLines.type = 5 # makes the markers a line that is on every 2 points, (line from 0-1, 2-3, 4-5, etc.) + tractorLines.points.append(Point(100,self.widthTractor / 2,0)) + tractorLines.points.append(Point(-100,self.widthTractor / 2,0)) + tractorLines.points.append(Point(-100,-self.widthTractor / 2,0)) + tractorLines.points.append(Point(100,-self.widthTractor / 2,0)) + else: + wheelAngle = self.ackermannData.steering_angle + tractorLines.type = 4 # makes the markers a line that is on every point, (line from 0-1, 1-2, 2-3, 3-4, 4-5, etc.) + x = 0.0 # initializes x, y, t + y = 0.0 + t = 0.0 + r = self.fbWheelDist / sin(wheelAngle) + while x**2 + y**2 < self.senseRange**2 and t < pi / 2: # stops at the sense range or if the angle gets above 90 degrees + rOuter = self.fbWheelDist / sin(wheelAngle) + self.widthTractor / 2 + x = abs(rOuter * sin(t)) # parametric equation for a circle + y = rOuter * cos(t) - r + if x**2 + y**2 < self.senseRange **2: + tractorLines.points.append(Point(x,y,0)) + t += 0.01 + while t >= -0.01: + rInner = self.fbWheelDist / sin(wheelAngle) - self.widthTractor / 2 + x = abs(rInner * sin(t)) + y = rInner * cos(t) - r + if(x**2 + y**2 < self.senseRange**2): + tractorLines.points.append(Point(x,y,0)) + t -= 0.01 + self.vis_pubLines.publish(tractorLines) + + def run(self,visualize = False): + # Runs the code + while not rospy.is_shutdown() and self.frontData == None and self.downData == None: + rospy.logwarn("ERR: Missing data: /front/scan and /down/scan") + while not rospy.is_shutdown() and self.frontData == None and self.downData != None: + rospy.logwarn("ERR: Missing data: /front/scan (Front Lidar Not Connected)") + self.convertToYDistAndZDistDown() + self.getNumberOfObstaclesDown(visualize) + self.sendMessagesDown() + if visualize: + self.publishTractorLines() + self.update_rate.sleep() + while not rospy.is_shutdown() and self.downData == None and self.frontData != None: + rospy.logwarn("ERR: Missing data: /down/scan (Down Lidar Not Connected)") + self.convertToXDistAndYDistFront() + self.getNumberOfObstaclesFront(visualize) + self.sendMessagesFront() + if visualize: + self.publishTractorLines() + self.update_rate.sleep() + rospy.loginfo("Running") + while not rospy.is_shutdown(): + self.convertToXDistAndYDistFront() + self.getNumberOfObstaclesFront(visualize) + self.sendMessagesFront() + self.convertToYDistAndZDistDown() + self.getNumberOfObstaclesDown(visualize) + self.sendMessagesDown() + if visualize: + self.publishTractorLines() + self.update_rate.sleep() + +if __name__ == '__main__': + obs = ObstacleDetection() + obs.run(True) \ No newline at end of file diff --git a/scripts/test_ackermann.py b/scripts/test_ackermann.py new file mode 100755 index 0000000..815e76d --- /dev/null +++ b/scripts/test_ackermann.py @@ -0,0 +1,30 @@ +#!/usr/bin/env python + +import rospy +import math +from std_msgs.msg import Bool +from sensor_msgs.msg import LaserScan +from visualization_msgs.msg import Marker +from geometry_msgs.msg import Twist, Vector3, Point, Quaternion +from std_msgs.msg import ColorRGBA, Header +from ackermann_msgs.msg import AckermannDrive + +class AckTest(): + + def __init__(self): + rospy.init_node('AckTest', anonymous=True) + self.ackPub = rospy.Publisher("/cmd_vel",AckermannDrive,queue_size=10) + self.update_rate = rospy.Rate(5) + + def run(self): + while not rospy.is_shutdown(): + ack = AckermannDrive() + for i in range(-70,70,1): + ack.steering_angle = i / 100.0 + self.ackPub.publish(ack) + self.update_rate.sleep() + + +if __name__ == '__main__': + obs = AckTest() + obs.run() \ No newline at end of file