Merge pull request #43 from vanttec/testing/roboboat

Testing/roboboat

rb_missions/launch/auto_nav.launch

@@ -0,0 +1,13 @@
+<launch>
+
+  <!-- Auto Nav -->
+
+  <include file="$(find vectornav)/launch/vectornav.launch" />
+
+  <include file="$(find arduino-br)/launch/arduino_br.launch" />
+
+  <node pkg="rb_missions" type="auto_nav_position.py" name="auto_nav_position" />
+
+  <include file="$(find usv_control)/launch/usv_control.launch" />
+
+</launch>

rb_missions/launch/obs_chan.launch

@@ -0,0 +1,15 @@
+<launch>
+
+  <!-- Auto Nav -->
+
+  <include file="$(find vectornav)/launch/vectornav.launch" />
+
+  <include file="$(find arduino-br)/launch/arduino_br.launch" />
+
+  <node pkg="rb_missions" type="obstacle_channel.py" name="obstacle_channel" />
+
+  <include file="$(find usv_control)/launch/asmc.launch" />
+
+  <node pkg="usv_avoidance" type="nmpc_guidance_ca1" name="nmpc_guidance_ca1" />
+
+</launch>

rb_missions/launch/sim/sim_auto_nav.launch

@@ -0,0 +1,16 @@
+<launch>
+
+  <!-- Auto Nav -->
+
+  <include file="$(find usv_control)/launch/usv_simulation.launch" />
+
+  <node pkg="rb_missions" type="auto_nav_position.py" name="auto_nav_position" />
+
+  <include file="$(find usv_control)/launch/usv_control.launch" />
+
+  <node name="obstacle_simulator" pkg="rb_missions" type="obstacle_simulator.py" >
+	<param name = "challenge_number" value = "0" />
+  </node>
+
+
+</launch>

rb_missions/launch/sim/sim_obs_chan.launch

@@ -0,0 +1,18 @@
+<launch>
+
+  <!-- Auto Nav -->
+
+  <include file="$(find usv_control)/launch/usv_simulation.launch" />
+
+  <node pkg="rb_missions" type="obstacle_channel.py" name="obstacle_channel" />
+
+  <include file="$(find usv_control)/launch/asmc.launch" />
+
+  <node pkg="usv_avoidance" type="nmpc_guidance_ca1" name="nmpc_guidance_ca1" />
+
+  <node name="obstacle_simulator" pkg="rb_missions" type="obstacle_simulator.py" >
+	<param name = "challenge_number" value = "2" />
+  </node>
+
+
+</launch>

rb_missions/launch/sim/sim_speed_ch.launch

@@ -0,0 +1,16 @@
+<launch>
+
+  <!-- Auto Nav -->
+
+  <include file="$(find usv_control)/launch/usv_simulation.launch" />
+
+  <node pkg="rb_missions" type="speed_challenge.py" name="speed_challenge" />
+
+  <include file="$(find usv_control)/launch/usv_control.launch" />
+
+  <node name="obstacle_simulator" pkg="rb_missions" type="obstacle_simulator.py" >
+	<param name = "challenge_number" value = "1" />
+  </node>
+
+
+</launch>

rb_missions/launch/speed_ch.launch

@@ -0,0 +1,13 @@
+<launch>
+
+  <!-- Auto Nav -->
+
+  <include file="$(find vectornav)/launch/vectornav.launch" />
+
+  <include file="$(find arduino-br)/launch/arduino_br.launch" />
+
+  <node pkg="rb_missions" type="speed_challenge.py" name="speed_challenge" />
+
+  <include file="$(find usv_control)/launch/usv_control.launch" />
+
+</launch>

rb_missions/scripts/auto_nav_position.py

@@ -42,7 +42,7 @@ def __init__(self):
         self.state = -1
         self.distance = 0
         self.InitTime = rospy.Time.now().secs
-        self.offset = .55 #camera to ins offset
+        self.offset = .25 #camera to ins offset
         self.target_x = 0
         self.target_y = 0
         self.ned_alpha = 0
@@ -65,7 +65,7 @@ def ins_pose_callback(self,pose):
     def objs_callback(self,data):
         self.objects_list = []
         for i in range(data.len):
-            if str(data.objects[i].clase) == 'buoy':
+            if str(data.objects[i].clase) == 'buoy' and (data.objects[i].X > 0.2) and (data.objects[i].X < 5) and (data.objects[i].Y < 5) and (data.objects[i].Y > -5):
                 self.objects_list.append({'X' : data.objects[i].X + self.offset, 
                                       'Y' : data.objects[i].Y, 
                                       'color' : data.objects[i].color, 
@@ -129,6 +129,8 @@ def center_point(self):
         path_array.layout.data_offset = 5
         path_array.data = [xc, yc, xm, ym, 2]
 
+
+
         self.desired(path_array)
 
     def calculate_distance_to_boat(self):
@@ -247,10 +249,11 @@ def desired(self, path):
 
 def main():
     rospy.init_node("auto_nav_position", anonymous=False)
-    rate = rospy.Rate(20)
+    rate = rospy.Rate(10)
     autoNav = AutoNav()
     autoNav.distance = 4
     last_detection = []
+    time.sleep(10)
     while not rospy.is_shutdown() and autoNav.activated:
         if autoNav.objects_list != last_detection:
             if autoNav.state == -1:
@@ -264,20 +267,28 @@ def main():
                 autoNav.test.publish(autoNav.state)
                 if len(autoNav.objects_list) >= 2:
                     autoNav.calculate_distance_to_boat()
-                if (len(autoNav.objects_list) >= 2) and (autoNav.distance >= 2):
+                if (len(autoNav.objects_list) >= 2) and (autoNav.distance >= 3):
                     autoNav.center_point()
                 else:
+                    x_dif = autoNav.target_x - autoNav.ned_x
+                    y_dif = autoNav.target_y - autoNav.ned_y
+                    dist = math.pow(x_dif**2 + y_dif**2, 0.5) 
+                    if dist < 3:
+                        autoNav.state = 1
+                        rate.sleep()
+                '''else:
                     initTime = rospy.Time.now().secs
                     while ((not rospy.is_shutdown()) and 
                         (len(autoNav.objects_list) < 2 or autoNav.distance < 2)):
-                        if rospy.Time.now().secs - initTime > 2:
+                        if rospy.Time.now().secs - initTime > 1:
                             autoNav.state = 1
                             rate.sleep()
-                            break
+                            break'''
                 last_detection = autoNav.objects_list
 
         if autoNav.state == 1:
             autoNav.test.publish(autoNav.state)
+            print(autoNav.objects_list)
             if len(autoNav.objects_list) >= 2:
                 autoNav.state = 2
             else:
@@ -295,16 +306,23 @@ def main():
                 autoNav.test.publish(autoNav.state)
                 if len(autoNav.objects_list) >= 2:
                     autoNav.calculate_distance_to_boat()
-                if len(autoNav.objects_list) >= 2 and autoNav.distance >= 2:
+                if len(autoNav.objects_list) >= 2 and autoNav.distance >= 3:
                     autoNav.center_point()
                 else:
+                    x_dif = autoNav.target_x - autoNav.ned_x
+                    y_dif = autoNav.target_y - autoNav.ned_y
+                    dist = math.pow(x_dif**2 + y_dif**2, 0.5) 
+                    if dist < 3:
+                        autoNav.state = 3
+                        rate.sleep()
+                '''else:
                     initTime = rospy.Time.now().secs
                     while ((not rospy.is_shutdown()) and 
                         (len(autoNav.objects_list) < 2 or autoNav.distance < 2)):
                         if rospy.Time.now().secs - initTime > 2:
                             autoNav.state = 3
                             rate.sleep()
-                            break
+                            break'''
                 last_detection = autoNav.objects_list
 
         elif autoNav.state == 3:

rb_missions/scripts/obstacle_channel.py

@@ -37,7 +37,7 @@ def __init__(self):
         self.state = -1
         self.distance = 0
         self.distance_to_last = 0
-        self.offset = .55 #camera to ins offset
+        self.offset = .25 #camera to ins offset
         self.ned_channel_origin_x = 0
         self.ned_channel_origin_y = 0
         self.ned_alpha = 0
@@ -66,7 +66,7 @@ def ins_pose_callback(self,pose):
     def objs_callback(self,data):
         self.objects_list = []
         for i in range(data.len):
-            if str(data.objects[i].clase) == 'buoy':
+            if str(data.objects[i].clase) == 'buoy' and data.objects[i].X > 0.0:
                 self.objects_list.append({'X' : data.objects[i].X + self.offset, 
                                       'Y' : -data.objects[i].Y, #Negate sensor input in Y
                                       'color' : data.objects[i].color, 
@@ -337,6 +337,7 @@ def main():
     rate = rospy.Rate(20)
     obsChan = ObsChan()
     last_detection = []
+    time.sleep(10)
     while not rospy.is_shutdown() and obsChan.activated:
         if obsChan.objects_list != last_detection:
             if obsChan.state == -1:

rb_missions/scripts/obstacle_simulator.py

@@ -26,6 +26,7 @@
 from visualization_msgs.msg import Marker
 from visualization_msgs.msg import MarkerArray
 
+challenge_number = rospy.get_param("obstacle_simulator/challenge_number", 0)
 
 class ObstacleSimulator:
     def __init__(self):
@@ -36,11 +37,11 @@ def __init__(self):
         self.ned_y = 0
         self.yaw = 0
 
-        self.challenge = 3 #0 for AutonomousNavigation, 1 for SpeedChallenge, 2 for ObstacleChannel, 3 for ObstacleField
+        self.challenge = challenge_number #0 for AutonomousNavigation, 1 for SpeedChallenge, 2 for ObstacleChannel, 3 for ObstacleField
         self.obstacle_list = []
 
         self.max_visible_radius = 10
-        self.sensor_to_usv_offset = 0.55
+        self.sensor_to_usv_offset = 0.25
 
         rospy.Subscriber("/vectornav/ins_2d/NED_pose", Pose2D, self.ins_pose_callback)
 

rb_missions/scripts/speed_challenge.py

@@ -74,7 +74,7 @@ def ins_pose_callback(self,pose):
     def objs_callback(self,data):
         self.objects_list = []
         for i in range(data.len):
-            if str(data.objects[i].clase) == 'buoy':
+            if str(data.objects[i].clase) == 'buoy' and data.objects[i].X > 0.0:
                 self.objects_list.append({'X' : data.objects[i].X + self.offset,
                                           'Y' : data.objects[i].Y,
                                           'color' : data.objects[i].color, 
@@ -130,8 +130,11 @@ def center_point(self):
         if (abs(self.ned_alpha) > (math.pi)):
             self.ned_alpha = (self.ned_alpha/abs(self.ned_alpha))*(abs(self.ned_alpha) - 2*math.pi)
 
-        xm, ym = self.gate_to_body(2,0,alpha,xc,yc)
+        xm, ym = self.gate_to_body(5,0,alpha,xc,yc)
         self.target_x, self.target_y = self.body_to_ned(xm, ym)
+
+        print(x1,y1,x2,y2,xc,yc)
+
         self.gate_x, self.gate_y = self.body_to_ned(xc, yc)
 
         path_array = Float32MultiArray()
@@ -179,7 +182,9 @@ def buoy_waypoints(self,buoy_x,buoy_y):
         '''
         rospy.loginfo("Buoy waypoints has just started")
 
-        radius = 3
+        radius = 2
+
+        print(buoy_x, buoy_y, self.gate_x, self.gate_y)
 
         w1 = [buoy_x, buoy_y + radius]
         w2 = [buoy_x + radius, buoy_y]
@@ -194,6 +199,8 @@ def buoy_waypoints(self,buoy_x,buoy_y):
         w5_x, w5_y = self.gate_to_ned(-5, 0, self.ned_alpha, self.gate_x, self.gate_y)
         path_array.data = [w1_x, w1_y, w2_x, w2_y, w3_x, w3_y,
                           self.gate_x, self.gate_y, w5_x, w5_y, 0]
+
+        print(path_array.data)
         self.desired(path_array)
 
         return(w5_x, w5_y)
@@ -289,6 +296,7 @@ def main():
     speedChallenge = SpeedChallenge()
     speedChallenge.distance = 4
     last_detection = []
+    time.sleep(10)
     while not rospy.is_shutdown() and speedChallenge.activated:
         if speedChallenge.objects_list != last_detection:
             if speedChallenge.state == -1:
@@ -297,21 +305,27 @@ def main():
                     rate.sleep()
                 speedChallenge.state = 0
                 last_detection = speedChallenge.objects_list
-            if speedChallenge.state == 0:
-                speedChallenge.test.publish(speedChallenge.state)
-                if len(speedChallenge.objects_list) >= 2:
-                    speedChallenge.calculate_distance_to_boat()
-                if len(speedChallenge.objects_list) >= 2 and speedChallenge.distance >= 2:
-                    speedChallenge.center_point()
-                else:
-                    initTime = rospy.Time.now().secs
-                    while ((not rospy.is_shutdown()) and
-                        (len(speedChallenge.objects_list) < 2 or speedChallenge.distance < 2)):
-                        if rospy.Time.now().secs - initTime > 2:
-                            speedChallenge.state = 1
-                            rate.sleep()
-                            break
-                last_detection = speedChallenge.objects_list
+        if speedChallenge.state == 0:
+            speedChallenge.test.publish(speedChallenge.state)
+            if len(speedChallenge.objects_list) >= 2:
+                speedChallenge.calculate_distance_to_boat()
+            if len(speedChallenge.objects_list) >= 2 and speedChallenge.distance >= 2:
+                speedChallenge.center_point()
+            else:
+                x_dif = speedChallenge.target_x - speedChallenge.ned_x
+                y_dif = speedChallenge.target_y - speedChallenge.ned_y
+                dist = math.pow(x_dif**2 + y_dif**2, 0.5) 
+                if dist < 2:
+                    speedChallenge.state = 1
+            '''else:
+                initTime = rospy.Time.now().secs
+                while ((not rospy.is_shutdown()) and
+                    (len(speedChallenge.objects_list) < 2 or speedChallenge.distance < 2)):
+                    if rospy.Time.now().secs - initTime > 2:
+                        speedChallenge.state = 1
+                        rate.sleep()
+                        break'''
+            last_detection = speedChallenge.objects_list
         if speedChallenge.state == 1:
             speedChallenge.test.publish(speedChallenge.state)
             x_list = []
@@ -350,7 +364,7 @@ def main():
                     distance_list.append(math.pow(x_list[i]**2 + y_list[i]**2, 0.5))
                     ind_0 = np.argsort(distance_list)[0]
                 if ((len(speedChallenge.objects_list) >= 1) and
-                    (speedChallenge.objects_list[ind_0]['X'] < 7)):
+                    (speedChallenge.objects_list[ind_0]['X'] < 5)):
                     buoy_x = speedChallenge.objects_list[ind_0]['X']
                     buoy_y = speedChallenge.objects_list[ind_0]['Y']
                     speedChallenge.state = 3