navigation fixes. sim parameters, correct node names

src/control/pure_pursuit_cpp/CMakeLists.txt

@@ -26,27 +26,27 @@ install(TARGETS pure_pursuit
         DESTINATION lib/${PROJECT_NAME})
 
 # Executable for node_pure_pursuit
-add_executable(node_pure_pursuit src/node_pure_pursuit.cpp)
-ament_target_dependencies(node_pure_pursuit ${Dependencies})
-target_include_directories(node_pure_pursuit PUBLIC
+add_executable(pure_pursuit_node src/node_pure_pursuit.cpp)
+ament_target_dependencies(pure_pursuit_node ${Dependencies})
+target_include_directories(pure_pursuit_node PUBLIC
         ${CMAKE_SOURCE_DIR}/../../common/driverless_common/include)
-target_include_directories(node_pure_pursuit PUBLIC
+target_include_directories(pure_pursuit_node PUBLIC
         $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
         $<INSTALL_INTERFACE:include>)
-target_link_libraries(node_pure_pursuit pure_pursuit)  # Link with the pure_pursuit library
-install(TARGETS node_pure_pursuit
+target_link_libraries(pure_pursuit_node pure_pursuit)  # Link with the pure_pursuit library
+install(TARGETS pure_pursuit_node
         DESTINATION lib/${PROJECT_NAME})
 
 # Executable for lifecycle_pure_pursuit
-add_executable(lifecycle_pure_pursuit src/lifecycle_pure_pursuit.cpp)
-ament_target_dependencies(lifecycle_pure_pursuit ${Dependencies})
-target_include_directories(lifecycle_pure_pursuit PUBLIC
+add_executable(pure_pursuit_lifecycle src/lifecycle_pure_pursuit.cpp)
+ament_target_dependencies(pure_pursuit_lifecycle ${Dependencies})
+target_include_directories(pure_pursuit_lifecycle PUBLIC
         ${CMAKE_SOURCE_DIR}/../../common/driverless_common/include)
-target_include_directories(lifecycle_pure_pursuit PUBLIC
+target_include_directories(pure_pursuit_lifecycle PUBLIC
         $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
         $<INSTALL_INTERFACE:include>)
-target_link_libraries(lifecycle_pure_pursuit pure_pursuit)  # Link with the pure_pursuit library
-install(TARGETS lifecycle_pure_pursuit
+target_link_libraries(pure_pursuit_lifecycle pure_pursuit)  # Link with the pure_pursuit library
+install(TARGETS pure_pursuit_lifecycle
         DESTINATION lib/${PROJECT_NAME})
 
 # config folder

src/control/pure_pursuit_cpp/config/pure_pursuit_cpp.yaml

@@ -1,9 +1,9 @@
 pure_pursuit_cpp_node:
   ros__parameters:
-    Kp_ang: -3.0
+    Kp_ang: -3.5
     lookahead: 2.5
-    vel_max: 3.0
-    vel_min: 2.0
+    vel_max: 4.0
+    vel_min: 3.0
     discovery_lookahead: 3.0
-    discovery_vel_max: 3.0
-    discovery_vel_min: 2.0
+    discovery_vel_max: 4.0
+    discovery_vel_min: 3.0

src/navigation/nav_bringup/config/localisation_params.yaml

@@ -1,6 +1,6 @@
 odom_filter_node:
   ros__parameters:
-    use_sim_time: true
+    use_sim_time: false
     # 1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system.
     #     1a. If your system does not have a map_frame, just remove it, and make sure "world_frame" is set to the value of odom_frame.
     # 2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set "world_frame"

src/navigation/nav_bringup/config/slam_toolbox_params.yaml

@@ -1,6 +1,6 @@
 slam_toolbox_node:
   ros__parameters:
-    use_sim_time: true
+    use_sim_time: false
 
     # Solver Params
     solver_plugin: solver_plugins::CeresSolver

src/navigation/planners/planners/node_boundary_interpolation.py

@@ -18,9 +18,9 @@
 from typing import List, Tuple
 
 # for colour gradient based on intensity
-SEARCH_RANGE = 5
+SEARCH_RANGE = 6
 SEARCH_ANGLE = pi / 3
-SEARCH_RANGE_LENIANCE = 0.1
+SEARCH_RANGE_LENIANCE = 0.3
 SEARCH_ANGLE_MIN_LENIANCE = 15 * pi / 180  # 10 degrees
 
 

src/operations/mission_controller/launch/trackdrive.launch.py

@@ -27,7 +27,7 @@ def generate_launch_description():
             ),
             Node(
                 package="pure_pursuit_cpp",
-                executable="pure_pursuit_cpp_node",
+                executable="pure_pursuit_node",
                 parameters=[
                     get_package_share_path("pure_pursuit_cpp") / "config" / "pure_pursuit_cpp.yaml",
                 ],

src/operations/mission_controller/mission_controller/node_trackdrive_handler.py

@@ -27,17 +27,16 @@ class TrackdriveHandler(ShutdownNode):
     goal_offet = 5.0
     process = None
 
-    sim = True
-
     def __init__(self):
         super().__init__("trackdrive_logic_node")
 
         self.declare_parameter("start_following", False)
+        self.declare_parameter("sim", False)
 
         self.create_subscription(State, "system/as_status", self.state_callback, 1)
-        self.create_subscription(Odometry, "odometry/sbg_ekf", self.odom_callback, 1)
+        self.create_subscription(Odometry, "imu/odometry", self.odom_callback, 1)
 
-        if not self.sim:
+        if not self.get_parameter("sim").value:
             # reset odom and pose from camera
             self.reset_odom_client = self.create_client(Trigger, "zed2i/zed_node/reset_odometry")
             self.reset_pose_client = self.create_client(Trigger, "zed2i/zed_node/reset_pos_tracking")
@@ -75,7 +74,7 @@ def state_callback(self, msg: State):
             self.last_lap_time = time.time()
             self.lap_trig_pub.publish(UInt8(data=0))
 
-            if not self.sim:
+            if not self.get_parameter("sim").value:
                 # reset odom and pose from camera
                 self.reset_odom_client.call_async(Trigger.Request())
                 self.reset_pose_client.call_async(Trigger.Request())