Feature/pdi steering

scripts/auto.py

@@ -7,6 +7,7 @@
 from nav_msgs.msg import Odometry
 from nova_common.msg import *
 from nova_common.srv import *
+import steeringPID
 
 testing = True
 #--**--..--**--..--**--..--**--..--**--..--**--..--**--..--**--..--**--
@@ -132,6 +133,10 @@ def waypointCallback(waypointData):
     lng = waypointData.lng
     waypoint.setCoords(lat,lng)
 
+def pdiSteer(controller, rawCommand):
+    controller.ComputeTurn(rawCommand)
+    return controller.turn_est 
+
 #--**--..--**--..--**--..--**--..--**--..--**--..--**--..--**--..--**--
 # getMode(): Retrieve Mode from parameter server.
 #--..--**--..--**--..--**--..--**--..--**--..--**--..--**--..--**--..-- 
@@ -149,7 +154,7 @@ def waypointCallback(waypointData):
 #    Main function
 #--..--**--..--**--..--**--..--**--..--**--..--**--..--**--..--**--..--
 def auto():
-
+    pdiSteering = steeringPID(0,2)
     global rovey_pos
     global waypoint
     global auto_engaged
@@ -186,10 +191,9 @@ def auto():
            # steer_limit = rospy.get_param('steer_limit')
 
             drive_msg = DriveCmd()
-            drive_msg.rpm       = 10
-            drive_msg.steer_pct = turn * 0.5
-            drive_pub.publish(drive_msg)
-
+            drive_msg.steer_pct = turn
+            drive_pub.publish(pdiSteer(pdiSteering, turn))
+            drive_msg.rpm       = 0
             #if distance < :
             #   desPos.set_coords(route[1][0], route[1][1])   
 

scripts/auto_controller.py

@@ -105,11 +105,11 @@ def auto_drive(self):
         rospy.loginfo("distance: %s", distance)
         rospy.loginfo("orientation: %s", self.orientation)
         rospy.loginfo("current pos: %s", self.waypoint)
-        rpm_limit   = rospy.get_param('rpm_limit',100)
-        steer_limit = rospy.get_param('steer_limit',100)
+        rpm_limit   = rospy.get_param('rpm_limit',10)
+        steer_limit = rospy.get_param('steer_limit',10)
         drive_msg = DriveCmd()
-        drive_msg.rpm       = rpm_limit*10
-        drive_msg.steer_pct = steer_limit*10*turn/180
+        drive_msg.rpm       = 10
+        drive_msg.steer_pct = 0.5*turn
         self.drive_pub.publish(drive_msg)
     def metricCalculation(self):
         self.orientation = self.rovey_pos.yaw
@@ -208,7 +208,7 @@ def auto_controller():
     ## testing
     if testing:
         time.sleep(2)
-        SM.des_pos = WaypointClass(-37.660970, 145.368935)
+        SM.des_pos = WaypointClass(-37.6617819, 145.3692175)
         SM.toTraverse()
     ## end testing
     while not rospy.is_shutdown():

scripts/auto_functions.py

@@ -1,5 +1,5 @@
 import rospy, math,numpy
-from auto_classes import WaypointClass
+from auto_classes import WaypointClass 
 #--**--..--**--..--**--..--**--..--**--..--**--..--**--..--**--..--**--
 # bearingInDegrees():
 #    Calculates the direction an object is pointing in relation to the
@@ -104,6 +104,6 @@ def spiralSearch(current_pos,no_of_waypoints,rang_min,rang_max):
     lng=r*numpy.cos(theta) + current_pos.longitude
     lat=r*numpy.sin(theta) + current_pos.latitude
     searchPath=list()
-    for i in range(len(x)):
-        searchPath.append(RoveyPosClass(lat[i],lng[i]))
+    for i in range(len(theta)):
+        searchPath.append(WaypointClass(lat[i],lng[i]))
     return searchPath

scripts/ert.py

@@ -0,0 +1,124 @@
+#!/usr/bin/env python
+import rospy
+import math
+import time
+from sensor_msgs.msg import NavSatFix, MagneticField, Imu
+from webots_ros.srv import set_float
+from nova_common.msg import *
+from nova_common.srv import *
+
+class RoveyPosClass(object):
+
+    def __init__(self, lat, lng, roll, pitch, yaw):
+        self.latitude = lat
+        self.longitude = lng  
+        self.roll = roll
+        self.pitch = pitch
+        self.yaw = yaw
+
+    def setCoords(self, lat, lng):
+        self.latitude = lat
+        self.longitude = lng 
+
+    def setOrientation(self, roll, pitch, yaw):
+        self.roll = roll
+        self.pitch = pitch
+        self.yaw = yaw  
+
+def distanceBetween(lat1, lng1, lat2, lng2):
+    distance = math.sqrt((lat2-lat1)**2 + (lng2-lng1)**2)
+    return distance
+
+def turnDirection(beta, orientation):
+    if beta < 180:
+        if (orientation < (beta+180)) & (orientation > beta):
+            rospy.logdebug("turn left")
+            turn = (orientation-beta) * -1
+        elif orientation < beta:
+            rospy.logdebug("turn right")
+            turn = beta-orientation
+        else:
+            rospy.logdebug("turn right")
+            turn = (360-orientation)+beta
+    else:
+        if (orientation > (beta-180)) & (orientation < beta):
+            rospy.logdebug("turn right")
+            turn = beta-orientation
+        elif orientation > beta:
+            rospy.logdebug("turn left")
+            turn = (orientation-beta) * -1
+        else:
+            rospy.logdebug("turn left")
+            turn = ((360-beta)+orientation) * -1
+
+    rospy.loginfo("turn: %s", turn)        
+    return turn
+
+def angleBetween(lat1, lng1, lat2, lng2):    
+    lat1 = math.radians(lat1)
+    lat2 = math.radians(lat2)
+
+    longDiff = math.radians(lng2 - lng1)
+    y = math.sin(longDiff) * math.cos(lat2)
+    x = math.cos(lat1) * math.sin(lat2) - math.sin(lat1) * math.cos(lat2) * math.cos(longDiff)
+
+    beta = math.atan2(y, x)
+    beta = math.degrees(beta)
+    beta = (beta+360) % 360
+
+    return beta
+
+def gpsCallback(gpsData):
+    global rovey_pos
+    lat = gpsData.latitude
+    lng = gpsData.longitude
+    rovey_pos.setCoords(lat,lng)
+    #rospy.logdebug("lat: %s, long: %s", lat, lng)
+
+def rpyCallback(rpyData):
+    global rovey_pos
+    rovey_pos.setOrientation(rpyData.roll, rpyData.pitch, rpyData.yaw)
+
+rovey_pos = RoveyPosClass(0,0,0,0,0)
+
+def ert():
+
+    global rovey_pos
+
+    rospy.init_node('auto', anonymous=True)
+    rate = rospy.Rate(2) # Loop rate in Hz
+
+    #how to get north direction from world info? or gps ref point?
+
+    gps_sub     = rospy.Subscriber("/nova_common/gps_data", NavSatFix, gpsCallback)
+    rpy_sub     = rospy.Subscriber("/nova_common/RPY", RPY, rpyCallback)
+    status_pub  = rospy.Publisher("/core_rover/auto_status", AutoStatus, queue_size=10)
+
+    waypointlat = float(input("Input lat pls:"))
+    waypointlon = float(input("Input lon pls:"))
+    time.sleep(5)
+    while not rospy.is_shutdown():
+
+    orientation = rovey_pos.yaw
+
+    if (True):
+
+        beta =angleBetween(rovey_pos.latitude, rovey_pos.longitude, waypointlat, waypointlon)
+        distance = 110000 * distanceBetween(rovey_pos.latitude, rovey_pos.longitude, waypointlat, waypointlon)
+        turn = turnDirection(beta, orientation)
+
+        rospy.loginfo("fish")
+        rospy.loginfo("beta: %s", beta)
+        rospy.loginfo("distance: %s", distance)
+        rospy.loginfo("orientation: %s", orientation)
+
+    status_msg = AutoStatus()
+    status_msg.latitude  = rovey_pos.latitude
+    status_msg.longitude = rovey_pos.longitude
+    status_msg.bearing   = orientation
+    status_pub.publish(status_msg)   
+
+    rate.sleep()
+
+if __name__ == '__main__':
+    auto()