From 42181d67f30d82b6839477f050c29013e3be601d Mon Sep 17 00:00:00 2001 From: nathanestill Date: Sat, 22 Sep 2018 16:56:21 -0400 Subject: [PATCH 01/12] added LidarCodeAdvanced --- scripts/LidarCodeAdvanced.py | 61 ++++++++++++++++++++++++++++++++++++ 1 file changed, 61 insertions(+) create mode 100644 scripts/LidarCodeAdvanced.py diff --git a/scripts/LidarCodeAdvanced.py b/scripts/LidarCodeAdvanced.py new file mode 100644 index 0000000..961c763 --- /dev/null +++ b/scripts/LidarCodeAdvanced.py @@ -0,0 +1,61 @@ +#!/usr/bin/env python + +import rospy +import math +from std_msgs.msg import Bool +from sensor_msgs.msg import LaserScan +class ObstacleDetection(): + + def __init__(self): + rospy.init_node('Obstacle Detection', anonymous=True) + self.hasSensed = False + self.pubEstop = rospy.Publisher('/softestop', Bool, queue_size=10) + self.laserSub = rospy.Subscriber('scan',LaserScan,self.callback) # Subscribes to the LIDAR, calls the callback function + self.DistanceToTheGround = 4.5 # Essentially the ground *** + self.lengthOfTheTractor = 1.25 # Horizontal length of the tractor *** + self.numberOfPointsNeededToTrigger = 15 # How many points must be seen to trigger a stop? *** + + def callback(self,data): + self.data = data #calling below code + + def convertToVerticalAndHorizontal(self): + self.totalDist = [] # Initializing arrays + self.verticalDistance = [] # Distance from front of tractor + self.horizontalDistance = [] # Distance from center of tractor + for i in range(len(self.data.ranges)): + self.totalDist.append(self.data.ranges[i]) + if self.totalDist[i] > 1000000: + self.totalDist[i] = 100000 + self.verticalDistance.append(abs(math.cos(math.radians((i - 380.0) * (190.0 / 760.0))) * self.totalDist[i])) # Computes the distance from the object + self.horizontalDistance.append(math.sin(math.radians((i - 380.0) * (190.0 / 760.0))) * self.totalDist[i]) # Computes the distance parallel to tractor + + def getNumberOfObstacles(self): + self.obstaclePoints = 0 # Counts how many points are not the ground + self.triggerPoints = 0 # Counts number of points breaking threshold + for i in range(len(self.totalDist)): # Sweep through the distances + if(self.totalDist[i] < self.DistanceToTheGround): # Is there an object that is not the ground? + self.obstaclePoints += 1 # Add a point into the number of obstacle points + if(abs(self.horizontalDistance[i]) < (self.lengthOfTheTractor / 2.0)): #Will the obstacle hit the tractor? + self.triggerPoints += 1 # Add a point the the number of triggers + + def sendMessages(self): + # Code is supposed to detect if there is an obstacle, and if so, stop the tractor + # Rougly complete, may not work in parcel B + if(triggerPoints > numberOfPointsNeededToTrigger and not self.hasSensed): # if there is an obstacle that will hit the tractor + # stop the tractor + pub0.publish(True) + self.hasSensed = True + if(triggerPoints <= numberOfPointsNeededToTrigger and self.hasSensed): + # don't stop the tractor + pub0.publish(False) + self.hasSensed = False + + def run(self): + self.convertToVerticalAndHorizontal() + self.getNumberOfObstacles() + self.sendMessages() + +if __name__ == '__main__': + obs = ObstacleDetection() + obs.listener() + rospy.spin() \ No newline at end of file From 3e48899669c64db65aa542142d667a1d52b0cf73 Mon Sep 17 00:00:00 2001 From: nathanestill Date: Sat, 22 Sep 2018 16:58:30 -0400 Subject: [PATCH 02/12] testing1 --- scripts/LidarCodeAdvanced.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/scripts/LidarCodeAdvanced.py b/scripts/LidarCodeAdvanced.py index 961c763..2ecd85a 100644 --- a/scripts/LidarCodeAdvanced.py +++ b/scripts/LidarCodeAdvanced.py @@ -21,7 +21,7 @@ def callback(self,data): def convertToVerticalAndHorizontal(self): self.totalDist = [] # Initializing arrays self.verticalDistance = [] # Distance from front of tractor - self.horizontalDistance = [] # Distance from center of tractor + self.horizontalDistance = [] # Distance from center of tractore for i in range(len(self.data.ranges)): self.totalDist.append(self.data.ranges[i]) if self.totalDist[i] > 1000000: From 74cb6cac652ad1dd6750a131ac0afec6bb3a162c Mon Sep 17 00:00:00 2001 From: nathanestill Date: Sat, 22 Sep 2018 17:00:19 -0400 Subject: [PATCH 03/12] testing2 --- scripts/LidarCodeAdvanced.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/scripts/LidarCodeAdvanced.py b/scripts/LidarCodeAdvanced.py index 2ecd85a..bfa7a3f 100644 --- a/scripts/LidarCodeAdvanced.py +++ b/scripts/LidarCodeAdvanced.py @@ -11,17 +11,17 @@ def __init__(self): self.hasSensed = False self.pubEstop = rospy.Publisher('/softestop', Bool, queue_size=10) self.laserSub = rospy.Subscriber('scan',LaserScan,self.callback) # Subscribes to the LIDAR, calls the callback function - self.DistanceToTheGround = 4.5 # Essentially the ground *** - self.lengthOfTheTractor = 1.25 # Horizontal length of the tractor *** - self.numberOfPointsNeededToTrigger = 15 # How many points must be seen to trigger a stop? *** + self.DistanceToTheGround = 4.5 # Essentially the ground + self.lengthOfTheTractor = 1.25 # Horizontal length of the tractor + self.numberOfPointsNeededToTrigger = 15 # How many points must be seen to trigger a stop? def callback(self,data): - self.data = data #calling below code + self.data = data # calling below code def convertToVerticalAndHorizontal(self): self.totalDist = [] # Initializing arrays self.verticalDistance = [] # Distance from front of tractor - self.horizontalDistance = [] # Distance from center of tractore + self.horizontalDistance = [] # Distance from center of tractor for i in range(len(self.data.ranges)): self.totalDist.append(self.data.ranges[i]) if self.totalDist[i] > 1000000: From bdf08f746337f97a8659d7f3aefec66b59141517 Mon Sep 17 00:00:00 2001 From: nathanestill Date: Wed, 26 Sep 2018 16:12:19 -0400 Subject: [PATCH 04/12] Making changes for Connor for pull request --- scripts/LidarCodeAdvanced.py | 57 +++++++++++++++++++++--------------- 1 file changed, 34 insertions(+), 23 deletions(-) diff --git a/scripts/LidarCodeAdvanced.py b/scripts/LidarCodeAdvanced.py index bfa7a3f..e622bf1 100644 --- a/scripts/LidarCodeAdvanced.py +++ b/scripts/LidarCodeAdvanced.py @@ -1,5 +1,14 @@ #!/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 import math from std_msgs.msg import Bool @@ -7,40 +16,41 @@ class ObstacleDetection(): def __init__(self): + # Variables explained in Github Wiki: https://github.com/olinrobotics/gravl/wiki/Vehicle-Safety rospy.init_node('Obstacle Detection', anonymous=True) - self.hasSensed = False - self.pubEstop = rospy.Publisher('/softestop', Bool, queue_size=10) + self.hasSensed = False # If there is an obstacle sensed + self.pubEstop = rospy.Publisher('/softestop', Bool, queue_size=10) # Publisher that publishes to the estop self.laserSub = rospy.Subscriber('scan',LaserScan,self.callback) # Subscribes to the LIDAR, calls the callback function self.DistanceToTheGround = 4.5 # Essentially the ground - self.lengthOfTheTractor = 1.25 # Horizontal length of the tractor + self.widthTractor = 1.25 # Horizontal length of the tractor self.numberOfPointsNeededToTrigger = 15 # How many points must be seen to trigger a stop? + self.update_rate = rospy.Rate(5) - def callback(self,data): - self.data = data # calling below code + def lidarCB(self,data): + # Collects data from the Hokuyo Lidar and stores it + self.data = data def convertToVerticalAndHorizontal(self): self.totalDist = [] # Initializing arrays - self.verticalDistance = [] # Distance from front of tractor - self.horizontalDistance = [] # Distance from center of tractor - for i in range(len(self.data.ranges)): - self.totalDist.append(self.data.ranges[i]) - if self.totalDist[i] > 1000000: - self.totalDist[i] = 100000 - self.verticalDistance.append(abs(math.cos(math.radians((i - 380.0) * (190.0 / 760.0))) * self.totalDist[i])) # Computes the distance from the object - self.horizontalDistance.append(math.sin(math.radians((i - 380.0) * (190.0 / 760.0))) * self.totalDist[i]) # Computes the distance parallel to tractor + self.xDist = [] # Distance from front of tractor + self.yDist = [] # Distance from center of tractor + for i in range(len(self.data.ranges)): # Puts the tuple of data into x and y Distances + self.totalDist.append(self.data.ranges[i]) + self.xDist.append(abs(math.cos(math.radians((i - 380.0) * (190.0 / 760.0))) * self.totalDist[i])) # Computes the distance from the object + self.yDist.append(math.sin(math.radians((i - 380.0) * (190.0 / 760.0))) * self.totalDist[i]) # Computes the distance parallel to tractor def getNumberOfObstacles(self): + # Calculates the number of points that pose a threat to the tractor self.obstaclePoints = 0 # Counts how many points are not the ground self.triggerPoints = 0 # Counts number of points breaking threshold for i in range(len(self.totalDist)): # Sweep through the distances if(self.totalDist[i] < self.DistanceToTheGround): # Is there an object that is not the ground? self.obstaclePoints += 1 # Add a point into the number of obstacle points - if(abs(self.horizontalDistance[i]) < (self.lengthOfTheTractor / 2.0)): #Will the obstacle hit the tractor? + if(abs(self.yDist[i]) < (self.widthTractor / 2.0)): #Will the obstacle hit the tractor? self.triggerPoints += 1 # Add a point the the number of triggers def sendMessages(self): - # Code is supposed to detect if there is an obstacle, and if so, stop the tractor - # Rougly complete, may not work in parcel B + # If the number of trigger points is greater than the threshold, send a singal message to the tractor if(triggerPoints > numberOfPointsNeededToTrigger and not self.hasSensed): # if there is an obstacle that will hit the tractor # stop the tractor pub0.publish(True) @@ -51,11 +61,12 @@ def sendMessages(self): self.hasSensed = False def run(self): - self.convertToVerticalAndHorizontal() - self.getNumberOfObstacles() - self.sendMessages() - + # Runs the code + while not rospy.is_shutdown() and (self.data == None): + self.convertToVerticalAndHorizontal() + self.getNumberOfObstacles() + self.sendMessages() + self.update_rate.sleep() if __name__ == '__main__': obs = ObstacleDetection() - obs.listener() - rospy.spin() \ No newline at end of file + obs.run() \ No newline at end of file From b1ef72c981c00e67fc3ef74d4f8d902fc4c4efcf Mon Sep 17 00:00:00 2001 From: nathanestill Date: Wed, 26 Sep 2018 17:56:10 -0400 Subject: [PATCH 05/12] add/modify launch files for lidar --- launch/downlidar.launch | 5 +++++ launch/frontlidar.launch | 5 +++++ launch/lidar.launch | 5 ++--- scripts/LidarCodeAdvanced.py | 19 ++++++++++++------- 4 files changed, 24 insertions(+), 10 deletions(-) create mode 100644 launch/downlidar.launch create mode 100644 launch/frontlidar.launch 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..500143e 100644 --- a/launch/lidar.launch +++ b/launch/lidar.launch @@ -1,5 +1,4 @@ - - - + + \ No newline at end of file diff --git a/scripts/LidarCodeAdvanced.py b/scripts/LidarCodeAdvanced.py index e622bf1..1280d21 100644 --- a/scripts/LidarCodeAdvanced.py +++ b/scripts/LidarCodeAdvanced.py @@ -20,22 +20,27 @@ def __init__(self): rospy.init_node('Obstacle Detection', anonymous=True) self.hasSensed = False # If there is an obstacle sensed self.pubEstop = rospy.Publisher('/softestop', Bool, queue_size=10) # Publisher that publishes to the estop - self.laserSub = rospy.Subscriber('scan',LaserScan,self.callback) # Subscribes to the LIDAR, calls the callback function + 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.backLidarCB) # Subscribes to the down Lidar, calls the cb function self.DistanceToTheGround = 4.5 # Essentially the ground self.widthTractor = 1.25 # Horizontal length of the tractor self.numberOfPointsNeededToTrigger = 15 # How many points must be seen to trigger a stop? self.update_rate = rospy.Rate(5) - def lidarCB(self,data): - # Collects data from the Hokuyo Lidar and stores it - self.data = data + 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 convertToVerticalAndHorizontal(self): self.totalDist = [] # Initializing arrays self.xDist = [] # Distance from front of tractor self.yDist = [] # Distance from center of tractor - for i in range(len(self.data.ranges)): # Puts the tuple of data into x and y Distances - self.totalDist.append(self.data.ranges[i]) + for i in range(len(self.frontData.ranges)): # Puts the tuple of data into x and y Distances + self.totalDist.append(self.frontData.ranges[i]) self.xDist.append(abs(math.cos(math.radians((i - 380.0) * (190.0 / 760.0))) * self.totalDist[i])) # Computes the distance from the object self.yDist.append(math.sin(math.radians((i - 380.0) * (190.0 / 760.0))) * self.totalDist[i]) # Computes the distance parallel to tractor @@ -62,7 +67,7 @@ def sendMessages(self): def run(self): # Runs the code - while not rospy.is_shutdown() and (self.data == None): + while not rospy.is_shutdown() and (self.frontData == None): self.convertToVerticalAndHorizontal() self.getNumberOfObstacles() self.sendMessages() From f2507e2d4493057d029d1105f1ce94b594fe2b44 Mon Sep 17 00:00:00 2001 From: nathanestill Date: Thu, 27 Sep 2018 12:29:05 -0400 Subject: [PATCH 06/12] starting to add Down Lidar Stuff --- scripts/LidarCodeAdvanced.py | 46 +++++++++++++++++++++++++++++++----- 1 file changed, 40 insertions(+), 6 deletions(-) diff --git a/scripts/LidarCodeAdvanced.py b/scripts/LidarCodeAdvanced.py index 1280d21..9caca46 100644 --- a/scripts/LidarCodeAdvanced.py +++ b/scripts/LidarCodeAdvanced.py @@ -23,6 +23,7 @@ def __init__(self): 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.backLidarCB) # Subscribes to the down Lidar, calls the cb function self.DistanceToTheGround = 4.5 # Essentially the ground + self.DownToGround = 1.3 self.widthTractor = 1.25 # Horizontal length of the tractor self.numberOfPointsNeededToTrigger = 15 # How many points must be seen to trigger a stop? self.update_rate = rospy.Rate(5) @@ -32,17 +33,19 @@ def frontLidarCB(self,data): self.frontData = data def downLidarCB(self,data): - # Collects data from the down Hokuyo Lidar and stores it + # Collects data from the Down Hokuyo Lidar and stores it self.downData = data - def convertToVerticalAndHorizontal(self): + def convertToXDistAndYDist(self): self.totalDist = [] # Initializing arrays self.xDist = [] # Distance from front of tractor self.yDist = [] # Distance from center of tractor + 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.totalDist.append(self.frontData.ranges[i]) - self.xDist.append(abs(math.cos(math.radians((i - 380.0) * (190.0 / 760.0))) * self.totalDist[i])) # Computes the distance from the object - self.yDist.append(math.sin(math.radians((i - 380.0) * (190.0 / 760.0))) * self.totalDist[i]) # Computes the distance parallel to tractor + self.xDist.append(abs(math.cos(math.radians((i - dataPoints / 2) * (angleSweep / dataPoints))) * self.totalDist[i])) # Computes the distance from the object + self.yDist.append(math.sin(math.radians((i - dataPoints / 2) * (angleSweep / dataPoints))) * self.totalDist[i]) # Computes the distance parallel to tractor def getNumberOfObstacles(self): # Calculates the number of points that pose a threat to the tractor @@ -65,10 +68,41 @@ def sendMessages(self): pub0.publish(False) self.hasSensed = False + def convertToYDistAndZDist(self): + self.totalDist = [] # Initializing arrays + self.xDist = [] # Distance from front of tractor + self.yDist = [] # Distance from center of tractor + dataPoints = len(self.frontData.ranges) + angleSweep = 270 + for i in range(len(self.frontData.ranges)): # Puts the tuple of data into x and y Distances + self.totalDist.append(self.frontData.ranges[i]) + self.zDist.append(abs(math.cos(math.radians((i - dataPoints / 2) * (angleSweep / dataPoints))) * self.totalDist[i])) # Computes the distance from the object + self.yDist.append(math.sin(math.radians((i - dataPoints / 2) * (angleSweep / dataPoints))) * self.totalDist[i]) # Computes the distance parallel to tractor + + def getNumberOfObstacles(self): + # Calculates the number of points that pose a threat to the tractor + self.obstaclePoints = 0 # Counts how many points are not the ground + self.triggerPoints = 0 # Counts number of points breaking threshold + for i in range(len(self.totalDist)): # Sweep through the distances + if(self.totalDist[i] < self.DownToGround): # Is there an object that is not the ground? + self.obstaclePoints += 1 # Add a point into the number of obstacle points + if(abs(self.yDist[i]) < (self.widthTractor / 2.0)): #Will the obstacle hit the tractor? + self.triggerPoints += 1 # Add a point the the number of triggers + + def sendMessages(self): + # If the number of trigger points is greater than the threshold, send a singal message to the tractor + if(triggerPoints > numberOfPointsNeededToTrigger and not self.hasSensed): # if there is an obstacle that will hit the tractor + # stop the tractor + pub0.publish(True) + self.hasSensed = True + if(triggerPoints <= numberOfPointsNeededToTrigger and self.hasSensed): + # don't stop the tractor + pub0.publish(False) + self.hasSensed = False def run(self): # Runs the code - while not rospy.is_shutdown() and (self.frontData == None): - self.convertToVerticalAndHorizontal() + while not rospy.is_shutdown() and (self.frontData == None or self.backData == None): + self.convertToXDistAndYDist() self.getNumberOfObstacles() self.sendMessages() self.update_rate.sleep() From 078a55c6efd5a47a5625ba072c3196f457dec26a Mon Sep 17 00:00:00 2001 From: nathanestill Date: Sat, 29 Sep 2018 16:58:24 -0400 Subject: [PATCH 07/12] modifying LIDarCode --- launch/lidar.launch | 6 +- scripts/LidarCodeAdvanced.py | 149 ++++++++++++++++++++++------------- 2 files changed, 97 insertions(+), 58 deletions(-) mode change 100644 => 100755 scripts/LidarCodeAdvanced.py diff --git a/launch/lidar.launch b/launch/lidar.launch index 500143e..27ba475 100644 --- a/launch/lidar.launch +++ b/launch/lidar.launch @@ -1,4 +1,4 @@ - - - \ No newline at end of file + + + diff --git a/scripts/LidarCodeAdvanced.py b/scripts/LidarCodeAdvanced.py old mode 100644 new mode 100755 index 9caca46..3c9e7fe --- a/scripts/LidarCodeAdvanced.py +++ b/scripts/LidarCodeAdvanced.py @@ -13,20 +13,27 @@ 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 class ObstacleDetection(): def __init__(self): # Variables explained in Github Wiki: https://github.com/olinrobotics/gravl/wiki/Vehicle-Safety - rospy.init_node('Obstacle Detection', anonymous=True) - self.hasSensed = False # If there is an obstacle sensed + 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.backLidarCB) # Subscribes to the down Lidar, calls the cb function + self.downLaserSub = rospy.Subscriber('/down/scan',LaserScan,self.downLidarCB) # Subscribes to the down Lidar, calls the cb function + self.frontData = None + self.downData = None self.DistanceToTheGround = 4.5 # Essentially the ground - self.DownToGround = 1.3 + self.DownToGround = 1.2 # Distance from down lidar to the ground self.widthTractor = 1.25 # Horizontal length of the tractor - self.numberOfPointsNeededToTrigger = 15 # How many points must be seen to trigger a stop? + self.threshold = 15 # How many points must be seen to trigger a stop? self.update_rate = rospy.Rate(5) + self.vis_pub = rospy.Publisher('/TractorLine_pt', Marker, queue_size=2) def frontLidarCB(self,data): # Collects data from the Front Hokuyo Lidar and stores it @@ -36,76 +43,108 @@ def downLidarCB(self,data): # Collects data from the Down Hokuyo Lidar and stores it self.downData = data - def convertToXDistAndYDist(self): - self.totalDist = [] # Initializing arrays - self.xDist = [] # Distance from front of tractor - self.yDist = [] # Distance from center of tractor + def convertToXDistAndYDistFront(self): + self.totalDistFront = [] # Initializing arrays + self.xDistFront = [] # Distance from front of tractor + self.yDistFront = [] # Distance from center of tractor 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.totalDist.append(self.frontData.ranges[i]) - self.xDist.append(abs(math.cos(math.radians((i - dataPoints / 2) * (angleSweep / dataPoints))) * self.totalDist[i])) # Computes the distance from the object - self.yDist.append(math.sin(math.radians((i - dataPoints / 2) * (angleSweep / dataPoints))) * self.totalDist[i]) # Computes the distance parallel to tractor + self.totalDistFront.append(self.frontData.ranges[i]) + angleRad = math.radians((i - dataPoints / 2) * (angleSweep / dataPoints)) + self.xDistFront.append(abs(math.cos(angleRad) * self.totalDistFront[i])) # Computes the distance from the object + self.yDistFront.append(math.sin(angleRad) * self.totalDistFront[i]) # Computes the distance parallel to tractor - def getNumberOfObstacles(self): + def getNumberOfObstaclesFront(self): # Calculates the number of points that pose a threat to the tractor - self.obstaclePoints = 0 # Counts how many points are not the ground - self.triggerPoints = 0 # Counts number of points breaking threshold + self.obstaclePointsFront = 0 # Counts how many points are not the ground + self.triggerPointsFront = 0 # Counts number of points breaking threshold for i in range(len(self.totalDist)): # Sweep through the distances - if(self.totalDist[i] < self.DistanceToTheGround): # Is there an object that is not the ground? - self.obstaclePoints += 1 # Add a point into the number of obstacle points - if(abs(self.yDist[i]) < (self.widthTractor / 2.0)): #Will the obstacle hit the tractor? - self.triggerPoints += 1 # Add a point the the number of triggers + if(self.totalDistFront[i] < self.DistanceToTheGround): # Is there an object that is not the ground? + self.obstaclePointsFront += 1 # Add a point into the number of obstacle points + if(abs(self.yDistFront[i]) < (self.widthTractor / 2.0)): #Will the obstacle hit the tractor? + self.triggerPointsFront += 1 # Add a point the the number of triggers - def sendMessages(self): + def sendMessagesFront(self): # If the number of trigger points is greater than the threshold, send a singal message to the tractor - if(triggerPoints > numberOfPointsNeededToTrigger and not self.hasSensed): # if there is an obstacle that will hit the tractor + if(self.triggerPointsFront > self.threshold and not self.hasSensedFront): # if there is an obstacle that will hit the tractor # stop the tractor - pub0.publish(True) - self.hasSensed = True - if(triggerPoints <= numberOfPointsNeededToTrigger and self.hasSensed): + self.pubEstop.publish(True) + self.hasSensedFront = True + if(self.triggerPointsFront <= self.threshold and self.hasSensedFront): # don't stop the tractor - pub0.publish(False) - self.hasSensed = False + self.pubEstop.publish(False) + self.hasSensedFront = False - def convertToYDistAndZDist(self): - self.totalDist = [] # Initializing arrays - self.xDist = [] # Distance from front of tractor - self.yDist = [] # Distance from center of tractor - dataPoints = len(self.frontData.ranges) - angleSweep = 270 - for i in range(len(self.frontData.ranges)): # Puts the tuple of data into x and y Distances - self.totalDist.append(self.frontData.ranges[i]) - self.zDist.append(abs(math.cos(math.radians((i - dataPoints / 2) * (angleSweep / dataPoints))) * self.totalDist[i])) # Computes the distance from the object - self.yDist.append(math.sin(math.radians((i - dataPoints / 2) * (angleSweep / dataPoints))) * self.totalDist[i]) # Computes the distance parallel to tractor + 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 = math.radians((i - dataPoints / 2) * (angleSweep / dataPoints)) + self.zDistDown.append(abs(math.cos(angleRad) * self.totalDistDown[i])) # Computes the distance from the object + self.yDistDown.append(abs(math.sin(angleRad) * self.totalDistDown[i])) # Computes the distance parallel to tractor - def getNumberOfObstacles(self): + def getNumberOfObstaclesDown(self): # Calculates the number of points that pose a threat to the tractor - self.obstaclePoints = 0 # Counts how many points are not the ground - self.triggerPoints = 0 # Counts number of points breaking threshold - for i in range(len(self.totalDist)): # Sweep through the distances - if(self.totalDist[i] < self.DownToGround): # Is there an object that is not the ground? - self.obstaclePoints += 1 # Add a point into the number of obstacle points - if(abs(self.yDist[i]) < (self.widthTractor / 2.0)): #Will the obstacle hit the tractor? - self.triggerPoints += 1 # Add a point the the number of triggers + 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 + used_pts = Marker() # Marker to visualize used lidar pts + used_pts.header.frame_id = "laser" + used_pts.header.stamp = rospy.Time.now() + used_pts.type = 8 + used_pts.scale = Vector3(100,0.01,0.01) + used_pts.color = ColorRGBA(1,1,1,1) + used_pts.points = [Point(0,1.25 / 2,0),Point(0,-1.25 / 2,0)] + used_pts.pose.orientation.w = 1.0 + self.vis_pub.publish(used_pts) + - def sendMessages(self): + + def sendMessagesDown(self): # If the number of trigger points is greater than the threshold, send a singal message to the tractor - if(triggerPoints > numberOfPointsNeededToTrigger and not self.hasSensed): # if there is an obstacle that will hit the tractor + if(self.triggerPointsDown > self.threshold and not self.hasSensedDown): # if there is an obstacle that will hit the tractor # stop the tractor - pub0.publish(True) - self.hasSensed = True - if(triggerPoints <= numberOfPointsNeededToTrigger and self.hasSensed): + self.pubEstop.publish(True) + self.hasSensedDown = True + if(self.triggerPointsDown <= self.threshold and self.hasSensedDown): # don't stop the tractor - pub0.publish(False) - self.hasSensed = False + self.pubEstop.publish(False) + self.hasSensedDown = False def run(self): # Runs the code - while not rospy.is_shutdown() and (self.frontData == None or self.backData == None): - self.convertToXDistAndYDist() - self.getNumberOfObstacles() - self.sendMessages() + 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() + self.sendMessagesDown() + 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() + self.sendMessagesFront() self.update_rate.sleep() + while not rospy.is_shutdown(): + rospy.loginfo("Running") + self.convertToXDistAndYDistFront() + self.getNumberOfObstaclesFront() + self.sendMessagesFront() + self.convertToYDistAndZDistDown() + self.getNumberOfObstaclesDown() + self.sendMessagesDown() + self.update_rate.sleep() + if __name__ == '__main__': obs = ObstacleDetection() obs.run() \ No newline at end of file From cdd20b9c53f787b29e7fa89e368c29fe0a2c3882 Mon Sep 17 00:00:00 2001 From: nathanestill Date: Wed, 3 Oct 2018 17:55:36 -0400 Subject: [PATCH 08/12] made front lidar better, made visualization pretty --- launch/localization.launch | 6 +-- scripts/LidarCodeAdvanced.py | 71 ++++++++++++++++++++++++------------ 2 files changed, 50 insertions(+), 27 deletions(-) diff --git a/launch/localization.launch b/launch/localization.launch index e374cb9..346a8ad 100644 --- a/launch/localization.launch +++ b/launch/localization.launch @@ -1,8 +1,8 @@ - - - + + + diff --git a/scripts/LidarCodeAdvanced.py b/scripts/LidarCodeAdvanced.py index 3c9e7fe..7752490 100755 --- a/scripts/LidarCodeAdvanced.py +++ b/scripts/LidarCodeAdvanced.py @@ -28,12 +28,13 @@ def __init__(self): self.downLaserSub = rospy.Subscriber('/down/scan',LaserScan,self.downLidarCB) # Subscribes to the down Lidar, calls the cb function self.frontData = None self.downData = None - self.DistanceToTheGround = 4.5 # Essentially the ground self.DownToGround = 1.2 # Distance from down lidar to the ground self.widthTractor = 1.25 # Horizontal length of the tractor self.threshold = 15 # How many points must be seen to trigger a stop? self.update_rate = rospy.Rate(5) - self.vis_pub = rospy.Publisher('/TractorLine_pt', Marker, queue_size=2) + self.vis_pubFront = rospy.Publisher('/TractorLineFront_pt', Marker, queue_size=2) + self.vis_pubDown = rospy.Publisher('/TractorLineDown_pt', Marker, queue_size=2) + def frontLidarCB(self,data): # Collects data from the Front Hokuyo Lidar and stores it @@ -47,23 +48,36 @@ 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 + angleOfIncline = 0.194724 # ?Empirically Discovered. Fix if necessary 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 = math.radians((i - dataPoints / 2) * (angleSweep / dataPoints)) - self.xDistFront.append(abs(math.cos(angleRad) * self.totalDistFront[i])) # Computes the distance from the object + self.xDistFront.append(abs(math.cos(angleOfIncline) * math.cos(angleRad) * self.totalDistFront[i])) # Computes the distance from the object self.yDistFront.append(math.sin(angleRad) * self.totalDistFront[i]) # Computes the distance parallel to tractor + self.zDistFront.append(math.sin(angleOfIncline) * math.cos(angleRad) * self.totalDistFront[i]) - def getNumberOfObstaclesFront(self): + def getNumberOfObstaclesFront(self,visualize): # Calculates the number of points that pose a threat to the tractor self.obstaclePointsFront = 0 # Counts how many points are not the ground - self.triggerPointsFront = 0 # Counts number of points breaking threshold - for i in range(len(self.totalDist)): # Sweep through the distances - if(self.totalDistFront[i] < self.DistanceToTheGround): # Is there an object that is not the ground? + self.triggerPointsFront = 0 # Counts number of points breaking threshold + for i in range(len(self.totalDistFront)): # Sweep through the distances + if(self.zDistFront[i] < self.DownToGround): # Is there an object that is not the ground? self.obstaclePointsFront += 1 # Add a point into the number of obstacle points if(abs(self.yDistFront[i]) < (self.widthTractor / 2.0)): #Will the obstacle hit the tractor? self.triggerPointsFront += 1 # Add a point the the number of triggers + if visualize: + used_pts = Marker() # Marker to visualize used lidar pts + used_pts.header.frame_id = "laser" + used_pts.header.stamp = rospy.Time.now() + used_pts.type = 8 + used_pts.scale = Vector3(0.02,0.02,0.02) + used_pts.color = ColorRGBA(0,0,1,1) + for i in range(len(self.totalDistFront)): + used_pts.points.append(Point(self.xDistFront[i],self.yDistFront[i],-self.zDistFront[i])) + self.vis_pubFront.publish(used_pts) def sendMessagesFront(self): # If the number of trigger points is greater than the threshold, send a singal message to the tractor @@ -85,10 +99,10 @@ def convertToYDistAndZDistDown(self): 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 = math.radians((i - dataPoints / 2) * (angleSweep / dataPoints)) - self.zDistDown.append(abs(math.cos(angleRad) * self.totalDistDown[i])) # Computes the distance from the object - self.yDistDown.append(abs(math.sin(angleRad) * self.totalDistDown[i])) # Computes the distance parallel to tractor + self.zDistDown.append(math.cos(angleRad) * self.totalDistDown[i]) # Computes the distance from the object + self.yDistDown.append(math.sin(angleRad) * self.totalDistDown[i]) # Computes the distance parallel to tractor - def getNumberOfObstaclesDown(self): + 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 @@ -97,15 +111,24 @@ def getNumberOfObstaclesDown(self): 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 - used_pts = Marker() # Marker to visualize used lidar pts - used_pts.header.frame_id = "laser" - used_pts.header.stamp = rospy.Time.now() - used_pts.type = 8 - used_pts.scale = Vector3(100,0.01,0.01) - used_pts.color = ColorRGBA(1,1,1,1) - used_pts.points = [Point(0,1.25 / 2,0),Point(0,-1.25 / 2,0)] - used_pts.pose.orientation.w = 1.0 - self.vis_pub.publish(used_pts) + if visualize: + # used_pts = Marker() # Marker to visualize used lidar pts + # used_pts.header.frame_id = "laser" + # used_pts.header.stamp = rospy.Time.now() + # used_pts.type = 8 + # used_pts.scale = Vector3(100,0.01,0.01) + # used_pts.color = ColorRGBA(1,1,1,1) + # used_pts.points = [Point(0,1.25 / 2,0),Point(0,-1.25 / 2,0)] + # self.vis_pubDown.publish(used_pts) + used_pts = Marker() # Marker to visualize used lidar pts + used_pts.header.frame_id = "laser" + used_pts.header.stamp = rospy.Time.now() + used_pts.type = 8 + used_pts.scale = Vector3(0.02,0.02,0.02) + used_pts.color = ColorRGBA(1,0,0,1) + for i in range(len(self.totalDistDown)): + used_pts.points.append(Point(0,self.yDistDown[i],-self.zDistDown[i])) + self.vis_pubDown.publish(used_pts) @@ -126,22 +149,22 @@ def run(self): 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() + self.getNumberOfObstaclesDown(True) self.sendMessagesDown() 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() + self.getNumberOfObstaclesFront(True) self.sendMessagesFront() self.update_rate.sleep() + rospy.loginfo("Running") while not rospy.is_shutdown(): - rospy.loginfo("Running") self.convertToXDistAndYDistFront() - self.getNumberOfObstaclesFront() + self.getNumberOfObstaclesFront(True) self.sendMessagesFront() self.convertToYDistAndZDistDown() - self.getNumberOfObstaclesDown() + self.getNumberOfObstaclesDown(True) self.sendMessagesDown() self.update_rate.sleep() From e26a66043409864a8647123b07adb21e06c70980 Mon Sep 17 00:00:00 2001 From: nathanestill Date: Sat, 6 Oct 2018 17:01:40 -0400 Subject: [PATCH 09/12] updating Lidar Code --- scripts/LidarCodeAdvanced.py | 129 ++++++++++++++++++++++------------- 1 file changed, 81 insertions(+), 48 deletions(-) diff --git a/scripts/LidarCodeAdvanced.py b/scripts/LidarCodeAdvanced.py index 7752490..8910abe 100755 --- a/scripts/LidarCodeAdvanced.py +++ b/scripts/LidarCodeAdvanced.py @@ -10,15 +10,18 @@ # publishing True to estop if an obstacle is seen ###################################################################### import rospy -import math +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): + def __init__(self,senseRange): # 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 @@ -26,14 +29,19 @@ def __init__(self): 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) self.frontData = None self.downData = None + self.ackermannData = None self.DownToGround = 1.2 # Distance from down lidar to the ground + self.senseRange = senseRange + self.mode = rospy.get_param('mode',"line") self.widthTractor = 1.25 # Horizontal length of the tractor - self.threshold = 15 # How many points must be seen to trigger a stop? + self.threshold = 5 # How many points must be seen to trigger a stop? self.update_rate = rospy.Rate(5) - self.vis_pubFront = rospy.Publisher('/TractorLineFront_pt', Marker, queue_size=2) - self.vis_pubDown = rospy.Publisher('/TractorLineDown_pt', Marker, queue_size=2) + 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): @@ -44,6 +52,9 @@ def downLidarCB(self,data): # Collects data from the Down Hokuyo Lidar and stores it self.downData = data + def ackermannCB(self,data): + self.ackermannData = data + def convertToXDistAndYDistFront(self): self.totalDistFront = [] # Initializing arrays self.xDistFront = [] # Distance from front of tractor @@ -54,30 +65,35 @@ def convertToXDistAndYDistFront(self): angleOfIncline = 0.194724 # ?Empirically Discovered. Fix if necessary 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 = math.radians((i - dataPoints / 2) * (angleSweep / dataPoints)) - self.xDistFront.append(abs(math.cos(angleOfIncline) * math.cos(angleRad) * self.totalDistFront[i])) # Computes the distance from the object - self.yDistFront.append(math.sin(angleRad) * self.totalDistFront[i]) # Computes the distance parallel to tractor - self.zDistFront.append(math.sin(angleOfIncline) * math.cos(angleRad) * self.totalDistFront[i]) + angleRad = radians((i - dataPoints / 2) * (angleSweep / dataPoints)) + self.xDistFront.append(abs(cos(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(angleOfIncline) * cos(angleRad) * self.totalDistFront[i]) + #rospy.loginfo(str(angleRad) + ": " + str(self.zDistFront[i])) def getNumberOfObstaclesFront(self,visualize): # Calculates the number of points that pose a threat to the tractor + if self.ackermannData == None: + 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 for i in range(len(self.totalDistFront)): # Sweep through the distances - if(self.zDistFront[i] < self.DownToGround): # Is there an object that is not the ground? + 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(abs(self.yDistFront[i]) < (self.widthTractor / 2.0)): #Will the obstacle hit the tractor? + 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 if visualize: - used_pts = Marker() # Marker to visualize used lidar pts - used_pts.header.frame_id = "laser" - used_pts.header.stamp = rospy.Time.now() - used_pts.type = 8 - used_pts.scale = Vector3(0.02,0.02,0.02) - used_pts.color = ColorRGBA(0,0,1,1) + 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)): - used_pts.points.append(Point(self.xDistFront[i],self.yDistFront[i],-self.zDistFront[i])) - self.vis_pubFront.publish(used_pts) + 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 @@ -98,9 +114,9 @@ def convertToYDistAndZDistDown(self): 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 = math.radians((i - dataPoints / 2) * (angleSweep / dataPoints)) - self.zDistDown.append(math.cos(angleRad) * self.totalDistDown[i]) # Computes the distance from the object - self.yDistDown.append(math.sin(angleRad) * self.totalDistDown[i]) # Computes the distance parallel to tractor + 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 @@ -112,26 +128,16 @@ def getNumberOfObstaclesDown(self,visualize): 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: - # used_pts = Marker() # Marker to visualize used lidar pts - # used_pts.header.frame_id = "laser" - # used_pts.header.stamp = rospy.Time.now() - # used_pts.type = 8 - # used_pts.scale = Vector3(100,0.01,0.01) - # used_pts.color = ColorRGBA(1,1,1,1) - # used_pts.points = [Point(0,1.25 / 2,0),Point(0,-1.25 / 2,0)] - # self.vis_pubDown.publish(used_pts) - used_pts = Marker() # Marker to visualize used lidar pts - used_pts.header.frame_id = "laser" - used_pts.header.stamp = rospy.Time.now() - used_pts.type = 8 - used_pts.scale = Vector3(0.02,0.02,0.02) - used_pts.color = ColorRGBA(1,0,0,1) + 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.totalDistDown)): - used_pts.points.append(Point(0,self.yDistDown[i],-self.zDistDown[i])) - self.vis_pubDown.publish(used_pts) - - - + 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 @@ -142,32 +148,59 @@ def sendMessagesDown(self): # don't stop the tractor self.pubEstop.publish(False) self.hasSensedDown = False - def run(self): + + def publishTractorLines(self): + if self.mode == "line": + tractorLines = Marker() + tractorLines.header.frame_id = "laser" + tractorLines.header.stamp = rospy.Time.now() + tractorLines.type = 5 + tractorLines.scale = Vector3(0.02, 0.01, 0.01) + tractorLines.color = ColorRGBA(1,1,1,1) + if self.ackermannData == None: + 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.points.append(Point(self.senseRange * cos(wheelAngle),self.widthTractor / 2 + sin(wheelAngle) * self.senseRange,0)) + 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) + 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(True) + 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(True) + 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(True) + self.getNumberOfObstaclesFront(visualize) self.sendMessagesFront() self.convertToYDistAndZDistDown() - self.getNumberOfObstaclesDown(True) + self.getNumberOfObstaclesDown(visualize) self.sendMessagesDown() + if visualize: + self.publishTractorLines() self.update_rate.sleep() if __name__ == '__main__': - obs = ObstacleDetection() - obs.run() \ No newline at end of file + obs = ObstacleDetection(float(rospy.get_param('senseRange','5.0'))) + obs.run(True) \ No newline at end of file From 68072de83acb46c8608dbdcc67ac3218bb0224af Mon Sep 17 00:00:00 2001 From: nathanestill Date: Wed, 10 Oct 2018 17:57:23 -0400 Subject: [PATCH 10/12] added Lidar Visualizer launch file and circle testing code --- launch/lidarvis.launch | 8 ++ rviz/lidarvis.rviz | 137 +++++++++++++++++++++++++++++++++++ scripts/LidarCodeAdvanced.py | 73 ++++++++++++++++--- 3 files changed, 207 insertions(+), 11 deletions(-) create mode 100644 launch/lidarvis.launch create mode 100644 rviz/lidarvis.rviz diff --git a/launch/lidarvis.launch b/launch/lidarvis.launch new file mode 100644 index 0000000..b7b169c --- /dev/null +++ b/launch/lidarvis.launch @@ -0,0 +1,8 @@ + + + + + + + + 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 index 8910abe..6e7acf0 100755 --- a/scripts/LidarCodeAdvanced.py +++ b/scripts/LidarCodeAdvanced.py @@ -21,7 +21,7 @@ class ObstacleDetection(): - def __init__(self,senseRange): + 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 @@ -34,9 +34,10 @@ def __init__(self,senseRange): self.downData = None self.ackermannData = None self.DownToGround = 1.2 # Distance from down lidar to the ground - self.senseRange = senseRange - self.mode = rospy.get_param('mode',"line") + 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.threshold = 5 # How many points must be seen to trigger a stop? self.update_rate = rospy.Rate(5) self.vis_pubFront = rospy.Publisher('/LidarFront_pt', Marker, queue_size=2) @@ -78,12 +79,26 @@ def getNumberOfObstaclesFront(self,visualize): 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 - 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 + self.triggerPointsFront = 0 # Counts number of points breaking threshold + if self.mode == "line" or wheelAngle == 0: + 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": + r = self.fbWheelDist / sin(wheelAngle) + if(wheelAngle > 0): + rInner = self.fbWheelDist / sin(wheelAngle) - self.widthTractor / 2 + 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): + self.obstaclePointsFront += 1 + if (rInner**2 < self.xDistFront[i]**2 + (self.yDistFront[i]-r)**2 < rOuter**2): + 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 @@ -133,7 +148,7 @@ def getNumberOfObstaclesDown(self,visualize): 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 + 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) @@ -169,6 +184,42 @@ def publishTractorLines(self): 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": + tractorLines = Marker() + 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) + if self.ackermannData == None or self.ackermannData.steering_angle == 0.0: + tractorLines.type = 5 + 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 + print(wheelAngle) + tractorLines.type = 4 + x = 0 + y = 0 + t = 0 + r = self.fbWheelDist / sin(wheelAngle) + while x**2 + y**2 < self.senseRange**2 and t < pi / 2: + rOuter = self.fbWheelDist / sin(wheelAngle) + self.widthTractor / 2 + x = abs(rOuter * sin(t)) + 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: @@ -202,5 +253,5 @@ def run(self,visualize = False): self.update_rate.sleep() if __name__ == '__main__': - obs = ObstacleDetection(float(rospy.get_param('senseRange','5.0'))) + obs = ObstacleDetection() obs.run(True) \ No newline at end of file From fd7d3b5bb76513948dc7f9d6ae4cd579bb6a9b35 Mon Sep 17 00:00:00 2001 From: nathanestill Date: Sat, 13 Oct 2018 16:58:38 -0400 Subject: [PATCH 11/12] added more updates to code --- launch/lidarvis.launch | 3 +- scripts/LidarCodeAdvanced.py | 81 ++++++++++++++++-------------------- 2 files changed, 39 insertions(+), 45 deletions(-) diff --git a/launch/lidarvis.launch b/launch/lidarvis.launch index b7b169c..629e641 100644 --- a/launch/lidarvis.launch +++ b/launch/lidarvis.launch @@ -1,6 +1,7 @@ - + diff --git a/scripts/LidarCodeAdvanced.py b/scripts/LidarCodeAdvanced.py index 6e7acf0..5f2e2f0 100755 --- a/scripts/LidarCodeAdvanced.py +++ b/scripts/LidarCodeAdvanced.py @@ -29,8 +29,8 @@ def __init__(self): 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) - self.frontData = None + 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 @@ -38,8 +38,9 @@ def __init__(self): 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) + 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) @@ -54,6 +55,7 @@ def downLidarCB(self,data): self.downData = data def ackermannCB(self,data): + # Collects data from the Wheels and stores it self.ackermannData = data def convertToXDistAndYDistFront(self): @@ -63,42 +65,41 @@ def convertToXDistAndYDistFront(self): self.zDistFront = [] # Distance from the tractor to the point, (usually the ground) dataPoints = len(self.frontData.ranges) angleSweep = 190.0 - angleOfIncline = 0.194724 # ?Empirically Discovered. Fix if necessary 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(angleOfIncline) * cos(angleRad) * self.totalDistFront[i])) # Computes the distance from the object + 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(angleOfIncline) * cos(angleRad) * self.totalDistFront[i]) - #rospy.loginfo(str(angleRad) + ": " + str(self.zDistFront[i])) + 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 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 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": - r = self.fbWheelDist / sin(wheelAngle) - if(wheelAngle > 0): - rInner = self.fbWheelDist / sin(wheelAngle) - self.widthTractor / 2 + 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): + 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): + 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 @@ -165,48 +166,40 @@ def sendMessagesDown(self): self.hasSensedDown = False def publishTractorLines(self): - if self.mode == "line": - tractorLines = Marker() - tractorLines.header.frame_id = "laser" - tractorLines.header.stamp = rospy.Time.now() - tractorLines.type = 5 - tractorLines.scale = Vector3(0.02, 0.01, 0.01) - tractorLines.color = ColorRGBA(1,1,1,1) + # 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: - 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)) + 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)) - 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)) + 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": - tractorLines = Marker() - 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) - if self.ackermannData == None or self.ackermannData.steering_angle == 0.0: - tractorLines.type = 5 + 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 - print(wheelAngle) - tractorLines.type = 4 - x = 0 - y = 0 - t = 0 + 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: + 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)) + 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)) From 9a8fb4cd4467e35c98eb109c0084c14c079e56bc Mon Sep 17 00:00:00 2001 From: nathanestill Date: Wed, 24 Oct 2018 15:34:15 -0400 Subject: [PATCH 12/12] adding test ackermann script, it creates a node that turns the wheels from left to right --- scripts/test_ackermann.py | 30 ++++++++++++++++++++++++++++++ 1 file changed, 30 insertions(+) create mode 100755 scripts/test_ackermann.py 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