work

workspace/src/common/launch/art_control_launch/launch/convoy_leader.launch.py

@@ -7,144 +7,87 @@
 from launch_ros.substitutions import FindPackageShare
 
 def generate_launch_description():
-    # Declare the launch arguments
     leader_ns = LaunchConfiguration('leader_ns')
     robot_ns = LaunchConfiguration('robot_ns')
     veh_config = LaunchConfiguration('veh_config_file')
-
-    # ---------------
-    # Vehicle Config
-    # ---------------
     package_share_directory = FindPackageShare('art_sensing_launch').find('art_sensing_launch')
     veh_config_file_path = PathJoinSubstitution([package_share_directory, 'config', veh_config])
 
-    return LaunchDescription([
-        DeclareLaunchArgument(
-            'leader_ns',
-            default_value='',
-            description='Namespace for leader vehicle'
+    common_nodes = [
+        Node(
+            package='convoy_controller',
+            executable='velocity',
+            name='velocity_controller',
+            namespace=robot_ns,
+            remappings=[
+                ('/leader/odometry/filtered', PythonExpression(['"', '/', leader_ns, "/odometry/filtered", '"'])),
+                ('/follower/odometry/filtered', PythonExpression(['"', '/', robot_ns, "/odometry/filtered", '"'])),
+                ('/odometry/filtered', PythonExpression(['"', '/', robot_ns, "/odometry/filtered", '"'])),
+                ('/driver_inputs', PythonExpression(['"', '/', robot_ns, "/input/long_input", '"']))
+            ],
+            parameters=[veh_config_file_path, { 'leader_ns': PythonExpression(['"', leader_ns, '"']) }],
+            output='screen'
         ),
-        DeclareLaunchArgument(
-            'robot_ns',
-            default_value='',
-            description='Namespace for robot vehicle'
+        Node(
+            package='convoy_controller',
+            executable='vehicle_traj',
+            name='vehicle_trajectory_publisher',
+            namespace=robot_ns,
+            remappings=[
+                ('/odometry/filtered', PythonExpression(['"', '/', robot_ns, "/odometry/filtered", '"'])),
+                ('/vehicle_traj', PythonExpression(['"', '/', robot_ns, "/vehicle_traj", '"']))
+            ],
+            parameters=[veh_config_file_path],
+            output='screen'
         ),
-        DeclareLaunchArgument(
-            'veh_config_file',
-            default_value='default_veh_config.yaml',
-            description='Vehicle configuration file'
+        Node(
+            package='convoy_controller',
+            executable='control_mux',
+            name='combined_input_node',
+            namespace=robot_ns,
+            remappings=[
+                ('/lateral_input', PythonExpression(['"', '/', robot_ns, "/input/lateral_input", '"'])),
+                ('/long_input', PythonExpression(['"', '/', robot_ns, "/input/long_input", '"'])),
+                ('/input/driver_inputs', PythonExpression(['"', '/', robot_ns, "/input/driver_inputs", '"']))
+            ],
+            parameters=[veh_config_file_path],
+            output='screen'
         ),
+        Node(
+            package='convoy_controller',
+            executable='follower',
+            name='follower_control_node',
+            namespace=robot_ns,
+            remappings=[
+                ('/leader_vehicle/odometry/filtered', PythonExpression(['"', '/', leader_ns, "/odometry/filtered", '"'])),
+                ('/follower_vehicle/odometry/filtered', PythonExpression(['"', '/', robot_ns, "/odometry/filtered", '"'])),
+                ('/follower_vehicle/driver_inputs', PythonExpression(['"', '/', robot_ns, "/input/lateral_input", '"'])),
+                ('/leader_vehicle/vehicle_traj', PythonExpression(['"', '/', leader_ns, "/vehicle_traj", '"'])),
+                ('/follower_vehicle/vehicle_traj', PythonExpression(['"', '/', robot_ns, "/vehicle_traj", '"']))
+            ],
+            parameters=[veh_config_file_path],
+            output='screen'
+        )
+
+    ]
+
+    follower_nodes = [
+    ]
+
+    return LaunchDescription([
+        DeclareLaunchArgument('leader_ns', default_value='', description='Namespace for leader vehicle'),
+        DeclareLaunchArgument('robot_ns', default_value='', description='Namespace for robot vehicle'),
+        DeclareLaunchArgument('veh_config_file', default_value='default_veh_config.yaml', description='Vehicle configuration file'),
 
-        # Conditionally launch the leader vehicle nodes
         GroupAction(
-            condition=IfCondition(PythonExpression([
-                "'", leader_ns, "'",
-                " != 'none'",
-            ])),
-            actions=[
-                Node(
-                    package='convoy_controller',
-                    executable='follower',  
-                    name='follower_control_node',
-                    namespace=robot_ns,
-                    remappings=[
-                        ('/leader_vehicle/odometry/filtered', PythonExpression(['"', '/', leader_ns, "/odometry/filtered", '"'])),
-                        ('/follower_vehicle/odometry/filtered', PythonExpression(['"', '/', robot_ns, "/odometry/filtered", '"'])),
-                        ('/follower_vehicle/driver_inputs', PythonExpression(['"', '/', robot_ns, "/input/lateral_input", '"'])),
-                        ('/leader_vehicle/vehicle_traj', PythonExpression(['"', '/', leader_ns, "/vehicle_traj", '"'])),
-                        ('/follower_vehicle/vehicle_traj', PythonExpression(['"', '/', robot_ns, "/vehicle_traj", '"']))
-                    ],
-                    parameters=[veh_config_file_path],
-                    output='screen'
-                ),
-                Node(
-                    package='convoy_controller',  
-                    executable='velocity',  
-                    name='velocity_controller',
-                    namespace=robot_ns,
-                    remappings=[
-                        ('/odometry/filtered', PythonExpression(['"', '/', robot_ns, "/odometry/filtered", '"'])),
-                        ('/driver_inputs', PythonExpression(['"', '/', robot_ns, "/input/long_input", '"']))
-                    ],
-                    parameters=[veh_config_file_path],
-                    output='screen'
-                ),
-                Node(
-                    package='convoy_controller',  
-                    executable='vehicle_traj',  
-                    name='vehicle_trajectory_publisher',
-                    namespace=robot_ns,
-                    remappings=[
-                        ('/odometry/filtered', PythonExpression(['"', '/', robot_ns, "/odometry/filtered", '"'])),
-                        ('/vehicle_traj', PythonExpression(['"', '/', robot_ns, "/vehicle_traj", '"']))
-                    ],
-                    parameters=[veh_config_file_path],
-                    output='screen'
-                ),
-                Node(
-                    package='convoy_controller',  
-                    executable='control_mux',  
-                    name='combined_input_node',
-                    namespace=robot_ns,
-                    remappings=[
-                        ('/lateral_input', PythonExpression(['"', '/', robot_ns, "/input/lateral_input", '"'])),
-                        ('/long_input', PythonExpression(['"', '/', robot_ns, "/input/long_input", '"'])),
-                        ('/input/driver_inputs', PythonExpression(['"', '/', robot_ns, "/input/driver_inputs", '"']))
-                    ],
-                    parameters=[veh_config_file_path],
-                    output='screen'
-                )
-            ]
+            condition=IfCondition(PythonExpression(["'", leader_ns, "' != 'none'"])),
+            actions=follower_nodes + common_nodes
         ),
-
-        # Conditionally launch the follower vehicle nodes
         GroupAction(
-            condition=IfCondition(PythonExpression([
-                "'", leader_ns, "'",
-                " == 'none'",
-            ])),
-            actions=[
-                Node(
-                    package='convoy_controller',  
-                    executable='velocity',  
-                    name='velocity_controller',
-                    namespace=robot_ns,
-                    remappings=[
-                        ('/odometry/filtered', PythonExpression(['"', '/', robot_ns, "/odometry/filtered", '"'])),
-                        ('/driver_inputs', PythonExpression(['"', '/', robot_ns, "/input/long_input", '"']))
-                    ],
-                    parameters=[veh_config_file_path],
-                    output='screen'
-                ),
-                Node(
-                    package='convoy_controller',  
-                    executable='vehicle_traj',  
-                    name='vehicle_trajectory_publisher',
-                    namespace=robot_ns,
-                    remappings=[
-                        ('/odometry/filtered', PythonExpression(['"', '/', robot_ns, "/odometry/filtered", '"'])),
-                        ('/vehicle_traj', PythonExpression(['"', '/', robot_ns, "/vehicle_traj", '"']))
-                    ],
-                    parameters=[veh_config_file_path],
-                    output='screen'
-                ),
-                Node(
-                    package='convoy_controller',  
-                    executable='control_mux',  
-                    name='combined_input_node',
-                    namespace=robot_ns,
-                    remappings=[
-                        ('/lateral_input', PythonExpression(['"', '/', robot_ns, "/input/lateral_input", '"'])),
-                        ('/long_input', PythonExpression(['"', '/', robot_ns, "/input/long_input", '"'])),
-                        ('/input/driver_inputs', PythonExpression(['"', '/', robot_ns, "/input/driver_inputs", '"']))
-                    ],
-                    parameters=[veh_config_file_path],
-                    output='screen'
-                )
-            ]
+            condition=IfCondition(PythonExpression(["'", leader_ns, "' == 'none'"])),
+            actions=common_nodes
         )
     ])
 
 if __name__ == '__main__':
     generate_launch_description()
-

workspace/src/common/launch/art_launch/launch/art.launch.py

@@ -141,7 +141,7 @@ def generate_launch_description():
     IncludeLaunchDescriptionWithCondition(
         ld, "art_localization_launch", "art_localization"
     )
-    ##IncludeLaunchDescriptionWithCondition(ld, "art_planning_launch", "art_planning")
+    IncludeLaunchDescriptionWithCondition(ld, "art_planning_launch", "art_planning")
     IncludeLaunchDescriptionWithCondition(ld, "art_control_launch", "art_control")  
     IncludeLaunchDescriptionWithCondition(ld, "art_sensing_launch", "art_sensing")
     IncludeLaunchDescriptionWithCondition(ld, "art_vehicle_launch", "art_vehicle") 

workspace/src/common/launch/art_planning_launch/launch/art_planning.launch.py

@@ -14,9 +14,12 @@ def generate_launch_description():
     # Launch Includes
     # ---------------
     IncludeLaunchDescriptionWithCondition(
-        ld, "art_planning_launch", "centerline_objects_path_planner"
-    )
-    IncludeLaunchDescriptionWithCondition(
-        ld, "art_planning_launch", "waypoints_path_planner"
+        ld, "art_planning_launch", "spline_path_planner"
     )
+    # IncludeLaunchDescriptionWithCondition(
+    #     ld, "art_planning_launch", "centerline_objects_path_planner"
+    # )
+    # IncludeLaunchDescriptionWithCondition(
+    #     ld, "art_planning_launch", "waypoints_path_planner"
+    # )
     return ld

workspace/src/common/launch/art_planning_launch/launch/spline_path_planner.launch.py

@@ -0,0 +1,76 @@
+#
+# BSD 3-Clause License
+#
+# Copyright (c) 2022 University of Wisconsin - Madison
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# * Redistributions of source code must retain the above copyright notice, this
+#   list of conditions and the following disclaimer.
+#
+# * 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.
+#
+# * Neither the name of the copyright holder nor the names of its
+#   contributors may be used to endorse or promote products derived from
+#   this software without specific prior written permission.
+#
+# 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.#
+
+# ros imports
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument, GroupAction
+from launch.conditions import IfCondition
+
+# internal imports
+from launch_utils import AddLaunchArgument, GetLaunchArgument
+from launch.substitutions import LaunchConfiguration, PythonExpression, PathJoinSubstitution
+
+
+def generate_launch_description():
+    ld = LaunchDescription()
+
+    # ----------------
+    # Launch Arguments
+    # ----------------
+    robot_ns = LaunchConfiguration('robot_ns')
+    leader_ns = LaunchConfiguration('leader_ns')
+
+    # -----
+    # Nodes
+    # -----
+
+    node = [Node(
+        package="spline_path_planner",
+        executable="bcurve_planner",
+        name="spline_planner",
+        remappings=[
+            ('/odometry/filtered', PythonExpression(['"', '/', robot_ns, "/odometry/filtered", '"'])),
+        ],
+        parameters=[
+        ],
+    )]
+
+    return LaunchDescription([
+        GroupAction(
+            condition=IfCondition(PythonExpression(["'", leader_ns, "' == 'none'"])),
+            actions=node
+        )
+    ])
+
+    # ld.add_action(node)
+
+    # return ld

workspace/src/common/launch/art_sensing_launch/config/sim.yaml

@@ -1,20 +1,24 @@
-/artcar_1/navsat_transform_node:
-  ros__parameters:
-    magnetic_declination_radians: 0.0
-    yaw_offset: 0.0
-    zero_altitude: True
-    use_odometry_yaw: True
-    wait_for_datum: False
-    publish_filtered_gps: True
-    broadcast_cartesian_transform: False
-
-/artcar_2/navsat_transform_node:
-  ros__parameters:
-    magnetic_declination_radians: 0.0
-    yaw_offset: 0.0
-    zero_altitude: True
-    use_odometry_yaw: True
-    wait_for_datum: False
-    publish_filtered_gps: True
-    broadcast_cartesian_transform: False
-
+/**:
+  navsat_transform_node:
+    ros__parameters:
+      magnetic_declination_radians: 0.0
+      yaw_offset: 0.0
+      zero_altitude: True
+      use_odometry_yaw: True
+      wait_for_datum: False 
+      publish_filtered_gps: True
+      broadcast_cartesian_transform: False
+  velocity_controller:
+    ros__parameters:
+      velocity_kp: 2.0
+      velocity_kd: 0.0
+      velocity_ki: 0.0
+      velocity_error_integral_limit: 1.0
+      target_velocity: 0.08
+  follower_control_node:
+    ros__parameters:
+      steering_kp: 4.3
+      steering_ki: 0.1 
+      steering_kd: -0.85
+      steering_cte_integral_limit: 5.0
+      predefined_path: True

workspace/src/common/meta/art_common_meta/package.xml

@@ -18,6 +18,7 @@
   <exec_depend>waypoints_path_planner</exec_depend>
   <exec_depend>pid_lateral_controller</exec_depend>
   <exec_depend>centerline_objects_path_planner</exec_depend>
+  <exec_depend>spline_path_planner</exec_depend>
   <exec_depend>pure_pursuit_follower</exec_depend>
   <exec_depend>convoy_controller</exec_depend>
   <exec_depend>navsat_util</exec_depend>

workspace/src/control/convoy_controller/convoy_controller/control_mux.py

@@ -9,7 +9,7 @@ def __init__(self):
 
         self.lateral_input = 0.0
         self.long_input = 0.0
-
+        self.braking = 0.0
         qos_profile = QoSProfile(
             reliability=QoSReliabilityPolicy.BEST_EFFORT,
             history=QoSHistoryPolicy.KEEP_LAST,
@@ -43,10 +43,12 @@ def lateral_input_callback(self, msg):
 
     def long_input_callback(self, msg):
         self.long_input = msg.throttle
+        self.braking = msg.braking
 
     def publish_combined_input(self):
         combined_msg = VehicleInput()
         combined_msg.steering = self.lateral_input
+        combined_msg.braking = self.braking
         combined_msg.throttle = self.long_input
         self.pub_combined_input.publish(combined_msg)
         #self.get_logger().info(f'Published combined input: Steering {self.lateral_input}, Throttle {self.long_input}.')