Feature/pid rewrite

create_container_intel.bash

@@ -32,7 +32,7 @@ $DOCKER_COMMAND -it -d\
     --privileged \
     -v /dev/bus/usb/:/dev/bus/usb \
     -v /dev:/dev \
-    -v "$PWD/docker_ws:/home/ws/src" \
+    -v "$PWD/workspace:/home/ws" \
     -v "/media/saveasmtz/pacman/rosbags:/home/ws/src/tests" \
     -v /var/run/docker.sock:/var/run/docker.sock \
     -v /run/dbus:/run/dbus:ro \

workspace/src/sdv_control/CMakeLists.txt

@@ -40,13 +40,25 @@ set(DEPENDENCIES
 add_subdirectory(libs/vanttec_controllers)
 ament_python_install_package(scripts)
 
-add_executable(vel_pid_node src/control/regulators/first_order/vel_pid_node.cpp)
-target_include_directories(vel_pid_node PUBLIC
+add_library(tf2_lib SHARED 
+    ${CMAKE_CURRENT_SOURCE_DIR}/src/simulation/sdc1_broadcaster.cpp
+    ${CMAKE_CURRENT_SOURCE_DIR}/src/simulation/tf2_6dof_broadcaster_ros2.cpp
+)
+ament_target_dependencies(tf2_lib ${DEPENDENCIES})
+
+install(TARGETS tf2_lib
+  LIBRARY DESTINATION lib
+  ARCHIVE DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+add_executable(sdc1_vel_pid_node src/control/regulators/first_order/sdc1_vel_pid_node.cpp)
+target_include_directories(sdc1_vel_pid_node PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include> libs/vanttec_controllers)
-target_compile_features(vel_pid_node PUBLIC c_std_99 cxx_std_17)
-ament_target_dependencies(vel_pid_node ${DEPENDENCIES})
-target_link_libraries(vel_pid_node vanttec_controllers)
+target_compile_features(sdc1_vel_pid_node PUBLIC c_std_99 cxx_std_17)
+ament_target_dependencies(sdc1_vel_pid_node ${DEPENDENCIES})
+target_link_libraries(sdc1_vel_pid_node vanttec_controllers)
 
 add_executable(stanley_controller_node src/control/guidance_laws/stanley_controller_node.cpp)
 target_include_directories(stanley_controller_node PUBLIC
@@ -56,35 +68,50 @@ target_compile_features(stanley_controller_node PUBLIC c_std_99 cxx_std_17)
 ament_target_dependencies(stanley_controller_node ${DEPENDENCIES})
 target_link_libraries(stanley_controller_node vanttec_controllers)
 
-add_executable(sdv_kinematic_sim src/model/sdv_kinematic_sim_node.cpp)
-target_include_directories(sdv_kinematic_sim PUBLIC
-  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-  $<INSTALL_INTERFACE:include> libs/vanttec_controllers)
-target_compile_features(sdv_kinematic_sim PUBLIC c_std_99 cxx_std_17)
-ament_target_dependencies(sdv_kinematic_sim ${DEPENDENCIES})
-target_link_libraries(sdv_kinematic_sim vanttec_controllers)
+# add_executable(sdv_kinematic_sim src/model/sdv_kinematic_sim_node.cpp)
+# target_include_directories(sdv_kinematic_sim PUBLIC
+#   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+#   $<INSTALL_INTERFACE:include> libs/vanttec_controllers)
+# target_compile_features(sdv_kinematic_sim PUBLIC c_std_99 cxx_std_17)
+# ament_target_dependencies(sdv_kinematic_sim ${DEPENDENCIES})
+# target_link_libraries(sdv_kinematic_sim vanttec_controllers)
 
-add_executable(velocity_regulator_node src/control/regulators/first_order/velocity_regulator_node.cpp)
-target_include_directories(velocity_regulator_node PUBLIC
-  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-  $<INSTALL_INTERFACE:include> libs/vanttec_controllers)
-target_compile_features(velocity_regulator_node PUBLIC c_std_99 cxx_std_17)
-ament_target_dependencies(velocity_regulator_node ${DEPENDENCIES})
-target_link_libraries(velocity_regulator_node vanttec_controllers)
+# add_executable(velocity_regulator_node src/control/regulators/first_order/velocity_regulator_node.cpp)
+# target_include_directories(velocity_regulator_node PUBLIC
+#   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+#   $<INSTALL_INTERFACE:include> libs/vanttec_controllers)
+# target_compile_features(velocity_regulator_node PUBLIC c_std_99 cxx_std_17)
+# ament_target_dependencies(velocity_regulator_node ${DEPENDENCIES})
+# target_link_libraries(velocity_regulator_node vanttec_controllers)
+
+add_executable(car_tf2_broadcast_node src/model/car_tf2_broadcast_node.cpp)
+# Include directories for project headers and other package headers
+target_include_directories(car_tf2_broadcast_node
+            PUBLIC
+            # $<BUILD_INTERFACE:${VANTTEC_CONTROLLER_PATHS}>
+            $<BUILD_INTERFACE:${sdv_msgs_INCLUDE_DIRS}>
+            $<INSTALL_INTERFACE:include>
+)
+target_compile_features(car_tf2_broadcast_node PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
+ament_target_dependencies(car_tf2_broadcast_node ${DEPENDENCIES})
+target_link_libraries(car_tf2_broadcast_node vanttec_controllers tf2_lib)
+set_target_properties(car_tf2_broadcast_node PROPERTIES
+  RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/lib
+)
 
 install(TARGETS 
-  vel_pid_node
+  sdc1_vel_pid_node
   stanley_controller_node
-  sdv_kinematic_sim
-  velocity_regulator_node
+  # sdv_kinematic_sim
+  # velocity_regulator_node
+  car_tf2_broadcast_node
   DESTINATION lib/${PROJECT_NAME})
 
 install(DIRECTORY 
   config 
   launch
   rviz
   DESTINATION share/${PROJECT_NAME})
-
 # Install Python executables
 install(PROGRAMS
   scripts/path_publisher_node.py

workspace/src/sdv_control/config/car_params.yaml

@@ -2,28 +2,28 @@
   ros__parameters:
     frequency: 100
     init_pose: [0.0, 0.0, 0.0] # x, y, psi
-    init_pose: [-63.0, -99.0, 3.1415] # x, y, psi
+    # init_pose: [-63.0, -99.0, 3.1415] # x, y, psi
     parent_frame: "odom"
     child_frame: "base_link"
 
 # PID Controller for the VTec SDV
 vel_pid_node:
   ros__parameters:
-    Kp: 1.0
-    Ki: 0.01
-    Kd: 0.2
-    U_MAX: 1.0
-    U_MIN: -1.0
+    Kp: 10.
+    Ki: 0.0
+    Kd: 10.
+    U_MAX: 180.0
+    U_MIN: -180.0
     enable_ramp: true
-    ramp_rate: 0.3
+    ramp_rate: 0.1
 
 # First Order Feedback Linearization 1-DOF PID Controller for the VTec SDC1
 sdc1_vel_pid_node:
   ros__parameters:
     Kp: 1.0
     Ki: 0.01
     Kd: 0.2 #0.01
-    D_MAX: 180
+    D_MAX: 90
 
 # First Order Feedback Linearization 1-DOF ASMC Controller for the VTec SDC1
 sdc1_vel_asmc_node:

workspace/src/sdv_control/launch/control_launch.py

@@ -16,7 +16,7 @@ def generate_launch_description():
 
    is_sim = DeclareLaunchArgument(
       'is_simulation',
-      default_value = 'false',
+      default_value = 'true',
       description = 'Defines if the application will run in simulation or in real life'
    )
 
@@ -165,16 +165,16 @@ def generate_launch_description():
          msg="Running in real robot mode."
       ),
 
-      # rviz,
+      rviz,
       # aitsmc_node,
       # asmc_node,
       pid_node,
-      # tf2_node,
-      # sdv_description_launch,
-      # sdv_loc_launch,
+      tf2_node,
+      sdv_description_launch,
+      sdv_loc_launch,
 
       # sdv_can_launch,
 
-      waypoint_handler,
-      car_guidance_node,
+      # waypoint_handler,
+      # car_guidance_node,
    ])

workspace/src/sdv_control/launch/dynamic_model_sim_launch.py

@@ -0,0 +1,49 @@
+import os
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+
+from launch.actions import IncludeLaunchDescription, LogInfo, DeclareLaunchArgument
+from launch.substitutions import PathJoinSubstitution, LaunchConfiguration
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+from launch_ros.substitutions import FindPackageShare
+
+from launch.substitutions import FindExecutable
+from launch.actions import ExecuteProcess
+import os
+
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+from ament_index_python.packages import get_package_share_path
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.actions import IncludeLaunchDescription, LogInfo
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import PathJoinSubstitution
+from launch.conditions import IfCondition, UnlessCondition
+from launch.substitutions import Command, LaunchConfiguration
+
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+def generate_launch_description():
+   dynamic_model = Node(
+      package="sdv_control",
+      executable="sdv_dynamic_sim_node",
+   )
+
+   rviz = IncludeLaunchDescription(
+      PythonLaunchDescriptionSource([
+         PathJoinSubstitution([
+               FindPackageShare('sdv_description'),
+               'launch',
+               'rviz_launch.py'
+         ])
+      ]),
+   )
+
+   return LaunchDescription([
+      rviz,
+      dynamic_model,
+   ])

workspace/src/sdv_control/launch/longitudinal_control_sim_launch.py

@@ -0,0 +1,57 @@
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription, LogInfo, DeclareLaunchArgument
+from launch.substitutions import PathJoinSubstitution, LaunchConfiguration
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+from launch.conditions import IfCondition, UnlessCondition
+
+def generate_launch_description():
+
+   car_params = os.path.join(
+      get_package_share_directory('sdv_control'),
+      'config',
+      'car_params.yaml'
+   )
+
+   pid_node = Node(
+      package='sdv_control',
+      executable='vel_pid_node',
+      parameters=[car_params],
+      output='screen',
+      emulate_tty=True,
+      arguments=[('__log_level:=debug')],
+   )
+
+   foxglove_bridge = Node(
+      name="foxglove_bridge",
+      package="foxglove_bridge",
+      executable="foxglove_bridge")
+
+   can_node = Node(
+      package='sdv_can',
+      executable='sdv_can_node',
+   )
+
+   dynamic_model_sim_launch = IncludeLaunchDescription(
+      PythonLaunchDescriptionSource([
+            PathJoinSubstitution([
+               FindPackageShare('sdv_control'),
+               'launch',
+               'dynamic_model_sim_launch.py'
+            ])
+      ])
+   )
+
+   return LaunchDescription([
+      pid_node,
+      foxglove_bridge,
+      # can_node,
+      dynamic_model_sim_launch,
+   ])

workspace/src/sdv_control/package.xml

@@ -7,6 +7,7 @@
   <maintainer email="aa@mail.com">root</maintainer>
   <license>TODO: License declaration</license>
 
+  <buildtool_depend>ament_cmake</buildtool_depend>
   <buildtool_depend>ament_cmake_ros</buildtool_depend>
   <buildtool_depend>ament_cmake_python</buildtool_depend>
 
@@ -23,6 +24,12 @@
   <depend>rclcpp</depend>
   <depend>rclpy</depend>
 
+  <exec_depend>launch</exec_depend>
+  <exec_depend>launch_ros</exec_depend>
+  <exec_depend>visualization_msgs</exec_depend>
+  <exec_depend>tf2_geometry_msgs</exec_depend>
+  <exec_depend>ros2launch</exec_depend>
+
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>