Launch files

.vscode/settings.json

@@ -0,0 +1,140 @@
+{
+    "python.autoComplete.extraPaths": [
+        "/home/milosz/autoware/install/velodyne_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/static_centerline_optimizer/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/planning_validator/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/operation_mode_transition_manager/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/control_performance_analysis/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/tswr_awsim/lib/python3.10/site-packages",
+        "/home/milosz/autoware/install/tier4_vehicle_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/tier4_v2x_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/tier4_api_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/tier4_system_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/tier4_simulation_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/tier4_rtc_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/planning_debug_tools/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/tier4_planning_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/dummy_perception_publisher/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/tier4_perception_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/tier4_localization_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/tier4_hmi_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/autoware_external_api_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/tier4_external_api_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/tier4_debug_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/tier4_control_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/build/simulator_compatibility_test",
+        "/home/milosz/autoware/install/simulator_compatibility_test/lib/python3.10/site-packages",
+        "/home/milosz/autoware/install/morai_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/autoware_sensing_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/autoware_planning_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/autoware_perception_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/autoware_map_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/autoware_localization_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/autoware_control_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/autoware_common_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/autoware_auto_vehicle_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/autoware_auto_system_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/autoware_auto_planning_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/autoware_auto_perception_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/autoware_auto_mapping_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/autoware_auto_geometry_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/TSwR/lib/python3.10/site-packages",
+        "/home/milosz/autoware/install/autoware_auto_control_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/autoware_adapi_version_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/build/ad_api_visualizers",
+        "/home/milosz/autoware/install/ad_api_visualizers/lib/python3.10/site-packages",
+        "/home/milosz/autoware/install/autoware_adapi_v1_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/ros2_ws/install/ur_robot_driver/local/lib/python3.10/dist-packages",
+        "/home/milosz/ros2_ws/install/ur_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/ros2_ws/install/ur_moveit_config/local/lib/python3.10/dist-packages",
+        "/home/milosz/ros2_ws/install/ur_dashboard_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/ros2_ws/install/rqt_joint_trajectory_controller/lib/python3.10/site-packages",
+        "/home/milosz/ros2_ws/install/rqt_controller_manager/lib/python3.10/site-packages",
+        "/home/milosz/ros2_ws/install/ros_object_detection/lib/python3.10/site-packages",
+        "/home/milosz/ros2_ws/install/ros_object_detection_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/ros2_ws/install/ros2controlcli/lib/python3.10/site-packages",
+        "/home/milosz/ros2_ws/install/ros2_controllers_test_nodes/lib/python3.10/site-packages",
+        "/home/milosz/ros2_ws/install/controller_manager/local/lib/python3.10/dist-packages",
+        "/home/milosz/ros2_ws/install/pcl_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/ros2_ws/install/octomap_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/ros2_ws/install/octomap_manipulation/lib/python3.10/site-packages",
+        "/home/milosz/ros2_ws/install/mapr_6_student/lib/python3.10/site-packages",
+        "/home/milosz/ros2_ws/install/mapr_5_student/lib/python3.10/site-packages",
+        "/home/milosz/ros2_ws/install/mapr_4_student/lib/python3.10/site-packages",
+        "/home/milosz/ros2_ws/install/map_manipulation/lib/python3.10/site-packages",
+        "/home/milosz/ros2_ws/install/interface_tutorial/local/lib/python3.10/dist-packages",
+        "/home/milosz/ros2_ws/install/controller_manager_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/ros2_ws/install/control_msgs/local/lib/python3.10/dist-packages",
+        "/opt/ros/humble/lib/python3.10/site-packages",
+        "/opt/ros/humble/local/lib/python3.10/dist-packages"
+    ],
+    "python.analysis.extraPaths": [
+        "/home/milosz/autoware/install/velodyne_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/static_centerline_optimizer/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/planning_validator/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/operation_mode_transition_manager/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/control_performance_analysis/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/tswr_awsim/lib/python3.10/site-packages",
+        "/home/milosz/autoware/install/tier4_vehicle_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/tier4_v2x_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/tier4_api_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/tier4_system_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/tier4_simulation_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/tier4_rtc_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/planning_debug_tools/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/tier4_planning_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/dummy_perception_publisher/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/tier4_perception_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/tier4_localization_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/tier4_hmi_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/autoware_external_api_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/tier4_external_api_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/tier4_debug_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/tier4_control_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/build/simulator_compatibility_test",
+        "/home/milosz/autoware/install/simulator_compatibility_test/lib/python3.10/site-packages",
+        "/home/milosz/autoware/install/morai_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/autoware_sensing_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/autoware_planning_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/autoware_perception_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/autoware_map_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/autoware_localization_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/autoware_control_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/autoware_common_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/autoware_auto_vehicle_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/autoware_auto_system_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/autoware_auto_planning_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/autoware_auto_perception_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/autoware_auto_mapping_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/autoware_auto_geometry_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/TSwR/lib/python3.10/site-packages",
+        "/home/milosz/autoware/install/autoware_auto_control_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/install/autoware_adapi_version_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/autoware/build/ad_api_visualizers",
+        "/home/milosz/autoware/install/ad_api_visualizers/lib/python3.10/site-packages",
+        "/home/milosz/autoware/install/autoware_adapi_v1_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/ros2_ws/install/ur_robot_driver/local/lib/python3.10/dist-packages",
+        "/home/milosz/ros2_ws/install/ur_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/ros2_ws/install/ur_moveit_config/local/lib/python3.10/dist-packages",
+        "/home/milosz/ros2_ws/install/ur_dashboard_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/ros2_ws/install/rqt_joint_trajectory_controller/lib/python3.10/site-packages",
+        "/home/milosz/ros2_ws/install/rqt_controller_manager/lib/python3.10/site-packages",
+        "/home/milosz/ros2_ws/install/ros_object_detection/lib/python3.10/site-packages",
+        "/home/milosz/ros2_ws/install/ros_object_detection_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/ros2_ws/install/ros2controlcli/lib/python3.10/site-packages",
+        "/home/milosz/ros2_ws/install/ros2_controllers_test_nodes/lib/python3.10/site-packages",
+        "/home/milosz/ros2_ws/install/controller_manager/local/lib/python3.10/dist-packages",
+        "/home/milosz/ros2_ws/install/pcl_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/ros2_ws/install/octomap_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/ros2_ws/install/octomap_manipulation/lib/python3.10/site-packages",
+        "/home/milosz/ros2_ws/install/mapr_6_student/lib/python3.10/site-packages",
+        "/home/milosz/ros2_ws/install/mapr_5_student/lib/python3.10/site-packages",
+        "/home/milosz/ros2_ws/install/mapr_4_student/lib/python3.10/site-packages",
+        "/home/milosz/ros2_ws/install/map_manipulation/lib/python3.10/site-packages",
+        "/home/milosz/ros2_ws/install/interface_tutorial/local/lib/python3.10/dist-packages",
+        "/home/milosz/ros2_ws/install/controller_manager_msgs/local/lib/python3.10/dist-packages",
+        "/home/milosz/ros2_ws/install/control_msgs/local/lib/python3.10/dist-packages",
+        "/opt/ros/humble/lib/python3.10/site-packages",
+        "/opt/ros/humble/local/lib/python3.10/dist-packages"
+    ]
+}

launch/iLQR_launch.py

@@ -0,0 +1,19 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    ld = LaunchDescription()
+
+    path_publisher = Node(
+        package="tswr_awsim",
+        executable="path_publisher"
+    )
+
+    controller = Node(
+        package="tswr_awsim",
+        executable="iLQR_controller"
+    )
+
+    ld.add_action(path_publisher)
+    ld.add_action(controller)
+    return ld

launch/lin_MPC_launch.py

@@ -0,0 +1,19 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    ld = LaunchDescription()
+
+    path_publisher = Node(
+        package="tswr_awsim",
+        executable="path_publisher"
+    )
+
+    lin_MPC_controller = Node(
+        package="tswr_awsim",
+        executable="lin_MPC_controller"
+    )
+
+    ld.add_action(path_publisher)
+    ld.add_action(lin_MPC_controller)
+    return ld

launch/pure_pursuit_launch.py

@@ -0,0 +1,19 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    ld = LaunchDescription()
+
+    path_publisher = Node(
+        package="tswr_awsim",
+        executable="path_publisher"
+    )
+
+    controller = Node(
+        package="tswr_awsim",
+        executable="pure_pursuit_controller"
+    )
+
+    ld.add_action(path_publisher)
+    ld.add_action(controller)
+    return ld

launch/stanley_launch.py

@@ -0,0 +1,19 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    ld = LaunchDescription()
+
+    path_publisher = Node(
+        package="tswr_awsim",
+        executable="path_publisher"
+    )
+
+    controller = Node(
+        package="tswr_awsim",
+        executable="stanley_controller"
+    )
+
+    ld.add_action(path_publisher)
+    ld.add_action(controller)
+    return ld

setup.py

@@ -1,5 +1,6 @@
 from setuptools import setup
-
+import os
+from glob import glob
 package_name = 'tswr_awsim'
 
 setup(
@@ -11,6 +12,7 @@
             ['resource/' + package_name]),
         ('share/' + package_name, ['package.xml']),
         ('share/' + package_name, ['resource/path.csv']),
+        (os.path.join('share', package_name), glob('launch/*.py')),
     ],
     install_requires=['setuptools'],
     zip_safe=True,

tswr_awsim/lin_MPC_controller.py

@@ -2,6 +2,33 @@
 from rclpy.node import Node
 from autoware_auto_control_msgs.msg import AckermannControlCommand 
 from geometry_msgs.msg import PoseStamped 
+from nav_msgs.msg import Path
+import math
+
+def quat2eulers(q0:float, q1:float, q2:float, q3:float) -> tuple:
+    """
+    Compute yaw-pitch-roll Euler angles from a quaternion.
+    @author: michaelwro
+    Args
+    ----
+        q0: Scalar component of quaternion. [qw]
+        q1, q2, q3: Vector components of quaternion. [qx, qy, qz]
+    
+    Returns
+    -------
+        (roll, pitch, yaw) (tuple): 321 Euler angles in radians
+    """
+    roll = math.atan2(
+        2 * ((q2 * q3) + (q0 * q1)),
+        q0**2 - q1**2 - q2**2 + q3**2
+    )  # radians
+    pitch = math.asin(2 * ((q1 * q3) - (q0 * q2)))
+    yaw = math.atan2(
+        2 * ((q1 * q2) + (q0 * q3)),
+        q0**2 + q1**2 - q2**2 - q3**2
+    )
+    return (roll, pitch, yaw)
+
 
 class linMPCNode(Node):
 
@@ -24,11 +51,9 @@ def __init__(self):
         # 
         # Reference trajectory
         # 
-        self.reference_trajectory_subscription = self.create_subscription(PoseStamped,'/control/reference_trajectory', 
-                                                                          self.reference_trajectory_listener_callback, 10) #TODO
+        self.reference_trajectory_subscription = self.create_subscription(Path, '/path_points', self.reference_trajectory_listener_callback, 10)
         # Position
-        self.ref_x = None #TODO
-        self.ref_y = None #TODO
+        self.ref_path = None
         # 
         # Control publisher
         # 
@@ -57,8 +82,16 @@ def current_pose_listener_callback(self, msg:PoseStamped):
         self.curr_qy = msg.pose.orientation.y
         self.curr_qz = msg.pose.orientation.z
 
-    def reference_trajectory_listener_callback(self, msg):
-        pass #TODO
+    def reference_trajectory_listener_callback(self, msg:Path):
+        self.ref_path = []
+        for pose in msg.poses:
+            x = pose.pose.position.x
+            y = pose.pose.position.y
+            qx = pose.pose.orientation.x
+            qy = pose.pose.orientation.y
+            qz = pose.pose.orientation.z
+            qw = pose.pose.orientation.w
+            self.ref_path.append([x, y, qx, qy, qz, qw])
 
     def publish_control(self, theta, accel):
         acc = AckermannControlCommand()
@@ -77,9 +110,11 @@ def control_timer_callback(self):
         # 
         # Calculate control
         # 
-        self.theta = 1.0
-        self.acceleration = 1.0
-        # TODO
+        if (self.ref_path is not None) and (self.curr_x is not None):
+            # TO IMPLEMENT
+            self.theta = 1.0
+            self.acceleration = 1.0
+            # TODO
 
 
 def main(args=None):

tswr_awsim/path_pub.py

@@ -56,7 +56,7 @@ def listener_callback(self, msg):
         delta_y = float(self.path[self.i % self.path_points][1]) - msg.pose.position.y
         distance = np.sqrt(delta_x ** 2 + delta_y ** 2)
 
-        if distance <= 0.1:
+        if distance <= 0.2:
             self.i += 1
 
         self.get_logger().info("Point nr: %f, distance: %f"%(self.i, distance))