Merge pull request #315 from QUT-Motorsport/track-dev-2-electric-boog…

…aloo

Track dev 2 electric boogaloo

src/common/driverless_common/driverless_common/shutdown_node.py

@@ -1,3 +1,6 @@
+import signal
+from subprocess import Popen
+
 from rclpy.node import Node
 
 from driverless_msgs.msg import State
@@ -11,15 +14,15 @@ class ShutdownNode(Node):
  def __init__(self, node_name: str, **kwargs) -> None:
  super().__init__(node_name, **kwargs)
  self.reset_sub = self.create_subscription(State, "/system/as_status", self.state_callback, QOS_LATEST)
-
- def set_process(self, process):
- self.process = process
+ self.process: Popen = None
 
  def state_callback(self, msg: State):
  if msg.state in [State.START, State.SELECT_MISSION, State.ACTIVATE_EBS, State.FINISHED, State.EMERGENCY]:
  if self.process is not None:
- self.process.terminate()
- self.get_logger().error("Terminated process")
+ self.process.send_signal(signal.SIGINT)
+ self.get_logger().error("Interrupted process")
+
  self.process = None
  self.get_logger().error("Exit Node - Shutdown Triggered")
  self.destroy_node()
+ exit(1)

src/control/controllers/controllers/node_vel_to_ackermann.py

@@ -0,0 +1,54 @@
+from math import atan, pi
+
+import rclpy
+from rclpy.node import Node
+
+from ackermann_msgs.msg import AckermannDriveStamped
+from geometry_msgs.msg import Twist
+
+
+def convert_trans_rot_vel_to_steering_angle(vel, omega, wheelbase):
+ if omega == 0 or vel == 0:
+ return 0.0
+
+ radius = vel / omega
+ return atan(wheelbase / radius) * (180 / pi) * 5
+
+
+class Vel2Ackermann(Node):
+ wheelbase = 1.5 # taken from sim config - measured on car
+
+ def __init__(self):
+ super().__init__("nav_cmd_translator")
+
+ self.create_subscription(Twist, "/control/nav_cmd_vel", self.cmd_callback, 1)
+
+ self.drive_pub = self.create_publisher(AckermannDriveStamped, "/control/driving_command", 1)
+
+ self.get_logger().info("---Nav2 control interpreter initalised---")
+
+ def cmd_callback(self, twist_msg: Twist):
+ vel = twist_msg.linear.x
+ steering = convert_trans_rot_vel_to_steering_angle(
+ vel,
+ twist_msg.angular.z,
+ self.wheelbase,
+ )
+
+ msg = AckermannDriveStamped()
+ # make time for msg id
+ # msg.header.stamp =
+ msg.header.frame_id = "base_footprint"
+ msg.drive.steering_angle = steering
+ msg.drive.speed = vel
+
+ self.drive_pub.publish(msg)
+
+
+def main(args=None):
+ # begin ros node
+ rclpy.init(args=args)
+ node = Vel2Ackermann()
+ rclpy.spin(node)
+ node.destroy_node()
+ rclpy.shutdown()

src/control/controllers/setup.py

@@ -25,6 +25,7 @@
  "point_fitting = controllers.node_point_fitting:main",
  "vector_spline = controllers.node_vector_spline:main",
  "point_fitting2 = controllers.node_point_fitting2:main",
+ "vel_to_ackermann_node = controllers.node_vel_to_ackermann:main",
  ],
  },
 )

src/control/pure_pursuit_cpp/config/pure_pursuit_cpp.yaml

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

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

@@ -16,8 +16,8 @@ def generate_launch_description():
  composable_node_descriptions=[
  ComposableNode(
  package="vehicle_supervisor",
- plugin="vehicle_supervisor::ASSupervisorLaunch",
- name="vehicle_supervisor_launch_node",
+ plugin="vehicle_supervisor::ASSupervisor",
+ name="vehicle_supervisor_node",
  parameters=[
  {"manual_override": False},
  ],
@@ -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,6 +69,10 @@ def generate_launch_description():
  package="lidar_pipeline",
  executable="lidar_detector_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/map_creation/include/map_creation/node_cone_placement.hpp

@@ -84,11 +84,13 @@ struct TrackedCone {
  colour = driverless_msgs::msg::Cone::YELLOW;
  } else if (blue_count > yellow_count && blue_count > orange_count) {
  colour = driverless_msgs::msg::Cone::BLUE;
+ } else if (orange_count > yellow_count && orange_count > blue_count) {
+ colour = driverless_msgs::msg::Cone::ORANGE_SMALL;
  }
 
- if (orange_count > 10 || orange_count > yellow_count || orange_count > blue_count) {
- colour = driverless_msgs::msg::Cone::ORANGE_BIG;
- }
+ // if (orange_count > 10 || orange_count > yellow_count || orange_count > blue_count) {
+ //  colour = driverless_msgs::msg::Cone::ORANGE_BIG;
+ // }
  }
 
  // transform map to car

src/navigation/map_creation/src/node_cone_placement.cpp

@@ -37,7 +37,8 @@ ConeAssociation::ConeAssociation() : Node("cone_placement_node") {
 
 void ConeAssociation::state_callback(const driverless_msgs::msg::State::SharedPtr msg) {
  // we haven't started driving yet
- if (msg->state == driverless_msgs::msg::State::DRIVING && msg->lap_count == 0) {
+ // if (msg->state == driverless_msgs::msg::State::DRIVING && msg->lap_count == 0) {
+ if (msg->lap_count == 0) {
  mapping = true;
  }
 
@@ -50,6 +51,7 @@ void ConeAssociation::state_callback(const driverless_msgs::msg::State::SharedPt
 
 void ConeAssociation::callback(const driverless_msgs::msg::ConeDetectionStamped::SharedPtr msg) {
  if (!mapping) {
+ RCLCPP_INFO_ONCE(get_logger(), "Not ready to map");
  return;
  }
 

src/navigation/nav_bringup/config/localisation_params.yaml

@@ -22,12 +22,12 @@ odom_filter_node:
  false, false, false, # roll_dot, pitch_dot, yaw_dot
  false, false, false] # x_ddot, y_ddot, z_ddot
 
- twist0: vehicle/wheel_twist
- twist0_config: [false, false, false, # x, y, z
- false, false, false, # roll, pitch, yaw
- true, false, false, # x_dot, y_dot, z_dot
- false, false, false, # roll_dot, pitch_dot, yaw_dot
- false, false, false] # x_ddot, y_ddot, z_ddot
+ # twist0: vehicle/wheel_twist
+ # twist0_config: [false, false, false, # x, y, z
+ #  false, false, false, # roll, pitch, yaw
+ #  true, false, false, # x_dot, y_dot, z_dot
+ #  false, false, false, # roll_dot, pitch_dot, yaw_dot
+ #  false, false, false] # x_ddot, y_ddot, z_ddot
 
  odom1: odometry/sbg_ekf
  odom1_config: [true, true, false, # x, y, z

src/navigation/nav_bringup/config/nav2_params.yaml

@@ -1,6 +1,6 @@
 /**: # stack-wide parameters
  ros__parameters:
- use_sim_time: True
+ use_sim_time: False
  global_frame: track
  local_frame: odom
  robot_base_frame: base_footprint
@@ -179,7 +179,7 @@ controller_server:
  min_x_velocity_threshold: 0.001
  min_y_velocity_threshold: 0.5
  min_theta_velocity_threshold: 0.001
- failure_tolerance: 0.3
+ failure_tolerance: 0.5
  progress_checker_plugin: "progress_checker"
  goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker"
  controller_plugins: ["RegulatedPurePursuit"]
@@ -198,8 +198,8 @@ controller_server:
  # Controller parameters
  RegulatedPurePursuit:
  plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"
- desired_linear_vel: 8.0 # The desired maximum linear velocity to use.
- lookahead_dist: 2.5 # The lookahead distance to use to find the lookahead point.
+ 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.
  lookahead_time: 1.5 # The time to project the velocity by to find the velocity scaled lookahead distance. Also known as the lookahead gain.

src/navigation/nav_bringup/launch/nav2_bringup.launch.py

@@ -45,24 +45,24 @@ def generate_launch_description():
  ("/tf", "tf"),
  ("/tf_static", "tf_static"),
  ("cmd_vel", "control/nav_cmd_vel"),
- ("plan", "/planning/global_path"),
+ ("plan", "planning/midline_path"),
+ ("map", "planning/boundary_grid"),
  ]
- # ('map', 'planning/boundary_grid')]
 
  # behaviour tree xml file location
  # uncomment the XML you want to test
  to_pose_bt_xml = os.path.join(
- get_package_share_directory("qutms_nav2"),
+ get_package_share_directory("nav_bringup"),
  "behaviour_trees",
  # 'plan_to_pose.xml')
  # 'replan_to_pose.xml')
- "plan_to_pose_and_follow.xml",
+ # "plan_to_pose_and_follow.xml",
+ #'replan_to_pose_and_follow.xml'
+ "follow_path.xml",
  )
- # 'replan_to_pose_and_follow.xml')
- # 'follow_path.xml')
 
  through_poses_bt_xml = os.path.join(
- get_package_share_directory("qutms_nav2"), "behaviour_trees", "plan_through_poses_and_follow.xml"
+ get_package_share_directory("nav_bringup"), "behaviour_trees", "plan_through_poses_and_follow.xml"
  )
 
  # Create our own temporary YAML files that include substitutions

src/navigation/planners/planners/node_ft_planner.py

@@ -173,14 +173,13 @@ class FaSTTUBeBoundaryExtractor(Node):
  following = False
  current_track = None
  spline_const = 10 # number of points per cone
- initialised = False
 
  def __init__(self):
  super().__init__("ft_planner_node")
 
  # 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)
@@ -196,8 +195,8 @@ def __init__(self):
  depth=1,
  durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
  )
- self.map_pub = self.create_publisher(OccupancyGrid, "/map", map_qos)
- self.map_meta_pub = self.create_publisher(MapMetaData, "/map_metadata", map_qos)
+ self.map_pub = self.create_publisher(OccupancyGrid, "/planning/boundary_grid", map_qos)
+ self.map_meta_pub = self.create_publisher(MapMetaData, "/planning/boundary_grid_metadata", map_qos)
 
  self.declare_parameter("start_following", False)
  if self.get_parameter("start_following").value:
@@ -206,6 +205,11 @@ def __init__(self):
 
  self.path_planner = PathPlanner(**self.get_planner_cfg())
 
+ init_plan_calcs = self.path_planner.calculate_path_in_global_frame(
+ pre_track, np.array([0.0, 0.0]), 0.0, return_intermediate_results=True
+ )
+ self.get_logger().info("Initialised planner calcs")
+
  self.get_logger().info("---Ordered path planner node initalised---")
 
  def get_planner_cfg(self):
@@ -216,8 +220,8 @@ def get_planner_cfg(self):
  self.declare_parameter("max_dist", 6.5)
  self.declare_parameter("max_dist_to_first", 6.0)
  self.declare_parameter("max_length", 12)
- self.declare_parameter("threshold_directional_angle", 40)
- self.declare_parameter("threshold_absolute_angle", 65)
+ self.declare_parameter("threshold_directional_angle", 30)
+ self.declare_parameter("threshold_absolute_angle", 55)
  self.declare_parameter("use_unknown_cones", True)
 
  cone_sorting_kwargs = {
@@ -253,7 +257,7 @@ def get_planner_cfg(self):
  }
 
  # cone matching
- self.declare_parameter("min_track_width", 4.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,23 +282,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 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
@@ -338,17 +325,21 @@ def planning_callback(self):
 
  global_cones = [unknown_cones, yellow_cones, blue_cones, orange_small_cones, orange_big_cones]
 
- (
- path,
- ordered_blues,
- ordered_yellows,
- virt_blues,
- virt_yellows,
- _,
- _,
- ) = self.path_planner.calculate_path_in_global_frame(
- global_cones, car_position, car_direction, return_intermediate_results=True
- )
+ try:
+ (
+ path,
+ ordered_blues,
+ ordered_yellows,
+ virt_blues,
+ virt_yellows,
+ _,
+ _,
+ ) = self.path_planner.calculate_path_in_global_frame(
+ global_cones, car_position, car_direction, return_intermediate_results=True
+ )
+ except Exception as e:
+ self.get_logger().warn("Cant plan, error" + str(e), throttle_duration_sec=1)
+ return
 
  use_virt = True
  if use_virt: