changes at track

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: 8.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/control/velocity_controller/config/velocity_controller.yaml

@@ -1,6 +1,6 @@
 velocity_controller_node:
   ros__parameters:
-      Kp: 0.05
+      Kp: 0.15
       Ki: 0.0
       max_integral_torque: 0.0
       histerisis_kickin_ms: 0.0

src/control/velocity_controller/include/component_velocity_controller.hpp

@@ -46,7 +46,7 @@ class VelocityController : public rclcpp::Node {
 
     float avg_velocity_;
     ackermann_msgs::msg::AckermannDriveStamped::SharedPtr target_ackermann_ = std::make_shared<ackermann_msgs::msg::AckermannDriveStamped>();
-    ackermann_msgs::msg::AckermannDriveStamped::UniquePtr prev_accel_cmd_ = std::make_unique<ackermann_msgs::msg::AckermannDriveStamped>();
+    float prev_accel_ = 0;
 
    public:
     VelocityController(const rclcpp::NodeOptions & options);

src/control/velocity_controller/src/component_velocity_controller.cpp

@@ -5,12 +5,12 @@ namespace velocity_controller {
 
 VelocityController::VelocityController(const rclcpp::NodeOptions & options) : Node("velocity_controller_node", options) {
     // PID controller parameters
-    this->declare_parameter<float>("Kp", 1.0);
+    this->declare_parameter<float>("Kp", 0.05);
     this->declare_parameter<float>("Ki", 0);
     this->declare_parameter<float>("max_integral_torque", 0);
     this->declare_parameter<float>("histerisis_kick_ms", 0);
     this->declare_parameter<float>("histerisis_reset_ms", 0);
-    this->declare_parameter<float>("min_time_to_max_accel_sec", 0);
+    this->declare_parameter<float>("min_time_to_max_accel_sec", 2.0);
 
     this->update_parameters(rcl_interfaces::msg::ParameterEvent());
 
@@ -107,8 +107,8 @@ void VelocityController::controller_callback() {
         accel += i_term;
     }
 
-    if ((accel - prev_accel_cmd_->drive.acceleration) > max_accel_per_tick_) {
-        accel = prev_accel_cmd_->drive.acceleration + max_accel_per_tick_;
+    if ((accel - prev_accel_) > max_accel_per_tick_) {
+        accel = prev_accel_ + max_accel_per_tick_;
     }
 
     // limit output accel to be between -1 (braking) and 1 (accel)
@@ -126,7 +126,7 @@ void VelocityController::controller_callback() {
 
     // publish accel
     accel_pub_->publish(std::move(accel_cmd));
-    prev_accel_cmd_ = std::move(accel_cmd);
+    prev_accel_ = accel;
 }
 
 void VelocityController::state_callback(const driverless_msgs::msg::State::SharedPtr msg) {

src/hardware/canbus/src/component_canbus_translator.cpp

@@ -48,6 +48,9 @@ void CANTranslator::canmsg_timer() {
         uint8_t vesc_id = msg->id & 0xFF;
         if (vesc_id < 4) {
             if (vesc_masked_id == VESC_CAN_PACKET_STATUS) {
+                // 3 wheel drive :/
+                if (vesc_id == 1) { continue; }
+
                 uint8_t data[8];
                 this->copy_data(msg->data, data, 8);
                 // extract and publish RPM
@@ -67,7 +70,7 @@ void CANTranslator::canmsg_timer() {
                 for (int i = 0; i < 4; i++) {
                     av_velocity += wheel_speeds_[i];
                 }
-                av_velocity = av_velocity / 4;
+                av_velocity = av_velocity / 3;
 
                 // publish velocity
                 driverless_msgs::msg::Float32Stamped::UniquePtr vel_msg(new driverless_msgs::msg::Float32Stamped());

src/hardware/sensors/launch/vlp32.launch.py

@@ -45,7 +45,7 @@ def generate_launch_description():
                     extra_arguments=[{'use_intra_process_comms': True}]),
                 ComposableNode(
                     package='velodyne_pointcloud',
-                    plugin='velodyne_pointcloud::Transform', #Transform',
+                    plugin='velodyne_pointcloud::Convert', #Transform',
                     name='velodyne_transform_node',
                     parameters=[transform_params],
                     extra_arguments=[{'use_intra_process_comms': True}]),

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

@@ -19,8 +19,8 @@
 
 # for colour gradient based on intensity
 SEARCH_RANGE = 6
-SEARCH_ANGLE = pi / 4
-SEARCH_RANGE_LENIANCE = 0.1
+SEARCH_ANGLE = pi / 3
+SEARCH_RANGE_LENIANCE = 0.3
 SEARCH_ANGLE_MIN_LENIANCE = 15 * pi / 180  # 10 degrees
 
 

src/operations/mission_controller/mission_controller/node_trackdrive_handler.py

@@ -34,7 +34,7 @@ def __init__(self):
         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.get_parameter("sim").value:
             # reset odom and pose from camera