diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 28a26df3..5246d6b6 100644 --- a/.github/CODEOWNERS +++ b/.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 diff --git a/.gitmodules b/.gitmodules index 823a0394..6704a99a 100644 --- a/.gitmodules +++ b/.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 diff --git a/.vscode/settings.json b/.vscode/settings.json index b2c23cc4..8fd57052 100644 --- a/.vscode/settings.json +++ b/.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" } } \ No newline at end of file diff --git a/rb_missions/launch/auto_nav.launch b/rb_missions/launch/auto_nav.launch new file mode 100644 index 00000000..4c1dcadb --- /dev/null +++ b/rb_missions/launch/auto_nav.launch @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/rb_missions/launch/obs_chan.launch b/rb_missions/launch/obs_chan.launch new file mode 100644 index 00000000..2df55dda --- /dev/null +++ b/rb_missions/launch/obs_chan.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/rb_missions/launch/sim/sim_auto_nav.launch b/rb_missions/launch/sim/sim_auto_nav.launch new file mode 100644 index 00000000..92759736 --- /dev/null +++ b/rb_missions/launch/sim/sim_auto_nav.launch @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/rb_missions/launch/sim/sim_obs_chan.launch b/rb_missions/launch/sim/sim_obs_chan.launch new file mode 100644 index 00000000..e5a6dec4 --- /dev/null +++ b/rb_missions/launch/sim/sim_obs_chan.launch @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + diff --git a/rb_missions/launch/sim/sim_speed_ch.launch b/rb_missions/launch/sim/sim_speed_ch.launch new file mode 100644 index 00000000..993f1188 --- /dev/null +++ b/rb_missions/launch/sim/sim_speed_ch.launch @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/rb_missions/launch/speed_ch.launch b/rb_missions/launch/speed_ch.launch new file mode 100644 index 00000000..4de1f05e --- /dev/null +++ b/rb_missions/launch/speed_ch.launch @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/rb_missions/scripts/auto_nav_position.py b/rb_missions/scripts/auto_nav_position.py index 72e8b862..8a62e2b4 100755 --- a/rb_missions/scripts/auto_nav_position.py +++ b/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: alexglzg97@gmail.com @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: diff --git a/rb_missions/scripts/obstacle_channel.py b/rb_missions/scripts/obstacle_channel.py index 4bdd2448..f0261162 100755 --- a/rb_missions/scripts/obstacle_channel.py +++ b/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: alexglzg97@gmail.com @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() diff --git a/rb_missions/scripts/obstacle_field.py b/rb_missions/scripts/obstacle_field.py new file mode 100755 index 00000000..fa38f363 --- /dev/null +++ b/rb_missions/scripts/obstacle_field.py @@ -0,0 +1,192 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +''' +---------------------------------------------------------- + @file: obstacle_field.py + @date: Sat May 15, 2021 + @author: Alejandro Gonzalez Garcia + @e-mail: alexglzg97@gmail.com + @brief: Motion planning. Script to select the marker + the USV has to circle in the Obstacle Field + challenge for the RoboBoat competition. Motion + planning selected to circle and send waypoints + to an obstacle avoidance script. + @version: 1.0 + Open source +--------------------------------------------------------- +''' + +import math +import time + +import matplotlib.pyplot as plt +import numpy as np +import rospy +from geometry_msgs.msg import Pose2D +from std_msgs.msg import Float32MultiArray, Int32, String +from sensor_msgs.msg import PointCloud2 +import sensor_msgs.point_cloud2 as pc2 + +from usv_perception.msg import obj_detected, obj_detected_list + +#EARTH_RADIUS = 6371000 + +# Class Definition +class ObsField: + def __init__(self): + self.objects_list = [] + self.activated = True + self.state = -1 + self.ned_x = 0 + self.ned_y = 0 + self.yaw = 0 + self.lat = 0 + self.lon = 0 + self.InitTime = rospy.Time.now().secs + self.distance = 0 + self.offset = .55 #camera to ins offset + self.target_x = 0 + self.target_y = 0 + self.gate_x = 0 + self.gate_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/lidar/objects_detected", obj_detected_list, self.objs_callback) + + # ROS Publishers + self.path_pub = rospy.Publisher("/mission/waypoints", Float32MultiArray, queue_size=10) + self.status_pub = rospy.Publisher("/mission/status", Int32, queue_size=10) + self.test = rospy.Publisher("/mission/state", Int32, queue_size=10) + + def ins_pose_callback(self,pose): + self.ned_x = pose.x + self.ned_y = pose.y + self.yaw = pose.theta + + def objs_callback(self,data): + self.objects_list = [] + for i in range(data.len): + if str(data.objects[i].clase) == 'marker': + self.objects_list.append({'X' : data.objects[i].X + self.offset, + 'Y' : data.objects[i].Y, + 'color' : data.objects[i].color, + 'class' : data.objects[i].clase}) + + def buoy_waypoints(self,buoy_x,buoy_y): + ''' + @name: buoy_waypoints + @brief: Returns 4 waypoints. The first three form a circle a certain radius + from the obstacle, and the next waypoints return the vehicle the starting + point + @param: buoy_x: buoy x coordinate + buoy_y: buoy y coordinate + @return: -- + ''' + rospy.loginfo("Marker waypoints has just started") + + radius = 2.0 + + w1 = [buoy_x, buoy_y + radius] + w2 = [buoy_x + radius, buoy_y] + w3 = [buoy_x, buoy_y - radius] + + path_array = Float32MultiArray() + path_array.layout.data_offset = 11 + + w1_x, w1_y = self.body_to_ned(w1[0], w1[1]) + w2_x, w2_y = self.body_to_ned(w2[0], w2[1]) + w3_x, w3_y = self.body_to_ned(w3[0], w3[1]) + path_array.data = [self.ned_x, self.ned_y, + w1_x, w1_y, w2_x, w2_y, w3_x, w3_y, + self.ned_x, self.ned_y, 0] + self.desired(path_array) + + return(self.ned_x, self.ned_y) + + def body_to_ned(self, x2, y2): + ''' + @name: body_to_ned + @brief: Coordinate transformation between body and NED reference frames. + @param: x2: target x coordinate in body reference frame + y2: target y coordinate in body reference frame + @return: ned_x2: target x coordinate in ned reference frame + ned_y2: target y coordinate in ned reference frame + ''' + p = np.array([x2, y2]) + J = self.rotation_matrix(self.yaw) + n = J.dot(p) + ned_x2 = n[0] + self.ned_x + ned_y2 = n[1] + self.ned_y + return (ned_x2, ned_y2) + + def rotation_matrix(self, angle): + ''' + @name: rotation_matrix + @brief: Transformation matrix template. + @param: angle: angle of rotation + @return: J: transformation matrix + ''' + J = np.array([[math.cos(angle), -1*math.sin(angle)], + [math.sin(angle), math.cos(angle)]]) + return (J) + + def desired(self, path): + self.path_pub.publish(path) + +def main(): + rospy.init_node("obstacle_field", anonymous=False) + rate = rospy.Rate(20) + obsField = ObsField() + last_detection = [] + while not rospy.is_shutdown() and obsField.activated: + if obsField.objects_list != last_detection: + if obsField.state == -1: + while (not rospy.is_shutdown()) and (len(obsField.objects_list) < 0): + obsField.test.publish(obsField.state) + rate.sleep() + obsField.state = 0 + last_detection = obsField.objects_list + if obsField.state == 0: + obsField.test.publish(obsField.state) + x_list = [] + y_list = [] + class_list = [] + distance_list = [] + for i in range(len(obsField.objects_list)): + x_list.append(obsField.objects_list[i]['X']) + y_list.append(obsField.objects_list[i]['Y']) + class_list.append(obsField.objects_list[i]['class']) + distance_list.append(math.pow(x_list[i]**2 + y_list[i]**2, 0.5)) + ind_0 = np.argsort(distance_list)[0] + if ((len(obsField.objects_list) >= 1)): + marker_x = obsField.objects_list[ind_0]['X'] + marker_y = - obsField.objects_list[ind_0]['Y'] + obsField.state = 1 + last_detection = obsField.objects_list + if obsField.state == 1: + obsField.test.publish(obsField.state) + x_final, y_final = obsField.buoy_waypoints(marker_x,marker_y) + obsField.state = 2 + last_detection = obsField.objects_list + if obsField.state == 2: + obsField.test.publish(obsField.state) + x_squared = math.pow(x_final - obsField.ned_x, 2) + y_squared = math.pow(y_final - obsField.ned_y, 2) + distance_final = math.pow(x_squared + y_squared, 0.5) + if distance_final > 1: + pass + else: + obsField.status_pub.publish(4) + last_detection = obsField.objects_list + rate.sleep() + rospy.spin() + +if __name__ == "__main__": + try: + main() + except rospy.ROSInterruptException: + pass diff --git a/rb_missions/scripts/obstacle_simulator.py b/rb_missions/scripts/obstacle_simulator.py index 170e02a2..b9950528 100755 --- a/rb_missions/scripts/obstacle_simulator.py +++ b/rb_missions/scripts/obstacle_simulator.py @@ -5,11 +5,11 @@ ---------------------------------------------------------- @file: obstacle_simulator.py @date: Sun Mar 22, 2020 - @modified: Mon Jun 8, 2020 + @modified: Sat May 15, 2021 @author: Alejandro Gonzalez Garcia @e-mail: alexglzg97@gmail.com @brief: Obstacle simulation for mission testing. - @version: 1.1 + @version: 1.2 Open source ---------------------------------------------------------- ''' @@ -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,15 +37,16 @@ def __init__(self): self.ned_y = 0 self.yaw = 0 - self.challenge = 1 #0 for AutonomousNavigation, 1 for SpeedChallenge, 2 for ObstacleChannel + 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) - self.detector_pub = rospy.Publisher('/usv_perception/yolo_zed/objects_detected', obj_detected_list, queue_size=10) + #self.detector_pub = rospy.Publisher('/usv_perception/yolo_zed/objects_detected', obj_detected_list, queue_size=10) + self.detector_pub = rospy.Publisher('/usv_perception/lidar/objects_detected', obj_detected_list, queue_size=10) self.marker_pub = rospy.Publisher("/usv_perception/lidar_detector/markers", MarkerArray, queue_size=10) def ins_pose_callback(self,pose): @@ -69,10 +71,11 @@ def simulate(self): distance = math.pow(delta_x*delta_x + delta_y*delta_y, 0.5) if (distance < self.max_visible_radius): x, y = self.ned_to_body(x, y) - if x > 1: + if x > 1 or self.challenge == 3: obstacle = obj_detected() obstacle.X = x - self.sensor_to_usv_offset obstacle.Y = -y + obstacle.R = self.obstacle_list[i]['R'] obstacle.color = self.obstacle_list[i]['color'] obstacle.clase = self.obstacle_list[i]['class'] list_length += 1 @@ -134,52 +137,68 @@ def rviz_markers(self): ''' marker_array = MarkerArray() for i in range(len(self.obstacle_list)): - x = self.obstacle_list[i]['X'] - y = -self.obstacle_list[i]['Y'] - if self.challenge == 0: - diameter = 0.254 - if self.challenge == 1: - diameter = 0.39 - if self.challenge == 2: - diameter = 0.21 - marker = Marker() - marker.header.frame_id = "/world" - marker.type = marker.SPHERE - marker.action = marker.ADD - if self.obstacle_list[i]['color'] == 'yellow': - marker.color.a = 1.0 - marker.color.r = 1.0 - marker.color.g = 1.0 - marker.color.b = 0.0 - diameter = 0.39 - elif self.obstacle_list[i]['color'] == 'red': - marker.color.a = 1.0 - marker.color.r = 1.0 - marker.color.g = 0.0 - marker.color.b = 0.0 - elif self.obstacle_list[i]['color'] == 'green': - marker.color.a = 1.0 - marker.color.r = 0.0 - marker.color.g = 1.0 - marker.color.b = 0.0 - elif self.obstacle_list[i]['color'] == 'blue': + if self.obstacle_list[i]['class'] == 'buoy': + x = self.obstacle_list[i]['X'] + y = -self.obstacle_list[i]['Y'] + diameter = 2*self.obstacle_list[i]['R'] + marker = Marker() + marker.header.frame_id = "/world" + marker.type = marker.SPHERE + marker.action = marker.ADD + if self.obstacle_list[i]['color'] == 'yellow': + marker.color.a = 1.0 + marker.color.r = 1.0 + marker.color.g = 1.0 + marker.color.b = 0.0 + elif self.obstacle_list[i]['color'] == 'red': + marker.color.a = 1.0 + marker.color.r = 1.0 + marker.color.g = 0.0 + marker.color.b = 0.0 + elif self.obstacle_list[i]['color'] == 'green': + marker.color.a = 1.0 + marker.color.r = 0.0 + marker.color.g = 1.0 + marker.color.b = 0.0 + elif self.obstacle_list[i]['color'] == 'blue': + marker.color.a = 1.0 + marker.color.r = 0.0 + marker.color.g = 0.0 + marker.color.b = 1.0 + marker.scale.x = diameter + marker.scale.y = diameter + marker.scale.z = diameter + marker.pose.orientation.w = 1.0 + marker.pose.position.x = x + marker.pose.position.y = y + marker.pose.position.z = 0 + if self.challenge == 0: + marker.type = marker.CYLINDER + marker.scale.z = 1 + marker.pose.position.z = 0.5 + marker.id = i + marker_array.markers.append(marker) + elif self.obstacle_list[i]['class'] == 'marker': + x = self.obstacle_list[i]['X'] + y = -self.obstacle_list[i]['Y'] + diameter = 2*self.obstacle_list[i]['R'] + marker = Marker() + marker.header.frame_id = "/world" + marker.type = marker.CYLINDER + marker.action = marker.ADD marker.color.a = 1.0 marker.color.r = 0.0 marker.color.g = 0.0 marker.color.b = 1.0 - marker.scale.x = diameter - marker.scale.y = diameter - marker.scale.z = diameter - marker.pose.orientation.w = 1.0 - marker.pose.position.x = x - marker.pose.position.y = y - marker.pose.position.z = 0 - if self.challenge == 0: - marker.type = marker.CYLINDER - marker.scale.z = 1 - marker.pose.position.z = 0.5 - marker.id = i - marker_array.markers.append(marker) + marker.scale.x = diameter + marker.scale.y = diameter + marker.scale.z = 1.54 + marker.pose.orientation.w = 1.0 + marker.pose.position.x = x + marker.pose.position.y = y + marker.pose.position.z = 0.77 + marker.id = i + marker_array.markers.append(marker) # Publish the MarkerArray self.marker_pub.publish(marker_array) @@ -190,115 +209,250 @@ def main(): if obstacleSimulator.challenge == 0: obstacleSimulator.obstacle_list.append({'X' : 6.5, 'Y' : -0.5, + 'R' : 0.127, 'color' : 'red', - 'class' : 'bouy'}) + 'class' : 'buoy'}) obstacleSimulator.obstacle_list.append({'X' : 3.5, 'Y' : 2.5, + 'R' : 0.127, 'color' : 'green', - 'class' : 'bouy'}) + 'class' : 'buoy'}) obstacleSimulator.obstacle_list.append({'X' : 22.5, 'Y' : 15.5, + 'R' : 0.127, 'color' : 'red', - 'class' : 'bouy'}) + 'class' : 'buoy'}) obstacleSimulator.obstacle_list.append({'X' : 19.5, 'Y' : 18.5, + 'R' : 0.127, 'color' : 'green', - 'class' : 'bouy'}) + 'class' : 'buoy'}) elif obstacleSimulator.challenge == 1: obstacleSimulator.obstacle_list.append({'X' : 6.5, 'Y' : -0.5, + 'R' : 0.195, 'color' : 'red', - 'class' : 'bouy'}) + 'class' : 'buoy'}) obstacleSimulator.obstacle_list.append({'X' : 3.5, 'Y' : 2.5, + 'R' : 0.195, 'color' : 'green', - 'class' : 'bouy'}) + 'class' : 'buoy'}) obstacleSimulator.obstacle_list.append({'X' : 21, 'Y' : 17, + 'R' : 0.195, 'color' : 'blue', - 'class' : 'bouy'}) + 'class' : 'buoy'}) elif obstacleSimulator.challenge == 2: obstacleSimulator.max_visible_radius = 50 obstacleSimulator.obstacle_list.append({'X' : 7, 'Y' : 2, + 'R' : 0.105, 'color' : 'green', - 'class' : 'bouy'}) + 'class' : 'buoy'}) obstacleSimulator.obstacle_list.append({'X' : 7, 'Y' : 5, + 'R' : 0.105, 'color' : 'red', - 'class' : 'bouy'}) + 'class' : 'buoy'}) obstacleSimulator.obstacle_list.append({'X' :14, 'Y' : 4.7, + 'R' : 0.105, 'color' : 'green', - 'class' : 'bouy'}) + 'class' : 'buoy'}) obstacleSimulator.obstacle_list.append({'X' : 14, 'Y' : 7.7, + 'R' : 0.105, 'color' : 'red', - 'class' : 'bouy'}) + 'class' : 'buoy'}) obstacleSimulator.obstacle_list.append({'X' : 16, 'Y' : 5.5, + 'R' : 0.195, 'color' : 'yellow', - 'class' : 'bouy'}) + 'class' : 'buoy'}) obstacleSimulator.obstacle_list.append({'X' : 21, 'Y' : 5.2, + 'R' : 0.105, 'color' : 'green', - 'class' : 'bouy'}) + 'class' : 'buoy'}) obstacleSimulator.obstacle_list.append({'X' :21, 'Y' : 8.2, + 'R' : 0.105, 'color' : 'red', - 'class' : 'bouy'}) + 'class' : 'buoy'}) obstacleSimulator.obstacle_list.append({'X' : 28, 'Y' : 4.3, + 'R' : 0.105, 'color' : 'green', - 'class' : 'bouy'}) + 'class' : 'buoy'}) obstacleSimulator.obstacle_list.append({'X' : 28, 'Y' : 7.3, + 'R' : 0.105, 'color' : 'red', - 'class' : 'bouy'}) + 'class' : 'buoy'}) obstacleSimulator.obstacle_list.append({'X' : 35, 'Y' : 1, + 'R' : 0.105, 'color' : 'green', - 'class' : 'bouy'}) + 'class' : 'buoy'}) obstacleSimulator.obstacle_list.append({'X' :35, 'Y' : 4, + 'R' : 0.105, 'color' : 'red', - 'class' : 'bouy'}) + 'class' : 'buoy'}) obstacleSimulator.obstacle_list.append({'X' : 40, 'Y' : 3, + 'R' : 0.195, 'color' : 'yellow', - 'class' : 'bouy'}) + 'class' : 'buoy'}) obstacleSimulator.obstacle_list.append({'X' : 42, 'Y' : 0.8, + 'R' : 0.105, 'color' : 'green', - 'class' : 'bouy'}) + 'class' : 'buoy'}) obstacleSimulator.obstacle_list.append({'X' : 42, 'Y' : 3.8, + 'R' : 0.105, 'color' : 'red', - 'class' : 'bouy'}) + 'class' : 'buoy'}) obstacleSimulator.obstacle_list.append({'X' :49, 'Y' : 1.5, + 'R' : 0.105, 'color' : 'green', - 'class' : 'bouy'}) + 'class' : 'buoy'}) obstacleSimulator.obstacle_list.append({'X' : 49, 'Y' : 4.5, + 'R' : 0.105, 'color' : 'red', - 'class' : 'bouy'}) + 'class' : 'buoy'}) obstacleSimulator.obstacle_list.append({'X' : 56, 'Y' : 3, + 'R' : 0.105, 'color' : 'green', - 'class' : 'bouy'}) + 'class' : 'buoy'}) obstacleSimulator.obstacle_list.append({'X' : 56, 'Y' : 6, + 'R' : 0.105, 'color' : 'red', - 'class' : 'bouy'}) + 'class' : 'buoy'}) obstacleSimulator.obstacle_list.append({'X' :63, 'Y' : 5, + 'R' : 0.105, 'color' : 'green', - 'class' : 'bouy'}) + 'class' : 'buoy'}) obstacleSimulator.obstacle_list.append({'X' : 63, 'Y' : 8, + 'R' : 0.105, + 'color' : 'red', + 'class' : 'buoy'}) + elif obstacleSimulator.challenge == 3: + obstacleSimulator.max_visible_radius = 50 + obstacleSimulator.obstacle_list.append({'X' : 3.1, + 'Y' : 5.1, + 'R' : 0.105, + 'color' : 'green', + 'class' : 'buoy'}) + obstacleSimulator.obstacle_list.append({'X' : 3.3, + 'Y' : 7.2, + 'R' : 0.105, + 'color' : 'red', + 'class' : 'buoy'}) + obstacleSimulator.obstacle_list.append({'X' : 3.2, + 'Y' : 1.3, + 'R' : 0.105, + 'color' : 'yellow', + 'class' : 'buoy'}) + obstacleSimulator.obstacle_list.append({'X' : 6.2, + 'Y' : 5.2, + 'R' : 0.105, + 'color' : 'green', + 'class' : 'buoy'}) + obstacleSimulator.obstacle_list.append({'X' : 4.1, + 'Y' : 0.2, + 'R' : 0.105, + 'color' : 'yellow', + 'class' : 'buoy'}) + obstacleSimulator.obstacle_list.append({'X' : 4.4, + 'Y' : 3.5, + 'R' : 0.105, + 'color' : 'yellow', + 'class' : 'buoy'}) + obstacleSimulator.obstacle_list.append({'X' : 5.8, + 'Y' : 1.4, + 'R' : 0.105, + 'color' : 'red', + 'class' : 'buoy'}) + obstacleSimulator.obstacle_list.append({'X' : 6.4, + 'Y' : 6.3, + 'R' : 0.105, + 'color' : 'green', + 'class' : 'buoy'}) + obstacleSimulator.obstacle_list.append({'X' : 9.1, + 'Y' : 0.4, + 'R' : 0.105, + 'color' : 'red', + 'class' : 'buoy'}) + obstacleSimulator.obstacle_list.append({'X' : 9.6, + 'Y' : 1.6, + 'R' : 0.105, + 'color' : 'green', + 'class' : 'buoy'}) + obstacleSimulator.obstacle_list.append({'X' : 12.6, + 'Y' : 7.4, + 'R' : 0.105, + 'color' : 'red', + 'class' : 'buoy'}) + obstacleSimulator.obstacle_list.append({'X' : 10.7, + 'Y' : 0.6, + 'R' : 0.105, + 'color' : 'green', + 'class' : 'buoy'}) + obstacleSimulator.obstacle_list.append({'X' : 10.3, + 'Y' : 10.1, + 'R' : 0.105, + 'color' : 'yellow', + 'class' : 'buoy'}) + obstacleSimulator.obstacle_list.append({'X' : 9.3, + 'Y' : 8.1, + 'R' : 0.105, 'color' : 'red', - 'class' : 'bouy'}) + 'class' : 'buoy'}) + obstacleSimulator.obstacle_list.append({'X' : 12.4, + 'Y' : 1.3, + 'R' : 0.105, + 'color' : 'yellow', + 'class' : 'buoy'}) + obstacleSimulator.obstacle_list.append({'X' : 13.6, + 'Y' : 3.6, + 'R' : 0.105, + 'color' : 'green', + 'class' : 'buoy'}) + obstacleSimulator.obstacle_list.append({'X' : 13.4, + 'Y' : 6.4, + 'R' : 0.105, + 'color' : 'red', + 'class' : 'buoy'}) + obstacleSimulator.obstacle_list.append({'X' : 14.2, + 'Y' : 4.2, + 'R' : 0.105, + 'color' : 'green', + 'class' : 'buoy'}) + obstacleSimulator.obstacle_list.append({'X' : 5.1, + 'Y' : 8.2, + 'R' : 0.105, + 'color' : 'yellow', + 'class' : 'buoy'}) + obstacleSimulator.obstacle_list.append({'X' : 7.7, + 'Y' : 9.6, + 'R' : 0.105, + 'color' : 'red', + 'class' : 'buoy'}) + obstacleSimulator.obstacle_list.append({'X' : 9.0, + 'Y' : 5.0, + 'R' : 0.114, + 'color' : 'blue', + 'class' : 'marker'}) + while not rospy.is_shutdown() and obstacleSimulator.active: obstacleSimulator.simulate() obstacleSimulator.rviz_markers() diff --git a/rb_missions/scripts/speed_challenge.py b/rb_missions/scripts/speed_challenge.py index 09ad1a25..e1924ba7 100755 --- a/rb_missions/scripts/speed_challenge.py +++ b/rb_missions/scripts/speed_challenge.py @@ -5,7 +5,7 @@ ---------------------------------------------------------- @file: speed_challenge.py @date: Thu 02 Jan, 2020 - @modified: Sat Mar 21, 2020 + @modified: Sat May 15, 2021 @author: Alejandro Gonzalez Garcia @e-mail: alexglzg97@gmail.com @co-author: Rodolfo Cuan Urquizo @@ -58,7 +58,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) @@ -73,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) == '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, 'color' : data.objects[i].color, @@ -129,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() @@ -178,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] @@ -193,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) @@ -288,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: @@ -296,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 = [] @@ -323,8 +338,9 @@ def main(): class_list.append(speedChallenge.objects_list[i]['class']) 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 - (str(speedChallenge.objects_list[ind_0]['color']) == 'blue')): + #if (len(speedChallenge.objects_list) >= 1 and + #(str(speedChallenge.objects_list[ind_0]['color']) == 'blue')): + if (len(speedChallenge.objects_list) >= 1): speedChallenge.state = 2 else: initTime = rospy.Time.now().secs @@ -348,9 +364,9 @@ 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)): - buoy_x = speedChallenge.objects_list[0]['X'] - buoy_y = speedChallenge.objects_list[0]['Y'] + (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 else: initTime = rospy.Time.now().secs diff --git a/usv_avoidance/CMakeLists.txt b/usv_avoidance/CMakeLists.txt new file mode 100644 index 00000000..850f3cb6 --- /dev/null +++ b/usv_avoidance/CMakeLists.txt @@ -0,0 +1,87 @@ +cmake_minimum_required(VERSION 3.0.2) +project(usv_avoidance) + +### Compile as C++11, supported in ROS Kinetic and newer +add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package( + catkin REQUIRED COMPONENTS + std_msgs + tf + roscpp + rospy + std_msgs + genmsg + sensor_msgs + tf2 + tf2_ros + tf2_geometry_msgs + usv_perception + visualization_msgs +) + +find_package( + Eigen3 REQUIRED +) + +set(acados_include "${PROJECT_SOURCE_DIR}/include/acados/include") +set(acados_lib "${PROJECT_SOURCE_DIR}/include/acados/lib") +#set(acados_include "/home/ivana/MPC_CollisionAvoidance/catkin_ws/src/nmpc_ca/acados/include") +#set(acados_lib "/home/ivana/MPC_CollisionAvoidance/catkin_ws/src/nmpc_ca/acados/lib") + +set(full_model_guidance_ca1 ${PROJECT_SOURCE_DIR}/scripts/usv_guidance_ca1/c_generated_code) + + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES usv_avoidance + CATKIN_DEPENDS rospy usv_perception +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( + ${catkin_INCLUDE_DIRS} + include + ${full_model_guidance_ca1} + ${acados_include} + ${acados_include}/blasfeo/include/ + ${acados_include}/hpipm/include/ + ${acados_include}/acados/ + ${acados_include}/qpOASES_e/ +) + +link_directories(${full_model_guidance_ca1}) + +# NODE +add_executable(nmpc_guidance_ca1 src/nmpc_guidance_ca1.cpp) +target_link_libraries(nmpc_guidance_ca1 + ${full_model_guidance_ca1}/libacados_ocp_solver_usv_model_guidance_ca1.so + #${acados_lib}/libacados.a + ${acados_lib}/libacados.so + ${catkin_LIBRARIES} +) +add_dependencies(nmpc_guidance_ca1 ${catkin_EXPORTED_TARGETS}) diff --git a/usv_avoidance/README.md b/usv_avoidance/README.md new file mode 100644 index 00000000..48eaa6e3 --- /dev/null +++ b/usv_avoidance/README.md @@ -0,0 +1,41 @@ +# usv_avoidance pkg + +## Setup +### Install Acados +``` + cd vanttec_usv_ws/src/usv_avoidance/ + git submodule update --recursive --init + cd vanttec_usv_ws/src/usv_avoidance/include/acados/ + mkdir -p build +``` +If on PC 64bit architecture platform: +``` + cd build + cmake -DACADOS_WITH_QPOASES=ON -DACADOS_WITH_OSQP=ON .. + make install +``` +If on JetsonTX2 platform: +``` + cd build + cmake -DACADOS_WITH_QPOASES=ON -DBLASFEO_TARGET=ARMV8A_ARM_CORTEX_A57 .. + make install +``` +Continue on all platforms: +``` + cd vanttec_usv_ws/src/usv_avoidance/include/acados/interfaces/acados_template + sudo python3.7 -m pip install -e . + cd tera_renderer + cargo build --verbose --release +``` +Replace t_render file in `vanttec_usv_ws/src/usv_avoidance/include/acados/bin` with t_renderer file generated in `vanttec_usv_ws/src/usv_avoidance/include/acados/interfaces/acados_template/t_renderer` + +### Run python script to generate C files +``` + cd vanttec_usv_ws/src/usv_avoidance/scripts/usv_guidance_ca1 + python3.7 main.py +``` +### Compile workspace +``` + cd vanttec_usv_ws + catkin_make +``` diff --git a/usv_avoidance/include/acados b/usv_avoidance/include/acados new file mode 160000 index 00000000..dfdc0734 --- /dev/null +++ b/usv_avoidance/include/acados @@ -0,0 +1 @@ +Subproject commit dfdc0734c8eeaf18705bebe6588d5dc88c1fc4ba diff --git a/usv_avoidance/package.xml b/usv_avoidance/package.xml new file mode 100644 index 00000000..d759d643 --- /dev/null +++ b/usv_avoidance/package.xml @@ -0,0 +1,65 @@ + + + usv_avoidance + 0.0.0 + The usv_avoidance package + + + + + ivana + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + rospy + roscpp + usv_perception + rospy + roscpp + usv_perception + + + + + + + + diff --git a/usv_avoidance/scripts/usv_guidance_ca1/__pycache__/acados_settings.cpython-37.pyc b/usv_avoidance/scripts/usv_guidance_ca1/__pycache__/acados_settings.cpython-37.pyc new file mode 100644 index 00000000..c746133d Binary files /dev/null and b/usv_avoidance/scripts/usv_guidance_ca1/__pycache__/acados_settings.cpython-37.pyc differ diff --git a/usv_avoidance/scripts/usv_guidance_ca1/__pycache__/plotFcn.cpython-37.pyc b/usv_avoidance/scripts/usv_guidance_ca1/__pycache__/plotFcn.cpython-37.pyc new file mode 100644 index 00000000..2bb21c78 Binary files /dev/null and b/usv_avoidance/scripts/usv_guidance_ca1/__pycache__/plotFcn.cpython-37.pyc differ diff --git a/usv_avoidance/scripts/usv_guidance_ca1/__pycache__/usv_model.cpython-37.pyc b/usv_avoidance/scripts/usv_guidance_ca1/__pycache__/usv_model.cpython-37.pyc new file mode 100644 index 00000000..23b36650 Binary files /dev/null and b/usv_avoidance/scripts/usv_guidance_ca1/__pycache__/usv_model.cpython-37.pyc differ diff --git a/usv_avoidance/scripts/usv_guidance_ca1/acados_ocp.json b/usv_avoidance/scripts/usv_guidance_ca1/acados_ocp.json new file mode 100644 index 00000000..895cfb09 --- /dev/null +++ b/usv_avoidance/scripts/usv_guidance_ca1/acados_ocp.json @@ -0,0 +1,797 @@ +{ + "acados_include_path": "/home/nvidia/vanttec_usv_ws/src/usv_avoidance/include/acados/include", + "acados_lib_path": "/home/nvidia/vanttec_usv_ws/src/usv_avoidance/include/acados/lib", + "constraints": { + "C": [], + "C_e": [], + "D": [], + "constr_type": "BGH", + "constr_type_e": "BGH", + "idxbu": [ + 0 + ], + "idxbx": [], + "idxbx_0": [ + 0, + 1, + 2, + 3, + 4, + 5, + 6, + 7 + ], + "idxbx_e": [], + "idxbxe_0": [ + 0, + 1, + 2, + 3, + 4, + 5, + 6, + 7 + ], + "idxsbu": [], + "idxsbx": [], + "idxsbx_e": [], + "idxsg": [], + "idxsg_e": [], + "idxsh": [ + 0, + 1, + 2, + 3, + 4, + 5, + 6, + 7 + ], + "idxsh_e": [], + "idxsphi": [], + "idxsphi_e": [], + "lbu": [ + -0.5 + ], + "lbx": [], + "lbx_0": [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "lbx_e": [], + "lg": [], + "lg_e": [], + "lh": [ + 1.5, + 1.5, + 1.5, + 1.5, + 1.5, + 1.5, + 1.5, + 1.5 + ], + "lh_e": [], + "lphi": [], + "lphi_e": [], + "lsbu": [], + "lsbx": [], + "lsbx_e": [], + "lsg": [], + "lsg_e": [], + "lsh": [ + -0.2, + -0.2, + -0.2, + -0.2, + -0.2, + -0.2, + -0.2, + -0.2 + ], + "lsh_e": [], + "lsphi": [], + "lsphi_e": [], + "ubu": [ + 0.5 + ], + "ubx": [], + "ubx_0": [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "ubx_e": [], + "ug": [], + "ug_e": [], + "uh": [ + 1000000, + 1000000, + 1000000, + 1000000, + 1000000, + 1000000, + 1000000, + 1000000 + ], + "uh_e": [], + "uphi": [], + "uphi_e": [], + "usbu": [], + "usbx": [], + "usbx_e": [], + "usg": [], + "usg_e": [], + "ush": [ + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0 + ], + "ush_e": [], + "usphi": [], + "usphi_e": [] + }, + "cost": { + "Vu": [ + [ + 0.0 + ], + [ + 0.0 + ], + [ + 0.0 + ], + [ + 0.0 + ], + [ + 0.0 + ], + [ + 0.0 + ], + [ + 0.0 + ], + [ + 0.0 + ], + [ + 1.0 + ] + ], + "Vx": [ + [ + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "Vx_e": [ + [ + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0 + ] + ], + "Vz": [], + "W": [ + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.05, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.01, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.2 + ] + ], + "W_e": [ + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.1, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.05, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "Zl": [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "Zl_e": [], + "Zu": [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "Zu_e": [], + "cost_type": "LINEAR_LS", + "cost_type_e": "LINEAR_LS", + "yref": [ + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0 + ], + "yref_e": [ + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0 + ], + "zl": [ + 10.0, + 10.0, + 10.0, + 10.0, + 10.0, + 10.0, + 10.0, + 10.0 + ], + "zl_e": [], + "zu": [ + 10.0, + 10.0, + 10.0, + 10.0, + 10.0, + 10.0, + 10.0, + 10.0 + ], + "zu_e": [] + }, + "dims": { + "N": 100, + "nbu": 1, + "nbx": 0, + "nbx_0": 8, + "nbx_e": 0, + "nbxe_0": 8, + "ng": 0, + "ng_e": 0, + "nh": 8, + "nh_e": 0, + "np": 16, + "nphi": 0, + "nphi_e": 0, + "nr": 0, + "nr_e": 0, + "ns": 8, + "ns_e": 0, + "nsbu": 0, + "nsbx": 0, + "nsbx_e": 0, + "nsg": 0, + "nsg_e": 0, + "nsh": 8, + "nsh_e": 0, + "nsphi": 0, + "nsphi_e": 0, + "nu": 1, + "nx": 8, + "ny": 9, + "ny_e": 8, + "nz": 0 + }, + "model": { + "name": "usv_model_guidance_ca1" + }, + "parameter_values": [ + 100, + 100, + 100, + 100, + 100, + 100, + 100, + 100, + 100, + 100, + 100, + 100, + 100, + 100, + 100, + 100 + ], + "problem_class": "OCP", + "solver_options": { + "Tsim": 0.05, + "exact_hess_constr": 1, + "exact_hess_cost": 1, + "exact_hess_dyn": 1, + "ext_cost_num_hess": 0, + "hessian_approx": "GAUSS_NEWTON", + "initialize_t_slacks": 0, + "integrator_type": "ERK", + "levenberg_marquardt": 0.0, + "model_external_shared_lib_dir": null, + "model_external_shared_lib_name": null, + "nlp_solver_max_iter": 100, + "nlp_solver_step_length": 1.0, + "nlp_solver_tol_comp": 1e-06, + "nlp_solver_tol_eq": 1e-06, + "nlp_solver_tol_ineq": 1e-06, + "nlp_solver_tol_stat": 1e-06, + "nlp_solver_type": "SQP_RTI", + "print_level": 0, + "qp_solver": "PARTIAL_CONDENSING_HPIPM", + "qp_solver_cond_N": null, + "qp_solver_iter_max": 50, + "qp_solver_tol_comp": null, + "qp_solver_tol_eq": null, + "qp_solver_tol_ineq": null, + "qp_solver_tol_stat": null, + "regularize_method": null, + "sim_method_jac_reuse": false, + "sim_method_newton_iter": 3, + "sim_method_num_stages": 4, + "sim_method_num_steps": 1, + "tf": 5.0, + "time_steps": [ + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05 + ] + } +} \ No newline at end of file diff --git a/usv_avoidance/scripts/usv_guidance_ca1/acados_settings.py b/usv_avoidance/scripts/usv_guidance_ca1/acados_settings.py new file mode 100644 index 00000000..3bea22a4 --- /dev/null +++ b/usv_avoidance/scripts/usv_guidance_ca1/acados_settings.py @@ -0,0 +1,209 @@ +# +# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl +# +# This file is part of acados. +# +# The 2-Clause BSD License +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE.; +# + +# author: Daniel Kloeser + +from acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver +from usv_model import usv_model +import scipy.linalg +import numpy as np + + +def acados_settings(Tf, N): + # create render arguments + ocp = AcadosOcp() + + # export model + model, constraint = usv_model() + + # define acados ODE + model_ac = AcadosModel() + model_ac.f_impl_expr = model.f_impl_expr + model_ac.f_expl_expr = model.f_expl_expr + model_ac.x = model.x + model_ac.xdot = model.xdot + model_ac.u = model.U + model_ac.z = model.z + model_ac.p = model.p + model_ac.name = model.name + ocp.model = model_ac + + # define constraint + model_ac.con_h_expr = constraint.expr + + # set dimensions + nx = model.x.size()[0] + nu = model.U.size()[0] + ny = nx + nu + ny_e = nx + + ocp.dims.N = N + ns = 8 + nsh = 8 + + # set cost + Q = np.diag([0, 0, 0.05, 0.01, 0, 0, 0, 0]) + + R = np.eye(nu) + R[0, 0] = 0.2 + + Qe = np.diag([0, 0, 0.1, 0.05, 0, 0, 0, 0]) + + + ocp.cost.cost_type = "LINEAR_LS" + ocp.cost.cost_type_e = "LINEAR_LS" + #unscale = N / Tf + + #ocp.cost.W = unscale * scipy.linalg.block_diag(Q, R) + #ocp.cost.W_e = Qe / unscale + ocp.cost.W = scipy.linalg.block_diag(Q, R) + ocp.cost.W_e = Qe + + Vx = np.zeros((ny, nx)) + Vx[:nx, :nx] = np.eye(nx) + ocp.cost.Vx = Vx + + Vu = np.zeros((ny, nu)) + Vu[8, 0] = 1.0 + + ocp.cost.Vu = Vu + + Vx_e = np.zeros((ny_e, nx)) + Vx_e[:nx, :nx] = np.eye(nx) + ocp.cost.Vx_e = Vx_e + + ocp.cost.zl = 10 * np.ones((ns,)) #previously 100 + ocp.cost.Zl = 0 * np.ones((ns,)) + ocp.cost.zu = 10 * np.ones((ns,)) #previously 100 + ocp.cost.Zu = 0 * np.ones((ns,)) + + # set intial references + ocp.cost.yref = np.array([0, 0, 0, 0, 0, 0, 0, 0, 0]) + ocp.cost.yref_e = np.array([0, 0, 0, 0, 0, 0, 0, 0]) + + # setting constraints + #ocp.constraints.lbx = np.array([model.psieddot_min]) + #ocp.constraints.ubx = np.array([model.psieddot_max]) + #ocp.constraints.idxbx = np.array([8]) + ocp.constraints.lbu = np.array([model.Upsieddot_min]) + ocp.constraints.ubu = np.array([model.Upsieddot_max]) + ocp.constraints.idxbu = np.array([0]) + + # ocp.constraints.lsbx=np.zero s([1]) + # ocp.constraints.usbx=np.zeros([1]) + # ocp.constraints.idxsbx=np.array([1]) + + ocp.constraints.lh = np.array( + [ + constraint.distance_min, + constraint.distance_min, + constraint.distance_min, + constraint.distance_min, + constraint.distance_min, + constraint.distance_min, + constraint.distance_min, + constraint.distance_min + ] + ) + ocp.constraints.uh = np.array( + [ + 1000000, + 1000000, + 1000000, + 1000000, + 1000000, + 1000000, + 1000000, + 1000000 + ] + ) + '''ocp.constraints.lsh = np.zeros(nsh) + ocp.constraints.ush = np.zeros(nsh) + ocp.constraints.idxsh = np.array([0, 2])''' + + ocp.constraints.lsh = np.array( + [ + -0.2, + -0.2, + -0.2, + -0.2, + -0.2, + -0.2, + -0.2, + -0.2 + ] + ) + ocp.constraints.ush = np.array( + [ + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0 + ] + ) + ocp.constraints.idxsh = np.array([0, 1, 2, 3, 4, 5, 6, 7]) + + + # set intial condition + ocp.constraints.x0 = model.x0 + + #ocp.parameter_values = np.array([4,4,4,8,4,12,4,20,100,100,100,100,100,100,100,100]) + ocp.parameter_values = np.array([100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100]) + + # set QP solver and integration + ocp.solver_options.tf = Tf + #ocp.solver_options.qp_solver = 'FULL_CONDENSING_QPOASES' + ocp.solver_options.qp_solver = "PARTIAL_CONDENSING_HPIPM" + ocp.solver_options.nlp_solver_type = "SQP_RTI" + #ocp.solver_options.nlp_solver_type = "SQP" + ocp.solver_options.hessian_approx = "GAUSS_NEWTON" + ocp.solver_options.integrator_type = "ERK" + #ocp.solver_options.sim_method_num_stages = 4 + #ocp.solver_options.sim_method_num_steps = 3 + + #ocp.solver_options.nlp_solver_max_iter = 200 + #ocp.solver_options.tol = 1e-4 + + #ocp.solver_options.qp_solver_tol_stat = 1e-2 + #ocp.solver_options.qp_solver_tol_eq = 1e-2 + #ocp.solver_options.qp_solver_tol_ineq = 1e-2 + #ocp.solver_options.qp_solver_tol_comp = 1e-2 + + # create solver + acados_solver = AcadosOcpSolver(ocp, json_file="acados_ocp.json") + + return constraint, model, acados_solver \ No newline at end of file diff --git a/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code.zip b/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code.zip new file mode 100644 index 00000000..f947959b Binary files /dev/null and b/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code.zip differ diff --git "a/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/\001\220#" "b/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/\001\220#" new file mode 100644 index 00000000..e69de29b diff --git a/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/Makefile b/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/Makefile new file mode 100644 index 00000000..950750dc --- /dev/null +++ b/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/Makefile @@ -0,0 +1,141 @@ +# +# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl +# +# This file is part of acados. +# +# The 2-Clause BSD License +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE.; +# + + +ACADOS_FLAGS = -fPIC -std=c99 #-fno-diagnostics-show-line-numbers -g +# # Debugging +# ACADOS_FLAGS += -g3 + +MODEL_OBJ= +MODEL_OBJ+= usv_model_guidance_ca1_model/usv_model_guidance_ca1_expl_ode_fun.o +MODEL_OBJ+= usv_model_guidance_ca1_model/usv_model_guidance_ca1_expl_vde_forw.o + + +OCP_OBJ= +OCP_OBJ+= usv_model_guidance_ca1_constraints/usv_model_guidance_ca1_constr_h_fun_jac_uxt_zt.o +OCP_OBJ+= usv_model_guidance_ca1_constraints/usv_model_guidance_ca1_constr_h_fun.o +OCP_OBJ+= acados_solver_usv_model_guidance_ca1.o + + +SIM_OBJ= +SIM_OBJ+= acados_sim_solver_usv_model_guidance_ca1.o + +EX_OBJ= +EX_OBJ+= main_usv_model_guidance_ca1.o + +EX_SIM_OBJ= +EX_SIM_OBJ+= main_sim_usv_model_guidance_ca1.o + +OBJ= +OBJ+= $(MODEL_OBJ) +OBJ+= $(SIM_OBJ) +OBJ+= $(OCP_OBJ) + +EXTERNAL_DIR= +EXTERNAL_LIB= + +INCLUDE_PATH = /home/nvidia/vanttec_usv_ws/src/usv_avoidance/include/acados/include +LIB_PATH = /home/nvidia/vanttec_usv_ws/src/usv_avoidance/include/acados/lib +all: clean casadi_fun example_sim example +shared_lib: bundled_shared_lib ocp_shared_lib sim_shared_lib + +CASADI_MODEL_SOURCE= +CASADI_MODEL_SOURCE+= usv_model_guidance_ca1_expl_ode_fun.c +CASADI_MODEL_SOURCE+= usv_model_guidance_ca1_expl_vde_forw.c +CASADI_CON_H_SOURCE= +CASADI_CON_H_SOURCE+= usv_model_guidance_ca1_constr_h_fun_jac_uxt_zt.c +CASADI_CON_H_SOURCE+= usv_model_guidance_ca1_constr_h_fun.c + + +casadi_fun: + ( cd usv_model_guidance_ca1_model; gcc $(ACADOS_FLAGS) -c $(CASADI_MODEL_SOURCE)) + ( cd usv_model_guidance_ca1_constraints; gcc $(ACADOS_FLAGS) -c $(CASADI_CON_H_SOURCE)) + +main: + gcc $(ACADOS_FLAGS) -c main_usv_model_guidance_ca1.c -I $(INCLUDE_PATH)/blasfeo/include/ -I $(INCLUDE_PATH)/hpipm/include/ \ + -I $(INCLUDE_PATH) -I $(INCLUDE_PATH)/acados/ \ + +main_sim: + gcc $(ACADOS_FLAGS) -c main_sim_usv_model_guidance_ca1.c -I $(INCLUDE_PATH)/blasfeo/include/ -I $(INCLUDE_PATH)/hpipm/include/ \ + -I $(INCLUDE_PATH) -I $(INCLUDE_PATH)/acados/ + +ocp_solver: + gcc $(ACADOS_FLAGS) -c acados_solver_usv_model_guidance_ca1.c -I $(INCLUDE_PATH)/blasfeo/include/ -I $(INCLUDE_PATH)/hpipm/include/ \ + -I $(INCLUDE_PATH) -I $(INCLUDE_PATH)/acados/ \ + +sim_solver: + gcc $(ACADOS_FLAGS) -c acados_sim_solver_usv_model_guidance_ca1.c -I $(INCLUDE_PATH)/blasfeo/include/ -I $(INCLUDE_PATH)/hpipm/include/ \ + -I $(INCLUDE_PATH) -I $(INCLUDE_PATH)/acados/ \ + +example: ocp_solver main + gcc $(ACADOS_FLAGS) -o main_usv_model_guidance_ca1 $(EX_OBJ) $(OBJ) -L $(LIB_PATH) \ + -lacados -lhpipm -lblasfeo \ + -lm \ + -I $(INCLUDE_PATH)/blasfeo/include/ \ + -I $(INCLUDE_PATH)/hpipm/include/ \ + -I $(INCLUDE_PATH) \ + -I $(INCLUDE_PATH)/acados/ \ + + +example_sim: sim_solver main_sim + gcc $(ACADOS_FLAGS) -o main_sim_usv_model_guidance_ca1 $(EX_SIM_OBJ) $(MODEL_OBJ) $(SIM_OBJ) -L $(LIB_PATH) \ + -lacados -lhpipm -lblasfeo \ + -lm \ + -I $(INCLUDE_PATH)/blasfeo/include/ \ + -I $(INCLUDE_PATH)/acados/ \ + +bundled_shared_lib: casadi_fun ocp_solver sim_solver + gcc $(ACADOS_FLAGS) -shared -o libacados_solver_usv_model_guidance_ca1.so $(OBJ) \ + -L $(LIB_PATH) \ + -lacados -lhpipm -lblasfeo \ + -lm \ + +ocp_shared_lib: casadi_fun ocp_solver + gcc $(ACADOS_FLAGS) -shared -o libacados_ocp_solver_usv_model_guidance_ca1.so $(OCP_OBJ) $(MODEL_OBJ) \ + -L$(EXTERNAL_DIR) -l$(EXTERNAL_LIB) \ + -L $(LIB_PATH) -lacados -lhpipm -lblasfeo \ + -lm \ + +sim_shared_lib: casadi_fun sim_solver + gcc $(ACADOS_FLAGS) -shared -o libacados_sim_solver_usv_model_guidance_ca1.so $(SIM_OBJ) $(MODEL_OBJ) -L$(EXTERNAL_DIR) -l$(EXTERNAL_LIB) \ + -L $(LIB_PATH) -lacados -lhpipm -lblasfeo \ + -lm \ + +clean: + rm -f *.o + rm -f *.so + rm -f main_usv_model_guidance_ca1 + +clean_ocp_shared_lib: + rm -f libacados_ocp_solver_usv_model_guidance_ca1.so + rm -f acados_solver_usv_model_guidance_ca1.o diff --git a/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/acados_sim_solver_usv_model_guidance_ca1.c b/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/acados_sim_solver_usv_model_guidance_ca1.c new file mode 100644 index 00000000..1ec2bc06 --- /dev/null +++ b/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/acados_sim_solver_usv_model_guidance_ca1.c @@ -0,0 +1,278 @@ +/* + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ +// standard +#include +#include + +// acados +#include "acados_c/external_function_interface.h" +#include "acados_c/sim_interface.h" +#include "acados_c/external_function_interface.h" + +#include "acados/sim/sim_common.h" +#include "acados/utils/external_function_generic.h" +#include "acados/utils/print.h" + + +// example specific +#include "usv_model_guidance_ca1_model/usv_model_guidance_ca1_model.h" +#include "acados_sim_solver_usv_model_guidance_ca1.h" + + +// ** global data ** +sim_config * usv_model_guidance_ca1_sim_config; +sim_in * usv_model_guidance_ca1_sim_in; +sim_out * usv_model_guidance_ca1_sim_out; +void * usv_model_guidance_ca1_sim_dims; +sim_opts * usv_model_guidance_ca1_sim_opts; +sim_solver * usv_model_guidance_ca1_sim_solver; + + +external_function_param_casadi * sim_forw_vde_casadi; +external_function_param_casadi * sim_expl_ode_fun_casadi; + + + +int usv_model_guidance_ca1_acados_sim_create() +{ + // initialize + int nx = 8; + int nu = 1; + int nz = 0; + + + double Tsim = 0.05; + + + // explicit ode + sim_forw_vde_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)); + sim_expl_ode_fun_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)); + + sim_forw_vde_casadi->casadi_fun = &usv_model_guidance_ca1_expl_vde_forw; + sim_forw_vde_casadi->casadi_n_in = &usv_model_guidance_ca1_expl_vde_forw_n_in; + sim_forw_vde_casadi->casadi_n_out = &usv_model_guidance_ca1_expl_vde_forw_n_out; + sim_forw_vde_casadi->casadi_sparsity_in = &usv_model_guidance_ca1_expl_vde_forw_sparsity_in; + sim_forw_vde_casadi->casadi_sparsity_out = &usv_model_guidance_ca1_expl_vde_forw_sparsity_out; + sim_forw_vde_casadi->casadi_work = &usv_model_guidance_ca1_expl_vde_forw_work; + external_function_param_casadi_create(sim_forw_vde_casadi, 16); + + sim_expl_ode_fun_casadi->casadi_fun = &usv_model_guidance_ca1_expl_ode_fun; + sim_expl_ode_fun_casadi->casadi_n_in = &usv_model_guidance_ca1_expl_ode_fun_n_in; + sim_expl_ode_fun_casadi->casadi_n_out = &usv_model_guidance_ca1_expl_ode_fun_n_out; + sim_expl_ode_fun_casadi->casadi_sparsity_in = &usv_model_guidance_ca1_expl_ode_fun_sparsity_in; + sim_expl_ode_fun_casadi->casadi_sparsity_out = &usv_model_guidance_ca1_expl_ode_fun_sparsity_out; + sim_expl_ode_fun_casadi->casadi_work = &usv_model_guidance_ca1_expl_ode_fun_work; + external_function_param_casadi_create(sim_expl_ode_fun_casadi, 16); + + + + // sim plan & config + sim_solver_plan plan; + plan.sim_solver = ERK; + + // create correct config based on plan + usv_model_guidance_ca1_sim_config = sim_config_create(plan); + + // sim dims + usv_model_guidance_ca1_sim_dims = sim_dims_create(usv_model_guidance_ca1_sim_config); + sim_dims_set(usv_model_guidance_ca1_sim_config, usv_model_guidance_ca1_sim_dims, "nx", &nx); + sim_dims_set(usv_model_guidance_ca1_sim_config, usv_model_guidance_ca1_sim_dims, "nu", &nu); + sim_dims_set(usv_model_guidance_ca1_sim_config, usv_model_guidance_ca1_sim_dims, "nz", &nz); + + + // sim opts + usv_model_guidance_ca1_sim_opts = sim_opts_create(usv_model_guidance_ca1_sim_config, usv_model_guidance_ca1_sim_dims); + int tmp_int = 4; + sim_opts_set(usv_model_guidance_ca1_sim_config, usv_model_guidance_ca1_sim_opts, "num_stages", &tmp_int); + tmp_int = 1; + sim_opts_set(usv_model_guidance_ca1_sim_config, usv_model_guidance_ca1_sim_opts, "num_steps", &tmp_int); + tmp_int = 3; + sim_opts_set(usv_model_guidance_ca1_sim_config, usv_model_guidance_ca1_sim_opts, "newton_iter", &tmp_int); + bool tmp_bool = false; + sim_opts_set(usv_model_guidance_ca1_sim_config, usv_model_guidance_ca1_sim_opts, "jac_reuse", &tmp_bool); + + + + // sim in / out + usv_model_guidance_ca1_sim_in = sim_in_create(usv_model_guidance_ca1_sim_config, usv_model_guidance_ca1_sim_dims); + usv_model_guidance_ca1_sim_out = sim_out_create(usv_model_guidance_ca1_sim_config, usv_model_guidance_ca1_sim_dims); + sim_in_set(usv_model_guidance_ca1_sim_config, usv_model_guidance_ca1_sim_dims, + usv_model_guidance_ca1_sim_in, "T", &Tsim); + + // model functions + usv_model_guidance_ca1_sim_config->model_set(usv_model_guidance_ca1_sim_in->model, + "expl_vde_for", sim_forw_vde_casadi); + usv_model_guidance_ca1_sim_config->model_set(usv_model_guidance_ca1_sim_in->model, + "expl_ode_fun", sim_expl_ode_fun_casadi); + + // sim solver + usv_model_guidance_ca1_sim_solver = sim_solver_create(usv_model_guidance_ca1_sim_config, + usv_model_guidance_ca1_sim_dims, usv_model_guidance_ca1_sim_opts); + + /* initialize parameter values */ + + // initialize parameters to nominal value + double p[16]; + + p[0] = 100; + p[1] = 100; + p[2] = 100; + p[3] = 100; + p[4] = 100; + p[5] = 100; + p[6] = 100; + p[7] = 100; + p[8] = 100; + p[9] = 100; + p[10] = 100; + p[11] = 100; + p[12] = 100; + p[13] = 100; + p[14] = 100; + p[15] = 100; + sim_forw_vde_casadi[0].set_param(sim_forw_vde_casadi, p); + sim_expl_ode_fun_casadi[0].set_param(sim_expl_ode_fun_casadi, p); + + + /* initialize input */ + // x + double x0[8]; + for (int ii = 0; ii < 8; ii++) + x0[ii] = 0.0; + + sim_in_set(usv_model_guidance_ca1_sim_config, usv_model_guidance_ca1_sim_dims, + usv_model_guidance_ca1_sim_in, "x", x0); + + + // u + double u0[1]; + for (int ii = 0; ii < 1; ii++) + u0[ii] = 0.0; + + sim_in_set(usv_model_guidance_ca1_sim_config, usv_model_guidance_ca1_sim_dims, + usv_model_guidance_ca1_sim_in, "u", u0); + + // S_forw + double S_forw[72]; + for (int ii = 0; ii < 72; ii++) + S_forw[ii] = 0.0; + for (int ii = 0; ii < 8; ii++) + S_forw[ii + ii * 8 ] = 1.0; + + + sim_in_set(usv_model_guidance_ca1_sim_config, usv_model_guidance_ca1_sim_dims, + usv_model_guidance_ca1_sim_in, "S_forw", S_forw); + + int status = sim_precompute(usv_model_guidance_ca1_sim_solver, usv_model_guidance_ca1_sim_in, usv_model_guidance_ca1_sim_out); + + return status; +} + + +int usv_model_guidance_ca1_acados_sim_solve() +{ + // integrate dynamics using acados sim_solver + int status = sim_solve(usv_model_guidance_ca1_sim_solver, + usv_model_guidance_ca1_sim_in, usv_model_guidance_ca1_sim_out); + if (status != 0) + printf("error in usv_model_guidance_ca1_acados_sim_solve()! Exiting.\n"); + + return status; +} + + +int usv_model_guidance_ca1_acados_sim_free() +{ + // free memory + sim_solver_destroy(usv_model_guidance_ca1_sim_solver); + sim_in_destroy(usv_model_guidance_ca1_sim_in); + sim_out_destroy(usv_model_guidance_ca1_sim_out); + sim_opts_destroy(usv_model_guidance_ca1_sim_opts); + sim_dims_destroy(usv_model_guidance_ca1_sim_dims); + sim_config_destroy(usv_model_guidance_ca1_sim_config); + + // free external function + external_function_param_casadi_free(sim_forw_vde_casadi); + external_function_param_casadi_free(sim_expl_ode_fun_casadi); + + return 0; +} + + +int usv_model_guidance_ca1_acados_sim_update_params(double *p, int np) +{ + int status = 0; + int casadi_np = 16; + + if (casadi_np != np) { + printf("usv_model_guidance_ca1_acados_sim_update_params: trying to set %i parameters for external functions." + " External function has %i parameters. Exiting.\n", np, casadi_np); + exit(1); + } + sim_forw_vde_casadi[0].set_param(sim_forw_vde_casadi, p); + sim_expl_ode_fun_casadi[0].set_param(sim_expl_ode_fun_casadi, p); + + return status; +} + +/* getters pointers to C objects*/ +sim_config * usv_model_guidance_ca1_acados_get_sim_config() +{ + return usv_model_guidance_ca1_sim_config; +}; + +sim_in * usv_model_guidance_ca1_acados_get_sim_in() +{ + return usv_model_guidance_ca1_sim_in; +}; + +sim_out * usv_model_guidance_ca1_acados_get_sim_out() +{ + return usv_model_guidance_ca1_sim_out; +}; + +void * usv_model_guidance_ca1_acados_get_sim_dims() +{ + return usv_model_guidance_ca1_sim_dims; +}; + +sim_opts * usv_model_guidance_ca1_acados_get_sim_opts() +{ + return usv_model_guidance_ca1_sim_opts; +}; + +sim_solver * usv_model_guidance_ca1_acados_get_sim_solver() +{ + return usv_model_guidance_ca1_sim_solver; +}; + diff --git a/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/acados_sim_solver_usv_model_guidance_ca1.h b/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/acados_sim_solver_usv_model_guidance_ca1.h new file mode 100644 index 00000000..a0fbc2dd --- /dev/null +++ b/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/acados_sim_solver_usv_model_guidance_ca1.h @@ -0,0 +1,73 @@ +/* + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + +#ifndef ACADOS_SIM_usv_model_guidance_ca1_H_ +#define ACADOS_SIM_usv_model_guidance_ca1_H_ + +#include "acados_c/sim_interface.h" +#include "acados_c/external_function_interface.h" + +#ifdef __cplusplus +extern "C" { +#endif + +int usv_model_guidance_ca1_acados_sim_create(); +int usv_model_guidance_ca1_acados_sim_solve(); +int usv_model_guidance_ca1_acados_sim_free(); +int usv_model_guidance_ca1_acados_sim_update_params(); + +sim_config * usv_model_guidance_ca1_acados_get_sim_config(); +sim_in * usv_model_guidance_ca1_acados_get_sim_in(); +sim_out * usv_model_guidance_ca1_acados_get_sim_out(); +void * usv_model_guidance_ca1_acados_get_sim_dims(); +sim_opts * usv_model_guidance_ca1_acados_get_sim_opts(); +sim_solver * usv_model_guidance_ca1_acados_get_sim_solver(); + +// ** global data ** +extern sim_config * usv_model_guidance_ca1_sim_config; +extern sim_in * usv_model_guidance_ca1_sim_in; +extern sim_out * usv_model_guidance_ca1_sim_out; +extern void * usv_model_guidance_ca1_sim_dims; +extern sim_opts * usv_model_guidance_ca1_sim_opts; +extern sim_solver * usv_model_guidance_ca1_sim_solver; + +#ifdef __cplusplus +} +#endif + + +extern external_function_param_casadi * sim_forw_vde_casadi; +extern external_function_param_casadi * sim_expl_ode_fun_casadi; + + +#endif // ACADOS_SIM_usv_model_guidance_ca1_H_ diff --git a/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/acados_solver_sfunction_usv_model_guidance_ca1.c b/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/acados_solver_sfunction_usv_model_guidance_ca1.c new file mode 100644 index 00000000..c11c4ff0 --- /dev/null +++ b/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/acados_solver_sfunction_usv_model_guidance_ca1.c @@ -0,0 +1,285 @@ +/* + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + +#define S_FUNCTION_NAME acados_solver_sfunction_usv_model_guidance_ca1 +#define S_FUNCTION_LEVEL 2 + +#define MDL_START + +// acados +#include "acados/utils/print.h" +#include "acados_c/sim_interface.h" +#include "acados_c/external_function_interface.h" + +// example specific +#include "usv_model_guidance_ca1_model/usv_model_guidance_ca1_model.h" +#include "acados_solver_usv_model_guidance_ca1.h" + +#include "simstruc.h" + +//#define SAMPLINGTIME -1 +#define SAMPLINGTIME 0.05 + +static void mdlInitializeSizes (SimStruct *S) +{ + // specify the number of continuous and discrete states + ssSetNumContStates(S, 0); + ssSetNumDiscStates(S, 0); + + + + // specify the number of input ports + if ( !ssSetNumInputPorts(S, 8) ) + return; + + // specify the number of output ports + if ( !ssSetNumOutputPorts(S, 6) ) + return; + + // specify dimension information for the input ports + // x0 + ssSetInputPortVectorDimension(S, 0, 8); + // y_ref + ssSetInputPortVectorDimension(S, 1, 100 * 9); + // y_ref_e + ssSetInputPortVectorDimension(S, 2, 8); + // parameters + ssSetInputPortVectorDimension(S, 3, (100+1) * 16); + // lbu + ssSetInputPortVectorDimension(S, 4, 1); + // ubu + ssSetInputPortVectorDimension(S, 5, 1); + // lh + ssSetInputPortVectorDimension(S, 6, 8); + // uh + ssSetInputPortVectorDimension(S, 7, 8); + + // specify dimension information for the output ports + ssSetOutputPortVectorDimension(S, 0, 1 ); // optimal input + ssSetOutputPortVectorDimension(S, 1, 1 ); // solver status + ssSetOutputPortVectorDimension(S, 2, 1 ); // KKT residuals + ssSetOutputPortVectorDimension(S, 3, 8 ); // first state + ssSetOutputPortVectorDimension(S, 4, 1); // computation times + ssSetOutputPortVectorDimension(S, 5, 1 ); // sqp iter + + // specify the direct feedthrough status + // should be set to 1 for all inputs used in mdlOutputs + ssSetInputPortDirectFeedThrough(S, 0, 1); + ssSetInputPortDirectFeedThrough(S, 1, 1); + ssSetInputPortDirectFeedThrough(S, 2, 1); + ssSetInputPortDirectFeedThrough(S, 3, 1); + ssSetInputPortDirectFeedThrough(S, 4, 1); + ssSetInputPortDirectFeedThrough(S, 5, 1); + ssSetInputPortDirectFeedThrough(S, 6, 1); + ssSetInputPortDirectFeedThrough(S, 7, 1); + + // one sample time + ssSetNumSampleTimes(S, 1); +} + + +#if defined(MATLAB_MEX_FILE) + +#define MDL_SET_INPUT_PORT_DIMENSION_INFO +#define MDL_SET_OUTPUT_PORT_DIMENSION_INFO + +static void mdlSetInputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo) +{ + if ( !ssSetInputPortDimensionInfo(S, port, dimsInfo) ) + return; +} + +static void mdlSetOutputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo) +{ + if ( !ssSetOutputPortDimensionInfo(S, port, dimsInfo) ) + return; +} + +#endif /* MATLAB_MEX_FILE */ + + +static void mdlInitializeSampleTimes(SimStruct *S) +{ + ssSetSampleTime(S, 0, SAMPLINGTIME); + ssSetOffsetTime(S, 0, 0.0); +} + + +static void mdlStart(SimStruct *S) +{ + acados_create(); +} + +static void mdlOutputs(SimStruct *S, int_T tid) +{ + InputRealPtrsType in_sign; + + + // local buffer + real_t buffer[16]; + + + /* go through inputs */ + // initial condition + in_sign = ssGetInputPortRealSignalPtrs(S, 0); + for (int i = 0; i < 8; i++) + buffer[i] = (double)(*in_sign[i]); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbx", buffer); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", buffer); + + + // y_ref - stage-variant !!! + in_sign = ssGetInputPortRealSignalPtrs(S, 1); + + for (int ii = 0; ii < 100; ii++) + { + for (int jj = 0; jj < 9; jj++) + buffer[jj] = (double)(*in_sign[ii*9+jj]); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, ii, "yref", (void *) buffer); + } + + + + // y_ref_e + in_sign = ssGetInputPortRealSignalPtrs(S, 2); + + for (int i = 0; i < 8; i++) + buffer[i] = (double)(*in_sign[i]); + + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 100, "yref", (void *) buffer); + + + // parameters - stage-variant !!! + in_sign = ssGetInputPortRealSignalPtrs(S, 3); + + // update value of parameters + for (int ii = 0; ii <= 100; ii++) + { + for (int jj = 0; jj < 16; jj++) + buffer[jj] = (double)(*in_sign[ii*16+jj]); + acados_update_params(ii, buffer, 16); + } + + + + + + // lbu + in_sign = ssGetInputPortRealSignalPtrs(S, 4); + + for (int i = 0; i < 1; i++) + buffer[i] = (double)(*in_sign[i]); + + for (int ii = 0; ii < 100; ii++) + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "lbu", buffer); + + // ubu + in_sign = ssGetInputPortRealSignalPtrs(S, 5); + + for (int i = 0; i < 1; i++) + buffer[i] = (double)(*in_sign[i]); + + for (int ii = 0; ii < 100; ii++) + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "ubu", buffer); + + + + + // lh + in_sign = ssGetInputPortRealSignalPtrs(S, 6); + + for (int i = 0; i < 8; i++) + buffer[i] = (double)(*in_sign[i]); + + for (int ii = 0; ii < 100; ii++) + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "lh", buffer); + + // uh + in_sign = ssGetInputPortRealSignalPtrs(S, 7); + + for (int i = 0; i < 8; i++) + buffer[i] = (double)(*in_sign[i]); + + for (int ii = 0; ii < 100; ii++) + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "uh", buffer); + + /* call solver */ + int rti_phase = 0; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "rti_phase", &rti_phase); + int acados_status = acados_solve(); + + + /* set outputs */ + // assign pointers to output signals + real_t *out_u0, *out_status, *out_sqp_iter, *out_KKT_res, *out_x1, *out_cpu_time; + int tmp_int; + + out_u0 = ssGetOutputPortRealSignal(S, 0); + out_status = ssGetOutputPortRealSignal(S, 1); + out_KKT_res = ssGetOutputPortRealSignal(S, 2); + out_x1 = ssGetOutputPortRealSignal(S, 3); + out_cpu_time = ssGetOutputPortRealSignal(S, 4); + out_sqp_iter = ssGetOutputPortRealSignal(S, 5); + + // extract solver info + *out_status = (real_t) acados_status; + *out_KKT_res = (real_t) nlp_out->inf_norm_res; +// *out_cpu_time = (real_t) nlp_out->total_time; + + // get solution time + ocp_nlp_get(nlp_config, nlp_solver, "time_tot", (void *) out_cpu_time); + + // get sqp iter + ocp_nlp_get(nlp_config, nlp_solver, "sqp_iter", (void *) &tmp_int); + *out_sqp_iter = (real_t) tmp_int; +// *out_sqp_iter = (real_t) nlp_out->sqp_iter; + + // get solution + ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 0, "u", (void *) out_u0); + + // get next state + ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 1, "x", (void *) out_x1); + +} + +static void mdlTerminate(SimStruct *S) +{ + acados_free(); +} + + +#ifdef MATLAB_MEX_FILE +#include "simulink.c" +#else +#include "cg_sfun.h" +#endif diff --git a/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/acados_solver_usv_model_guidance_ca1.c b/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/acados_solver_usv_model_guidance_ca1.c new file mode 100644 index 00000000..383ee356 --- /dev/null +++ b/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/acados_solver_usv_model_guidance_ca1.c @@ -0,0 +1,1266 @@ +/* + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + +// standard +#include +#include +// acados +#include "acados/utils/print.h" +#include "acados_c/ocp_nlp_interface.h" +#include "acados_c/external_function_interface.h" + +// example specific +#include "usv_model_guidance_ca1_model/usv_model_guidance_ca1_model.h" + + + +#include "usv_model_guidance_ca1_constraints/usv_model_guidance_ca1_h_constraint.h" + + + +#include "acados_solver_usv_model_guidance_ca1.h" + +#define NX 8 +#define NZ 0 +#define NU 1 +#define NP 16 +#define NBX 0 +#define NBX0 8 +#define NBU 1 +#define NSBX 0 +#define NSBU 0 +#define NSH 8 +#define NSG 0 +#define NSPHI 0 +#define NSHN 0 +#define NSGN 0 +#define NSPHIN 0 +#define NSBXN 0 +#define NS 8 +#define NSN 0 +#define NG 0 +#define NBXN 0 +#define NGN 0 +#define NY 9 +#define NYN 8 +#define N 100 +#define NH 8 +#define NPHI 0 +#define NHN 0 +#define NPHIN 0 +#define NR 0 + + +// ** global data ** +ocp_nlp_in * nlp_in; +ocp_nlp_out * nlp_out; +ocp_nlp_solver * nlp_solver; +void * nlp_opts; +ocp_nlp_plan * nlp_solver_plan; +ocp_nlp_config * nlp_config; +ocp_nlp_dims * nlp_dims; + +// number of expected runtime parameters +const unsigned int nlp_np = NP; + + +external_function_param_casadi * forw_vde_casadi; +external_function_param_casadi * expl_ode_fun; + + + +external_function_param_casadi * nl_constr_h_fun; +external_function_param_casadi * nl_constr_h_fun_jac; + + +external_function_param_casadi nl_constr_h_e_fun_jac; +external_function_param_casadi nl_constr_h_e_fun; + + +int acados_create() +{ + int status = 0; + + /************************************************ + * plan & config + ************************************************/ + nlp_solver_plan = ocp_nlp_plan_create(N); + nlp_solver_plan->nlp_solver = SQP_RTI; + + nlp_solver_plan->ocp_qp_solver_plan.qp_solver = PARTIAL_CONDENSING_HPIPM; + for (int i = 0; i < N; i++) + nlp_solver_plan->nlp_cost[i] = LINEAR_LS; + + nlp_solver_plan->nlp_cost[N] = LINEAR_LS; + + for (int i = 0; i < N; i++) + { + + nlp_solver_plan->nlp_dynamics[i] = CONTINUOUS_MODEL; + nlp_solver_plan->sim_solver_plan[i].sim_solver = ERK; + } + + for (int i = 0; i < N; i++) + { + nlp_solver_plan->nlp_constraints[i] = BGH; + } + nlp_solver_plan->nlp_constraints[N] = BGH; + nlp_config = ocp_nlp_config_create(*nlp_solver_plan); + + + /************************************************ + * dimensions + ************************************************/ + int nx[N+1]; + int nu[N+1]; + int nbx[N+1]; + int nbu[N+1]; + int nsbx[N+1]; + int nsbu[N+1]; + int nsg[N+1]; + int nsh[N+1]; + int nsphi[N+1]; + int ns[N+1]; + int ng[N+1]; + int nh[N+1]; + int nphi[N+1]; + int nz[N+1]; + int ny[N+1]; + int nr[N+1]; + int nbxe[N+1]; + + for (int i = 0; i < N+1; i++) + { + // common + nx[i] = NX; + nu[i] = NU; + nz[i] = NZ; + ns[i] = NS; + // cost + ny[i] = NY; + // constraints + nbx[i] = NBX; + nbu[i] = NBU; + nsbx[i] = NSBX; + nsbu[i] = NSBU; + nsg[i] = NSG; + nsh[i] = NSH; + nsphi[i] = NSPHI; + ng[i] = NG; + nh[i] = NH; + nphi[i] = NPHI; + nr[i] = NR; + nbxe[i] = 0; + } + + // for initial state + nbx[0] = NBX0; + nsbx[0] = 0; + ns[0] = NS - NSBX; + nbxe[0] = 8; + + // terminal - common + nu[N] = 0; + nz[N] = 0; + ns[N] = NSN; + // cost + ny[N] = NYN; + // constraint + nbx[N] = NBXN; + nbu[N] = 0; + ng[N] = NGN; + nh[N] = NHN; + nphi[N] = NPHIN; + nr[N] = 0; + + nsbx[N] = NSBXN; + nsbu[N] = 0; + nsg[N] = NSGN; + nsh[N] = NSHN; + nsphi[N] = NSPHIN; + + /* create and set ocp_nlp_dims */ + nlp_dims = ocp_nlp_dims_create(nlp_config); + + ocp_nlp_dims_set_opt_vars(nlp_config, nlp_dims, "nx", nx); + ocp_nlp_dims_set_opt_vars(nlp_config, nlp_dims, "nu", nu); + ocp_nlp_dims_set_opt_vars(nlp_config, nlp_dims, "nz", nz); + ocp_nlp_dims_set_opt_vars(nlp_config, nlp_dims, "ns", ns); + + for (int i = 0; i <= N; i++) + { + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nbx", &nbx[i]); + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nbu", &nbu[i]); + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nsbx", &nsbx[i]); + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nsbu", &nsbu[i]); + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "ng", &ng[i]); + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nsg", &nsg[i]); + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nbxe", &nbxe[i]); + } + + for (int i = 0; i < N; i++) + { + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nh", &nh[i]); + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nsh", &nsh[i]); + ocp_nlp_dims_set_cost(nlp_config, nlp_dims, i, "ny", &ny[i]); + } + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, N, "nh", &nh[N]); + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, N, "nsh", &nsh[N]); + ocp_nlp_dims_set_cost(nlp_config, nlp_dims, N, "ny", &ny[N]); + + + + /************************************************ + * external functions + ************************************************/ + nl_constr_h_fun_jac = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); + for (int i = 0; i < N; i++) { + nl_constr_h_fun_jac[i].casadi_fun = &usv_model_guidance_ca1_constr_h_fun_jac_uxt_zt; + nl_constr_h_fun_jac[i].casadi_n_in = &usv_model_guidance_ca1_constr_h_fun_jac_uxt_zt_n_in; + nl_constr_h_fun_jac[i].casadi_n_out = &usv_model_guidance_ca1_constr_h_fun_jac_uxt_zt_n_out; + nl_constr_h_fun_jac[i].casadi_sparsity_in = &usv_model_guidance_ca1_constr_h_fun_jac_uxt_zt_sparsity_in; + nl_constr_h_fun_jac[i].casadi_sparsity_out = &usv_model_guidance_ca1_constr_h_fun_jac_uxt_zt_sparsity_out; + nl_constr_h_fun_jac[i].casadi_work = &usv_model_guidance_ca1_constr_h_fun_jac_uxt_zt_work; + external_function_param_casadi_create(&nl_constr_h_fun_jac[i], 16); + } + nl_constr_h_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); + for (int i = 0; i < N; i++) { + nl_constr_h_fun[i].casadi_fun = &usv_model_guidance_ca1_constr_h_fun; + nl_constr_h_fun[i].casadi_n_in = &usv_model_guidance_ca1_constr_h_fun_n_in; + nl_constr_h_fun[i].casadi_n_out = &usv_model_guidance_ca1_constr_h_fun_n_out; + nl_constr_h_fun[i].casadi_sparsity_in = &usv_model_guidance_ca1_constr_h_fun_sparsity_in; + nl_constr_h_fun[i].casadi_sparsity_out = &usv_model_guidance_ca1_constr_h_fun_sparsity_out; + nl_constr_h_fun[i].casadi_work = &usv_model_guidance_ca1_constr_h_fun_work; + external_function_param_casadi_create(&nl_constr_h_fun[i], 16); + } + + + + + // explicit ode + forw_vde_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); + for (int i = 0; i < N; i++) { + forw_vde_casadi[i].casadi_fun = &usv_model_guidance_ca1_expl_vde_forw; + forw_vde_casadi[i].casadi_n_in = &usv_model_guidance_ca1_expl_vde_forw_n_in; + forw_vde_casadi[i].casadi_n_out = &usv_model_guidance_ca1_expl_vde_forw_n_out; + forw_vde_casadi[i].casadi_sparsity_in = &usv_model_guidance_ca1_expl_vde_forw_sparsity_in; + forw_vde_casadi[i].casadi_sparsity_out = &usv_model_guidance_ca1_expl_vde_forw_sparsity_out; + forw_vde_casadi[i].casadi_work = &usv_model_guidance_ca1_expl_vde_forw_work; + external_function_param_casadi_create(&forw_vde_casadi[i], 16); + } + + expl_ode_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); + for (int i = 0; i < N; i++) { + expl_ode_fun[i].casadi_fun = &usv_model_guidance_ca1_expl_ode_fun; + expl_ode_fun[i].casadi_n_in = &usv_model_guidance_ca1_expl_ode_fun_n_in; + expl_ode_fun[i].casadi_n_out = &usv_model_guidance_ca1_expl_ode_fun_n_out; + expl_ode_fun[i].casadi_sparsity_in = &usv_model_guidance_ca1_expl_ode_fun_sparsity_in; + expl_ode_fun[i].casadi_sparsity_out = &usv_model_guidance_ca1_expl_ode_fun_sparsity_out; + expl_ode_fun[i].casadi_work = &usv_model_guidance_ca1_expl_ode_fun_work; + external_function_param_casadi_create(&expl_ode_fun[i], 16); + } + + + + /************************************************ + * nlp_in + ************************************************/ + nlp_in = ocp_nlp_in_create(nlp_config, nlp_dims); + + double time_steps[N]; + time_steps[0] = 0.05; + time_steps[1] = 0.05; + time_steps[2] = 0.05; + time_steps[3] = 0.05; + time_steps[4] = 0.05; + time_steps[5] = 0.05; + time_steps[6] = 0.05; + time_steps[7] = 0.05; + time_steps[8] = 0.05; + time_steps[9] = 0.05; + time_steps[10] = 0.05; + time_steps[11] = 0.05; + time_steps[12] = 0.05; + time_steps[13] = 0.05; + time_steps[14] = 0.05; + time_steps[15] = 0.05; + time_steps[16] = 0.05; + time_steps[17] = 0.05; + time_steps[18] = 0.05; + time_steps[19] = 0.05; + time_steps[20] = 0.05; + time_steps[21] = 0.05; + time_steps[22] = 0.05; + time_steps[23] = 0.05; + time_steps[24] = 0.05; + time_steps[25] = 0.05; + time_steps[26] = 0.05; + time_steps[27] = 0.05; + time_steps[28] = 0.05; + time_steps[29] = 0.05; + time_steps[30] = 0.05; + time_steps[31] = 0.05; + time_steps[32] = 0.05; + time_steps[33] = 0.05; + time_steps[34] = 0.05; + time_steps[35] = 0.05; + time_steps[36] = 0.05; + time_steps[37] = 0.05; + time_steps[38] = 0.05; + time_steps[39] = 0.05; + time_steps[40] = 0.05; + time_steps[41] = 0.05; + time_steps[42] = 0.05; + time_steps[43] = 0.05; + time_steps[44] = 0.05; + time_steps[45] = 0.05; + time_steps[46] = 0.05; + time_steps[47] = 0.05; + time_steps[48] = 0.05; + time_steps[49] = 0.05; + time_steps[50] = 0.05; + time_steps[51] = 0.05; + time_steps[52] = 0.05; + time_steps[53] = 0.05; + time_steps[54] = 0.05; + time_steps[55] = 0.05; + time_steps[56] = 0.05; + time_steps[57] = 0.05; + time_steps[58] = 0.05; + time_steps[59] = 0.05; + time_steps[60] = 0.05; + time_steps[61] = 0.05; + time_steps[62] = 0.05; + time_steps[63] = 0.05; + time_steps[64] = 0.05; + time_steps[65] = 0.05; + time_steps[66] = 0.05; + time_steps[67] = 0.05; + time_steps[68] = 0.05; + time_steps[69] = 0.05; + time_steps[70] = 0.05; + time_steps[71] = 0.05; + time_steps[72] = 0.05; + time_steps[73] = 0.05; + time_steps[74] = 0.05; + time_steps[75] = 0.05; + time_steps[76] = 0.05; + time_steps[77] = 0.05; + time_steps[78] = 0.05; + time_steps[79] = 0.05; + time_steps[80] = 0.05; + time_steps[81] = 0.05; + time_steps[82] = 0.05; + time_steps[83] = 0.05; + time_steps[84] = 0.05; + time_steps[85] = 0.05; + time_steps[86] = 0.05; + time_steps[87] = 0.05; + time_steps[88] = 0.05; + time_steps[89] = 0.05; + time_steps[90] = 0.05; + time_steps[91] = 0.05; + time_steps[92] = 0.05; + time_steps[93] = 0.05; + time_steps[94] = 0.05; + time_steps[95] = 0.05; + time_steps[96] = 0.05; + time_steps[97] = 0.05; + time_steps[98] = 0.05; + time_steps[99] = 0.05; + + for (int i = 0; i < N; i++) + { + ocp_nlp_in_set(nlp_config, nlp_dims, nlp_in, i, "Ts", &time_steps[i]); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "scaling", &time_steps[i]); + } + + /**** Dynamics ****/ + for (int i = 0; i < N; i++) + { + ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "expl_vde_forw", &forw_vde_casadi[i]); + ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "expl_ode_fun", &expl_ode_fun[i]); + + } + + + /**** Cost ****/ + + double W[NY*NY]; + + W[0+(NY) * 0] = 0; + W[0+(NY) * 1] = 0; + W[0+(NY) * 2] = 0; + W[0+(NY) * 3] = 0; + W[0+(NY) * 4] = 0; + W[0+(NY) * 5] = 0; + W[0+(NY) * 6] = 0; + W[0+(NY) * 7] = 0; + W[0+(NY) * 8] = 0; + W[1+(NY) * 0] = 0; + W[1+(NY) * 1] = 0; + W[1+(NY) * 2] = 0; + W[1+(NY) * 3] = 0; + W[1+(NY) * 4] = 0; + W[1+(NY) * 5] = 0; + W[1+(NY) * 6] = 0; + W[1+(NY) * 7] = 0; + W[1+(NY) * 8] = 0; + W[2+(NY) * 0] = 0; + W[2+(NY) * 1] = 0; + W[2+(NY) * 2] = 0.05; + W[2+(NY) * 3] = 0; + W[2+(NY) * 4] = 0; + W[2+(NY) * 5] = 0; + W[2+(NY) * 6] = 0; + W[2+(NY) * 7] = 0; + W[2+(NY) * 8] = 0; + W[3+(NY) * 0] = 0; + W[3+(NY) * 1] = 0; + W[3+(NY) * 2] = 0; + W[3+(NY) * 3] = 0.01; + W[3+(NY) * 4] = 0; + W[3+(NY) * 5] = 0; + W[3+(NY) * 6] = 0; + W[3+(NY) * 7] = 0; + W[3+(NY) * 8] = 0; + W[4+(NY) * 0] = 0; + W[4+(NY) * 1] = 0; + W[4+(NY) * 2] = 0; + W[4+(NY) * 3] = 0; + W[4+(NY) * 4] = 0; + W[4+(NY) * 5] = 0; + W[4+(NY) * 6] = 0; + W[4+(NY) * 7] = 0; + W[4+(NY) * 8] = 0; + W[5+(NY) * 0] = 0; + W[5+(NY) * 1] = 0; + W[5+(NY) * 2] = 0; + W[5+(NY) * 3] = 0; + W[5+(NY) * 4] = 0; + W[5+(NY) * 5] = 0; + W[5+(NY) * 6] = 0; + W[5+(NY) * 7] = 0; + W[5+(NY) * 8] = 0; + W[6+(NY) * 0] = 0; + W[6+(NY) * 1] = 0; + W[6+(NY) * 2] = 0; + W[6+(NY) * 3] = 0; + W[6+(NY) * 4] = 0; + W[6+(NY) * 5] = 0; + W[6+(NY) * 6] = 0; + W[6+(NY) * 7] = 0; + W[6+(NY) * 8] = 0; + W[7+(NY) * 0] = 0; + W[7+(NY) * 1] = 0; + W[7+(NY) * 2] = 0; + W[7+(NY) * 3] = 0; + W[7+(NY) * 4] = 0; + W[7+(NY) * 5] = 0; + W[7+(NY) * 6] = 0; + W[7+(NY) * 7] = 0; + W[7+(NY) * 8] = 0; + W[8+(NY) * 0] = 0; + W[8+(NY) * 1] = 0; + W[8+(NY) * 2] = 0; + W[8+(NY) * 3] = 0; + W[8+(NY) * 4] = 0; + W[8+(NY) * 5] = 0; + W[8+(NY) * 6] = 0; + W[8+(NY) * 7] = 0; + W[8+(NY) * 8] = 0.2; + + double yref[NY]; + + yref[0] = 0; + yref[1] = 0; + yref[2] = 0; + yref[3] = 0; + yref[4] = 0; + yref[5] = 0; + yref[6] = 0; + yref[7] = 0; + yref[8] = 0; + + for (int i = 0; i < N; i++) + { + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "W", W); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "yref", yref); + } + + + double Vx[NY*NX]; + + Vx[0+(NY) * 0] = 1; + Vx[0+(NY) * 1] = 0; + Vx[0+(NY) * 2] = 0; + Vx[0+(NY) * 3] = 0; + Vx[0+(NY) * 4] = 0; + Vx[0+(NY) * 5] = 0; + Vx[0+(NY) * 6] = 0; + Vx[0+(NY) * 7] = 0; + Vx[1+(NY) * 0] = 0; + Vx[1+(NY) * 1] = 1; + Vx[1+(NY) * 2] = 0; + Vx[1+(NY) * 3] = 0; + Vx[1+(NY) * 4] = 0; + Vx[1+(NY) * 5] = 0; + Vx[1+(NY) * 6] = 0; + Vx[1+(NY) * 7] = 0; + Vx[2+(NY) * 0] = 0; + Vx[2+(NY) * 1] = 0; + Vx[2+(NY) * 2] = 1; + Vx[2+(NY) * 3] = 0; + Vx[2+(NY) * 4] = 0; + Vx[2+(NY) * 5] = 0; + Vx[2+(NY) * 6] = 0; + Vx[2+(NY) * 7] = 0; + Vx[3+(NY) * 0] = 0; + Vx[3+(NY) * 1] = 0; + Vx[3+(NY) * 2] = 0; + Vx[3+(NY) * 3] = 1; + Vx[3+(NY) * 4] = 0; + Vx[3+(NY) * 5] = 0; + Vx[3+(NY) * 6] = 0; + Vx[3+(NY) * 7] = 0; + Vx[4+(NY) * 0] = 0; + Vx[4+(NY) * 1] = 0; + Vx[4+(NY) * 2] = 0; + Vx[4+(NY) * 3] = 0; + Vx[4+(NY) * 4] = 1; + Vx[4+(NY) * 5] = 0; + Vx[4+(NY) * 6] = 0; + Vx[4+(NY) * 7] = 0; + Vx[5+(NY) * 0] = 0; + Vx[5+(NY) * 1] = 0; + Vx[5+(NY) * 2] = 0; + Vx[5+(NY) * 3] = 0; + Vx[5+(NY) * 4] = 0; + Vx[5+(NY) * 5] = 1; + Vx[5+(NY) * 6] = 0; + Vx[5+(NY) * 7] = 0; + Vx[6+(NY) * 0] = 0; + Vx[6+(NY) * 1] = 0; + Vx[6+(NY) * 2] = 0; + Vx[6+(NY) * 3] = 0; + Vx[6+(NY) * 4] = 0; + Vx[6+(NY) * 5] = 0; + Vx[6+(NY) * 6] = 1; + Vx[6+(NY) * 7] = 0; + Vx[7+(NY) * 0] = 0; + Vx[7+(NY) * 1] = 0; + Vx[7+(NY) * 2] = 0; + Vx[7+(NY) * 3] = 0; + Vx[7+(NY) * 4] = 0; + Vx[7+(NY) * 5] = 0; + Vx[7+(NY) * 6] = 0; + Vx[7+(NY) * 7] = 1; + Vx[8+(NY) * 0] = 0; + Vx[8+(NY) * 1] = 0; + Vx[8+(NY) * 2] = 0; + Vx[8+(NY) * 3] = 0; + Vx[8+(NY) * 4] = 0; + Vx[8+(NY) * 5] = 0; + Vx[8+(NY) * 6] = 0; + Vx[8+(NY) * 7] = 0; + for (int i = 0; i < N; i++) + { + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "Vx", Vx); + } + + + double Vu[NY*NU]; + + Vu[0+(NY) * 0] = 0; + Vu[1+(NY) * 0] = 0; + Vu[2+(NY) * 0] = 0; + Vu[3+(NY) * 0] = 0; + Vu[4+(NY) * 0] = 0; + Vu[5+(NY) * 0] = 0; + Vu[6+(NY) * 0] = 0; + Vu[7+(NY) * 0] = 0; + Vu[8+(NY) * 0] = 1; + + for (int i = 0; i < N; i++) + { + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "Vu", Vu); + } + + + + + + + double Zl[NS]; + double Zu[NS]; + double zl[NS]; + double zu[NS]; + + Zl[0] = 0; + Zl[1] = 0; + Zl[2] = 0; + Zl[3] = 0; + Zl[4] = 0; + Zl[5] = 0; + Zl[6] = 0; + Zl[7] = 0; + + + Zu[0] = 0; + Zu[1] = 0; + Zu[2] = 0; + Zu[3] = 0; + Zu[4] = 0; + Zu[5] = 0; + Zu[6] = 0; + Zu[7] = 0; + + + zl[0] = 10; + zl[1] = 10; + zl[2] = 10; + zl[3] = 10; + zl[4] = 10; + zl[5] = 10; + zl[6] = 10; + zl[7] = 10; + + + zu[0] = 10; + zu[1] = 10; + zu[2] = 10; + zu[3] = 10; + zu[4] = 10; + zu[5] = 10; + zu[6] = 10; + zu[7] = 10; + + for (int i = 0; i < N; i++) + { + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "Zl", Zl); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "Zu", Zu); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "zl", zl); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "zu", zu); + } + + + // terminal cost + + + double yref_e[NYN]; + + yref_e[0] = 0; + yref_e[1] = 0; + yref_e[2] = 0; + yref_e[3] = 0; + yref_e[4] = 0; + yref_e[5] = 0; + yref_e[6] = 0; + yref_e[7] = 0; + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "yref", yref_e); + + double W_e[NYN*NYN]; + + W_e[0+(NYN) * 0] = 0; + W_e[0+(NYN) * 1] = 0; + W_e[0+(NYN) * 2] = 0; + W_e[0+(NYN) * 3] = 0; + W_e[0+(NYN) * 4] = 0; + W_e[0+(NYN) * 5] = 0; + W_e[0+(NYN) * 6] = 0; + W_e[0+(NYN) * 7] = 0; + W_e[1+(NYN) * 0] = 0; + W_e[1+(NYN) * 1] = 0; + W_e[1+(NYN) * 2] = 0; + W_e[1+(NYN) * 3] = 0; + W_e[1+(NYN) * 4] = 0; + W_e[1+(NYN) * 5] = 0; + W_e[1+(NYN) * 6] = 0; + W_e[1+(NYN) * 7] = 0; + W_e[2+(NYN) * 0] = 0; + W_e[2+(NYN) * 1] = 0; + W_e[2+(NYN) * 2] = 0.1; + W_e[2+(NYN) * 3] = 0; + W_e[2+(NYN) * 4] = 0; + W_e[2+(NYN) * 5] = 0; + W_e[2+(NYN) * 6] = 0; + W_e[2+(NYN) * 7] = 0; + W_e[3+(NYN) * 0] = 0; + W_e[3+(NYN) * 1] = 0; + W_e[3+(NYN) * 2] = 0; + W_e[3+(NYN) * 3] = 0.05; + W_e[3+(NYN) * 4] = 0; + W_e[3+(NYN) * 5] = 0; + W_e[3+(NYN) * 6] = 0; + W_e[3+(NYN) * 7] = 0; + W_e[4+(NYN) * 0] = 0; + W_e[4+(NYN) * 1] = 0; + W_e[4+(NYN) * 2] = 0; + W_e[4+(NYN) * 3] = 0; + W_e[4+(NYN) * 4] = 0; + W_e[4+(NYN) * 5] = 0; + W_e[4+(NYN) * 6] = 0; + W_e[4+(NYN) * 7] = 0; + W_e[5+(NYN) * 0] = 0; + W_e[5+(NYN) * 1] = 0; + W_e[5+(NYN) * 2] = 0; + W_e[5+(NYN) * 3] = 0; + W_e[5+(NYN) * 4] = 0; + W_e[5+(NYN) * 5] = 0; + W_e[5+(NYN) * 6] = 0; + W_e[5+(NYN) * 7] = 0; + W_e[6+(NYN) * 0] = 0; + W_e[6+(NYN) * 1] = 0; + W_e[6+(NYN) * 2] = 0; + W_e[6+(NYN) * 3] = 0; + W_e[6+(NYN) * 4] = 0; + W_e[6+(NYN) * 5] = 0; + W_e[6+(NYN) * 6] = 0; + W_e[6+(NYN) * 7] = 0; + W_e[7+(NYN) * 0] = 0; + W_e[7+(NYN) * 1] = 0; + W_e[7+(NYN) * 2] = 0; + W_e[7+(NYN) * 3] = 0; + W_e[7+(NYN) * 4] = 0; + W_e[7+(NYN) * 5] = 0; + W_e[7+(NYN) * 6] = 0; + W_e[7+(NYN) * 7] = 0; + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "W", W_e); + double Vx_e[NYN*NX]; + + Vx_e[0+(NYN) * 0] = 1; + Vx_e[0+(NYN) * 1] = 0; + Vx_e[0+(NYN) * 2] = 0; + Vx_e[0+(NYN) * 3] = 0; + Vx_e[0+(NYN) * 4] = 0; + Vx_e[0+(NYN) * 5] = 0; + Vx_e[0+(NYN) * 6] = 0; + Vx_e[0+(NYN) * 7] = 0; + Vx_e[1+(NYN) * 0] = 0; + Vx_e[1+(NYN) * 1] = 1; + Vx_e[1+(NYN) * 2] = 0; + Vx_e[1+(NYN) * 3] = 0; + Vx_e[1+(NYN) * 4] = 0; + Vx_e[1+(NYN) * 5] = 0; + Vx_e[1+(NYN) * 6] = 0; + Vx_e[1+(NYN) * 7] = 0; + Vx_e[2+(NYN) * 0] = 0; + Vx_e[2+(NYN) * 1] = 0; + Vx_e[2+(NYN) * 2] = 1; + Vx_e[2+(NYN) * 3] = 0; + Vx_e[2+(NYN) * 4] = 0; + Vx_e[2+(NYN) * 5] = 0; + Vx_e[2+(NYN) * 6] = 0; + Vx_e[2+(NYN) * 7] = 0; + Vx_e[3+(NYN) * 0] = 0; + Vx_e[3+(NYN) * 1] = 0; + Vx_e[3+(NYN) * 2] = 0; + Vx_e[3+(NYN) * 3] = 1; + Vx_e[3+(NYN) * 4] = 0; + Vx_e[3+(NYN) * 5] = 0; + Vx_e[3+(NYN) * 6] = 0; + Vx_e[3+(NYN) * 7] = 0; + Vx_e[4+(NYN) * 0] = 0; + Vx_e[4+(NYN) * 1] = 0; + Vx_e[4+(NYN) * 2] = 0; + Vx_e[4+(NYN) * 3] = 0; + Vx_e[4+(NYN) * 4] = 1; + Vx_e[4+(NYN) * 5] = 0; + Vx_e[4+(NYN) * 6] = 0; + Vx_e[4+(NYN) * 7] = 0; + Vx_e[5+(NYN) * 0] = 0; + Vx_e[5+(NYN) * 1] = 0; + Vx_e[5+(NYN) * 2] = 0; + Vx_e[5+(NYN) * 3] = 0; + Vx_e[5+(NYN) * 4] = 0; + Vx_e[5+(NYN) * 5] = 1; + Vx_e[5+(NYN) * 6] = 0; + Vx_e[5+(NYN) * 7] = 0; + Vx_e[6+(NYN) * 0] = 0; + Vx_e[6+(NYN) * 1] = 0; + Vx_e[6+(NYN) * 2] = 0; + Vx_e[6+(NYN) * 3] = 0; + Vx_e[6+(NYN) * 4] = 0; + Vx_e[6+(NYN) * 5] = 0; + Vx_e[6+(NYN) * 6] = 1; + Vx_e[6+(NYN) * 7] = 0; + Vx_e[7+(NYN) * 0] = 0; + Vx_e[7+(NYN) * 1] = 0; + Vx_e[7+(NYN) * 2] = 0; + Vx_e[7+(NYN) * 3] = 0; + Vx_e[7+(NYN) * 4] = 0; + Vx_e[7+(NYN) * 5] = 0; + Vx_e[7+(NYN) * 6] = 0; + Vx_e[7+(NYN) * 7] = 1; + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "Vx", Vx_e); + + + + /**** Constraints ****/ + + // bounds for initial stage + + // x0 + int idxbx0[8]; + + idxbx0[0] = 0; + idxbx0[1] = 1; + idxbx0[2] = 2; + idxbx0[3] = 3; + idxbx0[4] = 4; + idxbx0[5] = 5; + idxbx0[6] = 6; + idxbx0[7] = 7; + + double lbx0[8]; + double ubx0[8]; + + lbx0[0] = 0; + ubx0[0] = 0; + lbx0[1] = 0; + ubx0[1] = 0; + lbx0[2] = 0; + ubx0[2] = 0; + lbx0[3] = 0; + ubx0[3] = 0; + lbx0[4] = 0; + ubx0[4] = 0; + lbx0[5] = 0; + ubx0[5] = 0; + lbx0[6] = 0; + ubx0[6] = 0; + lbx0[7] = 0; + ubx0[7] = 0; + + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "idxbx", idxbx0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbx", lbx0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", ubx0); + + + // idxbxe_0 + int idxbxe_0[8]; + + idxbxe_0[0] = 0; + idxbxe_0[1] = 1; + idxbxe_0[2] = 2; + idxbxe_0[3] = 3; + idxbxe_0[4] = 4; + idxbxe_0[5] = 5; + idxbxe_0[6] = 6; + idxbxe_0[7] = 7; + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "idxbxe", idxbxe_0); + + + /* constraints that are the same for initial and intermediate */ + + + + // u + int idxbu[NBU]; + + idxbu[0] = 0; + double lbu[NBU]; + double ubu[NBU]; + + lbu[0] = -0.5; + ubu[0] = 0.5; + + for (int i = 0; i < N; i++) + { + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "idxbu", idxbu); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lbu", lbu); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "ubu", ubu); + } + + + + + + + + // set up soft bounds for nonlinear constraints + int idxsh[NSH]; + + idxsh[0] = 0; + idxsh[1] = 1; + idxsh[2] = 2; + idxsh[3] = 3; + idxsh[4] = 4; + idxsh[5] = 5; + idxsh[6] = 6; + idxsh[7] = 7; + double lsh[NSH]; + double ush[NSH]; + + lsh[0] = -0.2; + ush[0] = 0; + lsh[1] = -0.2; + ush[1] = 0; + lsh[2] = -0.2; + ush[2] = 0; + lsh[3] = -0.2; + ush[3] = 0; + lsh[4] = -0.2; + ush[4] = 0; + lsh[5] = -0.2; + ush[5] = 0; + lsh[6] = -0.2; + ush[6] = 0; + lsh[7] = -0.2; + ush[7] = 0; + + for (int i = 0; i < N; i++) + { + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "idxsh", idxsh); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lsh", lsh); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "ush", ush); + } + + + + + + + + + + // set up nonlinear constraints for stage 0 to N-1 + double lh[NH]; + double uh[NH]; + + + lh[0] = 1.5; + lh[1] = 1.5; + lh[2] = 1.5; + lh[3] = 1.5; + lh[4] = 1.5; + lh[5] = 1.5; + lh[6] = 1.5; + lh[7] = 1.5; + + + uh[0] = 1000000; + uh[1] = 1000000; + uh[2] = 1000000; + uh[3] = 1000000; + uh[4] = 1000000; + uh[5] = 1000000; + uh[6] = 1000000; + uh[7] = 1000000; + + for (int i = 0; i < N; i++) + { + // nonlinear constraints for stages 0 to N-1 + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "nl_constr_h_fun_jac", + &nl_constr_h_fun_jac[i]); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "nl_constr_h_fun", + &nl_constr_h_fun[i]); + + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lh", lh); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "uh", uh); + } + + + + + /* terminal constraints */ + + + + + + + + + + + + + + + + + + /************************************************ + * opts + ************************************************/ + + nlp_opts = ocp_nlp_solver_opts_create(nlp_config, nlp_dims); + + + int num_steps_val = 1; + for (int i = 0; i < N; i++) + ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_num_steps", &num_steps_val); + + int ns_val = 4; + for (int i = 0; i < N; i++) + ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_num_stages", &ns_val); + + int newton_iter_val = 3; + for (int i = 0; i < N; i++) + ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_newton_iter", &newton_iter_val); + + bool tmp_bool = false; + for (int i = 0; i < N; i++) + ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_jac_reuse", &tmp_bool); + + double nlp_solver_step_length = 1; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "step_length", &nlp_solver_step_length); + + double levenberg_marquardt = 0; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "levenberg_marquardt", &levenberg_marquardt); + + /* options QP solver */ + int qp_solver_cond_N; + // NOTE: there is no condensing happening here! + qp_solver_cond_N = N; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_cond_N", &qp_solver_cond_N); + + + int qp_solver_iter_max = 50; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_iter_max", &qp_solver_iter_max); + + int print_level = 0; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "print_level", &print_level); + + + int ext_cost_num_hess = 0; + + + /* out */ + nlp_out = ocp_nlp_out_create(nlp_config, nlp_dims); + + // initialize primal solution + double x0[8]; + + // initialize with x0 + + x0[0] = 0; + x0[1] = 0; + x0[2] = 0; + x0[3] = 0; + x0[4] = 0; + x0[5] = 0; + x0[6] = 0; + x0[7] = 0; + + + double u0[NU]; + + u0[0] = 0.0; + + for (int i = 0; i < N; i++) + { + // x0 + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "x", x0); + // u0 + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "u", u0); + } + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, N, "x", x0); + + nlp_solver = ocp_nlp_solver_create(nlp_config, nlp_dims, nlp_opts); + + + + // initialize parameters to nominal value + double p[16]; + + p[0] = 100; + p[1] = 100; + p[2] = 100; + p[3] = 100; + p[4] = 100; + p[5] = 100; + p[6] = 100; + p[7] = 100; + p[8] = 100; + p[9] = 100; + p[10] = 100; + p[11] = 100; + p[12] = 100; + p[13] = 100; + p[14] = 100; + p[15] = 100; + + + for (int ii = 0; ii < N; ii++) + { + forw_vde_casadi[ii].set_param(forw_vde_casadi+ii, p); + expl_ode_fun[ii].set_param(expl_ode_fun+ii, p); + } + + + // cost + + // constraints + + for (int ii = 0; ii < N; ii++) + { + nl_constr_h_fun_jac[ii].set_param(nl_constr_h_fun_jac+ii, p); + nl_constr_h_fun[ii].set_param(nl_constr_h_fun+ii, p); + } + + status = ocp_nlp_precompute(nlp_solver, nlp_in, nlp_out); + + if (status != ACADOS_SUCCESS) + { + printf("\nocp_precompute failed!\n\n"); + exit(1); + } + + return status; +} + + +int acados_update_params(int stage, double *p, int np) +{ + int solver_status = 0; + + int casadi_np = 16; + if (casadi_np != np) { + printf("acados_update_params: trying to set %i parameters for external functions." + " External function has %i parameters. Exiting.\n", np, casadi_np); + exit(1); + } + if (stage < 100) + { + forw_vde_casadi[stage].set_param(forw_vde_casadi+stage, p); + expl_ode_fun[stage].set_param(expl_ode_fun+stage, p); + + + // constraints + + nl_constr_h_fun_jac[stage].set_param(nl_constr_h_fun_jac+stage, p); + nl_constr_h_fun[stage].set_param(nl_constr_h_fun+stage, p); + + // cost + + } + else // stage == N + { + // terminal shooting node has no dynamics + // cost + // constraints + + } + + + return solver_status; +} + + + +int acados_solve() +{ + // solve NLP + int solver_status = ocp_nlp_solve(nlp_solver, nlp_in, nlp_out); + + return solver_status; +} + + +int acados_free() +{ + // free memory + ocp_nlp_solver_opts_destroy(nlp_opts); + ocp_nlp_in_destroy(nlp_in); + ocp_nlp_out_destroy(nlp_out); + ocp_nlp_solver_destroy(nlp_solver); + ocp_nlp_dims_destroy(nlp_dims); + ocp_nlp_config_destroy(nlp_config); + ocp_nlp_plan_destroy(nlp_solver_plan); + + /* free external function */ + // dynamics + for (int i = 0; i < 100; i++) + { + external_function_param_casadi_free(&forw_vde_casadi[i]); + external_function_param_casadi_free(&expl_ode_fun[i]); + } + free(forw_vde_casadi); + free(expl_ode_fun); + + // cost + + // constraints + for (int i = 0; i < 100; i++) + { + external_function_param_casadi_free(&nl_constr_h_fun_jac[i]); + external_function_param_casadi_free(&nl_constr_h_fun[i]); + } + free(nl_constr_h_fun_jac); + free(nl_constr_h_fun); + + return 0; +} + +ocp_nlp_in * acados_get_nlp_in() { return nlp_in; } +ocp_nlp_out * acados_get_nlp_out() { return nlp_out; } +ocp_nlp_solver * acados_get_nlp_solver() { return nlp_solver; } +ocp_nlp_config * acados_get_nlp_config() { return nlp_config; } +void * acados_get_nlp_opts() { return nlp_opts; } +ocp_nlp_dims * acados_get_nlp_dims() { return nlp_dims; } +ocp_nlp_plan * acados_get_nlp_plan() { return nlp_solver_plan; } + + +void acados_print_stats() +{ + int sqp_iter, stat_m, stat_n, tmp_int; + ocp_nlp_get(nlp_config, nlp_solver, "sqp_iter", &sqp_iter); + ocp_nlp_get(nlp_config, nlp_solver, "stat_n", &stat_n); + ocp_nlp_get(nlp_config, nlp_solver, "stat_m", &stat_m); + + + double stat[1000]; + ocp_nlp_get(nlp_config, nlp_solver, "statistics", stat); + + int nrow = sqp_iter+1 < stat_m ? sqp_iter+1 : stat_m; + + printf("iter\tres_stat\tres_eq\t\tres_ineq\tres_comp\tqp_stat\tqp_iter\n"); + for (int i = 0; i < nrow; i++) + { + for (int j = 0; j < stat_n + 1; j++) + { + if (j == 0 || j > 4) + { + tmp_int = (int) stat[i + j * nrow]; + printf("%d\t", tmp_int); + } + else + { + printf("%e\t", stat[i + j * nrow]); + } + } + printf("\n"); + } +} diff --git a/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/acados_solver_usv_model_guidance_ca1.h b/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/acados_solver_usv_model_guidance_ca1.h new file mode 100644 index 00000000..f6e21cee --- /dev/null +++ b/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/acados_solver_usv_model_guidance_ca1.h @@ -0,0 +1,96 @@ +/* + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + +#ifndef ACADOS_SOLVER_usv_model_guidance_ca1_H_ +#define ACADOS_SOLVER_usv_model_guidance_ca1_H_ + +#include "acados_c/ocp_nlp_interface.h" +#include "acados_c/external_function_interface.h" + +#ifdef __cplusplus +extern "C" { +#endif + +int acados_create(); +int acados_update_params(int stage, double *value, int np); +int acados_solve(); +int acados_free(); +void acados_print_stats(); + +ocp_nlp_in * acados_get_nlp_in(); +ocp_nlp_out * acados_get_nlp_out(); +ocp_nlp_solver * acados_get_nlp_solver(); +ocp_nlp_config * acados_get_nlp_config(); +void * acados_get_nlp_opts(); +ocp_nlp_dims * acados_get_nlp_dims(); +ocp_nlp_plan * acados_get_nlp_plan(); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +// ** global data ** +// acados objects +extern ocp_nlp_in * nlp_in; +extern ocp_nlp_out * nlp_out; +extern ocp_nlp_solver * nlp_solver; +extern void * nlp_opts; +extern ocp_nlp_plan * nlp_solver_plan; +extern ocp_nlp_config * nlp_config; +extern ocp_nlp_dims * nlp_dims; + +// number of expected runtime parameters +extern const unsigned int nlp_np; + +/* external functions */ +// dynamics + +extern external_function_param_casadi * forw_vde_casadi; +extern external_function_param_casadi * expl_ode_fun; + + + +// cost + + + +// constraints +extern external_function_param_casadi * nl_constr_h_fun_jac; +extern external_function_param_casadi * nl_constr_h_fun; +extern external_function_param_casadi * nl_constr_h_fun_jac_hess; + + + + + +#endif // ACADOS_SOLVER_usv_model_guidance_ca1_H_ diff --git a/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/acados_solver_usv_model_guidance_ca1.o b/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/acados_solver_usv_model_guidance_ca1.o new file mode 100644 index 00000000..48e23508 Binary files /dev/null and b/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/acados_solver_usv_model_guidance_ca1.o differ diff --git a/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/libacados_ocp_solver_usv_model_guidance_ca1.so b/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/libacados_ocp_solver_usv_model_guidance_ca1.so new file mode 100755 index 00000000..20bb3867 Binary files /dev/null and b/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/libacados_ocp_solver_usv_model_guidance_ca1.so differ diff --git a/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/main_sim_usv_model_guidance_ca1.c b/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/main_sim_usv_model_guidance_ca1.c new file mode 100644 index 00000000..9a189f7d --- /dev/null +++ b/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/main_sim_usv_model_guidance_ca1.c @@ -0,0 +1,154 @@ +/* + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +// standard +#include +#include +// acados +#include "acados/utils/print.h" +#include "acados/utils/math.h" +#include "acados_c/sim_interface.h" +#include "acados_sim_solver_usv_model_guidance_ca1.h" + + +int main() +{ + int status = 0; + status = usv_model_guidance_ca1_acados_sim_create(); + + if (status) + { + printf("acados_create() returned status %d. Exiting.\n", status); + exit(1); + } + + // initial condition + double x_current[8]; + x_current[0] = 0.0; + x_current[1] = 0.0; + x_current[2] = 0.0; + x_current[3] = 0.0; + x_current[4] = 0.0; + x_current[5] = 0.0; + x_current[6] = 0.0; + x_current[7] = 0.0; + + + x_current[0] = 0; + x_current[1] = 0; + x_current[2] = 0; + x_current[3] = 0; + x_current[4] = 0; + x_current[5] = 0; + x_current[6] = 0; + x_current[7] = 0; + + + + + // initial value for control input + double u0[1]; + u0[0] = 0.0; + // set parameters + double p[16]; + + p[0] = 100; + + p[1] = 100; + + p[2] = 100; + + p[3] = 100; + + p[4] = 100; + + p[5] = 100; + + p[6] = 100; + + p[7] = 100; + + p[8] = 100; + + p[9] = 100; + + p[10] = 100; + + p[11] = 100; + + p[12] = 100; + + p[13] = 100; + + p[14] = 100; + + p[15] = 100; + + + usv_model_guidance_ca1_acados_sim_update_params(p, 16); + + + int n_sim_steps = 3; + // solve ocp in loop + for (int ii = 0; ii < n_sim_steps; ii++) + { + sim_in_set(usv_model_guidance_ca1_sim_config, usv_model_guidance_ca1_sim_dims, + usv_model_guidance_ca1_sim_in, "x", x_current); + status = usv_model_guidance_ca1_acados_sim_solve(); + + if (status != ACADOS_SUCCESS) + { + printf("acados_solve() failed with status %d.\n", status); + } + + sim_out_get(usv_model_guidance_ca1_sim_config, usv_model_guidance_ca1_sim_dims, + usv_model_guidance_ca1_sim_out, "x", x_current); + + printf("\nx_current, %d\n", ii); + for (int jj = 0; jj < 8; jj++) + { + printf("%e\n", x_current[jj]); + } + } + + printf("\nPerformed %d simulation steps with acados integrator successfully.\n\n", n_sim_steps); + + // free solver + status = usv_model_guidance_ca1_acados_sim_free(); + if (status) { + printf("usv_model_guidance_ca1_acados_sim_free() returned status %d. \n", status); + } + + return status; +} diff --git a/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/main_usv_model_guidance_ca1.c b/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/main_usv_model_guidance_ca1.c new file mode 100644 index 00000000..24262f55 --- /dev/null +++ b/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/main_usv_model_guidance_ca1.c @@ -0,0 +1,216 @@ +/* + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +// standard +#include +#include +// acados +#include "acados/utils/print.h" +#include "acados/utils/math.h" +#include "acados_c/ocp_nlp_interface.h" +#include "acados_c/external_function_interface.h" +#include "acados_solver_usv_model_guidance_ca1.h" + + +int main() +{ + + int status = 0; + status = acados_create(); + + if (status) + { + printf("acados_create() returned status %d. Exiting.\n", status); + exit(1); + } + + // initial condition + int idxbx0[8]; + idxbx0[0] = 0; + idxbx0[1] = 1; + idxbx0[2] = 2; + idxbx0[3] = 3; + idxbx0[4] = 4; + idxbx0[5] = 5; + idxbx0[6] = 6; + idxbx0[7] = 7; + + double lbx0[8]; + double ubx0[8]; + lbx0[0] = 0; + ubx0[0] = 0; + lbx0[1] = 0; + ubx0[1] = 0; + lbx0[2] = 0; + ubx0[2] = 0; + lbx0[3] = 0; + ubx0[3] = 0; + lbx0[4] = 0; + ubx0[4] = 0; + lbx0[5] = 0; + ubx0[5] = 0; + lbx0[6] = 0; + ubx0[6] = 0; + lbx0[7] = 0; + ubx0[7] = 0; + + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "idxbx", idxbx0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbx", lbx0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", ubx0); + + // initialization for state values + double x_init[8]; + x_init[0] = 0.0; + x_init[1] = 0.0; + x_init[2] = 0.0; + x_init[3] = 0.0; + x_init[4] = 0.0; + x_init[5] = 0.0; + x_init[6] = 0.0; + x_init[7] = 0.0; + + // initial value for control input + double u0[1]; + u0[0] = 0.0; + // set parameters + double p[16]; + + p[0] = 100; + + p[1] = 100; + + p[2] = 100; + + p[3] = 100; + + p[4] = 100; + + p[5] = 100; + + p[6] = 100; + + p[7] = 100; + + p[8] = 100; + + p[9] = 100; + + p[10] = 100; + + p[11] = 100; + + p[12] = 100; + + p[13] = 100; + + p[14] = 100; + + p[15] = 100; + + + for (int ii = 0; ii <= 100; ii++) + { + acados_update_params(ii, p, 16); + } + + + // prepare evaluation + int NTIMINGS = 1; + double min_time = 1e12; + double kkt_norm_inf; + double elapsed_time; + int sqp_iter; + + double xtraj[8 * (100+1)]; + double utraj[1 * (100)]; + + + // solve ocp in loop + int rti_phase = 0; + + for (int ii = 0; ii < NTIMINGS; ii++) + { + // initialize solution + for (int i = 0; i <= nlp_dims->N; i++) + { + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "x", x_init); + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "u", u0); + } + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "rti_phase", &rti_phase); + status = acados_solve(); + ocp_nlp_get(nlp_config, nlp_solver, "time_tot", &elapsed_time); + min_time = MIN(elapsed_time, min_time); + } + + /* print solution and statistics */ + for (int ii = 0; ii <= nlp_dims->N; ii++) + ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, "x", &xtraj[ii*8]); + for (int ii = 0; ii < nlp_dims->N; ii++) + ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, "u", &utraj[ii*1]); + + printf("\n--- xtraj ---\n"); + d_print_exp_tran_mat( 8, 100+1, xtraj, 8 ); + printf("\n--- utraj ---\n"); + d_print_exp_tran_mat( 1, 100, utraj, 1 ); + // ocp_nlp_out_print(nlp_solver->dims, nlp_out); + + printf("\nsolved ocp %d times, solution printed above\n\n", NTIMINGS); + + if (status == ACADOS_SUCCESS) + { + printf("acados_solve(): SUCCESS!\n"); + } + else + { + printf("acados_solve() failed with status %d.\n", status); + } + + // get solution + ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 0, "kkt_norm_inf", &kkt_norm_inf); + ocp_nlp_get(nlp_config, nlp_solver, "sqp_iter", &sqp_iter); + + acados_print_stats(); + + printf("\nSolver info:\n"); + printf(" SQP iterations %2d\n minimum time for %d solve %f [ms]\n KKT %e\n", + sqp_iter, NTIMINGS, min_time*1000, kkt_norm_inf); + + // free solver + status = acados_free(); + if (status) { + printf("acados_free() returned status %d. \n", status); + } + + return status; +} diff --git a/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/make_sfun.m b/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/make_sfun.m new file mode 100644 index 00000000..f6bf2bcb --- /dev/null +++ b/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/make_sfun.m @@ -0,0 +1,99 @@ +% +% Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +% Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +% Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +% Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl +% +% This file is part of acados. +% +% The 2-Clause BSD License +% +% Redistribution and use in source and binary forms, with or without +% modification, are permitted provided that the following conditions are met: +% +% 1. Redistributions of source code must retain the above copyright notice, +% this list of conditions and the following disclaimer. +% +% 2. Redistributions in binary form must reproduce the above copyright notice, +% this list of conditions and the following disclaimer in the documentation +% and/or other materials provided with the distribution. +% +% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +% POSSIBILITY OF SUCH DAMAGE.; +% + +SOURCES = [ ... + 'usv_model_guidance_ca1_model/usv_model_guidance_ca1_expl_ode_fun.c ', ... + 'usv_model_guidance_ca1_model/usv_model_guidance_ca1_expl_vde_forw.c ',... + 'usv_model_guidance_ca1_constraints/usv_model_guidance_ca1_constr_h_fun.c ', ... + 'usv_model_guidance_ca1_constraints/usv_model_guidance_ca1_constr_h_fun_jac_uxt_hess.c ', ... + 'usv_model_guidance_ca1_constraints/usv_model_guidance_ca1_constr_h_fun_jac_uxt_zt.c ', ... + 'acados_solver_sfunction_usv_model_guidance_ca1.c ', ... + 'acados_solver_usv_model_guidance_ca1.c ' + ]; + +INC_PATH = '/home/nvidia/vanttec_usv_ws/src/usv_avoidance/include/acados/include'; + +INCS = [ ' -I', fullfile(INC_PATH, 'blasfeo', 'include'), ... + ' -I', fullfile(INC_PATH, 'hpipm', 'include'), ... + ' -I', INC_PATH, ' -I', fullfile(INC_PATH, 'acados'), ' ']; + + + +CFLAGS = ' -O'; + + + +LIB_PATH = '/home/nvidia/vanttec_usv_ws/src/usv_avoidance/include/acados/lib'; + +LIBS = '-lacados -lhpipm -lblasfeo'; + + + +eval( [ 'mex -v -output acados_solver_sfunction_usv_model_guidance_ca1 ', ... + CFLAGS, INCS, ' ', SOURCES, ' -L', LIB_PATH, ' ', LIBS ]); + +fprintf( [ '\n\nSuccessfully created sfunction:\nacados_solver_sfunction_usv_model_guidance_ca1', '.', ... + eval('mexext')] ); + + +%% print note on usage of s-function +fprintf('\n\nNote: Usage of Sfunction is as follows:\n') +input_note = 'Inputs are:\n1) x0, initial state, size [8]\n '; +i_in = 2; +input_note = strcat(input_note, num2str(i_in), ') y_ref - concatenated for intermediate stages,',... + ' size [900]\n '); +i_in = i_in + 1; +input_note = strcat(input_note, num2str(i_in), ') y_ref_e, size [8]\n '); +i_in = i_in + 1; +input_note = strcat(input_note, num2str(i_in), ') parameters - concatenated for all stages,',... + ' size [1616]\n '); +i_in = i_in + 1; +input_note = strcat(input_note, num2str(i_in), ') lbu, size [1]\n '); +i_in = i_in + 1; +input_note = strcat(input_note, num2str(i_in), ') ubu, size [1]\n '); +i_in = i_in + 1; +input_note = strcat(input_note, num2str(i_in), ') lh, size [8]\n '); +i_in = i_in + 1; +input_note = strcat(input_note, num2str(i_in), ') uh, size [8]\n '); +i_in = i_in + 1; + +fprintf(input_note) + +disp(' ') + +output_note = strcat('Outputs are:\n', ... + '1) u0 - optimal input, size [1]\n',... + '2) acados solver status (0 = SUCCESS)\n3) KKT residual\n',... + '4) first state \n5) CPU time\n6) sqp iter\n'); + +fprintf(output_note) diff --git a/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/usv_model_guidance_ca1_constraints/usv_model_guidance_ca1_constr_h_fun.c b/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/usv_model_guidance_ca1_constraints/usv_model_guidance_ca1_constr_h_fun.c new file mode 100644 index 00000000..3c874533 --- /dev/null +++ b/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/usv_model_guidance_ca1_constraints/usv_model_guidance_ca1_constr_h_fun.c @@ -0,0 +1,293 @@ +/* This file was automatically generated by CasADi. + The CasADi copyright holders make no ownership claim of its contents. */ +#ifdef __cplusplus +extern "C" { +#endif + +/* How to prefix internal symbols */ +#ifdef CASADI_CODEGEN_PREFIX + #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID) + #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID + #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID) +#else + #define CASADI_PREFIX(ID) usv_model_guidance_ca1_constr_h_fun_ ## ID +#endif + +#include + +#ifndef casadi_real +#define casadi_real double +#endif + +#ifndef casadi_int +#define casadi_int int +#endif + +/* Add prefix to internal symbols */ +#define casadi_f0 CASADI_PREFIX(f0) +#define casadi_s0 CASADI_PREFIX(s0) +#define casadi_s1 CASADI_PREFIX(s1) +#define casadi_s2 CASADI_PREFIX(s2) +#define casadi_s3 CASADI_PREFIX(s3) +#define casadi_sq CASADI_PREFIX(sq) + +/* Symbol visibility in DLLs */ +#ifndef CASADI_SYMBOL_EXPORT + #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__) + #if defined(STATIC_LINKED) + #define CASADI_SYMBOL_EXPORT + #else + #define CASADI_SYMBOL_EXPORT __declspec(dllexport) + #endif + #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY) + #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default"))) + #else + #define CASADI_SYMBOL_EXPORT + #endif +#endif + +casadi_real casadi_sq(casadi_real x) { return x*x;} + +static const casadi_int casadi_s0[12] = {8, 1, 0, 8, 0, 1, 2, 3, 4, 5, 6, 7}; +static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0}; +static const casadi_int casadi_s2[3] = {0, 0, 0}; +static const casadi_int casadi_s3[20] = {16, 1, 0, 16, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15}; + +/* usv_model_guidance_ca1_constr_h_fun:(i0[8],i1,i2[],i3[16])->(o0[8]) */ +static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) { + casadi_real w0, w1, w2, w3; + /* #0: @0 = input[0][5] */ + w0 = arg[0] ? arg[0][5] : 0; + /* #1: @1 = input[3][0] */ + w1 = arg[3] ? arg[3][0] : 0; + /* #2: @1 = (@0-@1) */ + w1 = (w0-w1); + /* #3: @1 = sq(@1) */ + w1 = casadi_sq( w1 ); + /* #4: @2 = input[0][6] */ + w2 = arg[0] ? arg[0][6] : 0; + /* #5: @3 = input[3][1] */ + w3 = arg[3] ? arg[3][1] : 0; + /* #6: @3 = (@2-@3) */ + w3 = (w2-w3); + /* #7: @3 = sq(@3) */ + w3 = casadi_sq( w3 ); + /* #8: @1 = (@1+@3) */ + w1 += w3; + /* #9: @1 = sqrt(@1) */ + w1 = sqrt( w1 ); + /* #10: output[0][0] = @1 */ + if (res[0]) res[0][0] = w1; + /* #11: @1 = input[3][2] */ + w1 = arg[3] ? arg[3][2] : 0; + /* #12: @1 = (@0-@1) */ + w1 = (w0-w1); + /* #13: @1 = sq(@1) */ + w1 = casadi_sq( w1 ); + /* #14: @3 = input[3][3] */ + w3 = arg[3] ? arg[3][3] : 0; + /* #15: @3 = (@2-@3) */ + w3 = (w2-w3); + /* #16: @3 = sq(@3) */ + w3 = casadi_sq( w3 ); + /* #17: @1 = (@1+@3) */ + w1 += w3; + /* #18: @1 = sqrt(@1) */ + w1 = sqrt( w1 ); + /* #19: output[0][1] = @1 */ + if (res[0]) res[0][1] = w1; + /* #20: @1 = input[3][4] */ + w1 = arg[3] ? arg[3][4] : 0; + /* #21: @1 = (@0-@1) */ + w1 = (w0-w1); + /* #22: @1 = sq(@1) */ + w1 = casadi_sq( w1 ); + /* #23: @3 = input[3][5] */ + w3 = arg[3] ? arg[3][5] : 0; + /* #24: @3 = (@2-@3) */ + w3 = (w2-w3); + /* #25: @3 = sq(@3) */ + w3 = casadi_sq( w3 ); + /* #26: @1 = (@1+@3) */ + w1 += w3; + /* #27: @1 = sqrt(@1) */ + w1 = sqrt( w1 ); + /* #28: output[0][2] = @1 */ + if (res[0]) res[0][2] = w1; + /* #29: @1 = input[3][6] */ + w1 = arg[3] ? arg[3][6] : 0; + /* #30: @1 = (@0-@1) */ + w1 = (w0-w1); + /* #31: @1 = sq(@1) */ + w1 = casadi_sq( w1 ); + /* #32: @3 = input[3][7] */ + w3 = arg[3] ? arg[3][7] : 0; + /* #33: @3 = (@2-@3) */ + w3 = (w2-w3); + /* #34: @3 = sq(@3) */ + w3 = casadi_sq( w3 ); + /* #35: @1 = (@1+@3) */ + w1 += w3; + /* #36: @1 = sqrt(@1) */ + w1 = sqrt( w1 ); + /* #37: output[0][3] = @1 */ + if (res[0]) res[0][3] = w1; + /* #38: @1 = input[3][8] */ + w1 = arg[3] ? arg[3][8] : 0; + /* #39: @1 = (@0-@1) */ + w1 = (w0-w1); + /* #40: @1 = sq(@1) */ + w1 = casadi_sq( w1 ); + /* #41: @3 = input[3][9] */ + w3 = arg[3] ? arg[3][9] : 0; + /* #42: @3 = (@2-@3) */ + w3 = (w2-w3); + /* #43: @3 = sq(@3) */ + w3 = casadi_sq( w3 ); + /* #44: @1 = (@1+@3) */ + w1 += w3; + /* #45: @1 = sqrt(@1) */ + w1 = sqrt( w1 ); + /* #46: output[0][4] = @1 */ + if (res[0]) res[0][4] = w1; + /* #47: @1 = input[3][10] */ + w1 = arg[3] ? arg[3][10] : 0; + /* #48: @1 = (@0-@1) */ + w1 = (w0-w1); + /* #49: @1 = sq(@1) */ + w1 = casadi_sq( w1 ); + /* #50: @3 = input[3][11] */ + w3 = arg[3] ? arg[3][11] : 0; + /* #51: @3 = (@2-@3) */ + w3 = (w2-w3); + /* #52: @3 = sq(@3) */ + w3 = casadi_sq( w3 ); + /* #53: @1 = (@1+@3) */ + w1 += w3; + /* #54: @1 = sqrt(@1) */ + w1 = sqrt( w1 ); + /* #55: output[0][5] = @1 */ + if (res[0]) res[0][5] = w1; + /* #56: @1 = input[3][12] */ + w1 = arg[3] ? arg[3][12] : 0; + /* #57: @1 = (@0-@1) */ + w1 = (w0-w1); + /* #58: @1 = sq(@1) */ + w1 = casadi_sq( w1 ); + /* #59: @3 = input[3][13] */ + w3 = arg[3] ? arg[3][13] : 0; + /* #60: @3 = (@2-@3) */ + w3 = (w2-w3); + /* #61: @3 = sq(@3) */ + w3 = casadi_sq( w3 ); + /* #62: @1 = (@1+@3) */ + w1 += w3; + /* #63: @1 = sqrt(@1) */ + w1 = sqrt( w1 ); + /* #64: output[0][6] = @1 */ + if (res[0]) res[0][6] = w1; + /* #65: @1 = input[3][14] */ + w1 = arg[3] ? arg[3][14] : 0; + /* #66: @0 = (@0-@1) */ + w0 -= w1; + /* #67: @0 = sq(@0) */ + w0 = casadi_sq( w0 ); + /* #68: @1 = input[3][15] */ + w1 = arg[3] ? arg[3][15] : 0; + /* #69: @2 = (@2-@1) */ + w2 -= w1; + /* #70: @2 = sq(@2) */ + w2 = casadi_sq( w2 ); + /* #71: @0 = (@0+@2) */ + w0 += w2; + /* #72: @0 = sqrt(@0) */ + w0 = sqrt( w0 ); + /* #73: output[0][7] = @0 */ + if (res[0]) res[0][7] = w0; + return 0; +} + +CASADI_SYMBOL_EXPORT int usv_model_guidance_ca1_constr_h_fun(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){ + return casadi_f0(arg, res, iw, w, mem); +} + +CASADI_SYMBOL_EXPORT int usv_model_guidance_ca1_constr_h_fun_alloc_mem(void) { + return 0; +} + +CASADI_SYMBOL_EXPORT int usv_model_guidance_ca1_constr_h_fun_init_mem(int mem) { + return 0; +} + +CASADI_SYMBOL_EXPORT void usv_model_guidance_ca1_constr_h_fun_free_mem(int mem) { +} + +CASADI_SYMBOL_EXPORT int usv_model_guidance_ca1_constr_h_fun_checkout(void) { + return 0; +} + +CASADI_SYMBOL_EXPORT void usv_model_guidance_ca1_constr_h_fun_release(int mem) { +} + +CASADI_SYMBOL_EXPORT void usv_model_guidance_ca1_constr_h_fun_incref(void) { +} + +CASADI_SYMBOL_EXPORT void usv_model_guidance_ca1_constr_h_fun_decref(void) { +} + +CASADI_SYMBOL_EXPORT casadi_int usv_model_guidance_ca1_constr_h_fun_n_in(void) { return 4;} + +CASADI_SYMBOL_EXPORT casadi_int usv_model_guidance_ca1_constr_h_fun_n_out(void) { return 1;} + +CASADI_SYMBOL_EXPORT casadi_real usv_model_guidance_ca1_constr_h_fun_default_in(casadi_int i){ + switch (i) { + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const char* usv_model_guidance_ca1_constr_h_fun_name_in(casadi_int i){ + switch (i) { + case 0: return "i0"; + case 1: return "i1"; + case 2: return "i2"; + case 3: return "i3"; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const char* usv_model_guidance_ca1_constr_h_fun_name_out(casadi_int i){ + switch (i) { + case 0: return "o0"; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const casadi_int* usv_model_guidance_ca1_constr_h_fun_sparsity_in(casadi_int i) { + switch (i) { + case 0: return casadi_s0; + case 1: return casadi_s1; + case 2: return casadi_s2; + case 3: return casadi_s3; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const casadi_int* usv_model_guidance_ca1_constr_h_fun_sparsity_out(casadi_int i) { + switch (i) { + case 0: return casadi_s0; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT int usv_model_guidance_ca1_constr_h_fun_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { + if (sz_arg) *sz_arg = 6; + if (sz_res) *sz_res = 2; + if (sz_iw) *sz_iw = 0; + if (sz_w) *sz_w = 4; + return 0; +} + + +#ifdef __cplusplus +} /* extern "C" */ +#endif diff --git a/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/usv_model_guidance_ca1_constraints/usv_model_guidance_ca1_constr_h_fun.o b/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/usv_model_guidance_ca1_constraints/usv_model_guidance_ca1_constr_h_fun.o new file mode 100644 index 00000000..c67428bc Binary files /dev/null and b/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/usv_model_guidance_ca1_constraints/usv_model_guidance_ca1_constr_h_fun.o differ diff --git a/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/usv_model_guidance_ca1_constraints/usv_model_guidance_ca1_constr_h_fun_jac_uxt_zt.c b/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/usv_model_guidance_ca1_constraints/usv_model_guidance_ca1_constr_h_fun_jac_uxt_zt.c new file mode 100644 index 00000000..f620d7f2 --- /dev/null +++ b/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/usv_model_guidance_ca1_constraints/usv_model_guidance_ca1_constr_h_fun_jac_uxt_zt.c @@ -0,0 +1,478 @@ +/* This file was automatically generated by CasADi. + The CasADi copyright holders make no ownership claim of its contents. */ +#ifdef __cplusplus +extern "C" { +#endif + +/* How to prefix internal symbols */ +#ifdef CASADI_CODEGEN_PREFIX + #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID) + #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID + #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID) +#else + #define CASADI_PREFIX(ID) usv_model_guidance_ca1_constr_h_fun_jac_uxt_zt_ ## ID +#endif + +#include + +#ifndef casadi_real +#define casadi_real double +#endif + +#ifndef casadi_int +#define casadi_int int +#endif + +/* Add prefix to internal symbols */ +#define casadi_clear CASADI_PREFIX(clear) +#define casadi_copy CASADI_PREFIX(copy) +#define casadi_f0 CASADI_PREFIX(f0) +#define casadi_fill CASADI_PREFIX(fill) +#define casadi_s0 CASADI_PREFIX(s0) +#define casadi_s1 CASADI_PREFIX(s1) +#define casadi_s2 CASADI_PREFIX(s2) +#define casadi_s3 CASADI_PREFIX(s3) +#define casadi_s4 CASADI_PREFIX(s4) +#define casadi_s5 CASADI_PREFIX(s5) +#define casadi_sq CASADI_PREFIX(sq) + +/* Symbol visibility in DLLs */ +#ifndef CASADI_SYMBOL_EXPORT + #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__) + #if defined(STATIC_LINKED) + #define CASADI_SYMBOL_EXPORT + #else + #define CASADI_SYMBOL_EXPORT __declspec(dllexport) + #endif + #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY) + #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default"))) + #else + #define CASADI_SYMBOL_EXPORT + #endif +#endif + +casadi_real casadi_sq(casadi_real x) { return x*x;} + +void casadi_clear(casadi_real* x, casadi_int n) { + casadi_int i; + if (x) { + for (i=0; i(o0[8],o1[9x8,16nz],o2[8x0]) */ +static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) { + casadi_real *rr, *ss; + casadi_real w0, w1, w2, w3, w4, w5, w6, w7, w8, w9, w10, w11, w12, w13, w14, w15, w16, w17, w18, w19, w20, w21, w22, w23, w24, *w25=w+25, *w26=w+41; + /* #0: @0 = input[0][5] */ + w0 = arg[0] ? arg[0][5] : 0; + /* #1: @1 = input[3][0] */ + w1 = arg[3] ? arg[3][0] : 0; + /* #2: @1 = (@0-@1) */ + w1 = (w0-w1); + /* #3: @2 = sq(@1) */ + w2 = casadi_sq( w1 ); + /* #4: @3 = input[0][6] */ + w3 = arg[0] ? arg[0][6] : 0; + /* #5: @4 = input[3][1] */ + w4 = arg[3] ? arg[3][1] : 0; + /* #6: @4 = (@3-@4) */ + w4 = (w3-w4); + /* #7: @5 = sq(@4) */ + w5 = casadi_sq( w4 ); + /* #8: @2 = (@2+@5) */ + w2 += w5; + /* #9: @2 = sqrt(@2) */ + w2 = sqrt( w2 ); + /* #10: output[0][0] = @2 */ + if (res[0]) res[0][0] = w2; + /* #11: @5 = input[3][2] */ + w5 = arg[3] ? arg[3][2] : 0; + /* #12: @5 = (@0-@5) */ + w5 = (w0-w5); + /* #13: @6 = sq(@5) */ + w6 = casadi_sq( w5 ); + /* #14: @7 = input[3][3] */ + w7 = arg[3] ? arg[3][3] : 0; + /* #15: @7 = (@3-@7) */ + w7 = (w3-w7); + /* #16: @8 = sq(@7) */ + w8 = casadi_sq( w7 ); + /* #17: @6 = (@6+@8) */ + w6 += w8; + /* #18: @6 = sqrt(@6) */ + w6 = sqrt( w6 ); + /* #19: output[0][1] = @6 */ + if (res[0]) res[0][1] = w6; + /* #20: @8 = input[3][4] */ + w8 = arg[3] ? arg[3][4] : 0; + /* #21: @8 = (@0-@8) */ + w8 = (w0-w8); + /* #22: @9 = sq(@8) */ + w9 = casadi_sq( w8 ); + /* #23: @10 = input[3][5] */ + w10 = arg[3] ? arg[3][5] : 0; + /* #24: @10 = (@3-@10) */ + w10 = (w3-w10); + /* #25: @11 = sq(@10) */ + w11 = casadi_sq( w10 ); + /* #26: @9 = (@9+@11) */ + w9 += w11; + /* #27: @9 = sqrt(@9) */ + w9 = sqrt( w9 ); + /* #28: output[0][2] = @9 */ + if (res[0]) res[0][2] = w9; + /* #29: @11 = input[3][6] */ + w11 = arg[3] ? arg[3][6] : 0; + /* #30: @11 = (@0-@11) */ + w11 = (w0-w11); + /* #31: @12 = sq(@11) */ + w12 = casadi_sq( w11 ); + /* #32: @13 = input[3][7] */ + w13 = arg[3] ? arg[3][7] : 0; + /* #33: @13 = (@3-@13) */ + w13 = (w3-w13); + /* #34: @14 = sq(@13) */ + w14 = casadi_sq( w13 ); + /* #35: @12 = (@12+@14) */ + w12 += w14; + /* #36: @12 = sqrt(@12) */ + w12 = sqrt( w12 ); + /* #37: output[0][3] = @12 */ + if (res[0]) res[0][3] = w12; + /* #38: @14 = input[3][8] */ + w14 = arg[3] ? arg[3][8] : 0; + /* #39: @14 = (@0-@14) */ + w14 = (w0-w14); + /* #40: @15 = sq(@14) */ + w15 = casadi_sq( w14 ); + /* #41: @16 = input[3][9] */ + w16 = arg[3] ? arg[3][9] : 0; + /* #42: @16 = (@3-@16) */ + w16 = (w3-w16); + /* #43: @17 = sq(@16) */ + w17 = casadi_sq( w16 ); + /* #44: @15 = (@15+@17) */ + w15 += w17; + /* #45: @15 = sqrt(@15) */ + w15 = sqrt( w15 ); + /* #46: output[0][4] = @15 */ + if (res[0]) res[0][4] = w15; + /* #47: @17 = input[3][10] */ + w17 = arg[3] ? arg[3][10] : 0; + /* #48: @17 = (@0-@17) */ + w17 = (w0-w17); + /* #49: @18 = sq(@17) */ + w18 = casadi_sq( w17 ); + /* #50: @19 = input[3][11] */ + w19 = arg[3] ? arg[3][11] : 0; + /* #51: @19 = (@3-@19) */ + w19 = (w3-w19); + /* #52: @20 = sq(@19) */ + w20 = casadi_sq( w19 ); + /* #53: @18 = (@18+@20) */ + w18 += w20; + /* #54: @18 = sqrt(@18) */ + w18 = sqrt( w18 ); + /* #55: output[0][5] = @18 */ + if (res[0]) res[0][5] = w18; + /* #56: @20 = input[3][12] */ + w20 = arg[3] ? arg[3][12] : 0; + /* #57: @20 = (@0-@20) */ + w20 = (w0-w20); + /* #58: @21 = sq(@20) */ + w21 = casadi_sq( w20 ); + /* #59: @22 = input[3][13] */ + w22 = arg[3] ? arg[3][13] : 0; + /* #60: @22 = (@3-@22) */ + w22 = (w3-w22); + /* #61: @23 = sq(@22) */ + w23 = casadi_sq( w22 ); + /* #62: @21 = (@21+@23) */ + w21 += w23; + /* #63: @21 = sqrt(@21) */ + w21 = sqrt( w21 ); + /* #64: output[0][6] = @21 */ + if (res[0]) res[0][6] = w21; + /* #65: @23 = input[3][14] */ + w23 = arg[3] ? arg[3][14] : 0; + /* #66: @0 = (@0-@23) */ + w0 -= w23; + /* #67: @23 = sq(@0) */ + w23 = casadi_sq( w0 ); + /* #68: @24 = input[3][15] */ + w24 = arg[3] ? arg[3][15] : 0; + /* #69: @3 = (@3-@24) */ + w3 -= w24; + /* #70: @24 = sq(@3) */ + w24 = casadi_sq( w3 ); + /* #71: @23 = (@23+@24) */ + w23 += w24; + /* #72: @23 = sqrt(@23) */ + w23 = sqrt( w23 ); + /* #73: output[0][7] = @23 */ + if (res[0]) res[0][7] = w23; + /* #74: @25 = zeros(9x8,16nz) */ + casadi_clear(w25, 16); + /* #75: @1 = (2.*@1) */ + w1 = (2.* w1 ); + /* #76: @26 = ones(9x1,8nz) */ + casadi_fill(w26, 8, 1.); + /* #77: {NULL, NULL, NULL, NULL, NULL, NULL, @24, NULL, NULL} = vertsplit(@26) */ + w24 = w26[6]; + /* #78: @1 = (@1*@24) */ + w1 *= w24; + /* #79: @2 = (2.*@2) */ + w2 = (2.* w2 ); + /* #80: @1 = (@1/@2) */ + w1 /= w2; + /* #81: @5 = (2.*@5) */ + w5 = (2.* w5 ); + /* #82: @5 = (@5*@24) */ + w5 *= w24; + /* #83: @6 = (2.*@6) */ + w6 = (2.* w6 ); + /* #84: @5 = (@5/@6) */ + w5 /= w6; + /* #85: @8 = (2.*@8) */ + w8 = (2.* w8 ); + /* #86: @8 = (@8*@24) */ + w8 *= w24; + /* #87: @9 = (2.*@9) */ + w9 = (2.* w9 ); + /* #88: @8 = (@8/@9) */ + w8 /= w9; + /* #89: @11 = (2.*@11) */ + w11 = (2.* w11 ); + /* #90: @11 = (@11*@24) */ + w11 *= w24; + /* #91: @12 = (2.*@12) */ + w12 = (2.* w12 ); + /* #92: @11 = (@11/@12) */ + w11 /= w12; + /* #93: @14 = (2.*@14) */ + w14 = (2.* w14 ); + /* #94: @14 = (@14*@24) */ + w14 *= w24; + /* #95: @15 = (2.*@15) */ + w15 = (2.* w15 ); + /* #96: @14 = (@14/@15) */ + w14 /= w15; + /* #97: @17 = (2.*@17) */ + w17 = (2.* w17 ); + /* #98: @17 = (@17*@24) */ + w17 *= w24; + /* #99: @18 = (2.*@18) */ + w18 = (2.* w18 ); + /* #100: @17 = (@17/@18) */ + w17 /= w18; + /* #101: @20 = (2.*@20) */ + w20 = (2.* w20 ); + /* #102: @20 = (@20*@24) */ + w20 *= w24; + /* #103: @21 = (2.*@21) */ + w21 = (2.* w21 ); + /* #104: @20 = (@20/@21) */ + w20 /= w21; + /* #105: @0 = (2.*@0) */ + w0 = (2.* w0 ); + /* #106: @0 = (@0*@24) */ + w0 *= w24; + /* #107: @23 = (2.*@23) */ + w23 = (2.* w23 ); + /* #108: @0 = (@0/@23) */ + w0 /= w23; + /* #109: @26 = vertcat(@1, @5, @8, @11, @14, @17, @20, @0) */ + rr=w26; + *rr++ = w1; + *rr++ = w5; + *rr++ = w8; + *rr++ = w11; + *rr++ = w14; + *rr++ = w17; + *rr++ = w20; + *rr++ = w0; + /* #110: (@25[:16:2] = @26) */ + for (rr=w25+0, ss=w26; rr!=w25+16; rr+=2) *rr = *ss++; + /* #111: @4 = (2.*@4) */ + w4 = (2.* w4 ); + /* #112: @1 = ones(9x1,1nz) */ + w1 = 1.; + /* #113: {NULL, NULL, NULL, NULL, NULL, NULL, NULL, @5, NULL} = vertsplit(@1) */ + w5 = w1; + /* #114: @4 = (@4*@5) */ + w4 *= w5; + /* #115: @4 = (@4/@2) */ + w4 /= w2; + /* #116: @7 = (2.*@7) */ + w7 = (2.* w7 ); + /* #117: @7 = (@7*@5) */ + w7 *= w5; + /* #118: @7 = (@7/@6) */ + w7 /= w6; + /* #119: @10 = (2.*@10) */ + w10 = (2.* w10 ); + /* #120: @10 = (@10*@5) */ + w10 *= w5; + /* #121: @10 = (@10/@9) */ + w10 /= w9; + /* #122: @13 = (2.*@13) */ + w13 = (2.* w13 ); + /* #123: @13 = (@13*@5) */ + w13 *= w5; + /* #124: @13 = (@13/@12) */ + w13 /= w12; + /* #125: @16 = (2.*@16) */ + w16 = (2.* w16 ); + /* #126: @16 = (@16*@5) */ + w16 *= w5; + /* #127: @16 = (@16/@15) */ + w16 /= w15; + /* #128: @19 = (2.*@19) */ + w19 = (2.* w19 ); + /* #129: @19 = (@19*@5) */ + w19 *= w5; + /* #130: @19 = (@19/@18) */ + w19 /= w18; + /* #131: @22 = (2.*@22) */ + w22 = (2.* w22 ); + /* #132: @22 = (@22*@5) */ + w22 *= w5; + /* #133: @22 = (@22/@21) */ + w22 /= w21; + /* #134: @3 = (2.*@3) */ + w3 = (2.* w3 ); + /* #135: @3 = (@3*@5) */ + w3 *= w5; + /* #136: @3 = (@3/@23) */ + w3 /= w23; + /* #137: @26 = vertcat(@4, @7, @10, @13, @16, @19, @22, @3) */ + rr=w26; + *rr++ = w4; + *rr++ = w7; + *rr++ = w10; + *rr++ = w13; + *rr++ = w16; + *rr++ = w19; + *rr++ = w22; + *rr++ = w3; + /* #138: (@25[1:17:2] = @26) */ + for (rr=w25+1, ss=w26; rr!=w25+17; rr+=2) *rr = *ss++; + /* #139: output[1][0] = @25 */ + casadi_copy(w25, 16, res[1]); + return 0; +} + +CASADI_SYMBOL_EXPORT int usv_model_guidance_ca1_constr_h_fun_jac_uxt_zt(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){ + return casadi_f0(arg, res, iw, w, mem); +} + +CASADI_SYMBOL_EXPORT int usv_model_guidance_ca1_constr_h_fun_jac_uxt_zt_alloc_mem(void) { + return 0; +} + +CASADI_SYMBOL_EXPORT int usv_model_guidance_ca1_constr_h_fun_jac_uxt_zt_init_mem(int mem) { + return 0; +} + +CASADI_SYMBOL_EXPORT void usv_model_guidance_ca1_constr_h_fun_jac_uxt_zt_free_mem(int mem) { +} + +CASADI_SYMBOL_EXPORT int usv_model_guidance_ca1_constr_h_fun_jac_uxt_zt_checkout(void) { + return 0; +} + +CASADI_SYMBOL_EXPORT void usv_model_guidance_ca1_constr_h_fun_jac_uxt_zt_release(int mem) { +} + +CASADI_SYMBOL_EXPORT void usv_model_guidance_ca1_constr_h_fun_jac_uxt_zt_incref(void) { +} + +CASADI_SYMBOL_EXPORT void usv_model_guidance_ca1_constr_h_fun_jac_uxt_zt_decref(void) { +} + +CASADI_SYMBOL_EXPORT casadi_int usv_model_guidance_ca1_constr_h_fun_jac_uxt_zt_n_in(void) { return 4;} + +CASADI_SYMBOL_EXPORT casadi_int usv_model_guidance_ca1_constr_h_fun_jac_uxt_zt_n_out(void) { return 3;} + +CASADI_SYMBOL_EXPORT casadi_real usv_model_guidance_ca1_constr_h_fun_jac_uxt_zt_default_in(casadi_int i){ + switch (i) { + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const char* usv_model_guidance_ca1_constr_h_fun_jac_uxt_zt_name_in(casadi_int i){ + switch (i) { + case 0: return "i0"; + case 1: return "i1"; + case 2: return "i2"; + case 3: return "i3"; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const char* usv_model_guidance_ca1_constr_h_fun_jac_uxt_zt_name_out(casadi_int i){ + switch (i) { + case 0: return "o0"; + case 1: return "o1"; + case 2: return "o2"; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const casadi_int* usv_model_guidance_ca1_constr_h_fun_jac_uxt_zt_sparsity_in(casadi_int i) { + switch (i) { + case 0: return casadi_s0; + case 1: return casadi_s1; + case 2: return casadi_s2; + case 3: return casadi_s3; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const casadi_int* usv_model_guidance_ca1_constr_h_fun_jac_uxt_zt_sparsity_out(casadi_int i) { + switch (i) { + case 0: return casadi_s0; + case 1: return casadi_s4; + case 2: return casadi_s5; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT int usv_model_guidance_ca1_constr_h_fun_jac_uxt_zt_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { + if (sz_arg) *sz_arg = 12; + if (sz_res) *sz_res = 12; + if (sz_iw) *sz_iw = 0; + if (sz_w) *sz_w = 49; + return 0; +} + + +#ifdef __cplusplus +} /* extern "C" */ +#endif diff --git a/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/usv_model_guidance_ca1_constraints/usv_model_guidance_ca1_constr_h_fun_jac_uxt_zt.o b/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/usv_model_guidance_ca1_constraints/usv_model_guidance_ca1_constr_h_fun_jac_uxt_zt.o new file mode 100644 index 00000000..8049bb30 Binary files /dev/null and b/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/usv_model_guidance_ca1_constraints/usv_model_guidance_ca1_constr_h_fun_jac_uxt_zt.o differ diff --git a/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/usv_model_guidance_ca1_constraints/usv_model_guidance_ca1_h_constraint.h b/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/usv_model_guidance_ca1_constraints/usv_model_guidance_ca1_h_constraint.h new file mode 100644 index 00000000..f689c682 --- /dev/null +++ b/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/usv_model_guidance_ca1_constraints/usv_model_guidance_ca1_h_constraint.h @@ -0,0 +1,63 @@ +/* + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + +#ifndef usv_model_guidance_ca1_H_CONSTRAINT +#define usv_model_guidance_ca1_H_CONSTRAINT + +#ifdef __cplusplus +extern "C" { +#endif + + +int usv_model_guidance_ca1_constr_h_fun_jac_uxt_zt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int usv_model_guidance_ca1_constr_h_fun_jac_uxt_zt_work(int *, int *, int *, int *); +const int *usv_model_guidance_ca1_constr_h_fun_jac_uxt_zt_sparsity_in(int); +const int *usv_model_guidance_ca1_constr_h_fun_jac_uxt_zt_sparsity_out(int); +int usv_model_guidance_ca1_constr_h_fun_jac_uxt_zt_n_in(); +int usv_model_guidance_ca1_constr_h_fun_jac_uxt_zt_n_out(); + +int usv_model_guidance_ca1_constr_h_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int usv_model_guidance_ca1_constr_h_fun_work(int *, int *, int *, int *); +const int *usv_model_guidance_ca1_constr_h_fun_sparsity_in(int); +const int *usv_model_guidance_ca1_constr_h_fun_sparsity_out(int); +int usv_model_guidance_ca1_constr_h_fun_n_in(); +int usv_model_guidance_ca1_constr_h_fun_n_out(); + + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // usv_model_guidance_ca1_H_CONSTRAINT diff --git a/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/usv_model_guidance_ca1_model/usv_model_guidance_ca1_expl_ode_fun.c b/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/usv_model_guidance_ca1_model/usv_model_guidance_ca1_expl_ode_fun.c new file mode 100644 index 00000000..a7ce8764 --- /dev/null +++ b/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/usv_model_guidance_ca1_model/usv_model_guidance_ca1_expl_ode_fun.c @@ -0,0 +1,212 @@ +/* This file was automatically generated by CasADi. + The CasADi copyright holders make no ownership claim of its contents. */ +#ifdef __cplusplus +extern "C" { +#endif + +/* How to prefix internal symbols */ +#ifdef CASADI_CODEGEN_PREFIX + #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID) + #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID + #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID) +#else + #define CASADI_PREFIX(ID) usv_model_guidance_ca1_expl_ode_fun_ ## ID +#endif + +#include + +#ifndef casadi_real +#define casadi_real double +#endif + +#ifndef casadi_int +#define casadi_int int +#endif + +/* Add prefix to internal symbols */ +#define casadi_f0 CASADI_PREFIX(f0) +#define casadi_s0 CASADI_PREFIX(s0) +#define casadi_s1 CASADI_PREFIX(s1) +#define casadi_s2 CASADI_PREFIX(s2) + +/* Symbol visibility in DLLs */ +#ifndef CASADI_SYMBOL_EXPORT + #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__) + #if defined(STATIC_LINKED) + #define CASADI_SYMBOL_EXPORT + #else + #define CASADI_SYMBOL_EXPORT __declspec(dllexport) + #endif + #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY) + #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default"))) + #else + #define CASADI_SYMBOL_EXPORT + #endif +#endif + +static const casadi_int casadi_s0[12] = {8, 1, 0, 8, 0, 1, 2, 3, 4, 5, 6, 7}; +static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0}; +static const casadi_int casadi_s2[20] = {16, 1, 0, 16, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15}; + +/* usv_model_guidance_ca1_expl_ode_fun:(i0[8],i1,i2[16])->(o0[8]) */ +static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) { + casadi_real w0, w1, w2, w3, w4, w5, w6; + /* #0: @0 = 0 */ + w0 = 0.; + /* #1: output[0][0] = @0 */ + if (res[0]) res[0][0] = w0; + /* #2: @0 = 0 */ + w0 = 0.; + /* #3: output[0][1] = @0 */ + if (res[0]) res[0][1] = w0; + /* #4: @0 = input[0][0] */ + w0 = arg[0] ? arg[0][0] : 0; + /* #5: @1 = input[0][3] */ + w1 = arg[0] ? arg[0][3] : 0; + /* #6: @2 = input[0][1] */ + w2 = arg[0] ? arg[0][1] : 0; + /* #7: @3 = 0.001 */ + w3 = 1.0000000000000000e-03; + /* #8: @3 = (@3+@0) */ + w3 += w0; + /* #9: @3 = atan2(@2,@3) */ + w3 = atan2(w2,w3); + /* #10: @1 = (@1-@3) */ + w1 -= w3; + /* #11: @3 = sin(@1) */ + w3 = sin( w1 ); + /* #12: @3 = (@0*@3) */ + w3 = (w0*w3); + /* #13: @4 = cos(@1) */ + w4 = cos( w1 ); + /* #14: @4 = (@2*@4) */ + w4 = (w2*w4); + /* #15: @3 = (@3+@4) */ + w3 += w4; + /* #16: output[0][2] = @3 */ + if (res[0]) res[0][2] = w3; + /* #17: @3 = input[0][4] */ + w3 = arg[0] ? arg[0][4] : 0; + /* #18: @4 = (@3-@1) */ + w4 = (w3-w1); + /* #19: output[0][3] = @4 */ + if (res[0]) res[0][3] = w4; + /* #20: @4 = input[1][0] */ + w4 = arg[1] ? arg[1][0] : 0; + /* #21: output[0][4] = @4 */ + if (res[0]) res[0][4] = w4; + /* #22: @4 = input[0][7] */ + w4 = arg[0] ? arg[0][7] : 0; + /* #23: @5 = cos(@4) */ + w5 = cos( w4 ); + /* #24: @5 = (@0*@5) */ + w5 = (w0*w5); + /* #25: @6 = sin(@4) */ + w6 = sin( w4 ); + /* #26: @6 = (@2*@6) */ + w6 = (w2*w6); + /* #27: @5 = (@5-@6) */ + w5 -= w6; + /* #28: output[0][5] = @5 */ + if (res[0]) res[0][5] = w5; + /* #29: @5 = sin(@4) */ + w5 = sin( w4 ); + /* #30: @0 = (@0*@5) */ + w0 *= w5; + /* #31: @4 = cos(@4) */ + w4 = cos( w4 ); + /* #32: @2 = (@2*@4) */ + w2 *= w4; + /* #33: @0 = (@0+@2) */ + w0 += w2; + /* #34: output[0][6] = @0 */ + if (res[0]) res[0][6] = w0; + /* #35: @3 = (@3-@1) */ + w3 -= w1; + /* #36: output[0][7] = @3 */ + if (res[0]) res[0][7] = w3; + return 0; +} + +CASADI_SYMBOL_EXPORT int usv_model_guidance_ca1_expl_ode_fun(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){ + return casadi_f0(arg, res, iw, w, mem); +} + +CASADI_SYMBOL_EXPORT int usv_model_guidance_ca1_expl_ode_fun_alloc_mem(void) { + return 0; +} + +CASADI_SYMBOL_EXPORT int usv_model_guidance_ca1_expl_ode_fun_init_mem(int mem) { + return 0; +} + +CASADI_SYMBOL_EXPORT void usv_model_guidance_ca1_expl_ode_fun_free_mem(int mem) { +} + +CASADI_SYMBOL_EXPORT int usv_model_guidance_ca1_expl_ode_fun_checkout(void) { + return 0; +} + +CASADI_SYMBOL_EXPORT void usv_model_guidance_ca1_expl_ode_fun_release(int mem) { +} + +CASADI_SYMBOL_EXPORT void usv_model_guidance_ca1_expl_ode_fun_incref(void) { +} + +CASADI_SYMBOL_EXPORT void usv_model_guidance_ca1_expl_ode_fun_decref(void) { +} + +CASADI_SYMBOL_EXPORT casadi_int usv_model_guidance_ca1_expl_ode_fun_n_in(void) { return 3;} + +CASADI_SYMBOL_EXPORT casadi_int usv_model_guidance_ca1_expl_ode_fun_n_out(void) { return 1;} + +CASADI_SYMBOL_EXPORT casadi_real usv_model_guidance_ca1_expl_ode_fun_default_in(casadi_int i){ + switch (i) { + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const char* usv_model_guidance_ca1_expl_ode_fun_name_in(casadi_int i){ + switch (i) { + case 0: return "i0"; + case 1: return "i1"; + case 2: return "i2"; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const char* usv_model_guidance_ca1_expl_ode_fun_name_out(casadi_int i){ + switch (i) { + case 0: return "o0"; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const casadi_int* usv_model_guidance_ca1_expl_ode_fun_sparsity_in(casadi_int i) { + switch (i) { + case 0: return casadi_s0; + case 1: return casadi_s1; + case 2: return casadi_s2; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const casadi_int* usv_model_guidance_ca1_expl_ode_fun_sparsity_out(casadi_int i) { + switch (i) { + case 0: return casadi_s0; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT int usv_model_guidance_ca1_expl_ode_fun_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { + if (sz_arg) *sz_arg = 5; + if (sz_res) *sz_res = 2; + if (sz_iw) *sz_iw = 0; + if (sz_w) *sz_w = 7; + return 0; +} + + +#ifdef __cplusplus +} /* extern "C" */ +#endif diff --git a/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/usv_model_guidance_ca1_model/usv_model_guidance_ca1_expl_ode_fun.o b/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/usv_model_guidance_ca1_model/usv_model_guidance_ca1_expl_ode_fun.o new file mode 100644 index 00000000..b6f5bddc Binary files /dev/null and b/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/usv_model_guidance_ca1_model/usv_model_guidance_ca1_expl_ode_fun.o differ diff --git a/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/usv_model_guidance_ca1_model/usv_model_guidance_ca1_expl_vde_adj.c b/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/usv_model_guidance_ca1_model/usv_model_guidance_ca1_expl_vde_adj.c new file mode 100644 index 00000000..65b0c633 --- /dev/null +++ b/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/usv_model_guidance_ca1_model/usv_model_guidance_ca1_expl_vde_adj.c @@ -0,0 +1,296 @@ +/* This file was automatically generated by CasADi. + The CasADi copyright holders make no ownership claim of its contents. */ +#ifdef __cplusplus +extern "C" { +#endif + +/* How to prefix internal symbols */ +#ifdef CASADI_CODEGEN_PREFIX + #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID) + #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID + #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID) +#else + #define CASADI_PREFIX(ID) usv_model_guidance_ca1_expl_vde_adj_ ## ID +#endif + +#include + +#ifndef casadi_real +#define casadi_real double +#endif + +#ifndef casadi_int +#define casadi_int int +#endif + +/* Add prefix to internal symbols */ +#define casadi_copy CASADI_PREFIX(copy) +#define casadi_f0 CASADI_PREFIX(f0) +#define casadi_s0 CASADI_PREFIX(s0) +#define casadi_s1 CASADI_PREFIX(s1) +#define casadi_s2 CASADI_PREFIX(s2) +#define casadi_s3 CASADI_PREFIX(s3) +#define casadi_sq CASADI_PREFIX(sq) + +/* Symbol visibility in DLLs */ +#ifndef CASADI_SYMBOL_EXPORT + #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__) + #if defined(STATIC_LINKED) + #define CASADI_SYMBOL_EXPORT + #else + #define CASADI_SYMBOL_EXPORT __declspec(dllexport) + #endif + #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY) + #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default"))) + #else + #define CASADI_SYMBOL_EXPORT + #endif +#endif + +void casadi_copy(const casadi_real* x, casadi_int n, casadi_real* y) { + casadi_int i; + if (y) { + if (x) { + for (i=0; i(o0[9x1,6nz]) */ +static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) { + casadi_real w0, w1, *w2=w+2, w3, w4, w5, w6, w7, w8, w9, w10, w11, w12, w13, w14, w15, w16, w17; + /* #0: @0 = input[0][7] */ + w0 = arg[0] ? arg[0][7] : 0; + /* #1: @1 = sin(@0) */ + w1 = sin( w0 ); + /* #2: @2 = input[1][0] */ + casadi_copy(arg[1], 8, w2); + /* #3: {NULL, NULL, @3, @4, @5, @6, @7, @8} = vertsplit(@2) */ + w3 = w2[2]; + w4 = w2[3]; + w5 = w2[4]; + w6 = w2[5]; + w7 = w2[6]; + w8 = w2[7]; + /* #4: @1 = (@1*@7) */ + w1 *= w7; + /* #5: @9 = cos(@0) */ + w9 = cos( w0 ); + /* #6: @9 = (@9*@6) */ + w9 *= w6; + /* #7: @1 = (@1+@9) */ + w1 += w9; + /* #8: @9 = input[0][3] */ + w9 = arg[0] ? arg[0][3] : 0; + /* #9: @10 = input[0][1] */ + w10 = arg[0] ? arg[0][1] : 0; + /* #10: @11 = 0.001 */ + w11 = 1.0000000000000000e-03; + /* #11: @12 = input[0][0] */ + w12 = arg[0] ? arg[0][0] : 0; + /* #12: @11 = (@11+@12) */ + w11 += w12; + /* #13: @13 = atan2(@10,@11) */ + w13 = atan2(w10,w11); + /* #14: @9 = (@9-@13) */ + w9 -= w13; + /* #15: @13 = sin(@9) */ + w13 = sin( w9 ); + /* #16: @13 = (@13*@3) */ + w13 *= w3; + /* #17: @1 = (@1+@13) */ + w1 += w13; + /* #18: @13 = sq(@10) */ + w13 = casadi_sq( w10 ); + /* #19: @14 = sq(@11) */ + w14 = casadi_sq( w11 ); + /* #20: @13 = (@13+@14) */ + w13 += w14; + /* #21: @14 = (@10/@13) */ + w14 = (w10/w13); + /* #22: @15 = (-@4) */ + w15 = (- w4 ); + /* #23: @15 = (@15-@8) */ + w15 -= w8; + /* #24: @16 = sin(@9) */ + w16 = sin( w9 ); + /* #25: @17 = (@10*@3) */ + w17 = (w10*w3); + /* #26: @16 = (@16*@17) */ + w16 *= w17; + /* #27: @15 = (@15-@16) */ + w15 -= w16; + /* #28: @16 = cos(@9) */ + w16 = cos( w9 ); + /* #29: @17 = (@12*@3) */ + w17 = (w12*w3); + /* #30: @16 = (@16*@17) */ + w16 *= w17; + /* #31: @15 = (@15+@16) */ + w15 += w16; + /* #32: @14 = (@14*@15) */ + w14 *= w15; + /* #33: @1 = (@1+@14) */ + w1 += w14; + /* #34: output[0][0] = @1 */ + if (res[0]) res[0][0] = w1; + /* #35: @1 = cos(@0) */ + w1 = cos( w0 ); + /* #36: @1 = (@1*@7) */ + w1 *= w7; + /* #37: @14 = sin(@0) */ + w14 = sin( w0 ); + /* #38: @14 = (@14*@6) */ + w14 *= w6; + /* #39: @1 = (@1-@14) */ + w1 -= w14; + /* #40: @9 = cos(@9) */ + w9 = cos( w9 ); + /* #41: @9 = (@9*@3) */ + w9 *= w3; + /* #42: @1 = (@1+@9) */ + w1 += w9; + /* #43: @11 = (@11/@13) */ + w11 /= w13; + /* #44: @11 = (@11*@15) */ + w11 *= w15; + /* #45: @1 = (@1-@11) */ + w1 -= w11; + /* #46: output[0][1] = @1 */ + if (res[0]) res[0][1] = w1; + /* #47: output[0][2] = @15 */ + if (res[0]) res[0][2] = w15; + /* #48: @8 = (@8+@4) */ + w8 += w4; + /* #49: output[0][3] = @8 */ + if (res[0]) res[0][3] = w8; + /* #50: @8 = cos(@0) */ + w8 = cos( w0 ); + /* #51: @4 = (@12*@7) */ + w4 = (w12*w7); + /* #52: @8 = (@8*@4) */ + w8 *= w4; + /* #53: @4 = sin(@0) */ + w4 = sin( w0 ); + /* #54: @7 = (@10*@7) */ + w7 = (w10*w7); + /* #55: @4 = (@4*@7) */ + w4 *= w7; + /* #56: @8 = (@8-@4) */ + w8 -= w4; + /* #57: @4 = cos(@0) */ + w4 = cos( w0 ); + /* #58: @10 = (@10*@6) */ + w10 *= w6; + /* #59: @4 = (@4*@10) */ + w4 *= w10; + /* #60: @8 = (@8-@4) */ + w8 -= w4; + /* #61: @0 = sin(@0) */ + w0 = sin( w0 ); + /* #62: @12 = (@12*@6) */ + w12 *= w6; + /* #63: @0 = (@0*@12) */ + w0 *= w12; + /* #64: @8 = (@8-@0) */ + w8 -= w0; + /* #65: output[0][4] = @8 */ + if (res[0]) res[0][4] = w8; + /* #66: output[0][5] = @5 */ + if (res[0]) res[0][5] = w5; + return 0; +} + +CASADI_SYMBOL_EXPORT int usv_model_guidance_ca1_expl_vde_adj(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){ + return casadi_f0(arg, res, iw, w, mem); +} + +CASADI_SYMBOL_EXPORT int usv_model_guidance_ca1_expl_vde_adj_alloc_mem(void) { + return 0; +} + +CASADI_SYMBOL_EXPORT int usv_model_guidance_ca1_expl_vde_adj_init_mem(int mem) { + return 0; +} + +CASADI_SYMBOL_EXPORT void usv_model_guidance_ca1_expl_vde_adj_free_mem(int mem) { +} + +CASADI_SYMBOL_EXPORT int usv_model_guidance_ca1_expl_vde_adj_checkout(void) { + return 0; +} + +CASADI_SYMBOL_EXPORT void usv_model_guidance_ca1_expl_vde_adj_release(int mem) { +} + +CASADI_SYMBOL_EXPORT void usv_model_guidance_ca1_expl_vde_adj_incref(void) { +} + +CASADI_SYMBOL_EXPORT void usv_model_guidance_ca1_expl_vde_adj_decref(void) { +} + +CASADI_SYMBOL_EXPORT casadi_int usv_model_guidance_ca1_expl_vde_adj_n_in(void) { return 4;} + +CASADI_SYMBOL_EXPORT casadi_int usv_model_guidance_ca1_expl_vde_adj_n_out(void) { return 1;} + +CASADI_SYMBOL_EXPORT casadi_real usv_model_guidance_ca1_expl_vde_adj_default_in(casadi_int i){ + switch (i) { + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const char* usv_model_guidance_ca1_expl_vde_adj_name_in(casadi_int i){ + switch (i) { + case 0: return "i0"; + case 1: return "i1"; + case 2: return "i2"; + case 3: return "i3"; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const char* usv_model_guidance_ca1_expl_vde_adj_name_out(casadi_int i){ + switch (i) { + case 0: return "o0"; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const casadi_int* usv_model_guidance_ca1_expl_vde_adj_sparsity_in(casadi_int i) { + switch (i) { + case 0: return casadi_s0; + case 1: return casadi_s0; + case 2: return casadi_s1; + case 3: return casadi_s2; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const casadi_int* usv_model_guidance_ca1_expl_vde_adj_sparsity_out(casadi_int i) { + switch (i) { + case 0: return casadi_s3; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT int usv_model_guidance_ca1_expl_vde_adj_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { + if (sz_arg) *sz_arg = 6; + if (sz_res) *sz_res = 9; + if (sz_iw) *sz_iw = 0; + if (sz_w) *sz_w = 25; + return 0; +} + + +#ifdef __cplusplus +} /* extern "C" */ +#endif diff --git a/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/usv_model_guidance_ca1_model/usv_model_guidance_ca1_expl_vde_forw.c b/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/usv_model_guidance_ca1_model/usv_model_guidance_ca1_expl_vde_forw.c new file mode 100644 index 00000000..19559038 --- /dev/null +++ b/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/usv_model_guidance_ca1_model/usv_model_guidance_ca1_expl_vde_forw.c @@ -0,0 +1,1073 @@ +/* This file was automatically generated by CasADi. + The CasADi copyright holders make no ownership claim of its contents. */ +#ifdef __cplusplus +extern "C" { +#endif + +/* How to prefix internal symbols */ +#ifdef CASADI_CODEGEN_PREFIX + #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID) + #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID + #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID) +#else + #define CASADI_PREFIX(ID) usv_model_guidance_ca1_expl_vde_forw_ ## ID +#endif + +#include + +#ifndef casadi_real +#define casadi_real double +#endif + +#ifndef casadi_int +#define casadi_int int +#endif + +/* Add prefix to internal symbols */ +#define casadi_copy CASADI_PREFIX(copy) +#define casadi_f0 CASADI_PREFIX(f0) +#define casadi_project CASADI_PREFIX(project) +#define casadi_s0 CASADI_PREFIX(s0) +#define casadi_s1 CASADI_PREFIX(s1) +#define casadi_s2 CASADI_PREFIX(s2) +#define casadi_s3 CASADI_PREFIX(s3) +#define casadi_s4 CASADI_PREFIX(s4) +#define casadi_s5 CASADI_PREFIX(s5) +#define casadi_s6 CASADI_PREFIX(s6) +#define casadi_s7 CASADI_PREFIX(s7) +#define casadi_sq CASADI_PREFIX(sq) + +/* Symbol visibility in DLLs */ +#ifndef CASADI_SYMBOL_EXPORT + #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__) + #if defined(STATIC_LINKED) + #define CASADI_SYMBOL_EXPORT + #else + #define CASADI_SYMBOL_EXPORT __declspec(dllexport) + #endif + #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY) + #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default"))) + #else + #define CASADI_SYMBOL_EXPORT + #endif +#endif + +void casadi_copy(const casadi_real* x, casadi_int n, casadi_real* y) { + casadi_int i; + if (y) { + if (x) { + for (i=0; i(o0[8],o1[8x8,40nz],o2[8x1,6nz]) */ +static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) { + casadi_int i; + casadi_real *rr, *ss; + const casadi_real *cs; + casadi_real w0, w1, w2, w3, w4, w5, w6, w7, w8, w9, w10, w11, w12, w13, *w14=w+22, *w15=w+86, *w16=w+94, *w17=w+102, *w18=w+110, *w19=w+118, *w20=w+126, *w21=w+134, *w22=w+142, w23, w24, w25, w26, w27, w28, w29, w30, w31, w32, w33, w34, w35, *w36=w+163, *w40=w+169, *w41=w+174; + /* #0: @0 = 0 */ + w0 = 0.; + /* #1: output[0][0] = @0 */ + if (res[0]) res[0][0] = w0; + /* #2: @0 = 0 */ + w0 = 0.; + /* #3: output[0][1] = @0 */ + if (res[0]) res[0][1] = w0; + /* #4: @0 = input[0][0] */ + w0 = arg[0] ? arg[0][0] : 0; + /* #5: @1 = input[0][3] */ + w1 = arg[0] ? arg[0][3] : 0; + /* #6: @2 = input[0][1] */ + w2 = arg[0] ? arg[0][1] : 0; + /* #7: @3 = 0.001 */ + w3 = 1.0000000000000000e-03; + /* #8: @3 = (@3+@0) */ + w3 += w0; + /* #9: @4 = atan2(@2,@3) */ + w4 = atan2(w2,w3); + /* #10: @1 = (@1-@4) */ + w1 -= w4; + /* #11: @4 = sin(@1) */ + w4 = sin( w1 ); + /* #12: @5 = (@0*@4) */ + w5 = (w0*w4); + /* #13: @6 = cos(@1) */ + w6 = cos( w1 ); + /* #14: @7 = (@2*@6) */ + w7 = (w2*w6); + /* #15: @5 = (@5+@7) */ + w5 += w7; + /* #16: output[0][2] = @5 */ + if (res[0]) res[0][2] = w5; + /* #17: @5 = input[0][4] */ + w5 = arg[0] ? arg[0][4] : 0; + /* #18: @7 = (@5-@1) */ + w7 = (w5-w1); + /* #19: output[0][3] = @7 */ + if (res[0]) res[0][3] = w7; + /* #20: @7 = input[3][0] */ + w7 = arg[3] ? arg[3][0] : 0; + /* #21: output[0][4] = @7 */ + if (res[0]) res[0][4] = w7; + /* #22: @7 = input[0][7] */ + w7 = arg[0] ? arg[0][7] : 0; + /* #23: @8 = cos(@7) */ + w8 = cos( w7 ); + /* #24: @9 = (@0*@8) */ + w9 = (w0*w8); + /* #25: @10 = sin(@7) */ + w10 = sin( w7 ); + /* #26: @11 = (@2*@10) */ + w11 = (w2*w10); + /* #27: @9 = (@9-@11) */ + w9 -= w11; + /* #28: output[0][5] = @9 */ + if (res[0]) res[0][5] = w9; + /* #29: @9 = sin(@7) */ + w9 = sin( w7 ); + /* #30: @11 = (@0*@9) */ + w11 = (w0*w9); + /* #31: @12 = cos(@7) */ + w12 = cos( w7 ); + /* #32: @13 = (@2*@12) */ + w13 = (w2*w12); + /* #33: @11 = (@11+@13) */ + w11 += w13; + /* #34: output[0][6] = @11 */ + if (res[0]) res[0][6] = w11; + /* #35: @5 = (@5-@1) */ + w5 -= w1; + /* #36: output[0][7] = @5 */ + if (res[0]) res[0][7] = w5; + /* #37: @14 = input[1][0] */ + casadi_copy(arg[1], 64, w14); + /* #38: {@15, @16, @17, @18, @19, @20, @21, @22} = horzsplit(@14) */ + casadi_copy(w14, 8, w15); + casadi_copy(w14+8, 8, w16); + casadi_copy(w14+16, 8, w17); + casadi_copy(w14+24, 8, w18); + casadi_copy(w14+32, 8, w19); + casadi_copy(w14+40, 8, w20); + casadi_copy(w14+48, 8, w21); + casadi_copy(w14+56, 8, w22); + /* #39: {@5, @11, NULL, @13, @23, NULL, NULL, @24} = vertsplit(@15) */ + w5 = w15[0]; + w11 = w15[1]; + w13 = w15[3]; + w23 = w15[4]; + w24 = w15[7]; + /* #40: @25 = (@4*@5) */ + w25 = (w4*w5); + /* #41: @26 = cos(@1) */ + w26 = cos( w1 ); + /* #42: @27 = sq(@2) */ + w27 = casadi_sq( w2 ); + /* #43: @28 = sq(@3) */ + w28 = casadi_sq( w3 ); + /* #44: @27 = (@27+@28) */ + w27 += w28; + /* #45: @28 = (@3/@27) */ + w28 = (w3/w27); + /* #46: @29 = (@28*@11) */ + w29 = (w28*w11); + /* #47: @27 = (@2/@27) */ + w27 = (w2/w27); + /* #48: @30 = (@27*@5) */ + w30 = (w27*w5); + /* #49: @29 = (@29-@30) */ + w29 -= w30; + /* #50: @13 = (@13-@29) */ + w13 -= w29; + /* #51: @29 = (@26*@13) */ + w29 = (w26*w13); + /* #52: @29 = (@0*@29) */ + w29 = (w0*w29); + /* #53: @25 = (@25+@29) */ + w25 += w29; + /* #54: @29 = (@6*@11) */ + w29 = (w6*w11); + /* #55: @30 = sin(@1) */ + w30 = sin( w1 ); + /* #56: @31 = (@30*@13) */ + w31 = (w30*w13); + /* #57: @31 = (@2*@31) */ + w31 = (w2*w31); + /* #58: @29 = (@29-@31) */ + w29 -= w31; + /* #59: @25 = (@25+@29) */ + w25 += w29; + /* #60: output[1][0] = @25 */ + if (res[1]) res[1][0] = w25; + /* #61: @25 = (@23-@13) */ + w25 = (w23-w13); + /* #62: output[1][1] = @25 */ + if (res[1]) res[1][1] = w25; + /* #63: @25 = (@8*@5) */ + w25 = (w8*w5); + /* #64: @29 = sin(@7) */ + w29 = sin( w7 ); + /* #65: @31 = (@29*@24) */ + w31 = (w29*w24); + /* #66: @31 = (@0*@31) */ + w31 = (w0*w31); + /* #67: @25 = (@25-@31) */ + w25 -= w31; + /* #68: @31 = (@10*@11) */ + w31 = (w10*w11); + /* #69: @32 = cos(@7) */ + w32 = cos( w7 ); + /* #70: @33 = (@32*@24) */ + w33 = (w32*w24); + /* #71: @33 = (@2*@33) */ + w33 = (w2*w33); + /* #72: @31 = (@31+@33) */ + w31 += w33; + /* #73: @25 = (@25-@31) */ + w25 -= w31; + /* #74: output[1][2] = @25 */ + if (res[1]) res[1][2] = w25; + /* #75: @5 = (@9*@5) */ + w5 = (w9*w5); + /* #76: @25 = cos(@7) */ + w25 = cos( w7 ); + /* #77: @31 = (@25*@24) */ + w31 = (w25*w24); + /* #78: @31 = (@0*@31) */ + w31 = (w0*w31); + /* #79: @5 = (@5+@31) */ + w5 += w31; + /* #80: @11 = (@12*@11) */ + w11 = (w12*w11); + /* #81: @31 = sin(@7) */ + w31 = sin( w7 ); + /* #82: @24 = (@31*@24) */ + w24 = (w31*w24); + /* #83: @24 = (@2*@24) */ + w24 = (w2*w24); + /* #84: @11 = (@11-@24) */ + w11 -= w24; + /* #85: @5 = (@5+@11) */ + w5 += w11; + /* #86: output[1][3] = @5 */ + if (res[1]) res[1][3] = w5; + /* #87: @23 = (@23-@13) */ + w23 -= w13; + /* #88: output[1][4] = @23 */ + if (res[1]) res[1][4] = w23; + /* #89: {@23, @13, NULL, @5, @11, NULL, NULL, @24} = vertsplit(@16) */ + w23 = w16[0]; + w13 = w16[1]; + w5 = w16[3]; + w11 = w16[4]; + w24 = w16[7]; + /* #90: @33 = (@4*@23) */ + w33 = (w4*w23); + /* #91: @34 = (@28*@13) */ + w34 = (w28*w13); + /* #92: @35 = (@27*@23) */ + w35 = (w27*w23); + /* #93: @34 = (@34-@35) */ + w34 -= w35; + /* #94: @5 = (@5-@34) */ + w5 -= w34; + /* #95: @34 = (@26*@5) */ + w34 = (w26*w5); + /* #96: @34 = (@0*@34) */ + w34 = (w0*w34); + /* #97: @33 = (@33+@34) */ + w33 += w34; + /* #98: @34 = (@6*@13) */ + w34 = (w6*w13); + /* #99: @35 = (@30*@5) */ + w35 = (w30*w5); + /* #100: @35 = (@2*@35) */ + w35 = (w2*w35); + /* #101: @34 = (@34-@35) */ + w34 -= w35; + /* #102: @33 = (@33+@34) */ + w33 += w34; + /* #103: output[1][5] = @33 */ + if (res[1]) res[1][5] = w33; + /* #104: @33 = (@11-@5) */ + w33 = (w11-w5); + /* #105: output[1][6] = @33 */ + if (res[1]) res[1][6] = w33; + /* #106: @33 = (@8*@23) */ + w33 = (w8*w23); + /* #107: @34 = (@29*@24) */ + w34 = (w29*w24); + /* #108: @34 = (@0*@34) */ + w34 = (w0*w34); + /* #109: @33 = (@33-@34) */ + w33 -= w34; + /* #110: @34 = (@10*@13) */ + w34 = (w10*w13); + /* #111: @35 = (@32*@24) */ + w35 = (w32*w24); + /* #112: @35 = (@2*@35) */ + w35 = (w2*w35); + /* #113: @34 = (@34+@35) */ + w34 += w35; + /* #114: @33 = (@33-@34) */ + w33 -= w34; + /* #115: output[1][7] = @33 */ + if (res[1]) res[1][7] = w33; + /* #116: @23 = (@9*@23) */ + w23 = (w9*w23); + /* #117: @33 = (@25*@24) */ + w33 = (w25*w24); + /* #118: @33 = (@0*@33) */ + w33 = (w0*w33); + /* #119: @23 = (@23+@33) */ + w23 += w33; + /* #120: @13 = (@12*@13) */ + w13 = (w12*w13); + /* #121: @24 = (@31*@24) */ + w24 = (w31*w24); + /* #122: @24 = (@2*@24) */ + w24 = (w2*w24); + /* #123: @13 = (@13-@24) */ + w13 -= w24; + /* #124: @23 = (@23+@13) */ + w23 += w13; + /* #125: output[1][8] = @23 */ + if (res[1]) res[1][8] = w23; + /* #126: @11 = (@11-@5) */ + w11 -= w5; + /* #127: output[1][9] = @11 */ + if (res[1]) res[1][9] = w11; + /* #128: {@11, @5, NULL, @23, @13, NULL, NULL, @24} = vertsplit(@17) */ + w11 = w17[0]; + w5 = w17[1]; + w23 = w17[3]; + w13 = w17[4]; + w24 = w17[7]; + /* #129: @33 = (@4*@11) */ + w33 = (w4*w11); + /* #130: @34 = (@28*@5) */ + w34 = (w28*w5); + /* #131: @35 = (@27*@11) */ + w35 = (w27*w11); + /* #132: @34 = (@34-@35) */ + w34 -= w35; + /* #133: @23 = (@23-@34) */ + w23 -= w34; + /* #134: @34 = (@26*@23) */ + w34 = (w26*w23); + /* #135: @34 = (@0*@34) */ + w34 = (w0*w34); + /* #136: @33 = (@33+@34) */ + w33 += w34; + /* #137: @34 = (@6*@5) */ + w34 = (w6*w5); + /* #138: @35 = (@30*@23) */ + w35 = (w30*w23); + /* #139: @35 = (@2*@35) */ + w35 = (w2*w35); + /* #140: @34 = (@34-@35) */ + w34 -= w35; + /* #141: @33 = (@33+@34) */ + w33 += w34; + /* #142: output[1][10] = @33 */ + if (res[1]) res[1][10] = w33; + /* #143: @33 = (@13-@23) */ + w33 = (w13-w23); + /* #144: output[1][11] = @33 */ + if (res[1]) res[1][11] = w33; + /* #145: @33 = (@8*@11) */ + w33 = (w8*w11); + /* #146: @34 = (@29*@24) */ + w34 = (w29*w24); + /* #147: @34 = (@0*@34) */ + w34 = (w0*w34); + /* #148: @33 = (@33-@34) */ + w33 -= w34; + /* #149: @34 = (@10*@5) */ + w34 = (w10*w5); + /* #150: @35 = (@32*@24) */ + w35 = (w32*w24); + /* #151: @35 = (@2*@35) */ + w35 = (w2*w35); + /* #152: @34 = (@34+@35) */ + w34 += w35; + /* #153: @33 = (@33-@34) */ + w33 -= w34; + /* #154: output[1][12] = @33 */ + if (res[1]) res[1][12] = w33; + /* #155: @11 = (@9*@11) */ + w11 = (w9*w11); + /* #156: @33 = (@25*@24) */ + w33 = (w25*w24); + /* #157: @33 = (@0*@33) */ + w33 = (w0*w33); + /* #158: @11 = (@11+@33) */ + w11 += w33; + /* #159: @5 = (@12*@5) */ + w5 = (w12*w5); + /* #160: @24 = (@31*@24) */ + w24 = (w31*w24); + /* #161: @24 = (@2*@24) */ + w24 = (w2*w24); + /* #162: @5 = (@5-@24) */ + w5 -= w24; + /* #163: @11 = (@11+@5) */ + w11 += w5; + /* #164: output[1][13] = @11 */ + if (res[1]) res[1][13] = w11; + /* #165: @13 = (@13-@23) */ + w13 -= w23; + /* #166: output[1][14] = @13 */ + if (res[1]) res[1][14] = w13; + /* #167: {@13, @23, NULL, @11, @5, NULL, NULL, @24} = vertsplit(@18) */ + w13 = w18[0]; + w23 = w18[1]; + w11 = w18[3]; + w5 = w18[4]; + w24 = w18[7]; + /* #168: @33 = (@4*@13) */ + w33 = (w4*w13); + /* #169: @34 = (@28*@23) */ + w34 = (w28*w23); + /* #170: @35 = (@27*@13) */ + w35 = (w27*w13); + /* #171: @34 = (@34-@35) */ + w34 -= w35; + /* #172: @11 = (@11-@34) */ + w11 -= w34; + /* #173: @34 = (@26*@11) */ + w34 = (w26*w11); + /* #174: @34 = (@0*@34) */ + w34 = (w0*w34); + /* #175: @33 = (@33+@34) */ + w33 += w34; + /* #176: @34 = (@6*@23) */ + w34 = (w6*w23); + /* #177: @35 = (@30*@11) */ + w35 = (w30*w11); + /* #178: @35 = (@2*@35) */ + w35 = (w2*w35); + /* #179: @34 = (@34-@35) */ + w34 -= w35; + /* #180: @33 = (@33+@34) */ + w33 += w34; + /* #181: output[1][15] = @33 */ + if (res[1]) res[1][15] = w33; + /* #182: @33 = (@5-@11) */ + w33 = (w5-w11); + /* #183: output[1][16] = @33 */ + if (res[1]) res[1][16] = w33; + /* #184: @33 = (@8*@13) */ + w33 = (w8*w13); + /* #185: @34 = (@29*@24) */ + w34 = (w29*w24); + /* #186: @34 = (@0*@34) */ + w34 = (w0*w34); + /* #187: @33 = (@33-@34) */ + w33 -= w34; + /* #188: @34 = (@10*@23) */ + w34 = (w10*w23); + /* #189: @35 = (@32*@24) */ + w35 = (w32*w24); + /* #190: @35 = (@2*@35) */ + w35 = (w2*w35); + /* #191: @34 = (@34+@35) */ + w34 += w35; + /* #192: @33 = (@33-@34) */ + w33 -= w34; + /* #193: output[1][17] = @33 */ + if (res[1]) res[1][17] = w33; + /* #194: @13 = (@9*@13) */ + w13 = (w9*w13); + /* #195: @33 = (@25*@24) */ + w33 = (w25*w24); + /* #196: @33 = (@0*@33) */ + w33 = (w0*w33); + /* #197: @13 = (@13+@33) */ + w13 += w33; + /* #198: @23 = (@12*@23) */ + w23 = (w12*w23); + /* #199: @24 = (@31*@24) */ + w24 = (w31*w24); + /* #200: @24 = (@2*@24) */ + w24 = (w2*w24); + /* #201: @23 = (@23-@24) */ + w23 -= w24; + /* #202: @13 = (@13+@23) */ + w13 += w23; + /* #203: output[1][18] = @13 */ + if (res[1]) res[1][18] = w13; + /* #204: @5 = (@5-@11) */ + w5 -= w11; + /* #205: output[1][19] = @5 */ + if (res[1]) res[1][19] = w5; + /* #206: {@5, @11, NULL, @13, @23, NULL, NULL, @24} = vertsplit(@19) */ + w5 = w19[0]; + w11 = w19[1]; + w13 = w19[3]; + w23 = w19[4]; + w24 = w19[7]; + /* #207: @33 = (@4*@5) */ + w33 = (w4*w5); + /* #208: @34 = (@28*@11) */ + w34 = (w28*w11); + /* #209: @35 = (@27*@5) */ + w35 = (w27*w5); + /* #210: @34 = (@34-@35) */ + w34 -= w35; + /* #211: @13 = (@13-@34) */ + w13 -= w34; + /* #212: @34 = (@26*@13) */ + w34 = (w26*w13); + /* #213: @34 = (@0*@34) */ + w34 = (w0*w34); + /* #214: @33 = (@33+@34) */ + w33 += w34; + /* #215: @34 = (@6*@11) */ + w34 = (w6*w11); + /* #216: @35 = (@30*@13) */ + w35 = (w30*w13); + /* #217: @35 = (@2*@35) */ + w35 = (w2*w35); + /* #218: @34 = (@34-@35) */ + w34 -= w35; + /* #219: @33 = (@33+@34) */ + w33 += w34; + /* #220: output[1][20] = @33 */ + if (res[1]) res[1][20] = w33; + /* #221: @33 = (@23-@13) */ + w33 = (w23-w13); + /* #222: output[1][21] = @33 */ + if (res[1]) res[1][21] = w33; + /* #223: @33 = (@8*@5) */ + w33 = (w8*w5); + /* #224: @34 = (@29*@24) */ + w34 = (w29*w24); + /* #225: @34 = (@0*@34) */ + w34 = (w0*w34); + /* #226: @33 = (@33-@34) */ + w33 -= w34; + /* #227: @34 = (@10*@11) */ + w34 = (w10*w11); + /* #228: @35 = (@32*@24) */ + w35 = (w32*w24); + /* #229: @35 = (@2*@35) */ + w35 = (w2*w35); + /* #230: @34 = (@34+@35) */ + w34 += w35; + /* #231: @33 = (@33-@34) */ + w33 -= w34; + /* #232: output[1][22] = @33 */ + if (res[1]) res[1][22] = w33; + /* #233: @5 = (@9*@5) */ + w5 = (w9*w5); + /* #234: @33 = (@25*@24) */ + w33 = (w25*w24); + /* #235: @33 = (@0*@33) */ + w33 = (w0*w33); + /* #236: @5 = (@5+@33) */ + w5 += w33; + /* #237: @11 = (@12*@11) */ + w11 = (w12*w11); + /* #238: @24 = (@31*@24) */ + w24 = (w31*w24); + /* #239: @24 = (@2*@24) */ + w24 = (w2*w24); + /* #240: @11 = (@11-@24) */ + w11 -= w24; + /* #241: @5 = (@5+@11) */ + w5 += w11; + /* #242: output[1][23] = @5 */ + if (res[1]) res[1][23] = w5; + /* #243: @23 = (@23-@13) */ + w23 -= w13; + /* #244: output[1][24] = @23 */ + if (res[1]) res[1][24] = w23; + /* #245: {@23, @13, NULL, @5, @11, NULL, NULL, @24} = vertsplit(@20) */ + w23 = w20[0]; + w13 = w20[1]; + w5 = w20[3]; + w11 = w20[4]; + w24 = w20[7]; + /* #246: @33 = (@4*@23) */ + w33 = (w4*w23); + /* #247: @34 = (@28*@13) */ + w34 = (w28*w13); + /* #248: @35 = (@27*@23) */ + w35 = (w27*w23); + /* #249: @34 = (@34-@35) */ + w34 -= w35; + /* #250: @5 = (@5-@34) */ + w5 -= w34; + /* #251: @34 = (@26*@5) */ + w34 = (w26*w5); + /* #252: @34 = (@0*@34) */ + w34 = (w0*w34); + /* #253: @33 = (@33+@34) */ + w33 += w34; + /* #254: @34 = (@6*@13) */ + w34 = (w6*w13); + /* #255: @35 = (@30*@5) */ + w35 = (w30*w5); + /* #256: @35 = (@2*@35) */ + w35 = (w2*w35); + /* #257: @34 = (@34-@35) */ + w34 -= w35; + /* #258: @33 = (@33+@34) */ + w33 += w34; + /* #259: output[1][25] = @33 */ + if (res[1]) res[1][25] = w33; + /* #260: @33 = (@11-@5) */ + w33 = (w11-w5); + /* #261: output[1][26] = @33 */ + if (res[1]) res[1][26] = w33; + /* #262: @33 = (@8*@23) */ + w33 = (w8*w23); + /* #263: @34 = (@29*@24) */ + w34 = (w29*w24); + /* #264: @34 = (@0*@34) */ + w34 = (w0*w34); + /* #265: @33 = (@33-@34) */ + w33 -= w34; + /* #266: @34 = (@10*@13) */ + w34 = (w10*w13); + /* #267: @35 = (@32*@24) */ + w35 = (w32*w24); + /* #268: @35 = (@2*@35) */ + w35 = (w2*w35); + /* #269: @34 = (@34+@35) */ + w34 += w35; + /* #270: @33 = (@33-@34) */ + w33 -= w34; + /* #271: output[1][27] = @33 */ + if (res[1]) res[1][27] = w33; + /* #272: @23 = (@9*@23) */ + w23 = (w9*w23); + /* #273: @33 = (@25*@24) */ + w33 = (w25*w24); + /* #274: @33 = (@0*@33) */ + w33 = (w0*w33); + /* #275: @23 = (@23+@33) */ + w23 += w33; + /* #276: @13 = (@12*@13) */ + w13 = (w12*w13); + /* #277: @24 = (@31*@24) */ + w24 = (w31*w24); + /* #278: @24 = (@2*@24) */ + w24 = (w2*w24); + /* #279: @13 = (@13-@24) */ + w13 -= w24; + /* #280: @23 = (@23+@13) */ + w23 += w13; + /* #281: output[1][28] = @23 */ + if (res[1]) res[1][28] = w23; + /* #282: @11 = (@11-@5) */ + w11 -= w5; + /* #283: output[1][29] = @11 */ + if (res[1]) res[1][29] = w11; + /* #284: {@11, @5, NULL, @23, @13, NULL, NULL, @24} = vertsplit(@21) */ + w11 = w21[0]; + w5 = w21[1]; + w23 = w21[3]; + w13 = w21[4]; + w24 = w21[7]; + /* #285: @33 = (@4*@11) */ + w33 = (w4*w11); + /* #286: @34 = (@28*@5) */ + w34 = (w28*w5); + /* #287: @35 = (@27*@11) */ + w35 = (w27*w11); + /* #288: @34 = (@34-@35) */ + w34 -= w35; + /* #289: @23 = (@23-@34) */ + w23 -= w34; + /* #290: @34 = (@26*@23) */ + w34 = (w26*w23); + /* #291: @34 = (@0*@34) */ + w34 = (w0*w34); + /* #292: @33 = (@33+@34) */ + w33 += w34; + /* #293: @34 = (@6*@5) */ + w34 = (w6*w5); + /* #294: @35 = (@30*@23) */ + w35 = (w30*w23); + /* #295: @35 = (@2*@35) */ + w35 = (w2*w35); + /* #296: @34 = (@34-@35) */ + w34 -= w35; + /* #297: @33 = (@33+@34) */ + w33 += w34; + /* #298: output[1][30] = @33 */ + if (res[1]) res[1][30] = w33; + /* #299: @33 = (@13-@23) */ + w33 = (w13-w23); + /* #300: output[1][31] = @33 */ + if (res[1]) res[1][31] = w33; + /* #301: @33 = (@8*@11) */ + w33 = (w8*w11); + /* #302: @34 = (@29*@24) */ + w34 = (w29*w24); + /* #303: @34 = (@0*@34) */ + w34 = (w0*w34); + /* #304: @33 = (@33-@34) */ + w33 -= w34; + /* #305: @34 = (@10*@5) */ + w34 = (w10*w5); + /* #306: @35 = (@32*@24) */ + w35 = (w32*w24); + /* #307: @35 = (@2*@35) */ + w35 = (w2*w35); + /* #308: @34 = (@34+@35) */ + w34 += w35; + /* #309: @33 = (@33-@34) */ + w33 -= w34; + /* #310: output[1][32] = @33 */ + if (res[1]) res[1][32] = w33; + /* #311: @11 = (@9*@11) */ + w11 = (w9*w11); + /* #312: @33 = (@25*@24) */ + w33 = (w25*w24); + /* #313: @33 = (@0*@33) */ + w33 = (w0*w33); + /* #314: @11 = (@11+@33) */ + w11 += w33; + /* #315: @5 = (@12*@5) */ + w5 = (w12*w5); + /* #316: @24 = (@31*@24) */ + w24 = (w31*w24); + /* #317: @24 = (@2*@24) */ + w24 = (w2*w24); + /* #318: @5 = (@5-@24) */ + w5 -= w24; + /* #319: @11 = (@11+@5) */ + w11 += w5; + /* #320: output[1][33] = @11 */ + if (res[1]) res[1][33] = w11; + /* #321: @13 = (@13-@23) */ + w13 -= w23; + /* #322: output[1][34] = @13 */ + if (res[1]) res[1][34] = w13; + /* #323: {@13, @23, NULL, @11, @5, NULL, NULL, @24} = vertsplit(@22) */ + w13 = w22[0]; + w23 = w22[1]; + w11 = w22[3]; + w5 = w22[4]; + w24 = w22[7]; + /* #324: @33 = (@4*@13) */ + w33 = (w4*w13); + /* #325: @28 = (@28*@23) */ + w28 *= w23; + /* #326: @27 = (@27*@13) */ + w27 *= w13; + /* #327: @28 = (@28-@27) */ + w28 -= w27; + /* #328: @11 = (@11-@28) */ + w11 -= w28; + /* #329: @26 = (@26*@11) */ + w26 *= w11; + /* #330: @26 = (@0*@26) */ + w26 = (w0*w26); + /* #331: @33 = (@33+@26) */ + w33 += w26; + /* #332: @26 = (@6*@23) */ + w26 = (w6*w23); + /* #333: @30 = (@30*@11) */ + w30 *= w11; + /* #334: @30 = (@2*@30) */ + w30 = (w2*w30); + /* #335: @26 = (@26-@30) */ + w26 -= w30; + /* #336: @33 = (@33+@26) */ + w33 += w26; + /* #337: output[1][35] = @33 */ + if (res[1]) res[1][35] = w33; + /* #338: @33 = (@5-@11) */ + w33 = (w5-w11); + /* #339: output[1][36] = @33 */ + if (res[1]) res[1][36] = w33; + /* #340: @33 = (@8*@13) */ + w33 = (w8*w13); + /* #341: @29 = (@29*@24) */ + w29 *= w24; + /* #342: @29 = (@0*@29) */ + w29 = (w0*w29); + /* #343: @33 = (@33-@29) */ + w33 -= w29; + /* #344: @29 = (@10*@23) */ + w29 = (w10*w23); + /* #345: @32 = (@32*@24) */ + w32 *= w24; + /* #346: @32 = (@2*@32) */ + w32 = (w2*w32); + /* #347: @29 = (@29+@32) */ + w29 += w32; + /* #348: @33 = (@33-@29) */ + w33 -= w29; + /* #349: output[1][37] = @33 */ + if (res[1]) res[1][37] = w33; + /* #350: @13 = (@9*@13) */ + w13 = (w9*w13); + /* #351: @25 = (@25*@24) */ + w25 *= w24; + /* #352: @25 = (@0*@25) */ + w25 = (w0*w25); + /* #353: @13 = (@13+@25) */ + w13 += w25; + /* #354: @23 = (@12*@23) */ + w23 = (w12*w23); + /* #355: @31 = (@31*@24) */ + w31 *= w24; + /* #356: @31 = (@2*@31) */ + w31 = (w2*w31); + /* #357: @23 = (@23-@31) */ + w23 -= w31; + /* #358: @13 = (@13+@23) */ + w13 += w23; + /* #359: output[1][38] = @13 */ + if (res[1]) res[1][38] = w13; + /* #360: @5 = (@5-@11) */ + w5 -= w11; + /* #361: output[1][39] = @5 */ + if (res[1]) res[1][39] = w5; + /* #362: @5 = zeros(1x8,1nz) */ + w5 = 0.; + /* #363: @11 = 1 */ + w11 = 1.; + /* #364: (@5[0] = @11) */ + for (rr=(&w5)+0, ss=(&w11); rr!=(&w5)+1; rr+=1) *rr = *ss++; + /* #365: @5 = @5' */ + /* #366: @36 = project(@5) */ + casadi_project((&w5), casadi_s0, w36, casadi_s1, w); + /* #367: @37 = 00 */ + /* #368: @38 = 00 */ + /* #369: @22 = input[2][0] */ + casadi_copy(arg[2], 8, w22); + /* #370: {@5, @11, NULL, @13, @23, NULL, NULL, @31} = vertsplit(@22) */ + w5 = w22[0]; + w11 = w22[1]; + w13 = w22[3]; + w23 = w22[4]; + w31 = w22[7]; + /* #371: @4 = (@4*@5) */ + w4 *= w5; + /* #372: @24 = cos(@1) */ + w24 = cos( w1 ); + /* #373: @25 = sq(@2) */ + w25 = casadi_sq( w2 ); + /* #374: @33 = sq(@3) */ + w33 = casadi_sq( w3 ); + /* #375: @25 = (@25+@33) */ + w25 += w33; + /* #376: @3 = (@3/@25) */ + w3 /= w25; + /* #377: @3 = (@3*@11) */ + w3 *= w11; + /* #378: @25 = (@2/@25) */ + w25 = (w2/w25); + /* #379: @25 = (@25*@5) */ + w25 *= w5; + /* #380: @3 = (@3-@25) */ + w3 -= w25; + /* #381: @13 = (@13-@3) */ + w13 -= w3; + /* #382: @24 = (@24*@13) */ + w24 *= w13; + /* #383: @24 = (@0*@24) */ + w24 = (w0*w24); + /* #384: @4 = (@4+@24) */ + w4 += w24; + /* #385: @6 = (@6*@11) */ + w6 *= w11; + /* #386: @1 = sin(@1) */ + w1 = sin( w1 ); + /* #387: @1 = (@1*@13) */ + w1 *= w13; + /* #388: @1 = (@2*@1) */ + w1 = (w2*w1); + /* #389: @6 = (@6-@1) */ + w6 -= w1; + /* #390: @4 = (@4+@6) */ + w4 += w6; + /* #391: @6 = (@23-@13) */ + w6 = (w23-w13); + /* #392: @39 = 00 */ + /* #393: @8 = (@8*@5) */ + w8 *= w5; + /* #394: @1 = sin(@7) */ + w1 = sin( w7 ); + /* #395: @1 = (@1*@31) */ + w1 *= w31; + /* #396: @1 = (@0*@1) */ + w1 = (w0*w1); + /* #397: @8 = (@8-@1) */ + w8 -= w1; + /* #398: @10 = (@10*@11) */ + w10 *= w11; + /* #399: @1 = cos(@7) */ + w1 = cos( w7 ); + /* #400: @1 = (@1*@31) */ + w1 *= w31; + /* #401: @1 = (@2*@1) */ + w1 = (w2*w1); + /* #402: @10 = (@10+@1) */ + w10 += w1; + /* #403: @8 = (@8-@10) */ + w8 -= w10; + /* #404: @9 = (@9*@5) */ + w9 *= w5; + /* #405: @5 = cos(@7) */ + w5 = cos( w7 ); + /* #406: @5 = (@5*@31) */ + w5 *= w31; + /* #407: @0 = (@0*@5) */ + w0 *= w5; + /* #408: @9 = (@9+@0) */ + w9 += w0; + /* #409: @12 = (@12*@11) */ + w12 *= w11; + /* #410: @7 = sin(@7) */ + w7 = sin( w7 ); + /* #411: @7 = (@7*@31) */ + w7 *= w31; + /* #412: @2 = (@2*@7) */ + w2 *= w7; + /* #413: @12 = (@12-@2) */ + w12 -= w2; + /* #414: @9 = (@9+@12) */ + w9 += w12; + /* #415: @23 = (@23-@13) */ + w23 -= w13; + /* #416: @40 = vertcat(@37, @38, @4, @6, @39, @8, @9, @23) */ + rr=w40; + *rr++ = w4; + *rr++ = w6; + *rr++ = w8; + *rr++ = w9; + *rr++ = w23; + /* #417: @41 = project(@40) */ + casadi_project(w40, casadi_s2, w41, casadi_s1, w); + /* #418: @36 = (@36+@41) */ + for (i=0, rr=w36, cs=w41; i<6; ++i) (*rr++) += (*cs++); + /* #419: output[2][0] = @36 */ + casadi_copy(w36, 6, res[2]); + return 0; +} + +CASADI_SYMBOL_EXPORT int usv_model_guidance_ca1_expl_vde_forw(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){ + return casadi_f0(arg, res, iw, w, mem); +} + +CASADI_SYMBOL_EXPORT int usv_model_guidance_ca1_expl_vde_forw_alloc_mem(void) { + return 0; +} + +CASADI_SYMBOL_EXPORT int usv_model_guidance_ca1_expl_vde_forw_init_mem(int mem) { + return 0; +} + +CASADI_SYMBOL_EXPORT void usv_model_guidance_ca1_expl_vde_forw_free_mem(int mem) { +} + +CASADI_SYMBOL_EXPORT int usv_model_guidance_ca1_expl_vde_forw_checkout(void) { + return 0; +} + +CASADI_SYMBOL_EXPORT void usv_model_guidance_ca1_expl_vde_forw_release(int mem) { +} + +CASADI_SYMBOL_EXPORT void usv_model_guidance_ca1_expl_vde_forw_incref(void) { +} + +CASADI_SYMBOL_EXPORT void usv_model_guidance_ca1_expl_vde_forw_decref(void) { +} + +CASADI_SYMBOL_EXPORT casadi_int usv_model_guidance_ca1_expl_vde_forw_n_in(void) { return 5;} + +CASADI_SYMBOL_EXPORT casadi_int usv_model_guidance_ca1_expl_vde_forw_n_out(void) { return 3;} + +CASADI_SYMBOL_EXPORT casadi_real usv_model_guidance_ca1_expl_vde_forw_default_in(casadi_int i){ + switch (i) { + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const char* usv_model_guidance_ca1_expl_vde_forw_name_in(casadi_int i){ + switch (i) { + case 0: return "i0"; + case 1: return "i1"; + case 2: return "i2"; + case 3: return "i3"; + case 4: return "i4"; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const char* usv_model_guidance_ca1_expl_vde_forw_name_out(casadi_int i){ + switch (i) { + case 0: return "o0"; + case 1: return "o1"; + case 2: return "o2"; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const casadi_int* usv_model_guidance_ca1_expl_vde_forw_sparsity_in(casadi_int i) { + switch (i) { + case 0: return casadi_s3; + case 1: return casadi_s4; + case 2: return casadi_s3; + case 3: return casadi_s5; + case 4: return casadi_s6; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const casadi_int* usv_model_guidance_ca1_expl_vde_forw_sparsity_out(casadi_int i) { + switch (i) { + case 0: return casadi_s3; + case 1: return casadi_s7; + case 2: return casadi_s1; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT int usv_model_guidance_ca1_expl_vde_forw_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { + if (sz_arg) *sz_arg = 13; + if (sz_res) *sz_res = 11; + if (sz_iw) *sz_iw = 0; + if (sz_w) *sz_w = 180; + return 0; +} + + +#ifdef __cplusplus +} /* extern "C" */ +#endif diff --git a/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/usv_model_guidance_ca1_model/usv_model_guidance_ca1_expl_vde_forw.o b/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/usv_model_guidance_ca1_model/usv_model_guidance_ca1_expl_vde_forw.o new file mode 100644 index 00000000..5075affa Binary files /dev/null and b/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/usv_model_guidance_ca1_model/usv_model_guidance_ca1_expl_vde_forw.o differ diff --git a/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/usv_model_guidance_ca1_model/usv_model_guidance_ca1_model.h b/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/usv_model_guidance_ca1_model/usv_model_guidance_ca1_model.h new file mode 100644 index 00000000..edf150bb --- /dev/null +++ b/usv_avoidance/scripts/usv_guidance_ca1/c_generated_code/usv_model_guidance_ca1_model/usv_model_guidance_ca1_model.h @@ -0,0 +1,74 @@ +/* + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + +#ifndef usv_model_guidance_ca1_MODEL +#define usv_model_guidance_ca1_MODEL + +#ifdef __cplusplus +extern "C" { +#endif + + +/* explicit ODE */ + +// explicit ODE +int usv_model_guidance_ca1_expl_ode_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int usv_model_guidance_ca1_expl_ode_fun_work(int *, int *, int *, int *); +const int *usv_model_guidance_ca1_expl_ode_fun_sparsity_in(int); +const int *usv_model_guidance_ca1_expl_ode_fun_sparsity_out(int); +int usv_model_guidance_ca1_expl_ode_fun_n_in(); +int usv_model_guidance_ca1_expl_ode_fun_n_out(); + +// explicit forward VDE +int usv_model_guidance_ca1_expl_vde_forw(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int usv_model_guidance_ca1_expl_vde_forw_work(int *, int *, int *, int *); +const int *usv_model_guidance_ca1_expl_vde_forw_sparsity_in(int); +const int *usv_model_guidance_ca1_expl_vde_forw_sparsity_out(int); +int usv_model_guidance_ca1_expl_vde_forw_n_in(); +int usv_model_guidance_ca1_expl_vde_forw_n_out(); + +// explicit adjoint VDE +int usv_model_guidance_ca1_expl_vde_adj(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int usv_model_guidance_ca1_expl_vde_adj_work(int *, int *, int *, int *); +const int *usv_model_guidance_ca1_expl_vde_adj_sparsity_in(int); +const int *usv_model_guidance_ca1_expl_vde_adj_sparsity_out(int); +int usv_model_guidance_ca1_expl_vde_adj_n_in(); +int usv_model_guidance_ca1_expl_vde_adj_n_out(); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // usv_model_guidance_ca1_MODEL diff --git a/usv_avoidance/scripts/usv_guidance_ca1/main.py b/usv_avoidance/scripts/usv_guidance_ca1/main.py new file mode 100755 index 00000000..7447deab --- /dev/null +++ b/usv_avoidance/scripts/usv_guidance_ca1/main.py @@ -0,0 +1,210 @@ +# +# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl +# +# This file is part of acados. +# +# The 2-Clause BSD License +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE.; +# + +# author: Daniel Kloeser + +import time, os +import numpy as np +from random import random + +from acados_settings import * +from plotFcn import * +#from tracks.readDataFcn import getTrack +import matplotlib.pyplot as plt + +""" +Example of the frc_racecars in simulation without obstacle avoidance: +This example is for the optimal racing of the frc race cars. The model is a simple bycicle model and the lateral acceleration is constraint in order to validate the model assumptions. +The simulation starts at s=-2m until one round is completed(s=8.71m). The beginning is cut in the final plots to simulate a 'warm start'. +""" + +#track = "LMS_Track.txt" +#[Sref, _, _, _, _] = getTrack(track) + +Tf = 5.0 # prediction horizon +N = 100 # number of discretization steps +T = 50.00 # maximum simulation time[s] +#sref_N = 3 # reference for final reference progress + +# load model +constraint, model, acados_solver = acados_settings(Tf, N) + +# dimensions +nx = model.x.size()[0] +nu = model.U.size()[0] +ny = nx + nu +Nsim = int(T * N / Tf) + +# initialize data structs +simX = np.ndarray((Nsim, nx)) +simU = np.ndarray((Nsim, nu)) +simError = np.ndarray((Nsim, 3)) + +obsx = np.array([4,4,4,4]) +obsy = np.array([4,7.0,12,20]) +radius = np.array([1.5,1.5,1.5,1.5,0,0,0,0]) #0.5 +pobs = np.ones(16)*100 +robs = np.zeros(8) + +#s0 = model.x0[0] +tcomp_sum = 0 +tcomp_max = 0 + +x_pos = 0.0 +y_pos = 0.0 +x_vel_last = 0.0 +y_vel_last = 0.0 +psi_error = 0.0 +psi_mae = 0.0 # Mean Absolute Error +ye_mae = 0.0 +psi_mse = 0.0 # Mean Square Error +ye_mse = 0.0 + + +#Start values +nedx = 0 +nedy = 0 +psi = 0.0 +u = 0.7 +v = 0.0 + +x1 = 4.0 +y1 = -5.0 +x2 = 4.0 +y2 = 25. + +ak = np.math.atan2(y2-y1, x2-x1) +ye = -(nedx-x1)*np.sin(ak)+(nedy-y1)*np.cos(ak) +psie = psi - ak +x0 = np.array([u, v, ye, psie, psie, nedx, nedy, psi]) + +acados_solver.set(0, "lbx", x0) +acados_solver.set(0, "ubx", x0) + + +# simulate +for i in range(Nsim): + # update reference + #u_ref = 1.4 #u0 + #sref_N + for ii in range(len(obsx)): + pobs[2*ii] = obsx[ii] + pobs[2*ii+1] = obsy[ii] + robs[ii] = radius[ii] + for j in range(N): + yref=np.array([0, 0, 0, 0, 0, 0, 0, 0, 0]) + acados_solver.set(j, "yref", yref) + acados_solver.set(j, "p", pobs) + acados_solver.constraints_set(j, "lh", robs) + yref_N = np.array([0, 0, 0, 0, 0, 0, 0, 0]) + acados_solver.set(N, "yref", yref_N) + acados_solver.set(N, "p", pobs) + + # solve ocp + t = time.time() + + status = acados_solver.solve() + if status != 0: + print("acados returned status {} in closed loop iteration {}.".format(status, i)) + + elapsed = time.time() - t + + # manage timings + tcomp_sum += elapsed + if elapsed > tcomp_max: + tcomp_max = elapsed + + # get solution + x0 = acados_solver.get(0, "x") + u0 = acados_solver.get(0, "u") + + + for j in range(nx): + simX[i, j] = x0[j] + for j in range(nu): + simU[i, j] = u0[j] + + psi_error = x0[3] + ye_error = x0[2] + simError[i,0] = psi_error + simError[i,1] = ye_error + + if (i>400): + psi_mae += abs(psi_error) + ye_mae += abs(ye_error) + psi_mse += psi_error*psi_error + ye_mse += ye_error*ye_error + + + # update initial condition + x0 = acados_solver.get(1, "x") + # Add noise + #if (abs(x0[2]) > (np.pi)): + # x0[2] = (x0[2]/abs(x0[2]))*(abs(x0[2]) - 2*np.pi) + + acados_solver.set(0, "lbx", x0) + acados_solver.set(0, "ubx", x0) + #s0 = x0[0] + ''' + # check if one lap is done and break and remove entries beyond + if x0[0] > Sref[-1] + 0.1: + # find where vehicle first crosses start line + N0 = np.where(np.diff(np.sign(simX[:, 0])))[0][0] + Nsim = i - N0 # correct to final number of simulation steps for plotting + simX = simX[N0:i, :] + simU = simU[N0:i, :] + break + ''' + + +# Plot Results +t = np.linspace(0.0, Nsim * Tf / N, Nsim) + +plotRes(simX, simU, simError, obsx, obsy, radius, t) +#plotTrackProj(simX, track) +#plotalat(simX, simU, constraint, t) + +# Print some stats +print("Average computation time: {}".format(tcomp_sum / Nsim)) +print("Maximum computation time: {}".format(tcomp_max)) +#print("Average speed:{}m/s".format(np.average(simX[:, 3]))) +print("Lap time: {}s".format(Tf * Nsim / N)) + +#print("Mean Absolute Error psi: {}".format(psi_mae/600)) +print("Mean Square Error psi: {}".format(psi_mse/600)) +print("Mean Absolute Error ye: {}".format(ye_mae/600)) +print("Mean Square Error ye: {}".format(ye_mse/600)) + + +# avoid plotting when running on Travis +if os.environ.get("ACADOS_ON_TRAVIS") is None: + plt.show() \ No newline at end of file diff --git a/usv_avoidance/scripts/usv_guidance_ca1/plotFcn.py b/usv_avoidance/scripts/usv_guidance_ca1/plotFcn.py new file mode 100644 index 00000000..e6fe9423 --- /dev/null +++ b/usv_avoidance/scripts/usv_guidance_ca1/plotFcn.py @@ -0,0 +1,175 @@ +# +# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl +# +# This file is part of acados. +# +# The 2-Clause BSD License +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE.; +# + +# author: Daniel Kloeser + +#from tracks.readDataFcn import getTrack +#from time2spatial import transformProj2Orig,transformOrig2Proj +from matplotlib import cm +import matplotlib.pyplot as plt +import numpy as np +''' +def plotTrackProj(simX, T_opt=None): + # load track + s=simX[:,0] + n=simX[:,1] + alpha=simX[:,2] + v=simX[:,3] + distance=0.12 + # transform data + [x, y, _, _] = transformProj2Orig(s, n, alpha, v,filename) + # plot racetrack map + + #Setup plot + plt.figure() + plt.ylim(bottom=-1.75,top=0.35) + plt.xlim(left=-1.1,right=1.6) + plt.ylabel('y[m]') + plt.xlabel('x[m]') + + # Plot center line + [Sref,Xref,Yref,Psiref,_]=getTrack(filename) + plt.plot(Xref,Yref,'--',color='k') + + # Draw Trackboundaries + Xboundleft=Xref-distance*np.sin(Psiref) + Yboundleft=Yref+distance*np.cos(Psiref) + Xboundright=Xref+distance*np.sin(Psiref) + Yboundright=Yref-distance*np.cos(Psiref) + plt.plot(Xboundleft,Yboundleft,color='k',linewidth=1) + plt.plot(Xboundright,Yboundright,color='k',linewidth=1) + plt.plot(x,y, '-b') + + # Draw driven trajectory + heatmap = plt.scatter(x,y, c=v, cmap=cm.rainbow, edgecolor='none', marker='o') + cbar = plt.colorbar(heatmap, fraction=0.035) + cbar.set_label("velocity in [m/s]") + ax = plt.gca() + ax.set_aspect('equal', 'box') + + # Put markers for s values + xi=np.zeros(9) + yi=np.zeros(9) + xi1=np.zeros(9) + yi1=np.zeros(9) + xi2=np.zeros(9) + yi2=np.zeros(9) + for i in range(int(Sref[-1]) + 1): + try: + k = list(Sref).index(i + min(abs(Sref - i))) + except: + k = list(Sref).index(i - min(abs(Sref - i))) + [_,nrefi,_,_]=transformOrig2Proj(Xref[k],Yref[k],Psiref[k],0) + [xi[i],yi[i],_,_]=transformProj2Orig(Sref[k],nrefi+0.24,0,0) + # plt.text(xi[i], yi[i], f'{i}m', fontsize=12,horizontalalignment='center',verticalalignment='center') + plt.text(xi[i], yi[i], '{}m'.format(i), fontsize=12,horizontalalignment='center',verticalalignment='center') + [xi1[i],yi1[i],_,_]=transformProj2Orig(Sref[k],nrefi+0.12,0,0) + [xi2[i],yi2[i],_,_]=transformProj2Orig(Sref[k],nrefi+0.15,0,0) + plt.plot([xi1[i],xi2[i]],[yi1[i],yi2[i]],color='black') +''' +def plotRes(simX,simU, simError, obsx, obsy, radius, t): + # plot results + plt.figure(1) + plt.subplot(5, 1, 1) + plt.step(t, simU[:,0], color='r') + plt.title('closed-loop simulation') + plt.legend(['psieddot']) + plt.ylabel('rad/s') + plt.xlabel('t') + plt.grid(True) + plt.subplot(5, 1, 2) + plt.plot(t, simX[:,0:2]) + plt.ylabel('x') + plt.xlabel('t') + plt.legend(['u','v']) + plt.grid(True) + plt.subplot(5, 1, 3) + plt.step(t, simX[:,3], color='r') + plt.ylabel('rad') + plt.xlabel('t') + plt.legend(['psie']) + plt.grid(True) + plt.subplot(5, 1, 5) + plt.step(t, simX[:,2], color='r') + plt.ylabel('m') + plt.xlabel('t') + plt.legend(['ye']) + plt.grid(True) + plt.subplot(5, 1, 4) + plt.step(t, simX[:,4], color='r') + plt.ylabel('rad') + plt.xlabel('t') + plt.legend(['psied']) + plt.grid(True) + + fig = plt.figure(2) + plt.subplot(1, 1, 1) + plt.plot(simX[:,6:7], simX[:,5:6]) + ax = fig.gca() + for j in range(len(obsx)): + c = plt.Circle((obsy[j],obsx[j]),radius[j]) + ax.add_patch(c) + plt.ylabel('x') + plt.xlabel('y') + plt.legend(['trayectoria']) + + '''plt.figure(2) + plt.subplot(1, 1, 1) + plt.plot(simX[:,1:2], simX[:,0:1]) + plt.ylabel('x') + plt.xlabel('y') + plt.legend(['trayectoria']) + + plt.figure(3) + plt.subplot(1, 1, 1) + plt.plot(t, simError[:,1]) + plt.ylabel('m') + plt.xlabel('t') + plt.legend(['ye_error']) + plt.grid(True)''' + + +''' +def plotalat(simX,simU,constraint,t): + Nsim=t.shape[0] + plt.figure() + alat=np.zeros(Nsim) + for i in range(Nsim): + alat[i]=constraint.alat(simX[i,:],simU[i,:]) + plt.plot(t,alat) + plt.plot([t[0],t[-1]],[constraint.alat_min, constraint.alat_min],'k--') + plt.plot([t[0],t[-1]],[constraint.alat_max, constraint.alat_max],'k--') + plt.legend(['alat','alat_min/max']) + plt.xlabel('t') + plt.ylabel('alat[m/s^2]') +''' diff --git a/usv_avoidance/scripts/usv_guidance_ca1/time2spatial.py b/usv_avoidance/scripts/usv_guidance_ca1/time2spatial.py new file mode 100644 index 00000000..b52f868c --- /dev/null +++ b/usv_avoidance/scripts/usv_guidance_ca1/time2spatial.py @@ -0,0 +1,139 @@ +# +# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl +# +# This file is part of acados. +# +# The 2-Clause BSD License +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE.; +# + +# author: Daniel Kloeser + +import numpy as np +from tracks.readDataFcn import getTrack + + + +def transformProj2Orig(si,ni,alpha,v,filename='LMS_Track.txt'): + [sref,xref,yref,psiref,_]=getTrack(filename=filename) + tracklength=sref[-1] + si=si%tracklength + idxmindist=findClosestS(si,sref) + idxmindist2=findSecondClosestS(si,sref,idxmindist) + t=(si-sref[idxmindist])/(sref[idxmindist2]-sref[idxmindist]) + x0=(1-t)*xref[idxmindist]+t*xref[idxmindist2] + y0=(1-t)*yref[idxmindist]+t*yref[idxmindist2] + psi0=(1-t)*psiref[idxmindist]+t*psiref[idxmindist2] + + x=x0-ni*np.sin(psi0) + y=y0+ni*np.cos(psi0) + psi=psi0+alpha + v=v + return x,y,psi,v + + +def findClosestS(si,sref): + # Get number of elements + if(np.isscalar(si)): + N=1 + else: + N=np.array(si).shape[0] + mindist=100000*np.ones(N) + idxmindist=np.zeros(N) + for i in range(sref.size): + di=abs(si-sref[i]) + idxmindist = np.where(di < mindist,i, idxmindist) + mindist = np.where(di < mindist, di, mindist) + idxmindist = np.where(idxmindist==sref.size,1,idxmindist) + idxmindist = np.where(idxmindist<1,sref.size-1,idxmindist) + return idxmindist.astype(int) + + +def findSecondClosestS(si,sref,idxmindist): + d1=abs(si-sref[idxmindist-1]) # distance to node before + d2=abs(si-sref[(idxmindist+1)%sref.size]) # distance to node after + idxmindist2 = np.where(d1>d2,idxmindist+1,idxmindist-1) # decide which node is closer + idxmindist2 = np.where(idxmindist2==sref.size,0,idxmindist2) # if chosen node is too large + idxmindist2 = np.where(idxmindist2<0,sref.size-1,idxmindist2) # if chosen node is too small + + return idxmindist2 + +def transformOrig2Proj(x,y,psi,v,filename='LMS_Track.txt'): + [sref,xref,yref,psiref,_]=getTrack(filename=filename) + idxmindist=findClosestPoint(x,y,xref,yref) + idxmindist2=findClosestNeighbour(x,y,xref,yref,idxmindist) + t=findProjection(x,y,xref,yref,sref,idxmindist,idxmindist2) + s0=(1-t)*sref[idxmindist]+t*sref[idxmindist2] + x0=(1-t)*xref[idxmindist]+t*xref[idxmindist2] + y0=(1-t)*yref[idxmindist]+t*yref[idxmindist2] + psi0=(1-t)*psiref[idxmindist]+t*psiref[idxmindist2] + + s=s0 + n=np.cos(psi0)*(y-y0)-np.sin(psi0)*(x-x0) + alpha=psi-psi0 + v=v + return s,n,alpha,v + +def findProjection(x,y,xref,yref,sref,idxmindist,idxmindist2): + vabs=abs(sref[idxmindist]-sref[idxmindist2]) + vl=np.empty(2) + u=np.empty(2) + vl[0]=xref[idxmindist2]-xref[idxmindist] + vl[1]=yref[idxmindist2]-yref[idxmindist] + u[0]=x-xref[idxmindist] + u[1]=y-yref[idxmindist] + t=(vl[0]*u[0]+vl[1]*u[1])/vabs/vabs + return t + +def findClosestPoint(x,y,xref,yref): + mindist=1 + idxmindist=0 + for i in range(xref.size): + dist=dist2D(x,xref[i],y,yref[i]) + if dist +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include +#include +#include + +#include "acados/utils/print.h" +#include "acados_c/ocp_nlp_interface.h" +#include "acados_c/external_function_interface.h" +#include "acados/ocp_nlp/ocp_nlp_constraints_bgh.h" +#include "acados/ocp_nlp/ocp_nlp_cost_ls.h" + +#include "blasfeo/include/blasfeo_d_aux.h" +#include "blasfeo/include/blasfeo_d_aux_ext_dep.h" + +#include "usv_model_guidance_ca1_model/usv_model_guidance_ca1_model.h" +#include "acados_solver_usv_model_guidance_ca1.h" + +/** + * Represents a 3x3 matrix + * */ +typedef Eigen::Matrix xMatrix3; +/** + * Represents a 3x1 vector + * */ +typedef Eigen::Matrix xVector3; + +// global data +ocp_nlp_in * nlp_in; +ocp_nlp_out * nlp_out; +ocp_nlp_solver * nlp_solver; +void * nlp_opts; +ocp_nlp_plan * nlp_solver_plan; +ocp_nlp_config * nlp_config; +ocp_nlp_dims * nlp_dims; + +external_function_param_casadi * forw_vde_casadi; + +using namespace Eigen; +using std::ofstream; +using std::cout; +using std::endl; +using std::fixed; +using std::showpos; + +// acados dims + +// Number of intervals in the horizon +#define N 100 +// Number of differential state variables +#define NX 8 +// Number of control inputs +#define NU 1 +// Number of measurements/references on nodes 0..N-1 +#define NY 9 +// Number of measurements/references on node N +#define NYN 8 +// Number of obstacles allowed times 2 (x,y) +#define XYOBS 16 +// Number of obstalces +#define OBS 8 + +class NMPC + { + enum systemStates{ + u = 0, + v = 1, + ye = 2, + chie = 3, + psied = 4, + xned = 5, + yned = 6, + psi = 7 + }; + + enum controlInputs{ + psieddot = 0 + }; + + struct solver_output{ + double u0[NU]; + double x1[NX]; + }; + + struct solver_input{ + double x0[NX]; + double yref[NY]; + double yref_e[NYN]; + double p_obs[XYOBS]; + double r_obs[OBS]; + }; + + // publishers and subscribers + ros::Publisher desired_speed_pub; + ros::Publisher cross_track_error_pub; + ros::Publisher desired_heading_pub; + ros::Publisher desired_r_pub; + ros::Publisher marker_pub; + ros::Publisher obstacles_pub; + + ros::Subscriber local_vel_sub; + ros::Subscriber ins_pos_sub; + ros::Subscriber waypoints_sub; + ros::Subscriber obstacles_sub; + + unsigned int i,j,ii; + + // global variables + double u_des, v_des, ye_des, chie_des, psied_des; + double u_callback, v_callback; + double nedx_callback, nedy_callback; + std_msgs::Float64 eye; + std_msgs::Float64 d_heading; + std_msgs::Float64 d_speed; + std_msgs::Float64 d_r; + /** + * Safety visualization Markers + * */ + visualization_msgs::Marker marker; + + /** + * Obstacle list vector + * */ + std::vector obstacles_list_; + const double boat_radius_ = 0.5; + const unsigned int obs_num_ = 8; + const unsigned int init_obs_pos_ = 1000; + const double safety_radius_ = 0.2; + const double lidar_offset_ = 0.25; + usv_perception::obstacles_list obstacle_ned_list_; + + // acados struct + solver_input acados_in; + solver_output acados_out; + int acados_status; + +public: + + ros::Publisher target_pub; + geometry_msgs::Pose2D waypoint_path; + std::vector waypoints; + std::vector last_waypoints; + int k; + float past_psied; + float past_psieddot; + double psi_callback; + + + NMPC(ros::NodeHandle& n) + { + + int status = 0; + + status = acados_create(); + + if (status != 0){ + ROS_INFO_STREAM("acados_create() returned status " << status << ". Exiting." << endl); + exit(1); + } + + // ROS Publishers + cross_track_error_pub = n.advertise("/usv_control/controller/cross_track_error", 1); + target_pub = n.advertise("/guidance/target", 1); + desired_speed_pub = n.advertise("/guidance/desired_speed", 1); + desired_heading_pub = n.advertise("/guidance/desired_heading", 1); + desired_r_pub = n.advertise("/guidance/desired_r", 1); + marker_pub = n.advertise("/nmpc_ca/safety_vizualization", 1); + obstacles_pub = n.advertise("/nmpc_ca/obstacle_list", 1); + + // ROS Subscribers + local_vel_sub = n.subscribe("/vectornav/ins_2d/local_vel", 1, &NMPC::velocityCallback, this); + ins_pos_sub = n.subscribe("/vectornav/ins_2d/NED_pose", 1, &NMPC::positionCallback, this); + waypoints_sub = n.subscribe("/mission/waypoints", 1, &NMPC::waypointsCallback, this); + obstacles_sub = n.subscribe("/usv_perception/lidar/objects_detected", 1, &NMPC::obstaclesCallback, this); + + // Initializing control inputs + for(unsigned int i=0; i < NU; i++) acados_out.u0[i] = 0.0; + + // Define references (to be changed to subscribers) + u_des = 0.0; + v_des = 0.0; + ye_des = 0.0; + chie_des = 0.0; + psied_des = 0.0; + past_psied = 0.0;//-M_PI/2; + past_psieddot = 0; + + // Initialize state variables + nedx_callback = 0.0; + nedy_callback = 0.0; + u_callback = 0.001; + psi_callback = 0.0; + + // Initialize the state (x0) + acados_in.x0[u] = u_callback; + acados_in.x0[v] = 0.0; + acados_in.x0[ye] = 0.0; + acados_in.x0[chie] = 0.0; + acados_in.x0[psied] = 0.0; + + //Initialize Obstacles + obstacle_ned_list_.len = obs_num_; + initializeObstacles(); + + waypoints.clear(); + last_waypoints.clear(); + } + + void nmpcReset() + { + acados_free(); + } + + void velocityCallback(const geometry_msgs::Vector3::ConstPtr& _vel) + { + u_callback = _vel -> x; + if (u_callback == 0){ + u_callback = 0.001; + } + v_callback = _vel -> y; + } + + void positionCallback(const geometry_msgs::Pose2D::ConstPtr& _pos) + { + nedx_callback = _pos->x; + nedy_callback = _pos->y; + psi_callback = _pos->theta; + + circleDraw(nedx_callback, nedy_callback, boat_radius_, "boat_pose", 0); + } + + void waypointsCallback(const std_msgs::Float32MultiArray::ConstPtr& _msg) + { + waypoints.clear(); + int leng = _msg ->layout.data_offset; + + for (i=0; i<(leng-1); i++) + { + waypoints.push_back(_msg -> data[i]); + } + } + + void obstaclesCallback(const usv_perception::obj_detected_list::ConstPtr& _msg) + { + Eigen::Vector3f obstacle_body; + Eigen::Vector3f obstacle_ned; + geometry_msgs::Vector3 obs; + + // If there are more obstacles than MPC algorithim can handle + if (_msg->len > obs_num_) + { + Eigen::VectorXd obstacle_distances(_msg->len); + + // Calculate the distance to all obstacles + for (int i = 0; i < _msg->len; i++) + { + double body_x = _msg->objects[i].X + lidar_offset_; + double body_y = -(_msg->objects[i].Y); + double radius = _msg->objects[i].R + boat_radius_; + double distance = sqrt(body_x*body_x + body_y*body_y) - radius; + obstacle_distances(i) = distance; + if (distanceobjects[index].X + lidar_offset_; + double body_y = -(_msg->objects[index].Y); + double radius = _msg->objects[index].R + boat_radius_; + obstacle_body << body_x, body_y, 0; + obstacle_ned = body2NED(obstacle_body); + obstacle_ned(2) = radius; + //std::cout<<"Obstacles body x: "<< obstacle_ned[0] <<".\n"; + obstacles_list_[i] = obstacle_ned; + circleDraw(obstacle_ned(0), + obstacle_ned(1), + obstacle_ned(2), + "obstacle_circle", + i); + circleDraw(obstacle_ned(0), + obstacle_ned(1), + obstacle_ned(2) + safety_radius_, + "obstacle_circle", + i + obs_num_); + //Obstacle msg + obs.x = obstacle_ned(0); + obs.y = obstacle_ned(1); + obs.z = obstacle_ned(2); + double dist = sqrt((obs.x-nedx_callback)*(obs.x-nedx_callback)+(obs.y-nedy_callback)*(obs.y-nedy_callback)); + if (dist > 3.0) + { + obs.z = 0.0; + } + obstacle_ned_list_.obstacles[i] = obs; + } + } + + // If MPC can handle # of obstacles + else + { + // Initialize obstalces to a far distance with 0 radius + initializeObstacles(); + + // Obstain obstacle values + for (int i = 0; i < _msg->len; i++) + { + double body_x = _msg->objects[i].X + lidar_offset_; + double body_y = -(_msg->objects[i].Y); + double radius = _msg->objects[i].R + boat_radius_; + + double distance = sqrt(body_x*body_x + body_y*body_y); + //std::cout<<"Obstacle "<< i << " distance "<< distance << " radius "<< radius << ".\n"; + if (distance 3.0) + { + obs.z = 0.0; + } + obstacle_ned_list_.obstacles[i] = obs; + } + + } + + obstacles_pub.publish(obstacle_ned_list_); + + } + + Eigen::Vector3f body2NED(const Eigen::Vector3f _obstacle_body) + { + Eigen::Matrix3f R; + Eigen::Vector3f obstacle_ned; + + R << cos(psi_callback), -sin(psi_callback), 0.0, + sin(psi_callback), cos(psi_callback), 0.0, + 0.0, 0.0, 1.0; + + obstacle_ned = R*_obstacle_body; + + obstacle_ned(0) = obstacle_ned(0) + nedx_callback; + obstacle_ned(1) = obstacle_ned(1) + nedy_callback; + + return obstacle_ned; + } + + void initializeObstacles() + { + obstacles_list_.clear(); + obstacle_ned_list_.obstacles.clear(); + geometry_msgs::Vector3 obs; + + Eigen::Vector3f obstacle; + obstacle << init_obs_pos_, init_obs_pos_, 0; + + for(int i = 0; i < obs_num_; i++) + { + obstacles_list_.push_back(obstacle); + obs.x = 1000.0;//init_obs_pos_; + obs.y = 1000.0;//init_obs_pos_; + obs.z = 0.0; + obstacle_ned_list_.obstacles.push_back(obs); + } + + } + + void circleDraw(double h, double k, double r, std::string ns, int i) + { + marker.header.frame_id = "/world"; + marker.header.stamp = ros::Time::now(); + marker.ns = ns; + marker.id = i; + marker.type = visualization_msgs::Marker::LINE_STRIP; + marker.action = visualization_msgs::Marker::ADD; + marker.pose.position.x = h; + marker.pose.position.y = -k; + marker.pose.position.z = 0; + marker.pose.orientation.x = 0.0; + marker.pose.orientation.y = 0.0; + marker.pose.orientation.z = 0.0; + marker.pose.orientation.w = 1.0; + marker.color.b = 0.5; + marker.color.a = 1.0; + marker.scale.x = 0.1; + marker.lifetime = ros::Duration(); + geometry_msgs::Point p; + marker.points.clear(); + p.z = 0; + int numOfPoints = 30; + for (double i = -r; i <= r; i += 2 * r / numOfPoints) + { + p.y = i; + p.x = sqrt(pow(r, 2) - pow(i, 2)); + marker.points.push_back(p); + } + for (double i = r; i >= -r; i -= 2 * r / numOfPoints) + { + p.y = i; + p.x = -sqrt(pow(r, 2) - pow(i, 2)); + marker.points.push_back(p); + } + marker_pub.publish(marker); + } + + + /** sorts vectors from large to small + * vec: vector to be sorted + * sorted_vec: sorted results + * ind: the position of each element in the sort result in the original vector + */ + void sortVec(const VectorXd& vec, VectorXd& sorted_vec, VectorXi& index){ + index = VectorXi::LinSpaced(vec.size(),0,vec.size()-1);//[0 1 2 3 ... N-1] + + auto rule=[vec](int i, int j)->bool + { + return vec(i) 1) + { + waypoint_path.x = x2; + waypoint_path.y = y2; + target_pub.publish(waypoint_path); + double ye = -(nedx_callback-x1)*sin(ak) + + (nedy_callback-y1)*cos(ak); + control(x1, y1, ak, ye); + } + else + { + //std::cout<<"Next waypoint"< M_PI) + { + past_psied = (past_psied/fabs(past_psied))*(fabs(past_psied) - 2*M_PI); + } + control(x1, y1, ak2, ye); + } + } + else + { + d_speed.data = 0.0; + desired_speed_pub.publish(d_speed); + } + } + + void control(double _x1, double _y1, double _ak, double _ye) + { + double beta = std::atan2(v_callback, u_callback+.001); + if (v*v + u*u > 0){ + beta = std::atan2(v_callback, u_callback); + } + double chie_new = psi_callback + beta - _ak; + if (fabs(chie_new) > M_PI){ + chie_new = (chie_new/fabs(chie_new))*(fabs(chie_new) - 2*M_PI); + } + + acados_in.x0[u] = u_callback; + acados_in.x0[v] = v_callback; + acados_in.x0[ye] = _ye; + acados_in.x0[chie] = chie_new; + acados_in.x0[psied] = past_psied; + acados_in.x0[xned] = nedx_callback; + acados_in.x0[yned] = nedy_callback; + acados_in.x0[psi] = psi_callback; + + + // acados NMPC + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbx", acados_in.x0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", acados_in.x0); + + ye_des = 0.00; + + acados_in.yref[u] = u_des; // u + acados_in.yref[v] = v_des; // v + acados_in.yref[ye] = ye_des; // ye + acados_in.yref[chie] = chie_des; // chie + acados_in.yref[psied] = psied_des; // psied + acados_in.yref[xned] = 0.0; + acados_in.yref[yned] = 0.0; + acados_in.yref[psi] = 0.0; + acados_in.yref[psieddot] = 0.0; + + acados_in.yref_e[u] = u_des; // u + acados_in.yref_e[v] = v_des; // v + acados_in.yref_e[ye] = ye_des; // ye + acados_in.yref_e[chie] = chie_des; // chie + acados_in.yref_e[psied] = psied_des; // psied + acados_in.yref_e[xned] = 0.0; + acados_in.yref_e[yned] = 0.0; + acados_in.yref_e[psi] = 0.0; + + acados_in.p_obs[0] = obstacles_list_[0][0]; + acados_in.p_obs[1] = obstacles_list_[0][1]; + acados_in.p_obs[2] = obstacles_list_[1][0]; + acados_in.p_obs[3] = obstacles_list_[1][1]; + acados_in.p_obs[4] = obstacles_list_[2][0]; + acados_in.p_obs[5] = obstacles_list_[2][1]; + acados_in.p_obs[6] = obstacles_list_[3][0]; + acados_in.p_obs[7] = obstacles_list_[3][1]; + acados_in.p_obs[8] = obstacles_list_[4][0]; + acados_in.p_obs[9] = obstacles_list_[4][1]; + acados_in.p_obs[10] = obstacles_list_[5][0]; + acados_in.p_obs[11] = obstacles_list_[5][1]; + acados_in.p_obs[12] = obstacles_list_[6][0]; + acados_in.p_obs[13] = obstacles_list_[6][1]; + acados_in.p_obs[14] = obstacles_list_[7][0]; + acados_in.p_obs[15] = obstacles_list_[7][1]; + + //std::cout<<"Obstacle position: "<< obstacles_list_[0][0] <<".\n"; + //std::cout<<"Obstacle 0 min distance: "<< obstacles_list_[0][2] <<".\n"; + acados_in.r_obs[0] = obstacles_list_[0][2]; + acados_in.r_obs[1] = obstacles_list_[1][2]; + acados_in.r_obs[2] = obstacles_list_[2][2]; + acados_in.r_obs[3] = obstacles_list_[3][2]; + acados_in.r_obs[4] = obstacles_list_[4][2]; + acados_in.r_obs[5] = obstacles_list_[5][2]; + acados_in.r_obs[6] = obstacles_list_[6][2]; + acados_in.r_obs[7] = obstacles_list_[7][2]; + + for (ii = 0; ii < N; ii++) + { + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, ii, "yref", acados_in.yref); + acados_update_params( ii, acados_in.p_obs, 16); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "lh", acados_in.r_obs); + } + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "yref", acados_in.yref_e); + acados_update_params( N, acados_in.p_obs, 16); + + // call solver + acados_status = acados_solve(); + if (acados_status != 0){ + ROS_INFO_STREAM("acados returned status " << acados_status << endl); + } + + // get solution (as u) + ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 0, "u", (void *)acados_out.u0); + + // get solution at stage N = 1 (as thrust comes from x1 instead of u0 because of the derivative) + ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 1, "x", (void *)acados_out.x1); + float psid = acados_out.x1[psied] + _ak; + //std::cout<<"psieddot: "<< acados_out.u0[psieddot]<<".\n"; + //std::cout<<"psied: "<< acados_out.x1[psied]<<".\n"; + if (fabs(psid) > M_PI){ + psid = (psid/fabs(psid))*(fabs(psid) - 2*M_PI); + } + past_psied = acados_out.x1[psied]; + d_heading.data = psid; + desired_heading_pub.publish(d_heading); + d_r.data = acados_out.u0[psieddot]; + desired_r_pub.publish(d_r); + desired_speed_pub.publish(d_speed); + eye.data = _ye; + cross_track_error_pub.publish(eye); + } + + + }; + +int main(int argc, char **argv) +{ + + ros::init(argc, argv, "nmpc_guidance_ca1"); + + ros::NodeHandle n("~"); + NMPC nmpc(n); + ros::Rate loop_rate(20); + nmpc.last_waypoints.clear(); + + while(ros::ok()){ + if(nmpc.last_waypoints != nmpc.waypoints) + { + nmpc.k = 1; + nmpc.last_waypoints = nmpc.waypoints; + double x1 = nmpc.last_waypoints[0]; + double y1 = nmpc.last_waypoints[1]; + nmpc.waypoint_path.x = x1; + nmpc.waypoint_path.y = y1; + nmpc.target_pub.publish(nmpc.waypoint_path); + double x2 = nmpc.last_waypoints[2]; + double y2 = nmpc.last_waypoints[3]; + double ak = atan2(y2-y1, x2-x1); + nmpc.past_psied = nmpc.psi_callback - ak; + if (fabs(nmpc.past_psied) > M_PI){ + nmpc.past_psied = (nmpc.past_psied/fabs(nmpc.past_psied))*(fabs(nmpc.past_psied) - 2*M_PI); + } + } + + nmpc.waypoint_manager(); + + ros::spinOnce(); + loop_rate.sleep(); + } + return 0; +} diff --git a/usv_control/launch/asmc.launch b/usv_control/launch/asmc.launch index 2a1bd092..a6814586 100644 --- a/usv_control/launch/asmc.launch +++ b/usv_control/launch/asmc.launch @@ -1,14 +1,32 @@ - + + + - - + + + + + + + + + + + + + + + + + + diff --git a/usv_control/launch/usv_control.launch b/usv_control/launch/usv_control.launch index 33fcf8c5..7538651c 100644 --- a/usv_control/launch/usv_control.launch +++ b/usv_control/launch/usv_control.launch @@ -1,7 +1,12 @@ + + + + + @@ -13,6 +18,6 @@ - + \ No newline at end of file diff --git a/usv_control/scripts/ca.pyc b/usv_control/scripts/ca.pyc new file mode 100644 index 00000000..56d4ae7e Binary files /dev/null and b/usv_control/scripts/ca.pyc differ diff --git a/usv_control/scripts/lidar_obstacle_simulator.py b/usv_control/scripts/lidar_obstacle_simulator.py index 1002daa2..09581f14 100755 --- a/usv_control/scripts/lidar_obstacle_simulator.py +++ b/usv_control/scripts/lidar_obstacle_simulator.py @@ -65,8 +65,8 @@ def simulate(self): object_detected_list = obstacles_list() list_length = 0 for i in range(len(self.obstacle_list)): - x = self.obstacle_list[i]['X'] + 1.5 - y = self.obstacle_list[i]['Y'] - 4.0 + x = self.obstacle_list[i]['X'] + y = self.obstacle_list[i]['Y'] delta_x = x - self.ned_x delta_y = y - self.ned_y distance = math.pow(delta_x*delta_x + delta_y*delta_y, 0.5) @@ -75,7 +75,7 @@ def simulate(self): #if x > -0.5: obstacle = Vector3() obstacle.x = x - obstacle.y = -y + obstacle.y = y obstacle.z = self.obstacle_list[i]['R'] list_length += 1 object_detected_list.obstacles.append(obstacle) @@ -143,16 +143,16 @@ def rviz_markers(self): marker.header.frame_id = "/world" marker.type = marker.SPHERE marker.action = marker.ADD - marker.scale.x = radius +.5 - marker.scale.y = radius +.5 - marker.scale.z = radius +.5 + marker.scale.x = 2*radius + marker.scale.y = 2*radius + marker.scale.z = 2*radius marker.color.a = 1.0 marker.color.r = 1.0 marker.color.g = 1.0 marker.color.b = 0.0 marker.pose.orientation.w = 1.0 - marker.pose.position.x = x + 1.5 - marker.pose.position.y = y + 4.0 + marker.pose.position.x = x + marker.pose.position.y = y marker.pose.position.z = 0 marker.id = i marker_array.markers.append(marker) diff --git a/usv_control/scripts/los.py b/usv_control/scripts/los.py index c3f5f56e..695e1dd0 100755 --- a/usv_control/scripts/los.py +++ b/usv_control/scripts/los.py @@ -6,12 +6,15 @@ @file: los.py @date: Nov 2019 @date_modif: Sat Mar 21, 2020 + @date_modif: Sun Mar 28, 2021 @author: Alejandro Gonzalez @e-mail: alexglzg97@gmail.com @co-author: Sebastian Martinez Perez @e-mail: sebas.martp@gmail.com @brief: Implementation of line-of-sight (LOS) algorithm with inputs on NED, geodetic and body reference frames + @Note: Be careful when copying waypoint lists. la = lb, la is not + a copy, but a reference to lb, so modifying la will change lb. Open source ---------------------------------------------------------- ''' @@ -91,7 +94,8 @@ def waypoints_callback(self, msg): for i in range(int(leng)-1): waypoints.append(msg.data[i]) self.waypoint_mode = msg.data[-1] # 0 for NED, 1 for GPS, 2 for body - self.waypoint_array = waypoints + if waypoints != self.waypoint_array: + self.waypoint_array = waypoints def los_manager(self, listvar): ''' @@ -230,31 +234,31 @@ def main(): rate = rospy.Rate(100) # 100hz los = LOS() los.last_waypoint_array = [] - aux_waypoint_array = [] + NED_waypoint_array = [] while (not rospy.is_shutdown()) and los.active: if los.last_waypoint_array != los.waypoint_array: los.k = 1 los.last_waypoint_array = los.waypoint_array - aux_waypoint_array = los.last_waypoint_array + NED_waypoint_array = list(los.last_waypoint_array) # Make a copy, not a reference x_0 = los.ned_x y_0 = los.ned_y - + if los.waypoint_mode == 0: - aux_waypoint_array.insert(0,x_0) - aux_waypoint_array.insert(1,y_0) + pass elif los.waypoint_mode == 1: - for i in range(0, len(aux_waypoint_array), 2): - aux_waypoint_array[i], aux_waypoint_array[i+1] = los.gps_to_ned(aux_waypoint_array[i],aux_waypoint_array[i+1]) - aux_waypoint_array.insert(0,x_0) - aux_waypoint_array.insert(1,y_0) + for i in range(0, len(NED_waypoint_array), 2): + NED_waypoint_array[i], NED_waypoint_array[i+1] = los.gps_to_ned(NED_waypoint_array[i],NED_waypoint_array[i+1]) elif los.waypoint_mode == 2: - for i in range(0, len(aux_waypoint_array), 2): - aux_waypoint_array[i], aux_waypoint_array[i+1] = los.body_to_ned(aux_waypoint_array[i],aux_waypoint_array[i+1]) - aux_waypoint_array.insert(0,x_0) - aux_waypoint_array.insert(1,y_0) - if len(aux_waypoint_array) > 1: - los.los_manager(los.last_waypoint_array) + for i in range(0, len(NED_waypoint_array), 2): + NED_waypoint_array[i], NED_waypoint_array[i+1] = los.body_to_ned(NED_waypoint_array[i],NED_waypoint_array[i+1]) + + # Starting point + NED_waypoint_array.insert(0,x_0) + NED_waypoint_array.insert(1,y_0) + + if len(NED_waypoint_array) > 1: + los.los_manager(NED_waypoint_array) rate.sleep() los.desired(0, los.yaw) rospy.logwarn('Finished') diff --git a/usv_control/src/asmc.cpp b/usv_control/src/asmc.cpp old mode 100755 new mode 100644 index 9ba69b53..56ddff4b --- a/usv_control/src/asmc.cpp +++ b/usv_control/src/asmc.cpp @@ -1,18 +1,3 @@ -/* ----------------------------------------------------------- - @file: asmc.cpp - @date: Aug 8, 2019 - @date_modif: Mon May 18, 2020 - @author: Alejandro Gonzalez - @e-mail: alexglzg97@gmail.com - @co-author: Sebastian Martinez Perez - @e-mail: sebas.martp@gmail.com - @brief: Implementation of an adaptive sliding mode controller (ASMC) for - a USV given desired speed and heading. Requires gains as parameters. - Open source ----------------------------------------------------------- -*/ - #include #include @@ -22,6 +7,7 @@ #include "std_msgs/Float64.h" #include "std_msgs/UInt8.h" + class AdaptiveSlidingModeControl { public: @@ -47,6 +33,7 @@ class AdaptiveSlidingModeControl //Auxiliry variables float e_u_int; float e_u_last; + float psi_d_last; //Model pysical parameters float Xu; @@ -71,6 +58,16 @@ class AdaptiveSlidingModeControl float ua_u; float ua_psi; + float o_dot_dot; + float o_dot; + float o; + float o_last; + float o_dot_last; + float o_dot_dot_last; + static const float f1 = 2; + static const float f2 = 2; + static const float f3 = 2; + //Controller gains float k_u; float k_psi; @@ -94,6 +91,7 @@ class AdaptiveSlidingModeControl heading_sigma_pub = n.advertise("/usv_control/asmc/heading_sigma", 1000); heading_gain_pub = n.advertise("/usv_control/asmc/heading_gain", 1000); heading_error_pub = n.advertise("/usv_control/controller/heading_error", 1000); + control_input_pub = n.advertise("/usv_control/controller/control_input", 1000); //ROS Subscribers desired_speed_sub = n.subscribe("/guidance/desired_speed", 1000, &AdaptiveSlidingModeControl::desiredSpeedCallback, this); @@ -127,6 +125,7 @@ class AdaptiveSlidingModeControl u_d = 0; psi_d = 0; + psi_d_last = 0; testing = 0; arduino = 0; @@ -191,10 +190,26 @@ class AdaptiveSlidingModeControl e_u_int = (integral_step)*(e_u + e_u_last)/2 + e_u_int; //integral of the surge speed error e_u_last = e_u; - float e_psi_dot = 0 - r; + float psi_d_dif = psi_d - psi_d_last; + if (std::abs(psi_d_dif) > 3.141592){ + psi_d_dif = (psi_d_dif/std::abs(psi_d_dif))*(std::abs(psi_d_dif) - 2*3.141592); + } + float r_d = (psi_d_dif) / integral_step; + psi_d_last = psi_d; + o_dot_dot = (((r_d - o_last) * f1) - (f3 * o_dot_last)) * f2; + o_dot = (integral_step)*(o_dot_dot + o_dot_dot_last)/2 + o_dot; + o = (integral_step)*(o_dot + o_dot_last)/2 + o; + r_d = o; + o_last = o; + o_dot_last = o_dot; + o_dot_dot_last = o_dot_dot; + + float e_psi_dot = r_d - r; + //float e_psi_dot = 0 - r; float sigma_u = e_u + lambda_u * e_u_int; float sigma_psi = e_psi_dot + lambda_psi * e_psi; + //float sigma_psi = 0.1 * e_psi_dot + lambda_psi * e_psi; float sigma_u_abs = std::abs(sigma_u); float sigma_psi_abs = std::abs(sigma_psi); @@ -280,6 +295,14 @@ class AdaptiveSlidingModeControl Ka_dot_last_psi = 0; e_u_int = 0; e_u_last = 0; + o_dot_dot = 0; + o_dot = 0; + o = 0; + o_last = 0; + o_dot_last = 0; + o_dot_dot_last = 0; + psi_d = theta; + psi_d_last =theta; } port_t = (Tx / 2) + (Tz / B); @@ -311,6 +334,8 @@ class AdaptiveSlidingModeControl std_msgs::Float64 su; std_msgs::Float64 sp; + geometry_msgs::Pose2D ctrl_input; + rt.data = starboard_t; lt.data = port_t; @@ -323,6 +348,9 @@ class AdaptiveSlidingModeControl su.data = sigma_u; sp.data = sigma_psi; + ctrl_input.x = Tx; + ctrl_input.theta = Tz; + right_thruster_pub.publish(rt); left_thruster_pub.publish(lt); @@ -332,6 +360,7 @@ class AdaptiveSlidingModeControl heading_gain_pub.publish(hg); heading_error_pub.publish(epsi); heading_sigma_pub.publish(sp); + control_input_pub.publish(ctrl_input); } } @@ -346,6 +375,7 @@ class AdaptiveSlidingModeControl ros::Publisher heading_sigma_pub; ros::Publisher heading_gain_pub; ros::Publisher heading_error_pub; + ros::Publisher control_input_pub; ros::Subscriber desired_speed_sub; ros::Subscriber desired_heading_sub; @@ -370,4 +400,4 @@ int main(int argc, char *argv[]) loop_rate.sleep(); } return 0; -} +} \ No newline at end of file diff --git a/usv_master/CMakeLists.txt b/usv_master/CMakeLists.txt index 481acb28..e452bd99 100644 --- a/usv_master/CMakeLists.txt +++ b/usv_master/CMakeLists.txt @@ -13,6 +13,9 @@ find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs + # gazebo_msgs + geometry_msgs + tf2 ) ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) @@ -117,8 +120,8 @@ catkin_package( ## Specify additional locations of header files ## Your package locations should be listed before other locations include_directories( -# include -# ${catkin_INCLUDE_DIRS} + include + ${catkin_INCLUDE_DIRS} ) ## Declare a C++ library @@ -136,6 +139,8 @@ include_directories( ## The recommended prefix ensures that target names across packages don't collide # add_executable(${PROJECT_NAME}_node src/vanttec_usv_node.cpp) +#add_executable(gazebo_interface src/gazebo_interface.cpp) + ## Rename C++ executable without prefix ## The above recommended prefix causes long target names, the following renames the ## target back to the shorter version for ease of user use @@ -147,9 +152,9 @@ include_directories( # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) ## Specify libraries to link a library or executable target against -# target_link_libraries(${PROJECT_NAME}_node -# ${catkin_LIBRARIES} -# ) +#target_link_libraries(gazebo_interface +# ${catkin_LIBRARIES} +#) ############# ## Install ## diff --git a/usv_master/launch/usv_master.launch b/usv_master/launch/usv_master.launch index 3da644b6..ca48e431 100644 --- a/usv_master/launch/usv_master.launch +++ b/usv_master/launch/usv_master.launch @@ -1,17 +1,21 @@ - + - + - + - + + + + + + + - - \ No newline at end of file diff --git a/usv_master/scripts/waypoints.py b/usv_master/scripts/waypoints.py new file mode 100755 index 00000000..0fcd2fda --- /dev/null +++ b/usv_master/scripts/waypoints.py @@ -0,0 +1,31 @@ +#!/usr/bin/env python + +import os +import time +import rospy +from geometry_msgs.msg import Pose2D +from std_msgs.msg import Float64, Float32MultiArray + +class Test: + def __init__(self): + self.path_pub = rospy.Publisher("/mission/waypoints", Float32MultiArray, queue_size=10) + + def desired(self, path): + self.path_pub.publish(path) + +def main(): + rospy.init_node('waypoints', anonymous=False) + rate = rospy.Rate(100) + t = Test() + path_array = Float32MultiArray() + path_array.layout.data_offset = 9 + path_array.data = [5,5,5,-5,10,0,0,0,0] # Last should be waypoint mode: 0 for NED, 1 for GPS, 2 for body + while not rospy.is_shutdown(): + t.desired(path_array) + rate.sleep() + +if __name__ == "__main__": + try: + main() + except rospy.ROSInterruptException: + pass \ No newline at end of file diff --git a/usv_master/src/gazebo_interface.cpp b/usv_master/src/gazebo_interface.cpp new file mode 100644 index 00000000..b42afa41 --- /dev/null +++ b/usv_master/src/gazebo_interface.cpp @@ -0,0 +1,47 @@ +#include "ros/ros.h" +#include "gazebo_msgs/ModelState.h" +#include +#include + +class SetState{ +private: + ros::NodeHandle nh; + ros::Publisher pub_state = nh.advertise("/gazebo/set_model_state", 1000); + ros::Subscriber sub_state = nh.subscribe("/vectornav/ins_2d/NED_pose", 1000, &SetState::stateCallback, this); + tf2::Quaternion q; + gazebo_msgs::ModelState model_msg; + +public: + void stateCallback(const geometry_msgs::Pose2D::ConstPtr& msg); +}; + +void SetState::stateCallback(const geometry_msgs::Pose2D::ConstPtr& msg){ + model_msg.model_name = "vtec_s3"; + + // NED to ENU + q.setRPY(0,0,-msg->theta); + + model_msg.pose.position.x = msg->x; + model_msg.pose.position.y = -msg->y; + model_msg.pose.position.z = 0; + + model_msg.pose.orientation.x = q.x(); + model_msg.pose.orientation.y = q.y(); + model_msg.pose.orientation.z = q.z(); + model_msg.pose.orientation.w = q.w(); + + pub_state.publish(model_msg); +} + +int main(int argc, char **argv){ + ros::init(argc,argv, "gazebo_interface"); + SetState state; + ros::Rate rate(100); + + while (ros::ok){ + rate.sleep(); // wait 10 Hz + ros::spinOnce(); // will call all the callbacks waiting to be called at that point in time + } + + return 0; +} \ No newline at end of file diff --git a/usv_perception/CMakeLists.txt b/usv_perception/CMakeLists.txt index 43af2960..88c432b0 100644 --- a/usv_perception/CMakeLists.txt +++ b/usv_perception/CMakeLists.txt @@ -1,8 +1,8 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.0.2) project(usv_perception) ## Compile as C++11, supported in ROS Kinetic and newer -# add_compile_options(-std=c++11) +add_compile_options(-std=c++14) ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) @@ -19,6 +19,9 @@ message_generation find_package(OpenCV 3 REQUIRED) include_directories(${OpenCV_INCLUDE_DIRS}) +find_package(PCL REQUIRED) +include_directories(${PCL_INCLUDE_DIRS}) + ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) @@ -64,6 +67,7 @@ add_message_files( add_service_files( FILES color_id.srv + dock_corners.srv ) ## Generate actions in the 'action' folder # add_action_files( @@ -110,10 +114,8 @@ generate_messages( ## CATKIN_DEPENDS: catkin_packages dependent projects also need ## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( -# INCLUDE_DIRS include -# LIBRARIES usv_perception -# CATKIN_DEPENDS other_catkin_pkg -# DEPENDS system_lib + INCLUDE_DIRS include + LIBRARIES lib_VTPC ) ########### @@ -123,14 +125,22 @@ catkin_package( ## Specify additional locations of header files ## Your package locations should be listed before other locations include_directories( -# include + include ${catkin_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} ) ## Declare a C++ library -# add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/usv_perception.cpp -# ) +add_library(lib_VTPC + include/VTPC/VTPC.cpp + include/VTPC/VTPC.h + include/VTPC/ObjectDetectionVTPC.h +) +target_link_libraries(lib_VTPC + ${catkin_LIBRARIES} + ${PCL_LIBRARIES} + ${OpenCV_LIBRARIES} +) ## Add cmake target dependencies of the library ## as an example, code may need to be generated before libraries @@ -142,6 +152,15 @@ include_directories( ## The recommended prefix ensures that target names across packages don't collide # add_executable(${PROJECT_NAME}_node src/usv_perception_node.cpp) +add_executable(preprocessing_node + src/preprocessing_node.cpp +) +add_executable(obj_detector_node + src/obj_detector_node.cpp +) + + + ## Rename C++ executable without prefix ## The above recommended prefix causes long target names, the following renames the ## target back to the shorter version for ease of user use @@ -156,7 +175,17 @@ include_directories( # target_link_libraries(${PROJECT_NAME}_node # ${catkin_LIBRARIES} # ) +target_link_libraries(obj_detector_node + ${catkin_LIBRARIES} + ${PCL_LIBRARIES} + lib_VTPC +) +target_link_libraries(preprocessing_node + ${catkin_LIBRARIES} + ${PCL_LIBRARIES} + lib_VTPC +) ############# ## Install ## ############# @@ -211,3 +240,4 @@ include_directories( ## Add folders to be run by python nosetests # catkin_add_nosetests(test) + diff --git a/usv_perception/include/VTPC/ObjectDetectionVTPC.h b/usv_perception/include/VTPC/ObjectDetectionVTPC.h new file mode 100644 index 00000000..c98a1cc1 --- /dev/null +++ b/usv_perception/include/VTPC/ObjectDetectionVTPC.h @@ -0,0 +1,100 @@ +/** ---------------------------------------------------------------------------- + * @file: ObjectDetectionVTPC.cpp + * @date: January 11, 2021 + * @coauthor: Rodolfo Cuan + * @email: a01233155@itesm.mx + * + * @brief: Contains the description of the class use for object detection in + * VantTec Point Cloud (VTPC) class. + * ---------------------------------------------------------------------------*/ + +// IFNDEF ---------------------------------------------------------------------- +#ifndef ObjectDetectionVTPC_H_ +#define ObjectDetectionVTPC_H_ + + +// NAMESPACES ------------------------------------------------------------------ + +using namespace std; + +// INCLUDES -------------------------------------------------------------------- + +#include +#include +#include +#include + +// CLASS ---------------------------------------------------------------------- + +class gridObj{ + +public: + bool occupy; + int density; + int groupNum; + pcl::PointXYZ centerPoint; + + pcl::PointXYZ pt_min_x, pt_min_y, pt_max_x, pt_max_y; + pcl::PointXYZ pt_min, pt_max; + + string clase; + + std::vector indices; + Eigen::Vector3f voxel_min, voxel_max; + + //Eigen::Vector4f pt_min, pt_max; + + inline gridObj(){ + occupy = false; + density = 0; + groupNum = 0; + indices.clear(); + + } + + gridObj operator+(const gridObj& b){ + gridObj res; + res.density = this->density + b.density; + + res.pt_max.z = max(this->pt_max.z , b.pt_max.z); + res.pt_min.z = min(this->pt_min.z , b.pt_min.z); + res.pt_max.y = max(this->pt_max.y , b.pt_max.y); + res.pt_min.y = min(this->pt_min.y , b.pt_min.y); + res.pt_max.x = max(this->pt_max.x , b.pt_max.x); + res.pt_min.x = min(this->pt_min.x , b.pt_min.x); + + res.indices.insert(res.indices.end(),this->indices.begin(), this->indices.end()); + res.indices.insert(res.indices.end(),b.indices.begin(), b.indices.end()); + + res.voxel_min = {min(this->voxel_min(0) , b.voxel_min(0)), + min(this->voxel_min(1) , b.voxel_min(1)), + min(this->voxel_min(2) , b.voxel_min(2))}; + res.voxel_max = {max(this->voxel_max(0) , b.voxel_max(0)), + max(this->voxel_max(1) , b.voxel_max(1)), + max(this->voxel_max(2) , b.voxel_max(2))}; + + res.occupy = true; + + res.centerPoint.x = (res.voxel_max(0) + res.voxel_min(0)) /2; + res.centerPoint.y = (res.voxel_max(1) + res.voxel_min(1)) /2; + res.centerPoint.z = (res.pt_max.z + res.pt_min.z)/2; + + res.groupNum = this->groupNum; + + return res; + } + + void display(){ + cout<<"ind max: "<pt_max<pt_min<voxel_min(0)<<" "<voxel_min(1)<<" "<voxel_min(2)<voxel_max(0)<<" "<voxel_max(1)<<" "<voxel_max(2)<groupNum<density< +VTPC::VTPC(){ + //Initialize Pointer + cloudPtr_ = typename pcl::PointCloud::Ptr(new pcl::PointCloud); + + viewer_ = NULL; + + groupNumCnt_ = 0; + isFirstIteration_ = false; + +} + +template +VTPC::~VTPC() { +//Empty body +} + +// FUNCTIONS ------------------------------------------------------------------- +template +void VTPC::SetCloud(const pcl::PointCloud &in_cloud) { + *cloudPtr_ = in_cloud; +} + +template +pcl::PointCloud VTPC::GetCloud() { + return *cloudPtr_; +} + +template +sensor_msgs::PointCloud2 VTPC::GetCloudMsg() { + sensor_msgs::PointCloud2 output_msg; + pcl::PCLPointCloud2 point_cloud; + + //from PCL to PointCloud2 + pcl::toPCLPointCloud2(*cloudPtr_, point_cloud); + //from PointCloud2 to sensor_msgs::PointCloud2 + pcl_conversions::fromPCL(point_cloud,output_msg); + + return output_msg; +} + +template +void VTPC::PassThrough(const std::string &dim,const float &minLim, + const float &maxLim){ + pcl::PassThrough pass; + + pass.setInputCloud(cloudPtr_); + pass.setFilterFieldName (dim); + pass.setFilterLimits (minLim, maxLim); + pass.filter(*cloudPtr_); + +} + +template +void VTPC::CropBox(const float &minX, const float &maxX, const float &minY, + const float &maxY, const float &minZ, const float &maxZ){ + pcl::CropBox boxFilter; + + boxFilter.setMin(Eigen::Vector4f(minX, minY, minZ, 1.0)); + boxFilter.setMax(Eigen::Vector4f(maxX, maxY, maxZ, 1.0)); + boxFilter.setInputCloud(cloudPtr_); + boxFilter.filter(*cloudPtr_); + +} + +template +void VTPC::DownsampleVoxelGrid(const float &leaf_size){ + pcl::VoxelGrid vg; + + vg.setInputCloud(cloudPtr_); + vg.setLeafSize(leaf_size, leaf_size, leaf_size); + vg.filter(*cloudPtr_); + +} + +template +void VTPC::RadiusFilter(const float &radius){ + pcl::RadiusOutlierRemoval outrem; + + outrem.setInputCloud(cloudPtr_); + outrem.setRadiusSearch(radius); + outrem.setMinNeighborsInRadius(1); + outrem.setNegative(false); + outrem.setKeepOrganized(false); + + outrem.filter(*cloudPtr_); +} + +template +void VTPC::viewPointCloud(){ + + //pcl::visualization::PCLVisualizer::Ptr viewer_(new pcl::visualization::PCLVisualizer ("3D Viewer")); + + if(viewer_ == NULL){ + viewer_ = pcl::visualization::PCLVisualizer::Ptr(new pcl::visualization::PCLVisualizer ("3D Viewer")); + + + + viewer_->setBackgroundColor (0, 0, 0); + + viewer_->addCoordinateSystem (1.0); + viewer_->initCameraParameters (); + + viewer_->setCameraPosition(1.03362, 5.26269, 0.231358, -0.000555349, 0.0371576, 0.999309); + viewer_->setCameraFieldOfView(0.8574994601); + viewer_->setCameraClipDistances(0.382015, 8.84942); + viewer_->setPosition(3425, 294); + viewer_->setSize( 893, 440); + + } + + pcl::visualization::PointCloudColorHandlerGenericField intensity_distribution(cloudPtr_, "intensity"); + + + viewer_->addPointCloud (cloudPtr_, intensity_distribution, "sample cloud"); + viewer_->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud"); + + + + + std::vector insidePoints; + + Mat gridImg = Mat(512, 512, CV_8U, gridImg_); + + + + insidePoints.clear(); + + cv::findNonZero(gridImg, insidePoints); + + + + int cubeCnt = 0; + + /* + for( auto const &point : insidePoints){ + + + viewer_->addCube + (grid_[point.y][point.x].voxel_min(0),grid_[point.y][point.x].voxel_max(0), + grid_[point.y][point.x].voxel_min(1),grid_[point.y][point.x].voxel_max(1), + 0, 1, + 1, 1, 1, "cubee" + to_string(cubeCnt++), 0); + + + } + */ + + + std::vector dockcorners; + + for(auto const &object : objDetVec_){ + + auto obj = object.second.second; + + + + if(obj.clase != "undefined"){ + + + + viewer_->addSphere(obj.centerPoint, 0.01, 255,0,0,"Sphere" + to_string(cubeCnt++), 0); + //viewer_->addSphere(obj.pt_max, 0.01, 255,0,0,"Sphere" + to_string(cubeCnt++), 0); + //viewer_->addSphere(obj.pt_min, 0.01, 255,0,0,"Sphere" + to_string(cubeCnt++), 0); + + pcl::PointXYZ ptText; + std::stringstream ss; + + ptText.x = obj.voxel_max(0); + ptText.y = obj.voxel_max(1); + ptText.z = obj.pt_max.z; + + ss<addSphere(dockcorners[i], 0.05, 255,0,0,"Sphere" + to_string(cubeCnt++), 0); + } + + + } + + viewer_->addCube + (obj.voxel_min(0),obj.voxel_max(0), + obj.voxel_min(1),obj.voxel_max(1), + obj.pt_min.z, obj.pt_max.z, + 1, 1, 1, "cube" + to_string(cubeCnt++), 0); + + + viewer_->addText3D(ss.str(), ptText ,0.1,1,1,1, "text"+ to_string(cubeCnt++), 0); + + + } + } + + + + + viewer_->spinOnce(100); + + viewer_->removeAllPointClouds(); + viewer_->removeAllShapes(); + //viewer_->close(); + +} + +template +void VTPC::CreateGrid(const float &gridDim){ + + + typename pcl::PointCloud:: Ptr cloudProjectionPtr (new pcl::PointCloud); + + Eigen::Vector3f voxel_min, voxel_max; + std::vector point_indices; + Eigen::Vector4f min_point; + Eigen::Vector4f max_point; + double octree_min_x, octree_min_y, octree_min_z, + octree_max_x, octree_max_y, octree_max_z; + int x_grid_index, y_grid_index; + + gridObj gridObj; + + //clear gridImg + memset(gridImg_, 0, 512*512); + + //Copy cloud content to modify + *cloudProjectionPtr = *cloudPtr_; + + //Project PointCloud into the z axis + for(auto& pt : *cloudProjectionPtr) + pt.z = 0.0f; + + //Declaring and initialize octree + pcl::octree::OctreePointCloudPointVector octree(gridDim); + octree.setInputCloud(cloudProjectionPtr); + octree.defineBoundingBox(-5.5,-5.5,-0.4,5.5,5.5,2); + octree.addPointsFromInputCloud(); + + //Obtaining BoundingBox + octree.getBoundingBox(octree_min_x, octree_min_y, octree_min_z, + octree_max_x, octree_max_y, octree_max_z); + + + for(auto it = octree.leaf_begin(); it != octree.leaf_end(); ++it) + { + + + + auto leaf = it.getLeafContainer(); + + //Getting leaf's indices, leaf's Bounds, min and max points + point_indices.clear(); + leaf.getPointIndices(point_indices); + octree.getVoxelBounds(it, voxel_min, voxel_max); + pcl::getMinMax3D(*cloudPtr_, point_indices, min_point, max_point); + + cout<<"1"< min_density) && + (abs(min_point(2) - max_point(2)) > min_height) && + (abs(min_point(2) - max_point(2)) < max_height) + + ){ + + cout<<"2"< +void VTPC::ClusterGrid(const bool &tracking){ + + Mat gridImg = Mat(512, 512, CV_8U, gridImg_); + Mat prevGridImg = Mat(512, 512, CV_8U, prevGridImg_); + Mat contour_mask = Mat::zeros( gridImg.size(), CV_8U ); + Mat contour_mask_mod = Mat::zeros( gridImg.size(), CV_8U ); + + vector > contours; + vector hierarchy; + + std::vector non_zero_contour; + std::vector non_zero_tracking; + + int group_number; + float curr_time; + + //If not tracking reset objects detected vector + if(!tracking){ + objDetVec_.clear(); + groupNumCnt_ = 0; + } + + curr_time = float(cloudPtr_->header.stamp / 1000000.0); + findContours( gridImg, contours, hierarchy, RETR_TREE, CHAIN_APPROX_SIMPLE ); + + for( size_t i = 0; i< contours.size(); i++ ) + { + + non_zero_contour.clear(); + non_zero_tracking.clear(); + contour_mask = Mat::zeros( gridImg.size(), CV_8U ); + + drawContours( contour_mask, contours, i, cv::Scalar(255,255,255), cv::FILLED); + + //If tracking enable and is not the first iteration + //The current contour will be dilatated and will be compared with the previous image + //If there is an overlap it means the current object is close to an object in the previous frame + //the group number will be passed to the group new image + //If not a new group number will be assign + if((tracking) & (isFirstIteration_)){ + + dilate(contour_mask, contour_mask_mod, Mat(), Point(-1, -1), 2, 1, 1); + bitwise_and(contour_mask_mod, prevGridImg, contour_mask_mod); + findNonZero(contour_mask_mod, non_zero_tracking); + + if(non_zero_tracking.size() > 0){ + group_number = prevGrid_[non_zero_tracking[0].y][non_zero_tracking[0].x].groupNum; + } + else{ + group_number = groupNumCnt_++; + } + }else{ + group_number = groupNumCnt_++; + } + + //Look for the coordinates in the contour and group the gridObjs + cv::findNonZero(contour_mask, non_zero_contour); + + for( auto const &point : non_zero_contour){ + + grid_[point.y][point.x].groupNum = group_number; + + if(objDetVec_.find(group_number) == objDetVec_.end()){ + + objDetVec_[group_number].second = grid_[point.y][point.x]; + objDetVec_[group_number].first = curr_time; + + }else{ + + if(tracking && (objDetVec_[group_number].first != curr_time)){ + objDetVec_[group_number].second = grid_[point.y][point.x]; + objDetVec_[group_number].first = curr_time; + }else{ + objDetVec_[group_number].second = objDetVec_[group_number].second + grid_[point.y][point.x]; + } + + } + + //Visual Pourposes + for( auto const &idx: grid_[point.y][point.x].indices){ + cloudPtr_->points[idx].intensity = grid_[point.y][point.x].groupNum; + } + + + } + + } + + memcpy(prevGrid_, grid_, sizeof(grid_)); + memcpy(prevGridImg_, gridImg_, sizeof(gridImg_)); + isFirstIteration_ = true; + + if(tracking){ + eraseDisapearingObjects(1); + } + + +} + +template +void VTPC::ObjectDetectionPublish(const ros::Publisher &objDetPub){ + + usv_perception::obj_detected_list objDetList; + // objDetList.len = objDetVec_.size(); + + cout< +void VTPC::eraseDisapearingObjects(const float &seconds){ + + for (auto it = objDetVec_.cbegin(); it != objDetVec_.cend() ; ) + { + if (it->second.first < float((cloudPtr_->header.stamp / 1000000.0) - seconds)){ + it = objDetVec_.erase(it++); + } + else{ + ++it; + } + } +} + +template +string VTPC::ObjectClassifier(const gridObj &obj){ + std::stringstream ss; + ss.str(""); + if((abs(obj.pt_max.z-obj.pt_min.z) < 0.45) && + (abs(obj.pt_min.x - obj.pt_max.x) < 0.45) && + (abs(obj.pt_min.y - obj.pt_max.y) < 0.45) && + (abs(obj.pt_min.x - obj.pt_max.x) > 0) && + (abs(obj.pt_min.y - obj.pt_max.y) > 0) + ){ + ss <<"buoy"; + } + + /*else if( + (abs(obj.voxel_max(0) - obj.voxel_min(0)) > 1 ) || + (abs(obj.voxel_max(1) - obj.voxel_min(1)) > 1 ) + + ss<<"marker"; + }*/ + + else{ + ss<<"undefined"; + } + + return ss.str(); +} + +template +void VTPC::setDockService(const ros::ServiceClient &dockClientServer){ + dock_service_ = dockClientServer; +} + +template +std::vector VTPC::FindDockCorners(const gridObj &obj){ + + pcl::ExtractIndices extract; + typename pcl::PointCloud::Ptr cloud_p(new pcl::PointCloud); + pcl::PointIndices::Ptr inliers(new pcl::PointIndices()); + inliers->indices = obj.indices; + + extract.setInputCloud(cloudPtr_); + extract.setIndices(inliers); + extract.setNegative(false); + extract.filter(*cloud_p); + + Mat img = Mat::zeros(Size(500, 500) , CV_8U ); + Mat img2; + cvtColor( img,img2, CV_GRAY2RGB); + + usv_perception::dock_corners srv; + std::vector points; + + for(auto& pt : *cloud_p){ + + + geometry_msgs::Point32 point; + + point.x = pt.x - obj.centerPoint.x; + point.y = pt.y - obj.centerPoint.y; + point.z = pt.z; + + points.push_back(point); + + pt.x = ceil((pt.x - obj.centerPoint.x)*100)+ 250; + pt.y = ceil((pt.y - obj.centerPoint.y)*100)+ 250; + + img.at(pt.y,pt.x) = 255; + + } + + geometry_msgs::Point32 pointCenter; + + pointCenter.x =obj.centerPoint.x; + pointCenter.y= obj.centerPoint.y; + + srv.request.pointCoordinates = points; + srv.request.centerPoint = pointCenter; + + dock_service_.call(srv); + + std::vector dockcorners; + for(auto &pt: srv.response.dockCoordinates){ + + //pt.x += obj.centerPoint.x; + //pt.y += obj.centerPoint.y; + pt.z = obj.pt_max.z; + + pcl::PointXYZ pt2(pt.x, pt.y, pt.z); + + cout< +void VTPC::addNoise(const int &n){ + + + + + for(int i = 0; i < n; i++){ + + pcl::PointXYZI p; + + + p.x = (float) rand()/RAND_MAX*10 - 5; + p.y = (float) rand()/RAND_MAX*10 - 5; + p.z = 0; + + cloudPtr_->points.push_back(p); + } + + +} + + +//template class VTPC; +template class VTPC; +//template class VTPC; + diff --git a/usv_perception/include/VTPC/VTPC.h b/usv_perception/include/VTPC/VTPC.h new file mode 100644 index 00000000..333986b7 --- /dev/null +++ b/usv_perception/include/VTPC/VTPC.h @@ -0,0 +1,253 @@ +/** ---------------------------------------------------------------------------- + * @file: VTPC.h + * @date: Octubre 8, 2020 + * @coauthor: Ivana Collado + * @email: a00569475@itesm.mx + * @coauthor: Rodolfo Cuan + * @email: a01233155@itesm.mx + * + * @brief: Contains de definition of the VantTec Point Cloud (VTPC) class. + * ---------------------------------------------------------------------------*/ + +// IFNDEF ---------------------------------------------------------------------- +#ifndef VTPC_H_ +#define VTPC_H_ + +// INCLUDES -------------------------------------------------------------------- +// ROS +#include +#include +#include +#include + + +//Custom Msgs +#include +#include +#include + + +// PCL +#include +#include +#include + +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +// STD +#include +#include +#include +#include +#include +#include + +//OpenCV + +#include +#include +#include "opencv2/imgcodecs.hpp" +#include "opencv2/highgui.hpp" +#include "opencv2/imgproc.hpp" + +//Other Classes +#include "VTPC/ObjectDetectionVTPC.h" + +// NAMESPACES ------------------------------------------------------------------ + +using namespace std; +using namespace cv; + +// STRUCTS --------------------------------------------------------------------- + +// CLASS DECLARATION ----------------------------------------------------------- +template +class VTPC { +public: + // CONSTRUCTOR AND DESTRUCTOR ------------------------------------------------ + /** + * Constructor. + */ + VTPC(); + /** + * Destructor + * */ + ~VTPC(); + + + // FUNCTIONS ----------------------------------------------------------------- + /** + * Cloud Setter + * @param in_cloud[in]: PointCloud to process. + */ + void SetCloud(const pcl::PointCloud &in_cloud); + /** + * Cloud Getter + * @return pcl::PointCloud[out]: processed pointcloud in PCL format. + */ + pcl::PointCloud GetCloud(); + /** + * Cloud Sensor Msg Getter + * @return sensor_msgs::PointCloud2[out]: processed pointcloud as a ROS msg. + */ + sensor_msgs::PointCloud2 GetCloudMsg(); + /** + * Function to filter PointCloud in 1 Dimension + * @param dim[in]: dimension to filter (x, y or z) + * @param minLim[in]: Minimum limit in meters + * @param maxLim[in]: Maximum limit in meters + * @return void + */ + void PassThrough( const std::string &dim, const float &minLim, + const float &maxLim); + /** + * Function to filter PointCloud in a CropBox + * @param minX[in]: Minimum limits in X in meters + * @param maxX[in]: Maximum limits in X in meters + * @param minY[in]: Minimum limits in Y in meters + * @param maxY[in]: Maximum limits in Y in meters + * @param minZ[in]: Minimum limits in Z in meters + * @param maxZ[in]: Maximum limits in Z in meters + * @return void + */ + void CropBox( const float &minX,const float &maxX,const float &minY, + const float &maxY,const float &minZ,const float &maxZ); + /** + * Function to filter PointCloud within Radius + * @param radius[in]: radius in meters to be filtered + * @return void + */ + void RadiusFilter(const float &radius); + /** + * Downsample PointCloud using Voxel Gridding + * @param leaf_size[in]: Size of leaf to downsample (resolution). + * @return void + */ + void DownsampleVoxelGrid(const float &leaf_size); + /** + * Separates Point Cloud into 2D Plane Grid + * @param gridDim[in]: Size of Grid in meters + * @return void + */ + void CreateGrid(const float &gridDim); + /** + * Cluster objects on gridImg_ + * @param tracking[in]: Activate Tracking Mode + * @return void + */ + void ClusterGrid(const bool &tracking = false); + /** + * Uses PCL Visualizer to look at cloudPtr_ + * @return void + */ + void viewPointCloud(); + /** + * Classifies and Publishes the objects detected along their location + * @param objDetPub[in]: Ros Publisher which will publish the objects detected + * @return void + */ + void ObjectDetectionPublish(const ros::Publisher &objDetPub); + /** + * Given a gridObj it Classifies it by its dimension as a marker, bouy or marker. + * @param obj[in]: gridObj in quary + * @return string: with the class. + */ + string ObjectClassifier(const gridObj &obj); + /** + * Transforms the objet'c pointcloud to an image and sent to the Dock Server + */ + std::vector FindDockCorners(const gridObj &obj); + /** + * Set the server to calculate the dock Corners + * @param dockClientServer[in]: Dock Calc Server + * @return void + */ + void setDockService(const ros::ServiceClient &dockClientServer); + + /** + * Adds random noise + * @param n[in]: number of noise points + * @return void + */ + void addNoise(const int &n); + + // MEMBERS ------------------------------------------------------------------- + + + +private: + // FUNCTIONS ----------------------------------------------------------------- + + /** + * Erase objects from objDetVec_ which have not been detected for the last + * number of seconds indicated. + * @param seconds[in] + * @return void + */ + void eraseDisapearingObjects(const float &seconds); + + // MEMBERS ------------------------------------------------------------------- + /** + * Cloud Object to be processed + */ + typename pcl::PointCloud::Ptr cloudPtr_; + + pcl::visualization::PCLVisualizer::Ptr viewer_; + + + // MEMBERS FOR OBJECT DETECTION ---------------------------------------------- + + /** + * Current grid containing the pointcloud and image representation + */ + + gridObj grid_[512][512]; + uint8_t gridImg_[512][512]; + + /** + * Previous grid information + */ + gridObj prevGrid_[512][512]; + uint8_t prevGridImg_[512][512]; + + /** + * Map storing the detected objects. >. + */ + map> objDetVec_; + + + /** + * Group Number counter. + */ + int groupNumCnt_; + + /** + * Flag to start tracking in the second frame. + */ + bool isFirstIteration_; + + + /** + * ROS Server for calculating dock corners + */ + ros::ServiceClient dock_service_; + + +}; // End of class VTPC + +#endif \ No newline at end of file diff --git a/usv_perception/launch/VLP16_points.launch b/usv_perception/launch/VLP16_points.launch new file mode 100755 index 00000000..4970b1a3 --- /dev/null +++ b/usv_perception/launch/VLP16_points.launch @@ -0,0 +1,60 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/usv_perception/launch/usv_perception2.launch b/usv_perception/launch/usv_perception2.launch new file mode 100644 index 00000000..e2fd6493 --- /dev/null +++ b/usv_perception/launch/usv_perception2.launch @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + diff --git a/usv_perception/launch/usv_perception3.launch b/usv_perception/launch/usv_perception3.launch new file mode 100644 index 00000000..29cb122a --- /dev/null +++ b/usv_perception/launch/usv_perception3.launch @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + diff --git a/usv_perception/msg/obj_detected.msg b/usv_perception/msg/obj_detected.msg index e4b20680..8a447a0b 100644 --- a/usv_perception/msg/obj_detected.msg +++ b/usv_perception/msg/obj_detected.msg @@ -6,3 +6,5 @@ string color string clase float32 X float32 Y +int64 id +float32 R diff --git a/usv_perception/scripts/color_srv.py b/usv_perception/scripts/color_srv.py index 1acecc6b..43810dfb 100755 --- a/usv_perception/scripts/color_srv.py +++ b/usv_perception/scripts/color_srv.py @@ -7,6 +7,16 @@ from cv_bridge import CvBridge, CvBridgeError bridge = CvBridge() +red=['red' for i in range(2)] +orange=['orange' for i in range(2)] +yellow=['yellow' for i in range(3)] +green=['green' for i in range(11)] +blue=['blue' for i in range(8)] +pink=['-' for i in range(8)] +colors = red + orange + yellow + green + blue + pink + red + + + def callback_color(img): global bridge @@ -19,36 +29,25 @@ def callback_color(img): image = bridge.imgmsg_to_cv2(image, "bgr8") image = image[y:y+h,x:x+w] - + hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV) + # split each channel from hsv tensor (h, s, v) = cv2.split(image) - ret = "" - if np.median(s) < 50: - - predicted_color = "white" - else: - red=['red' for i in range(2)] - orange=['orange' for i in range(2)] - yellow=['yellow' for i in range(3)] - green=['green' for i in range(11)] - blue=['blue' for i in range(8)] - pink=['-' for i in range(8)] - - hist = cv2.calcHist([hsv], [0], None, [36], [0, 180]) - colors = red + orange + yellow + green + blue + pink + red + ret = "" + + + hist = cv2.calcHist([hsv], [0], None, [36], [0, 180]) + - hist_list = [hist[i][0] for i in range(len(hist))] - hist_dic = dict(zip(hist_list, colors)) + hist_list = [hist[i][0] for i in range(len(hist))] - # uncomment to see the frequencies of each color region - # print(hist_dic) + max_index = hist_list.index(max(hist_list)) - # predicted_color contains the region of maximum frequency - ret = str(hist_dic[max(hist_dic)]) + ret = colors[max_index] return ret diff --git a/usv_perception/scripts/detection_fusion.py b/usv_perception/scripts/detection_fusion.py new file mode 100755 index 00000000..8ade6237 --- /dev/null +++ b/usv_perception/scripts/detection_fusion.py @@ -0,0 +1,122 @@ +#!/usr/bin/env python + + +import rospy +import cv2 +from std_msgs.msg import String +from cv_bridge import CvBridge, CvBridgeError +import sensor_msgs.point_cloud2 as pc2 +from sensor_msgs.msg import PointCloud2 +#from sensor_msgs.msg import Image +import numpy as np +from usv_perception.msg import obj_detected +from usv_perception.msg import obj_detected_list +from sklearn.neighbors import KDTree +import matplotlib.pyplot as plt + + + + +class detection_fusion: + def __init__(self): + + rospy.Subscriber("/usv_perception/lidar/objects_detected", obj_detected_list, self.callback_lidar_det) + rospy.Subscriber("/usv_perception/yolo_zed/objects_detected", obj_detected_list, self.callback_yolo_det) + + self.detector_fusion_pub = rospy.Publisher('/usv_perception/sensor_fusion/objects_detected', obj_detected_list, queue_size=10) + + self.obj_detected_lidar = obj_detected_list() + self.obj_detected_yolo = obj_detected_list() + + self.obj_trackId = {} + + + def objDetToDict(self, obj_det_list): + + obj_dict = {} + for obj in obj_det_list.objects: + if(obj.id != -1): + obj_dict[obj.id] = obj + + return obj_dict + + def matchDetections(self, obj_det_lidar, obj_det_yolo): + + lidar_points = np.array([[obj.X, obj.Y] for obj in obj_det_lidar.objects]) + tree = KDTree(lidar_points, leaf_size=1) + + obj_detected_sf = obj_det_lidar + + for obj in obj_det_yolo.objects: + + query_point = np.array([obj.X + 0.30, obj.Y * -1]) + dist, ind = tree.query(query_point.reshape(1, -1), k=1) + + new_obj = obj_detected() + + if dist < 0.5: + + obj_detected_sf.objects[ind[0][0]].color = obj.color + + else: + new_obj = obj + obj_detected_sf.objects.append(new_obj) + + return obj_detected_sf + + def plotDetections(self, obj_dets): + + obj_list = [] + + plt.clf() + + plt.xlim(-5, 5) + plt.ylim(-5, 5) + + for obj in obj_dets.objects: + color = obj.color[0] if obj.color != "" else "b" + plt.scatter(obj.Y, obj.X, color = color) + plt.pause(0.001) + + + + def callback_yolo_det(self,msg): + + self.obj_detected_yolo = msg + + obj_det_lidar = self.obj_detected_lidar + obj_det_yolo =self.obj_detected_yolo + + if(bool(self.obj_trackId)): + for obj in obj_det_lidar.objects: + if(self.obj_trackId.has_key(obj.id)): + obj.color = self.obj_trackId[obj.id].color + + obj_detected_sf = self.matchDetections(obj_det_lidar, obj_det_yolo) + + self.obj_trackId = self.objDetToDict(obj_detected_sf) + + obj_detected_sf.len = len(obj_detected_sf.objects) + + self.plotDetections(obj_detected_sf) + + self.detector_fusion_pub.publish(obj_detected_sf) + + + def callback_lidar_det(self,msg): + self.obj_detected_lidar = msg + + + + + +if __name__ == '__main__': + try: + rospy.init_node('detector_fusion') + rate = rospy.Rate(5) + DF = detection_fusion() + rospy.spin() + + + except rospy.ROSInterruptException: + pass \ No newline at end of file diff --git a/usv_perception/scripts/dock_corner_srv.py b/usv_perception/scripts/dock_corner_srv.py new file mode 100755 index 00000000..6f839629 --- /dev/null +++ b/usv_perception/scripts/dock_corner_srv.py @@ -0,0 +1,135 @@ +#!/usr/bin/env python + +import rospy +import cv2 +import numpy as np +import math +from usv_perception.srv import dock_corners,dock_cornersResponse +from geometry_msgs.msg import Point32, PoseArray, Pose +from scipy.spatial import ConvexHull, convex_hull_plot_2d, distance +import matplotlib.pyplot as plt + +from cv_bridge import CvBridge, CvBridgeError + + + + + +c = 0 + +pub = rospy.Publisher('/usv_perception/lidar_detector/dock', PoseArray, queue_size=10) + + +def callback(req): + global c + + points = np.round(np.array([[point.x, point.y] for point in req.pointCoordinates]), 2) * 100 + 250 + + + #Fing hull shape and fill it + hull = ConvexHull(points) + + image=np.zeros((500,500,3),np.uint8) + image2=np.zeros((500,500,3),np.uint8) + + pts = np.array(points[hull.vertices,:], np.int32) + pts = pts.reshape((-1,1,2)) + cv2.fillPoly(image,[pts],(255,255,255)) + + + #Obtain corners from shape + gray = cv2.cvtColor(image,cv2.COLOR_BGR2GRAY) + gray = np.float32(gray) + dst = cv2.cornerHarris(gray,2,9,0.04) + dst = cv2.dilate(dst,None) + + + image2[dst>0.01*dst.max()]=[255,255,255] + gray2 = cv2.cvtColor(image2,cv2.COLOR_BGR2GRAY) + gray2 = cv2.dilate(gray2,None, iterations = 5) + + #find the centroid of the multiple corner detections + #cv2.imwrite("/home/fitocuan/Pictures/" + str(c) + ".jpg", gray2) + contours, hierarchy = cv2.findContours(gray2, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)[-2:] + + res = dock_cornersResponse() + + poseArray = PoseArray() + + cornersArray = [] + + for i in contours: + M = cv2.moments(i) + cX = int(M["m10"] / M["m00"]) + cY = int(M["m01"] / M["m00"]) + + + cX = (cX - 250 )/100.0 + cY = (cY - 250 )/100.0 + + #print(cX, cY) + + + + + cornersArray.append((cX + req.centerPoint.x, cX + req.centerPoint.y)) + + + #poseArray.poses.append(pose) + + point = Point32(cX+ req.centerPoint.x ,cY + req.centerPoint.y,0) + res.dockCoordinates.append(point) + + c+=1 + + + for point in cornersArray: + for point2 in cornersArray: + + + #print(distance.euclidean(point, point2)) + + if(distance.euclidean(point, point2) > 4): + + + pose = Pose() + pose2 = Pose() + pose.position.x = point[0] + req.centerPoint.x + pose.position.y = point[1] + req.centerPoint.y + + pose2.position.x = point2[0] + req.centerPoint.x + pose2.position.y = point2[1] + req.centerPoint.y + + poseArray.poses.append(pose) + poseArray.poses.append(pose2) + + + break + else: + continue + break + + + + + + pub.publish(poseArray) + + + + return res + + +if __name__ == '__main__': + + rospy.init_node('dock_corners_server') + rospy.loginfo("Node created!") + + service = rospy.Service("/get_dock_corners", dock_corners, callback) + + + + rate = rospy.Rate(10) + + while not rospy.is_shutdown(): + rate.sleep() diff --git a/usv_perception/scripts/include/__init__.pyc b/usv_perception/scripts/include/__init__.pyc index 9611a31c..acbcd22f 100644 Binary files a/usv_perception/scripts/include/__init__.pyc and b/usv_perception/scripts/include/__init__.pyc differ diff --git a/usv_perception/scripts/include/detector_lib.pyc b/usv_perception/scripts/include/detector_lib.pyc index 5f13d00d..19b76707 100644 Binary files a/usv_perception/scripts/include/detector_lib.pyc and b/usv_perception/scripts/include/detector_lib.pyc differ diff --git a/usv_perception/scripts/testColor.py b/usv_perception/scripts/testColor.py new file mode 100755 index 00000000..6f720ee1 --- /dev/null +++ b/usv_perception/scripts/testColor.py @@ -0,0 +1,251 @@ +#!/usr/bin/env python + +from include.detector_lib import Detector +from std_msgs.msg import String +from imutils.video import VideoStream +from imutils.video import FPS + +from usv_perception.srv import color_id +#from srv import DistanceCal +from cv_bridge import CvBridge, CvBridgeError +from sensor_msgs.msg import Image +from sensor_msgs.msg import PointCloud2 + +from usv_perception.msg import obj_detected +from usv_perception.msg import obj_detected_list + +import sensor_msgs.point_cloud2 as pc2 + +import matplotlib.pyplot as plt +import matplotlib.animation as animation + +import imutils +import argparse +import numpy as np +import time +import rospy +import cv2 +import math + +import os + +class Color(): + BLUE = '\033[94m' + GREEN = '\033[92m' + RED = '\033[91m' + DONE = '\033[0m' + + +class Detection_Node: + def __init__(self): + + self.bridge = CvBridge() + self.image = np.zeros((560,1000,3),np.uint8) + self.depth = np.zeros((560,1000,3),np.uint8) + self.points_list = [[0,0,0]] + + self.detector_pub = rospy.Publisher('/usv_perception/yolo_zed/objects_detected', obj_detected_list, queue_size=10) + + + def send_message(self, color, msg): + """ Publish message to ros node. """ + + msg = color + msg + Color.DONE + rospy.loginfo(msg) + + def calculate_color(self,img,x,y,w,h): + """ Calculates distance using get_distance service """ + + img = self.bridge.cv2_to_imgmsg(img, encoding = "bgr8") + rospy.wait_for_service("/get_color") + + service = rospy.ServiceProxy("/get_color", color_id) + color = service(img,x,y,w,h) + + return color + + + def detect(self): + """ Performs object detection and publishes coordinates. """ + + # Initialize detector + self.send_message(Color.GREEN, "[INFO] Initializing TinyYOLOv3 detector.") + dirname = os.path.dirname(__file__) + tiny3_file = dirname + "/yolo-config/tiny3.cfg" + weights_file = dirname + "/yolo-config/tiny3_68000.weights" + names_file = dirname + "/yolo-config/obj.names" + + det = Detector(tiny3_file, + weights_file, + names_file) + + (H, W) = (None, None) + + # Load model + self.send_message(Color.GREEN, "[INFO] Loading network model.") + net = det.load_model() + + # Initilialize Video Stream + self.send_message(Color.GREEN, "[INFO] Starting video stream.") + + counter = 0 + dets = 0 + nondets = 0 + detect = True + fps = FPS().start() + boxes, confidences, indices, cls_ids, colors, ids, distances = [], [], [], [], [], [], [] + + path = "/home/fitocuan/Downloads/colorTest/" + fileInPath = os.listdir(path) + + ret = True + while not rospy.is_shutdown(): + # Grab next frame + + self.image = cv2.imread(path + fileInPath[counter],1) + counter+=1 + zed_cam_size = self.image.shape[1] + frame = self.image.copy() + print_frame = self.image.copy() + curr_point_list = self.points_list + + color = "" + diststring = "" + + ##AQUI SE MODIFICA EL VIDEO + + #frame = add_brightness(frame) + #frame = add_darkness(frame) + + if cv2.waitKey(1) & 0xFF == ord ('q'): + self.send_message(Color.RED, "[DONE] Quitting program.") + break + + #frame = imutils.resize(frame, width=1000) + + (H, W) = frame.shape[:2] + print(H,W) + + det.set_h(H) + det.set_w(W) + + print((H, W)) + + # Perform detection + + detect = True + dets += 1 + # Get bounding boxes, condifences, indices and class IDs + boxes, confidences, indices, cls_ids = det.get_detections(net, frame) + # Publish detections + + # If there were any previous detections, draw them + colors = [] + distances = [] + obj_list = obj_detected_list() + len_list = 0 + + for ix in indices: + i = ix[0] + + box = boxes[i] + x, y, w, h = box + x, y, w, h = int(x), int(y), int(w), int(h) + print(len(indices)) + + + if detect == True: + + color = self.calculate_color(frame,x,y,h,w) + print("color: " + color.color) + + p1= int((x+w/2)) #1.28 hd + p2= int((y+h/2)) + + ind = p1 + p2*zed_cam_size + + d_list = [] + """ + d_list = [] + for yidx in range(-h/2,h/2): + ind_y = ind + yidx*640 + d_list.append(curr_point_list[ind_y-w/2:ind_y+w/2]) + """ + d_list = curr_point_list[ind:ind+1] + + + d_list_Y = np.array([point[2] for point in d_list]) + d_list_X = np.array([point[0] for point in d_list]) + + + if len(d_list_X) != 0: + dist_x = np.mean(d_list_X[np.isfinite(d_list_X)]) + else: + dist_x = 'nan' + + if len(d_list_Y) != 0: + dist = np.mean(d_list_Y[np.isfinite(d_list_Y)]) + else: + dist = 'nan' + + + if (dist < .30): + diststring = "OUT OF RANGE" + else: + diststring = str(dist) + " m" + + diststring = str(dist) + " m, " +str(dist_x) + "m" + color = str(color.color) + colors.append(color) + distances.append(dist) + + if str(dist) != 'nan' and str(dist_x) != 'nan': + obj = obj_detected() + obj.x = x + obj.y = y + obj.h = h + obj.w = w + obj.X = dist + obj.Y = dist_x + obj.color = color + obj.clase = 'bouy' if cls_ids[i] == 0 else 'marker' + len_list += 1 + obj_list.objects.append(obj) + + det.draw_prediction(print_frame, cls_ids[i], confidences[i], color,diststring, x, y, x+w, y+h) + + det_str = "Det: {}, BBoxes {}, Colors {}, Distance {}".format(dets, boxes, colors, distances) + self.send_message(Color.BLUE, det_str) + fps.update() + obj_list.len = len_list + self.detector_pub.publish(obj_list) + + fps.stop() + + info = [ + ("Detects: ", dets), + ("No detects: ", nondets), + ("FPS", "{:.2F}".format(fps.fps())), + ] + for (i, (k, v)) in enumerate(info): + text = "{}: {}".format(k, v) + cv2.putText(print_frame, text, (10, det.get_h() - ((i * 20) + 20)), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 0, 255), 2) + + # Show current frame + cv2.imshow("Frame", print_frame) + + #print(self.depth) + + cv2.waitKey(0) + rate.sleep() + + +if __name__ == '__main__': + try: + rospy.init_node('yolo_zed') + + rate = rospy.Rate(20) # 20Hz + D = Detection_Node() + D.detect() + except rospy.ROSInterruptException: + pass diff --git a/usv_perception/scripts/yolo_zed.py b/usv_perception/scripts/yolo_zed.py index e33f81a9..3f22ce70 100755 --- a/usv_perception/scripts/yolo_zed.py +++ b/usv_perception/scripts/yolo_zed.py @@ -42,11 +42,13 @@ def __init__(self): self.bridge = CvBridge() self.image = np.zeros((560,1000,3),np.uint8) self.depth = np.zeros((560,1000,3),np.uint8) - self.points_list = [[0,0,0]]*921600 + self.points_list = [[0,0,0]] - rospy.Subscriber("/zed/zed_node/rgb/image_rect_color", Image, self.callback_zed_img) - rospy.Subscriber("/zed/zed_node/point_cloud/cloud_registered", PointCloud2, self.callback_zed_cp) + #rospy.Subscriber("/zed/zed_node/left/image_rect_color", Image, self.callback_zed_img) + #rospy.Subscriber("/zed/zed_node/point_cloud/cloud_registered", PointCloud2, self.callback_zed_cp) + rospy.Subscriber("/r200/camera/color/image_raw", Image, self.callback_zed_img) + rospy.Subscriber("/r200/camera/depth_registered/points", PointCloud2, self.callback_zed_cp) self.detector_pub = rospy.Publisher('/usv_perception/yolo_zed/objects_detected', obj_detected_list, queue_size=10) @@ -57,8 +59,8 @@ def callback_zed_img(self,img): def callback_zed_cp(self,ros_cloud): - self.points_list = list(pc2.read_points(ros_cloud, skip_nans=True, field_names = ("x", "y", "z"))) - #skip_nans=True + """ ZED pointcloud callback""" + self.points_list = list(pc2.read_points(ros_cloud, skip_nans=False, field_names = ("x", "y", "z"))) def send_message(self, color, msg): """ Publish message to ros node. """ @@ -97,7 +99,6 @@ def detect(self): # Load model self.send_message(Color.GREEN, "[INFO] Loading network model.") net = det.load_model() - print(net) # Initilialize Video Stream self.send_message(Color.GREEN, "[INFO] Starting video stream.") @@ -108,16 +109,17 @@ def detect(self): detect = True fps = FPS().start() boxes, confidences, indices, cls_ids, colors, ids, distances = [], [], [], [], [], [], [] - zed_cam_size = 1280 + ret = True while not rospy.is_shutdown(): # Grab next frame - #ret, frame = video.read() + + zed_cam_size = self.image.shape[1] frame = self.image - #cap = cv2.VideoCapture("/home/nvidia/opencv_install/pajarito/bird.jpg") - #hasFrame, frame = cap.read() + curr_point_list = self.points_list + color = "" diststring = "" @@ -130,12 +132,14 @@ def detect(self): self.send_message(Color.RED, "[DONE] Quitting program.") break - frame = imutils.resize(frame, width=1000) + #frame = imutils.resize(frame, width=1000) (H, W) = frame.shape[:2] - if det.get_w() is None or det.get_h() is None: - det.set_h(H) - det.set_w(W) + + det.set_h(H) + det.set_w(W) + + print((H, W)) # Perform detection @@ -158,61 +162,60 @@ def detect(self): x, y, w, h = box x, y, w, h = int(x), int(y), int(w), int(h) + + if detect == True: color = self.calculate_color(frame,x,y,h,w) - print(zed_cam_size) - - p1= int((x+w/2)*zed_cam_size/1000) #1.28 hd - p2= int((y+h/2)*zed_cam_size/1000) - #1280 si es HD , 672 + p1= int((x+w/2)) #1.28 hd + p2= int((y+h/2)) ind = p1 + p2*zed_cam_size - d_list = self.points_list[ind-15:ind+15] + d_list = [] + """ + for yidx in range(-h/2,h/2): + ind_y = ind + yidx*640 + d_list.append(curr_point_list[ind_y-w/2:ind_y+w/2]) + """ + d_list = curr_point_list[ind-5:ind+5] - d_list2_Y = [] - for j in d_list: - if str(j[0]) != 'nan' and str(j[0]) != 'inf': - d_list2_Y.append(j[0]) + print(d_list) - d_list2_X = [] - for j in d_list: - if str(j[1]) != 'nan' and str(j[1]) != 'inf': - d_list2_X.append(j[1]) + d_list_Y = np.array([point[2] for point in d_list]) + d_list_X = np.array([point[0] for point in d_list]) - d_list = d_list2_Y - d_list_x = d_list2_X - if len(d_list_x) != 0: - dist_x = np.mean(d_list_x) + if len(d_list_X) != 0: + dist_x = np.mean(d_list_X[np.isfinite(d_list_X)])*-1 else: dist_x = 'nan' - if len(d_list) != 0: - dist = np.mean(d_list) + if len(d_list_Y) != 0: + dist = np.mean(d_list_Y[np.isfinite(d_list_Y)]) else: dist = 'nan' - + if (dist < .30): diststring = "OUT OF RANGE" else: diststring = str(dist) + " m" - + + diststring = str(round(float(dist),2)) + " m, " +str(round(float(dist_x),2)) + "m" color = str(color.color) colors.append(color) distances.append(dist) if str(dist) != 'nan' and str(dist_x) != 'nan': obj = obj_detected() - #print(p1,p2) obj.x = x obj.y = y obj.h = h obj.w = w obj.X = dist obj.Y = dist_x + obj.id = -1 obj.color = color obj.clase = 'bouy' if cls_ids[i] == 0 else 'marker' len_list += 1 @@ -225,7 +228,7 @@ def detect(self): fps.update() obj_list.len = len_list self.detector_pub.publish(obj_list) - cv2.line(frame, (500,560), (500,0), (255,0,0)) + fps.stop() info = [ @@ -238,10 +241,10 @@ def detect(self): cv2.putText(frame, text, (10, det.get_h() - ((i * 20) + 20)), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 0, 255), 2) # Show current frame - #cv2.imshow("Frame", frame) + cv2.imshow("Frame", frame) #print(self.depth) - #cv2.waitKey(3) + cv2.waitKey(1) rate.sleep() diff --git a/usv_perception/src/obj_detector_node.cpp b/usv_perception/src/obj_detector_node.cpp new file mode 100644 index 00000000..3a984ac1 --- /dev/null +++ b/usv_perception/src/obj_detector_node.cpp @@ -0,0 +1,161 @@ +/** ---------------------------------------------------------------------------- + * @file: obj_detector_node.cpp + * @date: February 18, 2021 + * @coauthor: Rodolfo Cuan + * @email: a01233155@itesm.mx + * + * @brief: Use case of the VantTec Point Cloud (VTPC) library. + * ---------------------------------------------------------------------------*/ + +// INCLUDES -------------------------------------------------------------------- +#include "VTPC/VTPC.h" +#include + +// NAMESPACES ------------------------------------------------------------------ + +// GLOBAL PARAMETERS ----------------------------------------------------------- +/** + * VTPC library object + * */ +VTPC vtpc_; +/** + * Callback Global Cloud + * */ +pcl::PointCloud cloud_; +/** + * Size of buffer queue + * */ +int queue_size_ = 10; +/** + * Subscribers + * */ +ros::Subscriber cloud_sub_; +/** + * Publishers + * */ +ros::Publisher cloud_pub_; +ros::Publisher obj_detected_pub_; +/** + * Services + */ +ros::ServiceClient dock_corners_client_; + + + +// LAUNCH PARAMETERS --------------------------------------------------------- +/** + * Topics + * */ +std::string topic_cloud_sub_ = ""; +std::string topic_cloud_pub_ = ""; +std::string topic_objdet_pub_ = ""; +/** + * Parameters + * */ +const std::string param_queue_size_ = "queue_size"; +bool viewPointCloudParam_ = false; + +// FUNCTIONS ------------------------------------------------------------------- + /** + * Initialize processing node + * @return void. + * */ +void initialize(ros::NodeHandle &node); + /** + * Callback to obtain point cloud. + * @param msg[in]: Received Point cloud. + * @return void. + */ +void on_cloud_msg(const sensor_msgs::PointCloud2::ConstPtr &msg); + +// MAIN PROGRAM ---------------------------------------------------------------- +int main(int argc, char** argv) +{ + ros::init(argc, argv, "obj_detector_node"); + ros::NodeHandle obj_detector_node("~"); + ros::Rate loop_rate(20); + initialize(obj_detector_node); + + + + + while (ros::ok()) + { + + + vtpc_.SetCloud(cloud_); + //vtpc_.addNoise(200); + + + //vtpc_.RadiusFilter(0.2); + vtpc_.PassThrough("z", -1, -0.1); + vtpc_.PassThrough("x", -5.0, 5.0); + vtpc_.PassThrough("y", -5.0, 5.0); + + + if (vtpc_.GetCloud().size() != 0){ + + vtpc_.CreateGrid(0.2); + + vtpc_.ClusterGrid(false); + vtpc_.ObjectDetectionPublish(obj_detected_pub_); + if(viewPointCloudParam_){ + vtpc_.viewPointCloud(); + } + + } + + + + + cloud_pub_.publish(vtpc_.GetCloudMsg()); + + + //Publish PointCloud + + + //ros::Duration(2.0).sleep(); + + ros::spinOnce(); + loop_rate.sleep(); + } + return 0; +} + +void initialize(ros::NodeHandle &node){ + + node.getParam("input_cloud", topic_cloud_sub_); + node.getParam("output_cloud", topic_cloud_pub_); + node.getParam("obj_detected_list", topic_objdet_pub_); + node.getParam("view_pointcloud", viewPointCloudParam_); + + // Subscribers + cloud_sub_ = node.subscribe(topic_cloud_sub_.c_str(), queue_size_, + &on_cloud_msg); + // Publishers + cloud_pub_ = node.advertise( + topic_cloud_pub_, queue_size_); + + obj_detected_pub_ = node.advertise( + topic_objdet_pub_, queue_size_); + + + + + dock_corners_client_ = node.serviceClient("/get_dock_corners"); + vtpc_.setDockService(dock_corners_client_); + + //Success + ROS_INFO("Velocity obstacle node is Ready!"); +} + +void on_cloud_msg(const sensor_msgs::PointCloud2::ConstPtr &msg){ + + pcl::PCLPointCloud2 pcl_pc2; + //From sensor_msgs::PointCloud2 to pcl::PCLPointCloud2 + pcl_conversions::toPCL(*msg,pcl_pc2); + //From pcl::PCLPointCloud2 to pcl::PointCloud + pcl::fromPCLPointCloud2(pcl_pc2,cloud_); + +} + diff --git a/usv_perception/src/preprocessing_node.cpp b/usv_perception/src/preprocessing_node.cpp new file mode 100644 index 00000000..8f7d17ef --- /dev/null +++ b/usv_perception/src/preprocessing_node.cpp @@ -0,0 +1,129 @@ +/** ---------------------------------------------------------------------------- + * @file: preprocessing_node.cpp + * @date: Octubre 8, 2020 + * @coauthor: Ivana Collado + * @email: a00569475@itesm.mx + * @coauthor: Rodolfo Cuan + * @email: a01233155@itesm.mx + * + * @brief: Use case of the VantTec Point Cloud (VTPC) library. + * ---------------------------------------------------------------------------*/ + +// INCLUDES -------------------------------------------------------------------- +#include "VTPC/VTPC.h" +#include + +// NAMESPACES ------------------------------------------------------------------ + +// GLOBAL PARAMETERS ----------------------------------------------------------- +/** + * VTPC library object + * */ +VTPC vtpc_; +/** + * Callback Global Cloud + * */ +pcl::PointCloud cloud_; +/** + * Size of buffer queue + * */ +int queue_size_ = 10; +/** + * Subscribers + * */ +ros::Subscriber cloud_sub_; +/** + * Publishers + * */ +ros::Publisher cloud_pub_; +ros::Publisher obj_detected_pub_; + + + +// LAUNCH PARAMETERS --------------------------------------------------------- +/** + * Topics + * */ +const std::string topic_cloud_sub_ = "/velodyne_points"; +const std::string topic_cloud_pub_ = "/output_pointcloud"; +const std::string topic_objdet_pub_ = "/obj_detected_lidar"; +/** + * Parameters + * */ +const std::string param_queue_size_ = "queue_size"; + +// FUNCTIONS ------------------------------------------------------------------- + /** + * Initialize processing node + * @return void. + * */ +void initialize(ros::NodeHandle &node); + /** + * Callback to obtain point cloud. + * @param msg[in]: Received Point cloud. + * @return void. + */ +void on_cloud_msg(const sensor_msgs::PointCloud2::ConstPtr &msg); + +// MAIN PROGRAM ---------------------------------------------------------------- +int main(int argc, char** argv) +{ + ros::init(argc, argv, "preprocessing"); + ros::NodeHandle preprocessing_node("preprocessing"); + ros::Rate loop_rate(100); + initialize(preprocessing_node); + + while (ros::ok()) + { + + vtpc_.SetCloud(cloud_); + + //Testing Functions + vtpc_.PassThrough("z", -0.4, 2.0); + vtpc_.RadiusFilter(2.5); + if (vtpc_.GetCloud().size() != 0){ + vtpc_.CreateGrid(0.2); + vtpc_.ClusterGrid(true); + vtpc_.ObjectDetectionPublish(obj_detected_pub_); + //vtpc_.viewPointCloud(); + cloud_pub_.publish(vtpc_.GetCloudMsg()); + } + + + //Publish PointCloud + + + //ros::Duration(2.0).sleep(); + + ros::spinOnce(); + loop_rate.sleep(); + } + return 0; +} + +void initialize(ros::NodeHandle &node){ + // Subscribers + cloud_sub_ = node.subscribe(topic_cloud_sub_, queue_size_, + &on_cloud_msg); + // Publishers + cloud_pub_ = node.advertise( + topic_cloud_pub_, queue_size_); + + obj_detected_pub_ = node.advertise( + topic_objdet_pub_, queue_size_); + //Success + ROS_INFO("Velocity obstacle node is Ready!"); +} + +void on_cloud_msg(const sensor_msgs::PointCloud2::ConstPtr &msg){ + + ROS_INFO("RECIEVED POINT CLOUD"); + + pcl::PCLPointCloud2 pcl_pc2; + //From sensor_msgs::PointCloud2 to pcl::PCLPointCloud2 + pcl_conversions::toPCL(*msg,pcl_pc2); + //From pcl::PCLPointCloud2 to pcl::PointCloud + pcl::fromPCLPointCloud2(pcl_pc2,cloud_); + +} + diff --git a/usv_perception/srv/dock_corners.srv b/usv_perception/srv/dock_corners.srv new file mode 100644 index 00000000..f8bcb7ae --- /dev/null +++ b/usv_perception/srv/dock_corners.srv @@ -0,0 +1,4 @@ +geometry_msgs/Point32[] pointCoordinates +geometry_msgs/Point32 centerPoint +--- +geometry_msgs/Point32[] dockCoordinates diff --git a/usv_research/CMakeLists.txt b/usv_research/CMakeLists.txt new file mode 100644 index 00000000..57fcb847 --- /dev/null +++ b/usv_research/CMakeLists.txt @@ -0,0 +1,202 @@ +cmake_minimum_required(VERSION 2.8.3) +project(usv_research) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs # Or other packages containing msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES usv_research +# CATKIN_DEPENDS other_catkin_pkg +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include +# ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/usv_research.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +# add_executable(${PROJECT_NAME}_node src/usv_research_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +# install(TARGETS ${PROJECT_NAME}_node +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark libraries for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +# install(TARGETS ${PROJECT_NAME} +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_usv_research.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/usv_research/bags/mpc_guidance_ca.m b/usv_research/bags/mpc_guidance_ca.m new file mode 100644 index 00000000..d492705d --- /dev/null +++ b/usv_research/bags/mpc_guidance_ca.m @@ -0,0 +1,235 @@ +%declare name of the bag +experimentbag = rosbag('mpc_guidance_exp4/vn_2021-06-11-13-37-23.bag'); +desiredheading = select(experimentbag, "Topic", '/guidance/desired_heading'); +desiredheadingts = timeseries(desiredheading, 'Data'); +start_time = desiredheadingts.get.TimeInfo.Start; + +%heading gain plot +heading = select(experimentbag, "Topic", '/usv_control/asmc/heading_gain'); +headingts = timeseries(heading, 'Data'); +t = headingts.get.Time - start_time; +headingdata = headingts.get.Data; +figure +plot(t,headingdata) +legend('$heading gain$', 'Interpreter', 'latex') +xlabel('Time [s]', 'Interpreter', 'latex') +ylabel('$\psi$ [rad]', 'Interpreter', 'latex') +title('Heading Gain') + +%heading sigma plot +heading = select(experimentbag, "Topic", '/usv_control/asmc/heading_sigma'); +headingts = timeseries(heading, 'Data'); +t = headingts.get.Time - start_time; +headingdata = headingts.get.Data; +figure +plot(t,headingdata) +hold on +heading = select(experimentbag, "Topic", '/usv_control/controller/heading_error'); +headingts = timeseries(heading, 'Data'); +t = headingts.get.Time - start_time; +headingdata = headingts.get.Data; +plot(t,headingdata) +hold off +legend('$\heading sigma$', '$\heading error$', 'Interpreter', 'latex') +xlabel('Time [s]', 'Interpreter', 'latex') +ylabel('$\psi$ [rad]', 'Interpreter', 'latex') +title('Heading Sigma') + +%control input plot +heading = select(experimentbag, "Topic", '/usv_control/controller/control_input'); +headingts = timeseries(heading, 'Theta'); +t = headingts.get.Time - start_time; +headingdata = headingts.get.Data; +figure +plot(t,headingdata) +hold off +legend('$\control input$', 'Interpreter', 'latex') +xlabel('Time [s]', 'Interpreter', 'latex') +ylabel('$\psi$ [rad]', 'Interpreter', 'latex') +title('Control input') + +%heading plot +%heading = select(experimentbag, "Topic", '/vectornav/ins_2d/local_vel'); +%headingts = timeseries(heading, 'Z'); +%t = headingts.get.Time - start_time; +%headingdata = headingts.get.Data; +%figure +%plot(t,headingdata) +%hold on +%desired heading plot +%desiredheading = select(experimentbag, "Topic", '/guidance/desired_r'); +%desiredheadingts = timeseries(desiredheading, 'Data'); +%t = desiredheadingts.get.Time - start_time; +%desiredheadingdata = desiredheadingts.get.Data; +%plot(t,desiredheadingdata) +%hold off +%legend('$\r$','$\r_{d}$', 'Interpreter', 'latex') +%xlabel('Time [s]', 'Interpreter', 'latex') +%ylabel('$\psi$ [rad]', 'Interpreter', 'latex') +%title(' Angular vel MPC') + +%heading plot +heading = select(experimentbag, "Topic", '/vectornav/ins_2d/NED_pose'); +headingts = timeseries(heading, 'Theta'); +t = headingts.get.Time - start_time; +headingdata = headingts.get.Data; +figure +plot(t,headingdata) +hold on +%desired heading plot +desiredheading = select(experimentbag, "Topic", '/guidance/desired_heading'); +desiredheadingts = timeseries(desiredheading, 'Data'); +t = desiredheadingts.get.Time - start_time; +desiredheadingdata = desiredheadingts.get.Data; +plot(t,desiredheadingdata) +hold off +legend('$\psi$','$\psi_{d}$', 'Interpreter', 'latex') +xlabel('Time [s]', 'Interpreter', 'latex') +ylabel('$\psi$ [rad]', 'Interpreter', 'latex') +title('Heading MPC') + +%x plot +figure +desiredheading = select(experimentbag, "Topic", '/vectornav/ins_2d/NED_pose'); +desiredheadingts = timeseries(desiredheading, 'X'); +t = desiredheadingts.get.Time - start_time; +desiredheadingdata = desiredheadingts.get.Data; +plot(t,desiredheadingdata) +hold on +%y plot +desiredheading = select(experimentbag, "Topic", '/vectornav/ins_2d/NED_pose'); +desiredheadingts = timeseries(desiredheading, 'Y'); +t = desiredheadingts.get.Time - start_time; +desiredheadingdata = desiredheadingts.get.Data; +plot(t,desiredheadingdata) +hold off +legend('$\X$','$\Y$', 'Interpreter', 'latex') +xlabel('Time [s]', 'Interpreter', 'latex') +ylabel('$\Pose$ [m]', 'Interpreter', 'latex') +title('Pose') + +%right thruster plot +right = select(experimentbag, "Topic", '/usv_control/controller/right_thruster'); +rightts = timeseries(right, 'Data'); +t = rightts.get.Time - start_time; +rightdata = rightts.get.Data; +figure +plot(t,rightdata) +hold on +%left thruster plot +left = select(experimentbag, "Topic", '/usv_control/controller/left_thruster'); +leftts = timeseries(left, 'Data'); +t = leftts.get.Time - start_time; +leftdata = leftts.get.Data; +plot(t,leftdata) +hold off +legend('$T_{stbd}$','$T_{port}$', 'Interpreter', 'latex') +xlabel('Time [s]', 'Interpreter', 'latex') +ylabel('Thrust [N]', 'Interpreter', 'latex') +title('Thruster MPC') +%speed plot +speed = select(experimentbag, "Topic", '/vectornav/ins_2d/local_vel'); +speedts = timeseries(speed, 'X'); +t = speedts.get.Time - start_time; +speeddata = speedts.get.Data; +figure +plot(t,speeddata) +hold on +%desired speed plot +desiredspeed = select(experimentbag, "Topic", '/guidance/desired_speed'); +desiredspeedts = timeseries(desiredspeed, 'Data'); +t = desiredspeedts.get.Time - start_time; +desiredspeeddata = desiredspeedts.get.Data; +plot(t,desiredspeeddata) +hold off +legend('$u$','$u_{d}$', 'Interpreter', 'latex') +xlabel('Time [s]', 'Interpreter', 'latex') +ylabel('$u$ [m/s]', 'Interpreter', 'latex') +title('Speed MPC') + +%Publish desired Path +desired_path = select(experimentbag, 'Topic', '/guidance/target'); +msgStructs = readMessages(desired_path,'DataFormat','struct'); +%msgStructs{1}; +figure +xPoints = cellfun(@(m) double(m.X),msgStructs); +yPoints = cellfun(@(m) double(m.Y),msgStructs); +plot(yPoints,xPoints) +hold on + +%publish actual path +position = select(experimentbag, 'Topic', '/vectornav/ins_2d/NED_pose'); +msgStructs = readMessages(position,'DataFormat','struct'); +%msgStructs{1}; +xlim([-30 0]), ylim([0 10]) +xPoints = cellfun(@(m) double(m.X),msgStructs); +yPoints = cellfun(@(m) double(m.Y),msgStructs); +plot(yPoints,xPoints) +xlabel('Y(m)', 'Interpreter', 'latex') +ylabel('X(m)', 'Interpreter', 'latex') +legend('Desired Path', 'Actual Path', 'Interpreter', 'latex') +title('Path') +hold on + +%publish obstacles +position = select(experimentbag, 'Topic', '/nmpc_ca/obstacle_list'); +msgStructs = readMessages(position,'DataFormat','struct'); +%figure +%xlim([-30 0]), ylim([0 10]) +xobs = zeros(8,length(msgStructs)); +yobs = zeros(8,length(msgStructs)); +zobs = zeros(8,length(msgStructs)); +for j=1:length(msgStructs) + for i=1:8 + fprintf('length %i index i %i index j %i. \n', length(msgStructs{j}.Obstacles), i ,j) + if length(msgStructs{j}.Obstacles)>0 + xobs(i, j) = msgStructs{j}.Obstacles(i).X; + yobs(i, j) = msgStructs{j}.Obstacles(i).Y; + zobs(i, j) = msgStructs{j}.Obstacles(i).Z; + if zobs(i,j)>0 + viscircles([yobs(i, j), xobs(i, j)], zobs(i, j)); + end + end + end +end +%xlabel('Y(m)', 'Interpreter', 'latex') +%ylabel('X(m)', 'Interpreter', 'latex') +title('Obstacles') +hold off + +%speed gain plot +heading = select(experimentbag, "Topic", '/usv_control/asmc/speed_gain'); +headingts = timeseries(heading, 'Data'); +t = headingts.get.Time - start_time; +headingdata = headingts.get.Data; +figure +plot(t,headingdata) +legend('$speed gain$', 'Interpreter', 'latex') +xlabel('Time [s]', 'Interpreter', 'latex') +ylabel('$u$ [m/s]', 'Interpreter', 'latex') +title('Speed Gain') + + +%speed error plot +heading = select(experimentbag, "Topic", '/usv_control/asmc/speed_error'); +headingts = timeseries(heading, 'Data'); +t = headingts.get.Time - start_time; +headingdata = headingts.get.Data; +figure +plot(t,headingdata) +legend('$speed error$', 'Interpreter', 'latex') +xlabel('Time [s]', 'Interpreter', 'latex') +ylabel('$u$ [m/s]', 'Interpreter', 'latex') +title('Speed Error') + +%speed sigma plot +heading = select(experimentbag, "Topic", '/usv_control/asmc/speed_sigma'); +headingts = timeseries(heading, 'Data'); +t = headingts.get.Time - start_time; +headingdata = headingts.get.Data; +figure +plot(t,headingdata) +legend('$speed sigma$', 'Interpreter', 'latex') +xlabel('Time [s]', 'Interpreter', 'latex') +ylabel('$u$ [m/s]', 'Interpreter', 'latex') +title('Speed Sigma') diff --git a/usv_research/bags/mpc_guidance_exp0/vn_2021-06-08-14-52-21.bag b/usv_research/bags/mpc_guidance_exp0/vn_2021-06-08-14-52-21.bag new file mode 100644 index 00000000..7f596bb2 Binary files /dev/null and b/usv_research/bags/mpc_guidance_exp0/vn_2021-06-08-14-52-21.bag differ diff --git a/usv_research/bags/mpc_guidance_exp0/vn_2021-06-08-14-53-56.bag b/usv_research/bags/mpc_guidance_exp0/vn_2021-06-08-14-53-56.bag new file mode 100644 index 00000000..ce6cf394 Binary files /dev/null and b/usv_research/bags/mpc_guidance_exp0/vn_2021-06-08-14-53-56.bag differ diff --git a/usv_research/bags/mpc_guidance_exp0/vn_2021-06-08-14-56-54.bag b/usv_research/bags/mpc_guidance_exp0/vn_2021-06-08-14-56-54.bag new file mode 100644 index 00000000..6ad59928 Binary files /dev/null and b/usv_research/bags/mpc_guidance_exp0/vn_2021-06-08-14-56-54.bag differ diff --git a/usv_research/bags/mpc_guidance_exp0/vn_2021-06-08-15-03-45.bag b/usv_research/bags/mpc_guidance_exp0/vn_2021-06-08-15-03-45.bag new file mode 100644 index 00000000..7c14e362 Binary files /dev/null and b/usv_research/bags/mpc_guidance_exp0/vn_2021-06-08-15-03-45.bag differ diff --git a/usv_research/bags/mpc_guidance_exp0/vn_2021-06-08-15-07-40.bag b/usv_research/bags/mpc_guidance_exp0/vn_2021-06-08-15-07-40.bag new file mode 100644 index 00000000..2ffb382d Binary files /dev/null and b/usv_research/bags/mpc_guidance_exp0/vn_2021-06-08-15-07-40.bag differ diff --git a/usv_research/bags/mpc_guidance_exp0/vn_2021-06-08-15-09-16.bag b/usv_research/bags/mpc_guidance_exp0/vn_2021-06-08-15-09-16.bag new file mode 100644 index 00000000..10084036 Binary files /dev/null and b/usv_research/bags/mpc_guidance_exp0/vn_2021-06-08-15-09-16.bag differ diff --git a/usv_research/bags/mpc_guidance_exp0/vn_2021-06-08-15-13-20.bag b/usv_research/bags/mpc_guidance_exp0/vn_2021-06-08-15-13-20.bag new file mode 100644 index 00000000..3ae249b0 Binary files /dev/null and b/usv_research/bags/mpc_guidance_exp0/vn_2021-06-08-15-13-20.bag differ diff --git a/usv_research/bags/mpc_guidance_exp0/vn_2021-06-08-15-15-20.bag b/usv_research/bags/mpc_guidance_exp0/vn_2021-06-08-15-15-20.bag new file mode 100644 index 00000000..d5b4e3bf Binary files /dev/null and b/usv_research/bags/mpc_guidance_exp0/vn_2021-06-08-15-15-20.bag differ diff --git a/usv_research/bags/mpc_guidance_exp0/vn_2021-06-08-15-17-05.bag b/usv_research/bags/mpc_guidance_exp0/vn_2021-06-08-15-17-05.bag new file mode 100644 index 00000000..55c8f4f4 Binary files /dev/null and b/usv_research/bags/mpc_guidance_exp0/vn_2021-06-08-15-17-05.bag differ diff --git a/usv_research/bags/mpc_guidance_exp0/vn_2021-06-08-15-19-02.bag b/usv_research/bags/mpc_guidance_exp0/vn_2021-06-08-15-19-02.bag new file mode 100644 index 00000000..660ee62f Binary files /dev/null and b/usv_research/bags/mpc_guidance_exp0/vn_2021-06-08-15-19-02.bag differ diff --git a/usv_research/bags/mpc_guidance_exp0/vn_2021-06-11-12-27-02.bag b/usv_research/bags/mpc_guidance_exp0/vn_2021-06-11-12-27-02.bag new file mode 100644 index 00000000..9b604584 Binary files /dev/null and b/usv_research/bags/mpc_guidance_exp0/vn_2021-06-11-12-27-02.bag differ diff --git a/usv_research/bags/mpc_guidance_exp0/vn_2021-06-11-12-29-16.bag b/usv_research/bags/mpc_guidance_exp0/vn_2021-06-11-12-29-16.bag new file mode 100644 index 00000000..e438de6c Binary files /dev/null and b/usv_research/bags/mpc_guidance_exp0/vn_2021-06-11-12-29-16.bag differ diff --git a/usv_research/bags/mpc_guidance_exp0/vn_2021-06-11-12-30-52.bag b/usv_research/bags/mpc_guidance_exp0/vn_2021-06-11-12-30-52.bag new file mode 100644 index 00000000..2da5b2f5 Binary files /dev/null and b/usv_research/bags/mpc_guidance_exp0/vn_2021-06-11-12-30-52.bag differ diff --git a/usv_research/bags/mpc_guidance_exp0/vn_2021-06-11-12-34-35.bag b/usv_research/bags/mpc_guidance_exp0/vn_2021-06-11-12-34-35.bag new file mode 100644 index 00000000..b29662fb Binary files /dev/null and b/usv_research/bags/mpc_guidance_exp0/vn_2021-06-11-12-34-35.bag differ diff --git a/usv_research/bags/mpc_guidance_exp1/vn_2021-06-08-18-23-49.bag b/usv_research/bags/mpc_guidance_exp1/vn_2021-06-08-18-23-49.bag new file mode 100644 index 00000000..7ea6f11a Binary files /dev/null and b/usv_research/bags/mpc_guidance_exp1/vn_2021-06-08-18-23-49.bag differ diff --git a/usv_research/bags/mpc_guidance_exp1/vn_2021-06-08-18-24-46.bag b/usv_research/bags/mpc_guidance_exp1/vn_2021-06-08-18-24-46.bag new file mode 100644 index 00000000..f68f6907 Binary files /dev/null and b/usv_research/bags/mpc_guidance_exp1/vn_2021-06-08-18-24-46.bag differ diff --git a/usv_research/bags/mpc_guidance_exp1/vn_2021-06-08-18-27-09.bag b/usv_research/bags/mpc_guidance_exp1/vn_2021-06-08-18-27-09.bag new file mode 100644 index 00000000..82016160 Binary files /dev/null and b/usv_research/bags/mpc_guidance_exp1/vn_2021-06-08-18-27-09.bag differ diff --git a/usv_research/bags/mpc_guidance_exp1/vn_2021-06-08-18-28-46.bag b/usv_research/bags/mpc_guidance_exp1/vn_2021-06-08-18-28-46.bag new file mode 100644 index 00000000..070c6bbb Binary files /dev/null and b/usv_research/bags/mpc_guidance_exp1/vn_2021-06-08-18-28-46.bag differ diff --git a/usv_research/bags/mpc_guidance_exp2/vn_2021-06-11-12-07-09.bag b/usv_research/bags/mpc_guidance_exp2/vn_2021-06-11-12-07-09.bag new file mode 100644 index 00000000..cbd96534 Binary files /dev/null and b/usv_research/bags/mpc_guidance_exp2/vn_2021-06-11-12-07-09.bag differ diff --git a/usv_research/bags/mpc_guidance_exp2/vn_2021-06-11-18-18-35.bag b/usv_research/bags/mpc_guidance_exp2/vn_2021-06-11-18-18-35.bag new file mode 100644 index 00000000..db6e5ef3 Binary files /dev/null and b/usv_research/bags/mpc_guidance_exp2/vn_2021-06-11-18-18-35.bag differ diff --git a/usv_research/bags/mpc_guidance_exp2/vn_2021-06-11-18-20-36.bag b/usv_research/bags/mpc_guidance_exp2/vn_2021-06-11-18-20-36.bag new file mode 100644 index 00000000..899a29b3 Binary files /dev/null and b/usv_research/bags/mpc_guidance_exp2/vn_2021-06-11-18-20-36.bag differ diff --git a/usv_research/bags/mpc_guidance_exp2/vn_2021-06-11-18-22-40.bag b/usv_research/bags/mpc_guidance_exp2/vn_2021-06-11-18-22-40.bag new file mode 100644 index 00000000..7c58e9f4 Binary files /dev/null and b/usv_research/bags/mpc_guidance_exp2/vn_2021-06-11-18-22-40.bag differ diff --git a/usv_research/bags/mpc_guidance_exp4/vn_2021-06-11-13-22-46.bag b/usv_research/bags/mpc_guidance_exp4/vn_2021-06-11-13-22-46.bag new file mode 100644 index 00000000..c9e652ce Binary files /dev/null and b/usv_research/bags/mpc_guidance_exp4/vn_2021-06-11-13-22-46.bag differ diff --git a/usv_research/bags/mpc_guidance_exp4/vn_2021-06-11-13-25-27.bag b/usv_research/bags/mpc_guidance_exp4/vn_2021-06-11-13-25-27.bag new file mode 100644 index 00000000..1f34cc9b Binary files /dev/null and b/usv_research/bags/mpc_guidance_exp4/vn_2021-06-11-13-25-27.bag differ diff --git a/usv_research/bags/mpc_guidance_exp4/vn_2021-06-11-13-29-34.bag b/usv_research/bags/mpc_guidance_exp4/vn_2021-06-11-13-29-34.bag new file mode 100644 index 00000000..9a579476 Binary files /dev/null and b/usv_research/bags/mpc_guidance_exp4/vn_2021-06-11-13-29-34.bag differ diff --git a/usv_research/bags/mpc_guidance_exp4/vn_2021-06-11-13-34-12.bag b/usv_research/bags/mpc_guidance_exp4/vn_2021-06-11-13-34-12.bag new file mode 100644 index 00000000..9e10379e Binary files /dev/null and b/usv_research/bags/mpc_guidance_exp4/vn_2021-06-11-13-34-12.bag differ diff --git a/usv_research/bags/mpc_guidance_exp4/vn_2021-06-11-13-37-23.bag b/usv_research/bags/mpc_guidance_exp4/vn_2021-06-11-13-37-23.bag new file mode 100644 index 00000000..39fe5ae7 Binary files /dev/null and b/usv_research/bags/mpc_guidance_exp4/vn_2021-06-11-13-37-23.bag differ diff --git a/usv_research/bags/mpc_guidance_exp4/vn_2021-06-11-13-40-21.bag b/usv_research/bags/mpc_guidance_exp4/vn_2021-06-11-13-40-21.bag new file mode 100644 index 00000000..2aeaf68c Binary files /dev/null and b/usv_research/bags/mpc_guidance_exp4/vn_2021-06-11-13-40-21.bag differ diff --git a/usv_research/bags/mpc_guidance_exp4/vn_2021-06-26-15-41-51.bag b/usv_research/bags/mpc_guidance_exp4/vn_2021-06-26-15-41-51.bag new file mode 100644 index 00000000..967d8dc0 Binary files /dev/null and b/usv_research/bags/mpc_guidance_exp4/vn_2021-06-26-15-41-51.bag differ diff --git a/usv_research/bags/mpc_guidance_exp4/vn_2021-06-26-15-48-24.bag b/usv_research/bags/mpc_guidance_exp4/vn_2021-06-26-15-48-24.bag new file mode 100644 index 00000000..0e92e4bf Binary files /dev/null and b/usv_research/bags/mpc_guidance_exp4/vn_2021-06-26-15-48-24.bag differ diff --git a/usv_research/bags/mpc_guidance_exp4/vn_2021-06-26-15-51-22.bag b/usv_research/bags/mpc_guidance_exp4/vn_2021-06-26-15-51-22.bag new file mode 100644 index 00000000..70d3f56b Binary files /dev/null and b/usv_research/bags/mpc_guidance_exp4/vn_2021-06-26-15-51-22.bag differ diff --git a/usv_research/bags/mpc_guidance_exp4/vn_2021-06-26-15-55-30.bag b/usv_research/bags/mpc_guidance_exp4/vn_2021-06-26-15-55-30.bag new file mode 100644 index 00000000..8537caa2 Binary files /dev/null and b/usv_research/bags/mpc_guidance_exp4/vn_2021-06-26-15-55-30.bag differ diff --git a/usv_research/bags/mpc_guidance_exp4/vn_2021-06-26-15-59-20.bag b/usv_research/bags/mpc_guidance_exp4/vn_2021-06-26-15-59-20.bag new file mode 100644 index 00000000..19dcbc82 Binary files /dev/null and b/usv_research/bags/mpc_guidance_exp4/vn_2021-06-26-15-59-20.bag differ diff --git a/usv_research/bags/mpc_guidance_exp4/vn_2021-06-26-16-00-06.bag b/usv_research/bags/mpc_guidance_exp4/vn_2021-06-26-16-00-06.bag new file mode 100644 index 00000000..de7a478c Binary files /dev/null and b/usv_research/bags/mpc_guidance_exp4/vn_2021-06-26-16-00-06.bag differ diff --git a/usv_research/bags/mpc_guidance_exp4/vn_2021-06-26-16-02-50.bag b/usv_research/bags/mpc_guidance_exp4/vn_2021-06-26-16-02-50.bag new file mode 100644 index 00000000..13ad5cde Binary files /dev/null and b/usv_research/bags/mpc_guidance_exp4/vn_2021-06-26-16-02-50.bag differ diff --git a/usv_research/launch/mpc_guidance_ca_exp0.launch b/usv_research/launch/mpc_guidance_ca_exp0.launch new file mode 100644 index 00000000..fa86c374 --- /dev/null +++ b/usv_research/launch/mpc_guidance_ca_exp0.launch @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/usv_research/launch/mpc_guidance_ca_exp1.launch b/usv_research/launch/mpc_guidance_ca_exp1.launch new file mode 100644 index 00000000..c9f56f57 --- /dev/null +++ b/usv_research/launch/mpc_guidance_ca_exp1.launch @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/usv_research/launch/mpc_guidance_ca_exp2.launch b/usv_research/launch/mpc_guidance_ca_exp2.launch new file mode 100644 index 00000000..204b040f --- /dev/null +++ b/usv_research/launch/mpc_guidance_ca_exp2.launch @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/usv_research/launch/mpc_guidance_ca_exp3.launch b/usv_research/launch/mpc_guidance_ca_exp3.launch new file mode 100644 index 00000000..90322757 --- /dev/null +++ b/usv_research/launch/mpc_guidance_ca_exp3.launch @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/usv_research/launch/mpc_guidance_ca_exp4.launch b/usv_research/launch/mpc_guidance_ca_exp4.launch new file mode 100644 index 00000000..4f2b6a2a --- /dev/null +++ b/usv_research/launch/mpc_guidance_ca_exp4.launch @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/usv_research/launch/sim/sim_mpc_guidance_ca_exp0.launch b/usv_research/launch/sim/sim_mpc_guidance_ca_exp0.launch new file mode 100644 index 00000000..fd2836e4 --- /dev/null +++ b/usv_research/launch/sim/sim_mpc_guidance_ca_exp0.launch @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/usv_research/launch/sim/sim_mpc_guidance_ca_exp1.launch b/usv_research/launch/sim/sim_mpc_guidance_ca_exp1.launch new file mode 100644 index 00000000..873bef9f --- /dev/null +++ b/usv_research/launch/sim/sim_mpc_guidance_ca_exp1.launch @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/usv_research/launch/sim/sim_mpc_guidance_ca_exp2.launch b/usv_research/launch/sim/sim_mpc_guidance_ca_exp2.launch new file mode 100644 index 00000000..2243eb6e --- /dev/null +++ b/usv_research/launch/sim/sim_mpc_guidance_ca_exp2.launch @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/usv_research/launch/sim/sim_mpc_guidance_ca_exp3.launch b/usv_research/launch/sim/sim_mpc_guidance_ca_exp3.launch new file mode 100644 index 00000000..909376d0 --- /dev/null +++ b/usv_research/launch/sim/sim_mpc_guidance_ca_exp3.launch @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/usv_research/launch/sim/sim_mpc_guidance_ca_exp4.launch b/usv_research/launch/sim/sim_mpc_guidance_ca_exp4.launch new file mode 100644 index 00000000..6da6c971 --- /dev/null +++ b/usv_research/launch/sim/sim_mpc_guidance_ca_exp4.launch @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/usv_research/launch/vn_nmpc_guidance_ca_exp0.launch b/usv_research/launch/vn_nmpc_guidance_ca_exp0.launch new file mode 100644 index 00000000..972dd7b7 --- /dev/null +++ b/usv_research/launch/vn_nmpc_guidance_ca_exp0.launch @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/usv_research/launch/vn_nmpc_guidance_ca_exp1.launch b/usv_research/launch/vn_nmpc_guidance_ca_exp1.launch new file mode 100644 index 00000000..12603d78 --- /dev/null +++ b/usv_research/launch/vn_nmpc_guidance_ca_exp1.launch @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/usv_research/launch/vn_nmpc_guidance_ca_exp2.launch b/usv_research/launch/vn_nmpc_guidance_ca_exp2.launch new file mode 100644 index 00000000..fba04702 --- /dev/null +++ b/usv_research/launch/vn_nmpc_guidance_ca_exp2.launch @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/usv_research/launch/vn_nmpc_guidance_ca_exp3.launch b/usv_research/launch/vn_nmpc_guidance_ca_exp3.launch new file mode 100644 index 00000000..aa2352e5 --- /dev/null +++ b/usv_research/launch/vn_nmpc_guidance_ca_exp3.launch @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/usv_research/launch/vn_nmpc_guidance_ca_exp4.launch b/usv_research/launch/vn_nmpc_guidance_ca_exp4.launch new file mode 100644 index 00000000..0f8f995d --- /dev/null +++ b/usv_research/launch/vn_nmpc_guidance_ca_exp4.launch @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + diff --git a/usv_research/package.xml b/usv_research/package.xml new file mode 100644 index 00000000..a5d0f27e --- /dev/null +++ b/usv_research/package.xml @@ -0,0 +1,59 @@ + + + usv_research + 0.0.0 + The usv_research package + + + + + nvidia + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + + + + + + + + diff --git a/usv_research/scripts/mpc_obstacle_simulator.py b/usv_research/scripts/mpc_obstacle_simulator.py new file mode 100755 index 00000000..fc591ac4 --- /dev/null +++ b/usv_research/scripts/mpc_obstacle_simulator.py @@ -0,0 +1,340 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +''' +---------------------------------------------------------- + @file: obstacle_simulator.py + @date: Sun Mar 22, 2020 + @modified: Sat May 15, 2021 + @author: Alejandro Gonzalez Garcia + @e-mail: alexglzg97@gmail.com + @brief: Obstacle simulation for mission testing. + @version: 1.2 + Open source +---------------------------------------------------------- +''' + +import math +import time +import os + +import numpy as np +import rospy +from geometry_msgs.msg import Pose2D +from usv_perception.msg import obj_detected +from usv_perception.msg import obj_detected_list +from visualization_msgs.msg import Marker +from visualization_msgs.msg import MarkerArray + + +class ObstacleSimulator: + def __init__(self): + + self.active = True + + self.ned_x = 0 + self.ned_y = 0 + self.yaw = 0 + + self.challenge = 4 #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.25 + + rospy.Subscriber("/vectornav/ins_2d/NED_pose", Pose2D, self.ins_pose_callback) + + #self.detector_pub = rospy.Publisher('/usv_perception/yolo_zed/objects_detected', obj_detected_list, queue_size=10) + self.detector_pub = rospy.Publisher('/usv_perception/lidar/objects_detected', obj_detected_list, queue_size=10) + self.marker_pub = rospy.Publisher("/usv_perception/lidar_detector/markers", MarkerArray, queue_size=10) + + def ins_pose_callback(self,pose): + self.ned_x = pose.x + self.ned_y = pose.y + self.yaw = pose.theta + + def simulate(self): + ''' + @name: simulate + @brief: Simulates the obstacles the USV should be looking at any moment in time + @param: -- + @return: -- + ''' + object_detected_list = obj_detected_list() + list_length = 0 + for i in range(len(self.obstacle_list)): + x = self.obstacle_list[i]['X'] + y = self.obstacle_list[i]['Y'] + delta_x = x - self.ned_x + delta_y = y - self.ned_y + distance = math.pow(delta_x*delta_x + delta_y*delta_y, 0.5) + if (distance < self.max_visible_radius): + x, y = self.ned_to_body(x, y) + #if x > 1 or self.challenge == 3: + if x != 0: + obstacle = obj_detected() + obstacle.X = x - self.sensor_to_usv_offset + obstacle.Y = -y + obstacle.R = self.obstacle_list[i]['R'] + obstacle.color = self.obstacle_list[i]['color'] + obstacle.clase = self.obstacle_list[i]['class'] + list_length += 1 + object_detected_list.objects.append(obstacle) + object_detected_list.len = list_length + self.detector_pub.publish(object_detected_list) + + + def body_to_ned(self, x2, y2): + ''' + @name: body_to_ned + @brief: Coordinate transformation between body and NED reference frames. + @param: x2: target x coordinate in body reference frame + y2: target y coordinate in body reference frame + @return: ned_x2: target x coordinate in ned reference frame + ned_y2: target y coordinate in ned reference frame + ''' + p = np.array([x2, y2]) + J = self.rotation_matrix(self.yaw) + n = J.dot(p) + ned_x2 = n[0] + self.ned_x + ned_y2 = n[1] + self.ned_y + return (ned_x2, ned_y2) + + def ned_to_body(self, ned_x2, ned_y2): + ''' + @name: ned_to_ned + @brief: Coordinate transformation between NED and body reference frames. + @param: ned_x2: target x coordinate in ned reference frame + ned_y2: target y coordinate in ned reference frame + @return: body_x2: target x coordinate in body reference frame + body_y2: target y coordinate in body reference frame + ''' + n = np.array([ned_x2 - self.ned_x, ned_y2 - self.ned_y]) + J = self.rotation_matrix(self.yaw) + J = np.linalg.inv(J) + b = J.dot(n) + body_x2 = b[0] + body_y2 = b[1] + return (body_x2, body_y2) + + def rotation_matrix(self, angle): + ''' + @name: rotation_matrix + @brief: Transformation matrix template. + @param: angle: angle of rotation + @return: J: transformation matrix + ''' + J = np.array([[math.cos(angle), -1*math.sin(angle)], + [math.sin(angle), math.cos(angle)]]) + return (J) + + def rviz_markers(self): + ''' + @name: rviz_markers + @brief: Publishes obstacles as rviz markers. + @param: -- + @return: -- + ''' + marker_array = MarkerArray() + for i in range(len(self.obstacle_list)): + if self.obstacle_list[i]['class'] == 'buoy': + x = self.obstacle_list[i]['X'] + y = -self.obstacle_list[i]['Y'] + diameter = 2*self.obstacle_list[i]['R'] + marker = Marker() + marker.header.frame_id = "/world" + marker.type = marker.SPHERE + marker.action = marker.ADD + if self.obstacle_list[i]['color'] == 'yellow': + marker.color.a = 1.0 + marker.color.r = 1.0 + marker.color.g = 1.0 + marker.color.b = 0.0 + elif self.obstacle_list[i]['color'] == 'red': + marker.color.a = 1.0 + marker.color.r = 1.0 + marker.color.g = 0.0 + marker.color.b = 0.0 + elif self.obstacle_list[i]['color'] == 'green': + marker.color.a = 1.0 + marker.color.r = 0.0 + marker.color.g = 1.0 + marker.color.b = 0.0 + elif self.obstacle_list[i]['color'] == 'blue': + marker.color.a = 1.0 + marker.color.r = 0.0 + marker.color.g = 0.0 + marker.color.b = 1.0 + marker.scale.x = diameter + marker.scale.y = diameter + marker.scale.z = diameter + marker.pose.orientation.w = 1.0 + marker.pose.position.x = x + marker.pose.position.y = y + marker.pose.position.z = 0 + if self.challenge == 0: + marker.type = marker.CYLINDER + marker.scale.z = 1 + marker.pose.position.z = 0.5 + marker.id = i + marker_array.markers.append(marker) + elif self.obstacle_list[i]['class'] == 'marker': + x = self.obstacle_list[i]['X'] + y = -self.obstacle_list[i]['Y'] + diameter = 2*self.obstacle_list[i]['R'] + marker = Marker() + marker.header.frame_id = "/world" + marker.type = marker.CYLINDER + marker.action = marker.ADD + marker.color.a = 1.0 + marker.color.r = 0.0 + marker.color.g = 0.0 + marker.color.b = 1.0 + marker.scale.x = diameter + marker.scale.y = diameter + marker.scale.z = 1.54 + marker.pose.orientation.w = 1.0 + marker.pose.position.x = x + marker.pose.position.y = y + marker.pose.position.z = 0.77 + marker.id = i + marker_array.markers.append(marker) + # Publish the MarkerArray + self.marker_pub.publish(marker_array) + +def main(): + rospy.init_node('mpc_obstacle_simulator', anonymous=False) + rate = rospy.Rate(20) # 20hz + obstacleSimulator = ObstacleSimulator() + if obstacleSimulator.challenge == 0: + obstacleSimulator.obstacle_list.append({'X' : 3.8, + 'Y' : -15, + 'R' : 0.127, + 'color' : 'yellow', + 'class' : 'buoy'}) + elif obstacleSimulator.challenge == 1: + obstacleSimulator.obstacle_list.append({'X' : 3.6, + 'Y' : -12, + 'R' : 0.105, + 'color' : 'green', + 'class' : 'buoy'}) + obstacleSimulator.obstacle_list.append({'X' : 4.2, + 'Y' : -15, + 'R' : 0.105, + 'color' : 'red', + 'class' : 'buoy'}) + obstacleSimulator.obstacle_list.append({'X' : 3.6, + 'Y' : -18, + 'R' : 0.105, + 'color' : 'yellow', + 'class' : 'buoy'}) + obstacleSimulator.obstacle_list.append({'X' : 3.7, + 'Y' : -20, + 'R' : 0.105, + 'color' : 'green', + 'class' : 'buoy'}) + obstacleSimulator.obstacle_list.append({'X' : 4.1, + 'Y' : -23, + 'R' : 0.105, + 'color' : 'yellow', + 'class' : 'buoy'}) + obstacleSimulator.obstacle_list.append({'X' : 3.1, + 'Y' : -25, + 'R' : 0.105, + 'color' : 'yellow', + 'class' : 'buoy'}) + elif obstacleSimulator.challenge == 2: + obstacleSimulator.obstacle_list.append({'X' : 2.5, + 'Y' : -2, + 'R' : 0.105, + 'color' : 'green', + 'class' : 'buoy'}) + obstacleSimulator.obstacle_list.append({'X' : 2.4, + 'Y' : -10, + 'R' : 0.105, + 'color' : 'red', + 'class' : 'buoy'}) + obstacleSimulator.obstacle_list.append({'X' : -0.5, + 'Y' : -10.5, + 'R' : 0.105, + 'color' : 'yellow', + 'class' : 'buoy'}) + obstacleSimulator.obstacle_list.append({'X' : -1.3, + 'Y' : -15, + 'R' : 0.105, + 'color' : 'green', + 'class' : 'buoy'}) + obstacleSimulator.obstacle_list.append({'X' : 0, + 'Y' : -25, + 'R' : 0.105, + 'color' : 'yellow', + 'class' : 'buoy'}) + obstacleSimulator.obstacle_list.append({'X' : 0.2, + 'Y' : -28, + 'R' : 0.105, + 'color' : 'yellow', + 'class' : 'buoy'}) + elif obstacleSimulator.challenge == 3: + obstacleSimulator.obstacle_list.append({'X' : -0.5, + 'Y' : -6, + 'R' : 0.105, + 'color' : 'green', + 'class' : 'buoy'}) + obstacleSimulator.obstacle_list.append({'X' : 0.3, + 'Y' : -7, + 'R' : 0.105, + 'color' : 'red', + 'class' : 'buoy'}) + obstacleSimulator.obstacle_list.append({'X' : 0.9, + 'Y' : -8, + 'R' : 0.105, + 'color' : 'yellow', + 'class' : 'buoy'}) + obstacleSimulator.obstacle_list.append({'X' : 0.2, + 'Y' : -9, + 'R' : 0.105, + 'color' : 'green', + 'class' : 'buoy'}) + obstacleSimulator.obstacle_list.append({'X' : 1.8, + 'Y' : -10, + 'R' : 0.105, + 'color' : 'yellow', + 'class' : 'buoy'}) + obstacleSimulator.obstacle_list.append({'X' : 0, + 'Y' : -10.2, + 'R' : 0.105, + 'color' : 'yellow', + 'class' : 'buoy'}) + elif obstacleSimulator.challenge == 4: + obstacleSimulator.obstacle_list.append({'X' : 3.0, + 'Y' : -8.0, + 'R' : 0.105, + 'color' : 'green', + 'class' : 'buoy'}) + obstacleSimulator.obstacle_list.append({'X' : 3.0, + 'Y' : -12.0, + 'R' : 0.105, + 'color' : 'red', + 'class' : 'buoy'}) + obstacleSimulator.obstacle_list.append({'X' : 4.0, + 'Y' : -10.0, + 'R' : 0.105, + 'color' : 'yellow', + 'class' : 'buoy'}) + obstacleSimulator.obstacle_list.append({'X' : 2.0, + 'Y' : -10.0, + 'R' : 0.105, + 'color' : 'green', + 'class' : 'buoy'}) + + while not rospy.is_shutdown() and obstacleSimulator.active: + obstacleSimulator.simulate() + obstacleSimulator.rviz_markers() + rate.sleep() + rospy.spin() + +if __name__ == "__main__": + try: + main() + except rospy.ROSInterruptException: + pass diff --git a/usv_research/scripts/straight_path_waypoints.py b/usv_research/scripts/straight_path_waypoints.py new file mode 100755 index 00000000..9d166a8b --- /dev/null +++ b/usv_research/scripts/straight_path_waypoints.py @@ -0,0 +1,34 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +import os +import time +import rospy +from geometry_msgs.msg import Pose2D +from std_msgs.msg import Float64, Float32MultiArray + +class Test: + def __init__(self): + self.path_pub = rospy.Publisher("/mission/waypoints", Float32MultiArray, queue_size=10) + + def desired(self, path): + self.path_pub.publish(path) + +def main(): + rospy.init_node('straight_path_waypoints', anonymous=False) + rate = rospy.Rate(100) + t = Test() + time.sleep(10) + rospy.logwarn("Start") + path_array = Float32MultiArray() + path_array.layout.data_offset = 5 + path_array.data = [3.0,0,3.0,-30.0,0] # Last should be waypoint mode: 0 for NED, 1 for GPS, 2 for body + while not rospy.is_shutdown(): + t.desired(path_array) + rate.sleep() + +if __name__ == "__main__": + try: + main() + except rospy.ROSInterruptException: + pass diff --git a/usv_research/scripts/uturn_path_waypoints.py b/usv_research/scripts/uturn_path_waypoints.py new file mode 100755 index 00000000..fadcbf37 --- /dev/null +++ b/usv_research/scripts/uturn_path_waypoints.py @@ -0,0 +1,33 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +import os +import time +import rospy +from geometry_msgs.msg import Pose2D +from std_msgs.msg import Float64, Float32MultiArray + +class Test: + def __init__(self): + self.path_pub = rospy.Publisher("/mission/waypoints", Float32MultiArray, queue_size=10) + + def desired(self, path): + self.path_pub.publish(path) + +def main(): + rospy.init_node('uturn_path_waypoints', anonymous=False) + rate = rospy.Rate(100) + t = Test() + time.sleep(5) + path_array = Float32MultiArray() + path_array.layout.data_offset = 9 + path_array.data = [0,-1,0,-15,4,-15,4,-1,0] # Last should be waypoint mode: 0 for NED, 1 for GPS, 2 for body + while not rospy.is_shutdown(): + t.desired(path_array) + rate.sleep() + +if __name__ == "__main__": + try: + main() + except rospy.ROSInterruptException: + pass \ No newline at end of file diff --git a/usv_research/scripts/zigzag_path_waypoints.py b/usv_research/scripts/zigzag_path_waypoints.py new file mode 100755 index 00000000..f2abf580 --- /dev/null +++ b/usv_research/scripts/zigzag_path_waypoints.py @@ -0,0 +1,33 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +import os +import time +import rospy +from geometry_msgs.msg import Pose2D +from std_msgs.msg import Float64, Float32MultiArray + +class Test: + def __init__(self): + self.path_pub = rospy.Publisher("/mission/waypoints", Float32MultiArray, queue_size=10) + + def desired(self, path): + self.path_pub.publish(path) + +def main(): + rospy.init_node('zigzag_path_waypoints', anonymous=False) + rate = rospy.Rate(100) + t = Test() + time.sleep(10) + path_array = Float32MultiArray() + path_array.layout.data_offset = 11 + path_array.data = [-1,-1,3.5,-6,-1,-15,2,-23,-1.5,-30,0] # Last should be waypoint mode: 0 for NED, 1 for GPS, 2 for body + while not rospy.is_shutdown(): + t.desired(path_array) + rate.sleep() + +if __name__ == "__main__": + try: + main() + except rospy.ROSInterruptException: + pass