Skip to content

Commit

Permalink
Feature/post texas features 25 (#128)
Browse files Browse the repository at this point in the history 
# PR Summary
PR Link: [Link](#128)

### Description
Merges improvements and changes from the 2025 Texas VEXU Regional.
Coauthored with @karmanyaahm @melissajcruz @JakeWendling @Fyfth 

### Reviewers
Tag reviewers.
@JakeWendling @MaxxWilson @melissajcruz @karmanyaahm @Fyfth 

---
### Changelog
- Fixes typo in full field filename causing crash
- Adds functionality to reset particle filter from competition state
machine
- Minor QoL in V5RobotBase
    - Change configured flag to atomic
- Disallow sensor callbacks until configuration is complete (potential
race condition)
- Adds Goal Rush solenoids to robot_hardware_config
- Adds neutral stake mech to robot_hardware_config
- Adds move_to_pose_test example BT
- Re-enables color sensor and classifier nodes
- Adds BT Nodes for autonomy
- Adds "smart" conveyor controls
    - Auto-reject for rings
    - Auto hook alignment
- Variety of bugfixes and improvements to Boomerang planner and
Drivetrain PD class

### Testing
#### Automatic
- Describe test cases that are covered by unit tests
#### Manual
- It was nearly undefeated in quals, and then made it to finals. Well
tested by my standards!

---------

Co-authored-by: Omega Jerry <[email protected]>
Co-authored-by: JakeWendling <[email protected]>
Co-authored-by: MELISSA CRUZ <[email protected]>
  • Loading branch information
@MaxxWilson @JakeWendling @melissajcruz
4 people authored Mar 2, 2025
1 parent e898566 commit cf9040c
Show file tree
Hide file tree
Showing 40 changed files with 2,990 additions and 494 deletions.
0 ...ghost_localization/maps/VEXFieldZeroLHtxt → ...host_localization/maps/VEXFieldZeroLH.txt
View file
File renamed without changes.
10 changes: 5 additions & 5 deletions 03_ROS/ghost_localization/src/ekf_pf_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -72,9 +72,12 @@ EkfPfNode::EkfPfNode()

LoadROSParams();

rclcpp::QoS qos_profile(1);
qos_profile.transient_local();

set_pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
rviz_set_pose_topic_,
10,
"/set_pf_pose",
qos_profile,
std::bind(&EkfPfNode::InitialPoseCallback, this, _1)
);

Expand All @@ -97,9 +100,6 @@ void EkfPfNode::LoadROSParams()
declare_parameter("particle_filter.world_frame", "");
config_params.world_frame = get_parameter("particle_filter.world_frame").as_string();

declare_parameter("particle_filter.rviz_set_pose_topic", "");
rviz_set_pose_topic_ = get_parameter("particle_filter.rviz_set_pose_topic").as_string();

declare_parameter("particle_filter.map", "");
config_params.map = get_parameter("particle_filter.map").as_string();
RCLCPP_INFO(
Expand Down
3 changes: 2 additions & 1 deletion 03_ROS/ghost_ros_interfaces/include/ghost_ros_interfaces/competition/v5_robot_base.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,7 @@

#include <chrono>
#include <memory>
#include <atomic>

#include <rclcpp/rclcpp.hpp>
#include <yaml-cpp/yaml.h>
Expand Down Expand Up @@ -145,7 +146,7 @@ class V5RobotBase
void updateCompetitionState(bool is_disabled, bool is_autonomous);
void trajectoryCallback(const ghost_msgs::msg::RobotTrajectory::SharedPtr msg);

bool configured_ = false;
std::atomic_bool configured_ = false;
bool should_record_ = false;
robot_state_e last_comp_state_ = robot_state_e::TELEOP;
robot_state_e curr_comp_state_ = robot_state_e::TELEOP;
Expand Down
4 changes: 4 additions & 0 deletions 03_ROS/ghost_ros_interfaces/src/competition/v5_robot_base.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -94,6 +94,10 @@ void V5RobotBase::loadRobotHardwareInterface()

void V5RobotBase::sensorUpdateCallback(const ghost_msgs::msg::V5SensorUpdate::SharedPtr msg)
{
if(!configured_){
return;
}

// Update Competition State Machine
updateCompetitionState(
msg->competition_status.is_disabled,
Expand Down
11 changes: 11 additions & 0 deletions 11_Robots/ghost_high_stakes/config/robot_hardware_config_tank.yaml
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
port_configuration:
digital_io:
A: IN
F: OUT
G: OUT
H: OUT
devices:
Expand All @@ -14,6 +15,16 @@ port_configuration:
type: MOTOR
reversed: false
config: intake_motor_config
neutral_stake_motor_l:
port: 9
type: MOTOR
reversed: false
config: intake_motor_config
neutral_stake_motor_r:
port: 10
type: MOTOR
reversed: true
config: intake_motor_config
imu:
port: 14
type: INERTIAL_SENSOR
Expand Down
117 changes: 82 additions & 35 deletions 11_Robots/ghost_high_stakes/config/ros_config.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -56,21 +56,28 @@ competition_state_machine_node:
odom_topic: "/sensors/wheel_odom"
backup_pose_topic: "/odom_ekf/odometry"
pose_topic: "/map_ekf/odometry"

set_pf_pose_topic: "/set_pf_pose"

tank_robot_plugin:
conveyor_num_links: 69.0
conveyor_sprocket_teeth: 12.0
conveyor_num_hooks: 3.0

conveyor_hook_align_threshold: 0.85
conveyor_hook_align_power: 0.5

conveyor_hook_throw_fraction: 0.5
conveyor_hook_throw_duration: 0.25

# motion planning
move_to_pose_kp_xy: 0.7
move_to_pose_kd_xy: 0.0
move_to_pose_kp_theta: 0.2
move_to_pose_kd_theta: 0.00
max_speed_linear: 1.0
max_speed_angular: 1.0
search_radius: 0.25

drive_motor_ticks_per_rotation: 600.0
move_to_pose_kp_xy: 2.2
move_to_pose_kd_xy: 0.2
move_to_pose_kp_theta: 1.75
move_to_pose_kd_theta: 0.2

drive_motor_ticks_per_rotation: 360.0
drive_gear_ratio: 1.15 # 23/20
drive_wheel_size_inches: 2.75
drive_wheel_rad_in: 1.375
wheel_base_inches: 12.5

cmd_twist_topic: "/cmd_vel" # from ekf
Expand All @@ -88,15 +95,33 @@ competition_state_machine_node:

/**:
ros__parameters:
map_ekf:
initial_state:
[
1.2, 0.508, 0.0,
0.0, 0.0, 1.75,
0.0, 0.0, 0.0,
0.0, 0.0, 0.0,
0.0, 0.0, 0.0,
]

initial_estimate_covariance:
[
0.05, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.05, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 1e-9, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 1e-9, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 1e-9, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.1,
]
particle_filter:
world_frame: "map"
rviz_set_pose_topic: "/set_pf_pose"
map: "/home/ghost/VEXU_GHOST/03_ROS/ghost_localization/maps/VEXFieldsmallZeroLH.txt"
map: "/home/ghost/VEXU_GHOST/03_ROS/ghost_localization/maps/VEXFieldZeroLH.txt"

# Initial position
init_x: 0.56
init_y: 0.28
init_r: 0.7853
# init_x: 0.508
# init_y: 0.28
# init_r: 0.0

# Initial cloud distribution
init_x_sigma: 0.05 #
Expand Down Expand Up @@ -142,6 +167,22 @@ competition_state_machine_node:
use_sim_time: false
should_record: false

color_classification:
blue:
hue_center: 200
hue_range: 30
sat_min: 0.2
sat_max: 1.0
val_min: 0.02
val_max: 1.0
red:
hue_center: 0
hue_range: 20
sat_min: 0.5
sat_max: 1.0
val_min: 0.01
val_max: 1.0

# If you are using the sweve model and you are tuning the k1-9 parameters,
# the swerve model plugin has its own duplicate (11_Robots/ghost_high_stakes/config/ros_config.yaml) of the k1-9 parameters that must
# be changed as well as this file. TODO: make a class for this
Expand Down Expand Up @@ -170,6 +211,11 @@ ekf_pf_node:
min_update_angle: 10.0 #0.005
max_update_angular_velocity: 1.0

# Disables LIDAR Updates for pure-odom auton testing
# min_update_dist: 10000.0 #0.05
# min_update_angle: 10000.0 #0.005
# max_update_angular_velocity: 0.0

# Computation Factors
num_particles: 200 # Increase until computation runs out
resize_factor: 10.0 # num_points / resize_factor = num_rays
Expand Down Expand Up @@ -444,11 +490,11 @@ map_ekf_node:

initial_state:
[
0.28, 0.28, 0.0,
0.0, 1.5708, 0.0,
0.0, 0.0, 0.0,
0.0, 0.0, 0.0,
0.0, 0.0, 0.0,
1.2, 0.508, 0.0,
0.0, 0.0, 1.75,
0.0, 0.0, 0.0,
0.0, 0.0, 0.0,
0.0, 0.0, 0.0,
]

# The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin
Expand Down Expand Up @@ -665,19 +711,20 @@ map_ekf_node:
initial_estimate_covariance:
[
0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-5, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-5, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9,
0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-5, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-5, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9,
]

dynamic_process_noise_covariance: true
8 changes: 5 additions & 3 deletions 11_Robots/ghost_high_stakes/launch/hardware.launch.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,6 +33,8 @@ def generate_launch_description():
bt_path_interaction = os.path.join(ghost_tank_share_dir, "config", "bt_interaction.xml")
config_path = os.path.join(ghost_tank_share_dir, "config")

mtp_test = os.path.join(ghost_tank_share_dir, "config", "move_to_pose_test.xml")

########################
### Node Definitions ###
########################
Expand Down Expand Up @@ -163,14 +165,14 @@ def generate_launch_description():

return LaunchDescription([
serial_node,
competition_state_machine_node,
bag_recorder_service,
ekf_pf_node,
# realsense_node,
imu_filter_node,
odom_ekf_node,
map_ekf_node,
rplidar_node,
# color_classifier_node,
# color_sensor_node
competition_state_machine_node,
color_classifier_node,
color_sensor_node
])
Loading

0 comments on commit cf9040c

Please sign in to comment.