Merge branch 'main' into drive-controller-rework

f1tenth_racecar_bringup/config/vehicle_params.yaml

@@ -2,4 +2,4 @@ vehicle_length: 0.5 # meters
 vehicle_width: 0.27 # meters
 base_link_to_center_dist: 0.165 # meters
 wheelbase: 0.33 # meters
-max_steering_angle: 0.4189 # radians
+max_steering_angle: 0.4698 # radians

f1tenth_racecar_bringup/config/vesc_ackermann.yaml

@@ -1,4 +1,4 @@
-speed_to_erpm_gain: 4614
+speed_to_erpm_gain: 7800
 speed_to_erpm_offset: 0
-steering_angle_to_servo_gain: -1.2135
-steering_angle_to_servo_offset: 0.5
+steering_angle_to_servo_gain: -1.0643
+steering_angle_to_servo_offset: 0.51

f1tenth_racecar_bringup/config/vesc_driver.yaml

@@ -9,5 +9,5 @@ position_max: 0.0
 position_min: 0.0
 servo_max: 1.0
 servo_min: 0.0
-speed_max: 23250.0
-speed_min: -23250.0
+speed_max: 200000.0
+speed_min: -200000.0

f1tenth_racecar_bringup/config/vesc_to_odom.yaml

@@ -1,4 +1,4 @@
 odom_frame: odom
 base_frame: base_link
 publish_tf: true
-use_servo_cmd_to_calc_angular_velocity: true
+use_servo_cmd_to_calc_angular_velocity: true

local_planner/src/lattice.cpp

@@ -117,8 +117,14 @@ Lattice::Generator::Generator(const int num_layers, const double layer_spacing,
 
   collision_checker_ptr_ = collision_checker_ptr;
 }
+
 Lattice Lattice::Generator::generate(const geometry_msgs::Pose& source_pose) const
 {
+  if (global_path_ == nullptr || global_path_->poses.empty())
+  {
+    throw std::runtime_error("Global path has not been set");
+  }
+
   std::vector<geometry_msgs::Pose> reference_poses = getReferencePoses(source_pose);
   PositionMap map(reference_poses.size());
 
@@ -180,11 +186,6 @@ Lattice Lattice::Generator::generate(const geometry_msgs::Pose& source_pose) con
 
 int Lattice::Generator::getNearestWaypointId(const geometry_msgs::Pose& current_pose) const
 {
-  if (global_path_->poses.empty())
-  {
-    throw std::runtime_error("Global path has not been set");
-  }
-
   int nearest_wp_id = -1;
   double nearest_wp_dist = std::numeric_limits<double>::max();
 

local_planner/src/local_planner.cpp

@@ -23,24 +23,22 @@
 LocalPlanner::LocalPlanner()
 {
   ros::NodeHandle private_nh("~");
-  initCallbacks(private_nh);
   initPlanner(private_nh);
-
-  /* --------------------------- Dynamic Reconfigure -------------------------- */
-
-  f_ = boost::bind(&LocalPlanner::configCallback, this, _1, _2);
-  server_.setCallback(f_);
+  initCallbacks(private_nh);
 }
 
 void LocalPlanner::initCallbacks(const ros::NodeHandle& private_nh)
 {
+  trajectory_pub_ = nh_.advertise<f1tenth_msgs::Trajectory>("local_trajectory", 1);
+  viz_pub_ = nh_.advertise<visualization_msgs::MarkerArray>("visualization/local_planner", 1);
+
   global_path_sub_ = nh_.subscribe("global_path", 1, &LocalPlanner::globalPathCallback, this);
   odom_sub_ = nh_.subscribe("odom", 1, &LocalPlanner::odomCallback, this);
   drive_sub_ = nh_.subscribe("drive_feedback", 1, &LocalPlanner::driveCallback, this);
   timer_ = nh_.createTimer(ros::Duration(0.1), &LocalPlanner::timerCallback, this);
 
-  trajectory_pub_ = nh_.advertise<f1tenth_msgs::Trajectory>("local_trajectory", 1);
-  viz_pub_ = nh_.advertise<visualization_msgs::MarkerArray>("visualization/local_planner", 1);
+  f_ = boost::bind(&LocalPlanner::configCallback, this, _1, _2);
+  server_.setCallback(f_);
 }
 
 void LocalPlanner::initPlanner(const ros::NodeHandle& private_nh)
@@ -96,6 +94,7 @@ void LocalPlanner::initPlanner(const ros::NodeHandle& private_nh)
   lat_gen_ptr_ = std::make_shared<Lattice::Generator>(lattice_num_layers, lattice_layer_spacing,
                                                       lattice_num_lateral_samples_per_side, lattice_lateral_spacing,
                                                       max_curvature, lattice_k_length, collision_checker_ptr_);
+
   acc_regulator_ptr_ =
       std::make_shared<AccelerationRegulator>(ref_max_speed, ref_max_lat_acc, ref_max_lon_acc, ref_max_lon_dec);