Adds worlds code to develop (#79)

# PR Summary
PR Link: INSERT-LINK-HERE

Issue Link: INSERT-LINK-HERE

### Description
Adds code from last day of worlds that wasnt pushed.

### Reviewers

- Required:
@MaxxWilson or @melissajcruz 

---
### Changelog
- Adds a bt node that signifies the end of the auton.
- Adds parameter for the starting intake position.
- Fixes runtime error where ros parameters were being registered
multiple times by BT nodes.
- changes various auton parameters in ros config

### Reviewer Guide
- Requires no input, just updating final values from last day of worlds

### Testing
#### Automatic
- Default tests
#### Manual
- Should run correctly with hardware launch

### Checklist
- [ ] Confirmed all tests pass on a clean build
- [ ] Added reviewers in Github
- [ ] Posted PR Summary to Discord PR's Channel
- [ ] Ran uncrustify on any modified C++ files
- [ ] Ran Colcon Lint for any modified CMakeLists.txt or Package.xml

---------

Co-authored-by: Swerve Robot <[email protected]>

03_ROS/ghost_motion_planner_core/include/ghost_motion_planner_core/motion_planner.hpp

@@ -27,7 +27,6 @@
 
 #include "ghost_msgs/msg/drivetrain_command.hpp"
 #include "ghost_msgs/msg/robot_trajectory.hpp"
-#include "ghost_msgs/msg/v5_sensor_update.hpp"
 #include "ghost_util/angle_util.hpp"
 #include "nav_msgs/msg/odometry.hpp"
 
@@ -92,7 +91,6 @@ class MotionPlanner
   }
 
 protected:
-  std::shared_ptr<ghost_v5_interfaces::RobotHardwareInterface> robot_hardware_interface_ptr_;
   rclcpp::Publisher<ghost_msgs::msg::RobotTrajectory>::SharedPtr trajectory_pub_;
   std::shared_ptr<rclcpp::Node> node_ptr_;
   double current_x_ = 0.0;
@@ -103,14 +101,11 @@ class MotionPlanner
   double current_theta_vel_rad_ = 0.0;
 
 private:
-  void sensorUpdateCallback(const ghost_msgs::msg::V5SensorUpdate::SharedPtr msg);
-  void loadRobotHardwareInterface();
   void setNewCommand(const ghost_msgs::msg::DrivetrainCommand::SharedPtr cmd);
   void odomCallback(nav_msgs::msg::Odometry::SharedPtr msg);
 
   bool configured_ = false;
   std::atomic_bool planning_ = false;
-  rclcpp::Subscription<ghost_msgs::msg::V5SensorUpdate>::SharedPtr sensor_update_sub_;
   rclcpp::Subscription<ghost_msgs::msg::DrivetrainCommand>::SharedPtr pose_command_sub_;
   rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
 };

03_ROS/ghost_motion_planner_core/src/motion_planner.cpp

@@ -50,12 +50,6 @@ void MotionPlanner::configure(std::string node_name)
   node_ptr_->declare_parameter("odom_topic", "/map_ekf/odometry");
   std::string odom_topic = node_ptr_->get_parameter("odom_topic").as_string();
 
-  sensor_update_sub_ = node_ptr_->create_subscription<ghost_msgs::msg::V5SensorUpdate>(
-    sensor_update_topic,
-    10,
-    std::bind(&MotionPlanner::sensorUpdateCallback, this, _1)
-  );
-
   pose_command_sub_ = node_ptr_->create_subscription<ghost_msgs::msg::DrivetrainCommand>(
     command_topic,
     10,
@@ -76,15 +70,6 @@ void MotionPlanner::configure(std::string node_name)
   configured_ = true;
 }
 
-void MotionPlanner::sensorUpdateCallback(const ghost_msgs::msg::V5SensorUpdate::SharedPtr msg)
-{
-  if (!planning_) {
-    fromROSMsg(*robot_hardware_interface_ptr_, *msg);
-    // make sure it doesnt overwrite values during trajectory calculation
-  }
-  // update values for trajectory calculation
-}
-
 void MotionPlanner::setNewCommand(const ghost_msgs::msg::DrivetrainCommand::SharedPtr cmd)
 {
   planning_ = true;

11_Robots/ghost_over_under/config/ros_config.yaml

@@ -93,9 +93,9 @@ competition_state_machine_node:
       k9: 0.5 # theta error from rotation   # 25% error in primary direction
 
       # motion planning
-      move_to_pose_kp_xy: 2.0
+      move_to_pose_kp_xy: 1.0
       move_to_pose_kd_xy: 0.7
-      move_to_pose_kp_theta: 0.9
+      move_to_pose_kp_theta: 0.6
       move_to_pose_kd_theta: 0.7
 
       # Steering Controller
@@ -122,6 +122,8 @@ competition_state_machine_node:
       lift_kP: .05
       lift_speed: -10. #TODO
 
+      intake_setpoint: 7.0
+
       # TODO MAXX ALL OF THESE
       stick_gear_ratio: 3.
       # 0 is the position we start on, slightly crooked

11_Robots/ghost_over_under/launch/hardware.launch.py

@@ -72,7 +72,6 @@ def generate_launch_description():
             #     "robot_config_yaml_path": robot_config_yaml_path
             # }
         ],
-        arguments=[plugin_type, robot_name],
         # arguments=["--ros-args", "--log-level", "debug"]
     )
 

11_Robots/ghost_swerve/CMakeLists.txt

@@ -116,7 +116,7 @@ set(NODES
   swipeTail
   intakeCmd
   climb
-  # autoDone
+  autoDone
 )
 
 foreach(NODE ${NODES})