nav/sim fixes, some param changes

src/simulation/launch/control.launch.py

@@ -14,7 +14,7 @@ def generate_launch_description():
     ackermann_controller_spawner = Node(
         package='controller_manager',
         executable='spawner',
-        arguments=['rear_ackermann_controller'],
+        arguments=['front_ackermann_controller'],
         output='screen',
         remappings=[
         ("/ackermann_steering_controller/tf_odometry", "/tf"),

src/subsystems/drive/drive_controllers/src/front_ackermann_controller.cpp

@@ -223,17 +223,17 @@ controller_interface::return_type FrontAckermannController::update(
 
     // Assign angles and velocities based on the hardware's actual behavior
     if (steer_cmd > 0.0) {  // LEFT TURN: left wheel is INNER
-      front_left_steer_angle = -inner_angle;
-      front_right_steer_angle = -outer_angle;
+      front_left_steer_angle = inner_angle;
+      front_right_steer_angle = outer_angle;
 
       front_left_vel = inner_front_vel;
       front_right_vel = outer_front_vel;
       rear_left_vel = inner_rear_vel;
       rear_right_vel = outer_rear_vel;
 
     } else {  // RIGHT TURN: right wheel is INNER
-      front_left_steer_angle = outer_angle;
-      front_right_steer_angle = inner_angle;
+      front_left_steer_angle = -outer_angle;
+      front_right_steer_angle = -inner_angle;
 
       front_left_vel = outer_front_vel;
       front_right_vel = inner_front_vel;

src/subsystems/navigation/athena_planner/behavior_trees/gnss_navigation.xml

@@ -2,7 +2,7 @@
   <BehaviorTree ID="GNSSNavigation">
     <Sequence> 
       <PipelineSequence name="NavigateWithReplanning">
-        <RateController hz="5.0">
+        <RateController hz="0.2">
           <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
         </RateController>
         <FollowPath path="{path}" controller_id="FollowPath"/>

src/subsystems/navigation/athena_planner/config/nav2_params.yaml

@@ -48,15 +48,15 @@ planner_server:
     planner_plugins: ["GridBased"]
 
     GridBased:
-      plugin: "nav2_smac_planner/SmacPlannerHybrid"
-      tolerance: 0.5
+      plugin: "nav2_smac_planner/SmacPlanner2D"
+      tolerance: 2.0
       downsample_costmap: false
       downsampling_factor: 1
       allow_unknown: true
       max_iterations: 1000000
       max_planning_time: 7.5
 
-      motion_model_for_search: "REEDS_SHEPP"
+      motion_model_for_search: "DUBIN"
       angle_quantization_bins: 72
       analytic_expansion_ratio: 3.5
       analytic_expansion_max_length: 3.0
@@ -90,10 +90,10 @@ controller_server:
       movement_time_allowance: 10.0
 
     general_goal_checker:
-      stateful: true
+      stateful: false
       plugin: "nav2_controller::SimpleGoalChecker"
-      xy_goal_tolerance: 0.5
-      yaw_goal_tolerance: 0.3
+      xy_goal_tolerance: 1.0
+      yaw_goal_tolerance: 6.283
 
     FollowPath:
       plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"
@@ -114,7 +114,7 @@ controller_server:
       regulated_linear_scaling_min_radius: 1.5
       regulated_linear_scaling_min_speed: 0.25
       use_rotate_to_heading: false
-      allow_reversing: true
+      allow_reversing: false
       rotate_to_heading_min_angle: 0.785
       max_linear_accel: 1.0
       max_linear_decel: 1.5

src/subsystems/navigation/athena_planner/include/athena_planner/led_status_node.hpp

@@ -34,6 +34,7 @@ class LedStatusNode : public BT::SyncActionNode
   rclcpp::Publisher<msgs::msg::LedStatus>::SharedPtr led_pub_;
 
   std::string topic_name_;
+  std::string last_logged_color_;
 };
 
 }  // namespace bt_nodes

src/subsystems/navigation/athena_planner/launch/navigation.launch.py

@@ -32,6 +32,9 @@ def generate_launch_description():
 
     waypoint_share = get_package_share_directory('waypoint_manager')
     waypoint_launch_file = os.path.join(waypoint_share, 'launch', 'waypoint_manager.launch.py')
+
+    yolo_ros_bt_share = get_package_share_directory('yolo_ros_bt')
+    yolo_ros_launch_file = os.path.join(yolo_ros_bt_share, 'launch', 'yolo.ros.launch.py')
 
     default_params = PathJoinSubstitution([
         FindPackageShare('athena_planner'), 'config', 'nav2_params.yaml'
@@ -56,6 +59,11 @@ def generate_launch_description():
     publish_zed_odom = PythonExpression(
         ["'true' if '", use_localizer, "' == 'false' else 'false'"]
     )
+
+    cmd_vel_out_topic = PythonExpression([
+        "'/front_ackermann_controller/reference' if '", sim,
+        "' == 'true' else '/rear_ackermann_controller/reference'"
+    ])
 
     twist_stamper_node = Node(
         package='twist_stamper',
@@ -64,7 +72,7 @@ def generate_launch_description():
         parameters=[{'use_sim_time': sim}],
         remappings=[
             ('cmd_vel_in', '/cmd_vel_nav'),
-            ('cmd_vel_out', '/rear_ackermann_controller/reference'),
+            ('cmd_vel_out', cmd_vel_out_topic),
         ],
     )
 
@@ -108,6 +116,12 @@ def generate_launch_description():
         PythonLaunchDescriptionSource(gps_goal_launch_file),
         launch_arguments={'use_sim_time': sim}.items(),
     )
+
+    yolo_ros_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(yolo_ros_launch_file),
+        launch_arguments={'use_sim_time': sim}.items(),
+        condition=IfCondition(sim),
+    )   
 
     point_cloud_filterer_sim = Node(
         package='point_cloud_filterer',
@@ -180,6 +194,7 @@ def generate_launch_description():
         sensors_launch,
         aruco_launch,
         waypoint_launch,
+        yolo_ros_launch,
         point_cloud_filterer_sim,
         point_cloud_relay,
         gps_goal_launch,

src/subsystems/navigation/athena_planner/package.xml

@@ -39,6 +39,9 @@
   <exec_depend>localizer</exec_depend>
   <exec_depend>gps_goal</exec_depend>
   <exec_depend>point_cloud_filterer</exec_depend>
+  <exec_depend>aruco_bt</exec_depend>
+  <exec_depend>waypoint_manager</exec_depend>
+  <exec_depend>yolo_ros_bt</exec_depend>
   <exec_depend>topic_tools</exec_depend>
 
   <test_depend>ament_lint_auto</test_depend>

src/subsystems/navigation/athena_planner/plugins/led_status_node.cpp

@@ -1,5 +1,8 @@
 #include "athena_planner/led_status_node.hpp"
 
+#include <algorithm>
+#include <string>
+
 namespace bt_nodes
 {
 
@@ -26,19 +29,21 @@ BT::NodeStatus LedStatusNode::tick()
     return BT::NodeStatus::FAILURE;
   }
 
+  std::transform(color.begin(), color.end(), color.begin(), ::tolower);
+
   msgs::msg::LedStatus msg;
   msg.cmd = msgs::msg::LedStatus::CMD_SOLID;
   msg.r = 0;
   msg.g = 0;
   msg.b = 0;
   msg.param = 0;
 
-  if (color == "red" || color == "RED") {
+  if (color == "red") {
     msg.cmd = msgs::msg::LedStatus::CMD_SOLID;
     msg.r = 255;
     msg.g = 0;
     msg.b = 0;
-  } else if (color == "green" || color == "GREEN") {
+  } else if (color == "green") {
     msg.cmd = msgs::msg::LedStatus::CMD_FLASH;
     msg.r = 0;
     msg.g = 255;
@@ -53,13 +58,17 @@ BT::NodeStatus LedStatusNode::tick()
 
   led_pub_->publish(msg);
 
-  RCLCPP_INFO(
-    node_->get_logger(),
-    "Published LED status: color=%s rgb=[%u, %u, %u]",
-    color.c_str(),
-    msg.r,
-    msg.g,
-    msg.b);
+  if (color != last_logged_color_) {
+    RCLCPP_INFO(
+      node_->get_logger(),
+      "Published LED status: color=%s rgb=[%u, %u, %u]",
+      color.c_str(),
+      msg.r,
+      msg.g,
+      msg.b);
+
+    last_logged_color_ = color;
+  }
 
   return BT::NodeStatus::SUCCESS;
 }

src/subsystems/navigation/athena_planner/plugins/nav_selector_node.cpp

@@ -9,6 +9,7 @@
 #include "athena_planner/get_aruco_pose_node.hpp"
 #include "athena_planner/spiral_coverage_action_node.hpp"
 #include "athena_planner/object_detected_node.hpp"
+#include "athena_planner/led_status_node.hpp"
 
 #include "rclcpp/rclcpp.hpp"
 
@@ -81,4 +82,5 @@ BT_REGISTER_NODES(factory)
   factory.registerNodeType<bt_nodes::GetArucoPose>("GetArucoPose");
   factory.registerNodeType<bt_nodes::SpiralCoverageAction>("SpiralCoverageAction");
   factory.registerNodeType<bt_nodes::ObjectDetected>("ObjectDetected");
+  factory.registerNodeType<bt_nodes::LedStatusNode>("LedStatus");
 }

src/subsystems/navigation/yolo_ros_bt/launch/yolo.ros.launch.py

@@ -5,7 +5,7 @@ def generate_launch_description():
     return LaunchDescription([
         Node(
             package='yolo_ros_bt',
-            executable='yolo_ros_node',
+            executable='yolo_node',
             name='yolo_node',
             output='screen',
             parameters=[{

src/subsystems/navigation/yolo_ros_bt/setup.py

@@ -21,6 +21,10 @@
             os.path.join('share', package_name, 'launch'),
             glob('launch/*.py')
         ),
+        (
+            os.path.join('share', package_name, 'models'),
+            glob('models/*')
+        ),
     ],
     install_requires=['setuptools'],
     zip_safe=True,

src/subsystems/navigation/yolo_ros_bt/yolo_ros_bt/yolo_ros_node.py

@@ -29,7 +29,6 @@ def __init__(self) -> None:
         pkg_share = get_package_share_directory('yolo_ros_bt')
         default_model_path = os.path.join(pkg_share, 'models', 'bestNoPreProcessingStep.pt')
 
-        self.declare_parameter('use_sim_time', False)
         self.declare_parameter('conf_thres', 0.5)
         self.declare_parameter('model_path', default_model_path)
         self.declare_parameter('image_topic', '/camera/image_raw')