diff --git a/src/control/pure_pursuit_cpp/config/pure_pursuit_cpp.yaml b/src/control/pure_pursuit_cpp/config/pure_pursuit_cpp.yaml index 8efd9242e..1d9325e1f 100644 --- a/src/control/pure_pursuit_cpp/config/pure_pursuit_cpp.yaml +++ b/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 diff --git a/src/control/velocity_controller/config/velocity_controller.yaml b/src/control/velocity_controller/config/velocity_controller.yaml index dbc4cd9ae..bdc5608eb 100644 --- a/src/control/velocity_controller/config/velocity_controller.yaml +++ b/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 diff --git a/src/control/velocity_controller/include/component_velocity_controller.hpp b/src/control/velocity_controller/include/component_velocity_controller.hpp index 53d20370b..4200f4300 100644 --- a/src/control/velocity_controller/include/component_velocity_controller.hpp +++ b/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::UniquePtr prev_accel_cmd_ = std::make_unique(); + float prev_accel_ = 0; public: VelocityController(const rclcpp::NodeOptions & options); diff --git a/src/control/velocity_controller/src/component_velocity_controller.cpp b/src/control/velocity_controller/src/component_velocity_controller.cpp index 866cc60e0..36240f41f 100644 --- a/src/control/velocity_controller/src/component_velocity_controller.cpp +++ b/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("Kp", 1.0); + this->declare_parameter("Kp", 0.05); this->declare_parameter("Ki", 0); this->declare_parameter("max_integral_torque", 0); this->declare_parameter("histerisis_kick_ms", 0); this->declare_parameter("histerisis_reset_ms", 0); - this->declare_parameter("min_time_to_max_accel_sec", 0); + this->declare_parameter("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) { diff --git a/src/hardware/canbus/src/component_canbus_translator.cpp b/src/hardware/canbus/src/component_canbus_translator.cpp index 2d21985a6..a59644a6c 100644 --- a/src/hardware/canbus/src/component_canbus_translator.cpp +++ b/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()); diff --git a/src/hardware/sensors/launch/vlp32.launch.py b/src/hardware/sensors/launch/vlp32.launch.py index ab02fe365..83e4afca8 100644 --- a/src/hardware/sensors/launch/vlp32.launch.py +++ b/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}]), diff --git a/src/navigation/nav_bringup/config/localisation_params.yaml b/src/navigation/nav_bringup/config/localisation_params.yaml index 571c22107..563c16ffb 100644 --- a/src/navigation/nav_bringup/config/localisation_params.yaml +++ b/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" diff --git a/src/navigation/nav_bringup/config/slam_toolbox_params.yaml b/src/navigation/nav_bringup/config/slam_toolbox_params.yaml index 3d892e605..04cd871a5 100644 --- a/src/navigation/nav_bringup/config/slam_toolbox_params.yaml +++ b/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 diff --git a/src/navigation/planners/planners/node_boundary_interpolation.py b/src/navigation/planners/planners/node_boundary_interpolation.py index 96e184927..7672527e5 100644 --- a/src/navigation/planners/planners/node_boundary_interpolation.py +++ b/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 diff --git a/src/operations/mission_controller/mission_controller/node_trackdrive_handler.py b/src/operations/mission_controller/mission_controller/node_trackdrive_handler.py index e8d94699f..443c65247 100644 --- a/src/operations/mission_controller/mission_controller/node_trackdrive_handler.py +++ b/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