few tweaks to planner and controller

src/machines/roscube_machine/launch/machine.launch.py

@@ -57,10 +57,10 @@ def generate_launch_description():
  return LaunchDescription(
  [
  scs_container,
- Node(
- package="rosboard",
- executable="rosboard_node",
- ),
+ # Node(
+ #  package="rosboard",
+ #  executable="rosboard_node",
+ # ),
  Node(
  package="driverless_common",
  executable="display",
@@ -69,10 +69,10 @@ def generate_launch_description():
  package="lidar_pipeline",
  executable="lidar_detector_node",
  ),
- Node(
- package="mission_controller",
- executable="mission_control_node",
- ),
+ # Node(
+ #  package="mission_controller",
+ #  executable="mission_launcher_node",
+ # ),
  IncludeLaunchDescription(
  launch_description_source=PythonLaunchDescriptionSource(
  launch_file_path=str(get_package_share_path("sensors") / "launch" / "vlp32.launch.py")

src/navigation/nav_bringup/config/nav2_params.yaml

@@ -198,7 +198,7 @@ controller_server:
  # Controller parameters
  RegulatedPurePursuit:
  plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"
- desired_linear_vel: 5.0 # The desired maximum linear velocity to use.
+ desired_linear_vel: 4.0 # The desired maximum linear velocity to use.
  lookahead_dist: 4.0 # The lookahead distance to use to find the lookahead point.
  # min_lookahead_dist: 3.0 # The minimum lookahead distance threshold when using velocity scaled lookahead distances.
  # max_lookahead_dist: 5.0 # The maximum lookahead distance threshold when using velocity scaled lookahead distances.

src/navigation/planners/planners/node_ft_planner.py

@@ -180,7 +180,7 @@ def __init__(self):
 
  # sub to track for all cone locations relative to car start point
  self.create_subscription(ConeDetectionStamped, "/slam/global_map", self.map_callback, QOS_LATEST)
- self.create_timer(1 / 20, self.planning_callback)
+ self.create_timer(1 / 10, self.planning_callback)
 
  self.tf_buffer = Buffer()
  self.tf_listener = TransformListener(self.tf_buffer, self)
@@ -206,6 +206,22 @@ def __init__(self):
 
  self.path_planner = PathPlanner(**self.get_planner_cfg())
 
+
+ # skip if we haven't completed a lap yet
+ (
+ path,
+ ordered_blues,
+ ordered_yellows,
+ virt_blues,
+ virt_yellows,
+ _,
+ _,
+ ) = self.path_planner.calculate_path_in_global_frame(
+ pre_track, np.array([0.0,0.0]), 0.0, return_intermediate_results=True
+ )
+ self.initialised = True
+ self.get_logger().info("Initialised planner calcs")
+
  self.get_logger().info("---Ordered path planner node initalised---")
 
  def get_planner_cfg(self):
@@ -253,7 +269,7 @@ def get_planner_cfg(self):
  }
 
  # cone matching
- self.declare_parameter("min_track_width", 3.5)
+ self.declare_parameter("min_track_width", 4.0)
  self.declare_parameter("max_search_range", 5)
  self.declare_parameter("max_search_angle", 50)
  self.declare_parameter("matches_should_be_monotonic", True)
@@ -278,39 +294,6 @@ def map_callback(self, track_msg: ConeDetectionStamped):
  self.current_track = track_msg
 
  def planning_callback(self):
- if not self.initialised:
- (
- path,
- ordered_blues,
- ordered_yellows,
- virt_blues,
- virt_yellows,
- _,
- _,
- ) = self.path_planner.calculate_path_in_global_frame(
- pre_track, np.array([0.0, 0.0]), 0.0, return_intermediate_results=True
- )
- self.initialised = True
- self.get_logger().info("Initialised planner calcs")
- return
-
- # skip if we haven't completed a lap yet
- if not self.initialised:
- (
- path,
- ordered_blues,
- ordered_yellows,
- virt_blues,
- virt_yellows,
- _,
- _,
- ) = self.path_planner.calculate_path_in_global_frame(
- pre_track, np.array([0.0,0.0]), 0.0, return_intermediate_results=True
- )
- self.initialised = True
- self.get_logger().info("Initialised planner calcs")
- return
-
  if self.current_track is None or len(self.current_track.cones) == 0:
  self.get_logger().warn("No track data received", throttle_duration_sec=1)
  return