Merge pull request #44 from vanttec/feature/release_candidate_v2.1

Feature/release candidate v2.1
.vscode/settings.json file could be eliminated

.github/CODEOWNERS

@@ -2,4 +2,4 @@
 # Each line is a file pattern followed by one or more owners.
 
 # These owners will be the default owners for everything in the repo.
-*       @pedrosc97 @ivanacoll
+*       @alexglzg @vSebas

.gitmodules

@@ -9,4 +9,7 @@
 	url = http://github.com/vanttec/velodyne_ros.git
 [submodule "zed_ros_wrapper"]
 	path = zed_ros_wrapper
-	url = http://github.com/vanttec/zed_ros_wrapper.git
+  url = http://github.com/vanttec/zed_ros_wrapper.git
+[submodule "usv_avoidance/include/acados"]
+	path = usv_avoidance/include/acados
+	url = https://github.com/acados/acados.git

.vscode/settings.json

@@ -1,5 +1,60 @@
 {
     "files.associations": {
-        "*.cuh": "cpp"
+      "*.txt": "makefile",
+      "*.cuh": "cpp",
+      "cctype": "cpp",
+      "clocale": "cpp",
+      "cmath": "cpp",
+      "csignal": "cpp",
+      "cstdarg": "cpp",
+      "cstddef": "cpp",
+      "cstdio": "cpp",
+      "cstdlib": "cpp",
+      "cstring": "cpp",
+      "ctime": "cpp",
+      "cwchar": "cpp",
+      "cwctype": "cpp",
+      "array": "cpp",
+      "atomic": "cpp",
+      "strstream": "cpp",
+      "*.tcc": "cpp",
+      "bitset": "cpp",
+      "chrono": "cpp",
+      "complex": "cpp",
+      "condition_variable": "cpp",
+      "cstdint": "cpp",
+      "deque": "cpp",
+      "list": "cpp",
+      "unordered_map": "cpp",
+      "vector": "cpp",
+      "exception": "cpp",
+      "algorithm": "cpp",
+      "functional": "cpp",
+      "ratio": "cpp",
+      "system_error": "cpp",
+      "tuple": "cpp",
+      "type_traits": "cpp",
+      "fstream": "cpp",
+      "initializer_list": "cpp",
+      "iomanip": "cpp",
+      "iosfwd": "cpp",
+      "iostream": "cpp",
+      "istream": "cpp",
+      "limits": "cpp",
+      "memory": "cpp",
+      "mutex": "cpp",
+      "new": "cpp",
+      "ostream": "cpp",
+      "numeric": "cpp",
+      "sstream": "cpp",
+      "stdexcept": "cpp",
+      "streambuf": "cpp",
+      "thread": "cpp",
+      "cfenv": "cpp",
+      "cinttypes": "cpp",
+      "utility": "cpp",
+      "typeindex": "cpp",
+      "typeinfo": "cpp",
+      "valarray": "cpp"
     }
 }

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

@@ -5,7 +5,7 @@
 ----------------------------------------------------------
     @file: auto_nav_position.py
     @date: Thu Dec 26, 2019
-    @modified: Sat Mar 21, 2020
+    @modified: Sat May 15, 2021
     @author: Alejandro Gonzalez Garcia
     @e-mail: [email protected]
     @co-author: Rodolfo Cuan Urquizo
@@ -42,14 +42,15 @@ 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
 
         # ROS Subscribers
         rospy.Subscriber("/vectornav/ins_2d/NED_pose", Pose2D, self.ins_pose_callback)
-        rospy.Subscriber("/usv_perception/yolo_zed/objects_detected", obj_detected_list, self.objs_callback)
+        #rospy.Subscriber("/usv_perception/yolo_zed/objects_detected", obj_detected_list, self.objs_callback)
+        rospy.Subscriber("/usv_perception/lidar/objects_detected", obj_detected_list, self.objs_callback)
 
         # ROS Publishers
         self.path_pub = rospy.Publisher("/mission/waypoints", Float32MultiArray, queue_size=10)
@@ -64,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) == 'bouy':
+            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, 
@@ -128,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):
@@ -246,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:
@@ -263,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:
@@ -294,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

@@ -5,6 +5,7 @@
 ----------------------------------------------------------
     @file: obstacle_channel.py
     @date: Mon Jun 8, 2020
+    @modified: Sat May 15, 2021
     @author: Alejandro Gonzalez Garcia
     @e-mail: [email protected]
     @brief: Motion planning. Script to navigate a USV through
@@ -36,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
@@ -49,7 +50,8 @@ def __init__(self):
 
         # ROS Subscribers
         rospy.Subscriber("/vectornav/ins_2d/NED_pose", Pose2D, self.ins_pose_callback)
-        rospy.Subscriber("/usv_perception/yolo_zed/objects_detected", obj_detected_list, self.objs_callback)
+        #rospy.Subscriber("/usv_perception/yolo_zed/objects_detected", obj_detected_list, self.objs_callback)
+        rospy.Subscriber("/usv_perception/lidar/objects_detected", obj_detected_list, self.objs_callback)
 
         # ROS Publishers
         self.path_pub = rospy.Publisher("/mission/waypoints", Float32MultiArray, queue_size=10)
@@ -64,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) == 'bouy':
+            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, 
@@ -335,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:
@@ -361,7 +364,7 @@ def main():
         elif obsChan.state == 1:
             obsChan.test.publish(obsChan.state)
             time.sleep(1)
-            obsChan.status_pub.publish(1)
+            obsChan.status_pub.publish(3)
 
         rate.sleep()
     rospy.spin()