From 5c82b7c3061d8cc78a02f216f95464bd410944b1 Mon Sep 17 00:00:00 2001 From: Swerve Robot Date: Sat, 25 May 2024 18:30:02 -0500 Subject: [PATCH 1/4] saving commit from worlds bot --- .../ghost_over_under/config/ros_config.yaml | 6 +- 11_Robots/ghost_swerve/config/bt.xml | 203 ++++++++++-------- .../ghost_swerve/bt_nodes/autoDone.hpp | 32 +++ .../ghost_swerve/bt_nodes/intakeCmd.hpp | 4 +- .../ghost_swerve/swerve_robot_plugin.hpp | 1 + .../include/ghost_swerve/swerve_tree.hpp | 3 +- .../ghost_swerve/src/bt_nodes/autoDone.cpp | 40 ++++ .../ghost_swerve/src/bt_nodes/intakeCmd.cpp | 6 +- .../src/swerve_motion_planner.cpp | 2 +- .../ghost_swerve/src/swerve_robot_plugin.cpp | 43 ++-- 11_Robots/ghost_swerve/src/swerve_tree.cpp | 6 +- 11 files changed, 229 insertions(+), 117 deletions(-) create mode 100644 11_Robots/ghost_swerve/include/ghost_swerve/bt_nodes/autoDone.hpp create mode 100644 11_Robots/ghost_swerve/src/bt_nodes/autoDone.cpp diff --git a/11_Robots/ghost_over_under/config/ros_config.yaml b/11_Robots/ghost_over_under/config/ros_config.yaml index b96d5ef4..14718cfa 100644 --- a/11_Robots/ghost_over_under/config/ros_config.yaml +++ b/11_Robots/ghost_over_under/config/ros_config.yaml @@ -88,9 +88,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 @@ -117,6 +117,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 diff --git a/11_Robots/ghost_swerve/config/bt.xml b/11_Robots/ghost_swerve/config/bt.xml index 4e2cff6d..c38ec7e5 100644 --- a/11_Robots/ghost_swerve/config/bt.xml +++ b/11_Robots/ghost_swerve/config/bt.xml @@ -1,6 +1,6 @@ + main_tree_to_execute="both_triballs"> @@ -22,7 +22,7 @@ omega="0.0" threshold="0.05" angle_threshold="5.0" - timeout="-3000" + timeout="-1000" speed="0.5"/> @@ -45,70 +45,80 @@ omega="0.0" threshold="0.05" angle_threshold="5.0" - timeout="-2000" + timeout="-1000" speed="0.5"/> - + + timeout="-2000" + speed="0.5"/> + timeout="-1500" + speed="0.5"/> + timeout="-2000" + speed="0.5"/> + timeout="-1000" + speed="0.5"/> + @@ -183,19 +193,18 @@ - - + + - - - - + timeout="-1000" + speed="0.5"/> + + @@ -233,75 +242,93 @@ + timeout="-1500" + speed="0.5"/> + + + + + + + - + - + + + + + + + - + + + + + + + - - - diff --git a/11_Robots/ghost_swerve/include/ghost_swerve/bt_nodes/autoDone.hpp b/11_Robots/ghost_swerve/include/ghost_swerve/bt_nodes/autoDone.hpp new file mode 100644 index 00000000..24c42369 --- /dev/null +++ b/11_Robots/ghost_swerve/include/ghost_swerve/bt_nodes/autoDone.hpp @@ -0,0 +1,32 @@ +#pragma once + +#include "behaviortree_cpp/behavior_tree.h" +#include "ghost_v5_interfaces/robot_hardware_interface.hpp" +#include "rclcpp/rclcpp.hpp" +#include "ghost_swerve/swerve_model.hpp" + +namespace ghost_swerve { + +class AutoDone : public BT::SyncActionNode, + public rclcpp::Node { + +public: + AutoDone(const std::string& name, const BT::NodeConfig& config, + std::shared_ptr rhi_ptr, + std::shared_ptr swerve_ptr); + + // It is mandatory to define this STATIC method. + static BT::PortsList providedPorts(); + + BT::NodeStatus tick(); + +private: + template + T get_input(std::string key); + + std::shared_ptr rhi_ptr_; + std::shared_ptr swerve_ptr_; +}; + + +} // namespace ghost_swerve diff --git a/11_Robots/ghost_swerve/include/ghost_swerve/bt_nodes/intakeCmd.hpp b/11_Robots/ghost_swerve/include/ghost_swerve/bt_nodes/intakeCmd.hpp index 92ade359..c848a5ea 100644 --- a/11_Robots/ghost_swerve/include/ghost_swerve/bt_nodes/intakeCmd.hpp +++ b/11_Robots/ghost_swerve/include/ghost_swerve/bt_nodes/intakeCmd.hpp @@ -16,7 +16,8 @@ class IntakeCmd : public BT::SyncActionNode, std::shared_ptr swerve_ptr, double burnout_absolute_rpm_threshold, double burnout_stall_duration_ms, - double burnout_cooldown_duration_ms); + double burnout_cooldown_duration_ms, + double lift_setpoint); // It is mandatory to define this STATIC method. static BT::PortsList providedPorts(); @@ -32,6 +33,7 @@ class IntakeCmd : public BT::SyncActionNode, double burnout_absolute_rpm_threshold_; double burnout_stall_duration_ms_; double burnout_cooldown_duration_ms_; + double lift_setpoint_; std::chrono::time_point intake_stall_start_; std::chrono::time_point intake_cooldown_start_; diff --git a/11_Robots/ghost_swerve/include/ghost_swerve/swerve_robot_plugin.hpp b/11_Robots/ghost_swerve/include/ghost_swerve/swerve_robot_plugin.hpp index 967a157e..7d5d4481 100644 --- a/11_Robots/ghost_swerve/include/ghost_swerve/swerve_robot_plugin.hpp +++ b/11_Robots/ghost_swerve/include/ghost_swerve/swerve_robot_plugin.hpp @@ -108,6 +108,7 @@ class SwerveRobotPlugin : public ghost_ros_interfaces::V5RobotBase { double m_init_world_y = 0.0; double m_init_world_theta = 0.0; bool m_use_backup_estimator = false; + double m_intake_setpoint = 7.0; // Digital IO std::vector m_digital_io; diff --git a/11_Robots/ghost_swerve/include/ghost_swerve/swerve_tree.hpp b/11_Robots/ghost_swerve/include/ghost_swerve/swerve_tree.hpp index 01fd1d95..db04c318 100644 --- a/11_Robots/ghost_swerve/include/ghost_swerve/swerve_tree.hpp +++ b/11_Robots/ghost_swerve/include/ghost_swerve/swerve_tree.hpp @@ -25,7 +25,8 @@ class SwerveTree { std::shared_ptr swerve_ptr, double burnout_absolute_rpm_threshold, double burnout_stall_duration_ms, - double burnout_cooldown_duration_ms); + double burnout_cooldown_duration_ms, + double lift_setpoint); void tick_tree(); private: diff --git a/11_Robots/ghost_swerve/src/bt_nodes/autoDone.cpp b/11_Robots/ghost_swerve/src/bt_nodes/autoDone.cpp new file mode 100644 index 00000000..badd9971 --- /dev/null +++ b/11_Robots/ghost_swerve/src/bt_nodes/autoDone.cpp @@ -0,0 +1,40 @@ +#include "ghost_swerve/bt_nodes/autoDone.hpp" + +namespace ghost_swerve { + +AutoDone::AutoDone(const std::string& name, const BT::NodeConfig& config, + std::shared_ptr rhi_ptr, + std::shared_ptr swerve_ptr) : + BT::SyncActionNode(name, config), + rclcpp::Node("intake_cmd"), + rhi_ptr_(rhi_ptr), + swerve_ptr_(swerve_ptr){ +} + +// It is mandatory to define this STATIC method. +BT::PortsList AutoDone::providedPorts(){ + return { + // BT::InputPort("in"), + }; +} + +template +T AutoDone::get_input(std::string key){ + BT::Expected input = getInput(key); + // Check if expected is valid. If not, throw its error + if(!input){ + throw BT::RuntimeError("missing required input [" + key + "]: ", + input.error() ); + } + return input.value(); +} + +BT::NodeStatus AutoDone::tick() { + auto status = BT::NodeStatus::FAILURE; + + swerve_ptr_->setAutoStatus(true); + return status; +} + + +} // namespace ghost_swerve diff --git a/11_Robots/ghost_swerve/src/bt_nodes/intakeCmd.cpp b/11_Robots/ghost_swerve/src/bt_nodes/intakeCmd.cpp index b925adce..14c4b652 100644 --- a/11_Robots/ghost_swerve/src/bt_nodes/intakeCmd.cpp +++ b/11_Robots/ghost_swerve/src/bt_nodes/intakeCmd.cpp @@ -7,14 +7,16 @@ IntakeCmd::IntakeCmd(const std::string& name, const BT::NodeConfig& config, std::shared_ptr swerve_ptr, double burnout_absolute_rpm_threshold, double burnout_stall_duration_ms, - double burnout_cooldown_duration_ms) : + double burnout_cooldown_duration_ms, + double lift_setpoint) : BT::SyncActionNode(name, config), rclcpp::Node("intake_cmd"), rhi_ptr_(rhi_ptr), swerve_ptr_(swerve_ptr), burnout_absolute_rpm_threshold_(burnout_absolute_rpm_threshold), burnout_stall_duration_ms_(burnout_stall_duration_ms), - burnout_cooldown_duration_ms_(burnout_cooldown_duration_ms){ + burnout_cooldown_duration_ms_(burnout_cooldown_duration_ms), + lift_setpoint_(lift_setpoint){ } // It is mandatory to define this STATIC method. diff --git a/11_Robots/ghost_swerve/src/swerve_motion_planner.cpp b/11_Robots/ghost_swerve/src/swerve_motion_planner.cpp index 64752050..00922120 100644 --- a/11_Robots/ghost_swerve/src/swerve_motion_planner.cpp +++ b/11_Robots/ghost_swerve/src/swerve_motion_planner.cpp @@ -8,7 +8,7 @@ using std::placeholders::_1; void SwerveMotionPlanner::initialize(){ odom_sub_ = create_subscription( - "/odom_ekf/odometry", + "/map_ekf/odometry", 10, std::bind(&SwerveMotionPlanner::odomCallback, this, _1) ); diff --git a/11_Robots/ghost_swerve/src/swerve_robot_plugin.cpp b/11_Robots/ghost_swerve/src/swerve_robot_plugin.cpp index 42cdb8b0..4da915c6 100644 --- a/11_Robots/ghost_swerve/src/swerve_robot_plugin.cpp +++ b/11_Robots/ghost_swerve/src/swerve_robot_plugin.cpp @@ -161,6 +161,9 @@ void SwerveRobotPlugin::initialize(){ auto wheel_rad_per_sec = ghost_util::RPM_TO_RAD_PER_SEC * swerve_model_config.max_wheel_actuator_vel * swerve_model_config.wheel_ratio; swerve_model_config.max_wheel_lin_vel = wheel_rad_per_sec * swerve_model_config.wheel_radius * ghost_util::INCHES_TO_METERS; + + node_ptr_->declare_parameter("swerve_robot_plugin.intake_setpoint", 7.0); + m_intake_setpoint = node_ptr_->get_parameter("swerve_robot_plugin.intake_setpoint").as_double(); node_ptr_->declare_parameter("swerve_robot_plugin.max_ang_vel_slew", 0.); // 0 should stop the robot from moving when the param is not set swerve_model_config.max_ang_vel_slew = node_ptr_->get_parameter("swerve_robot_plugin.max_ang_vel_slew").as_double(); @@ -217,7 +220,7 @@ void SwerveRobotPlugin::initialize(){ "/cmd_vel", 10); - bt_ = std::make_shared(bt_path, rhi_ptr_, m_swerve_model_ptr, m_burnout_absolute_rpm_threshold, m_burnout_stall_duration_ms, m_burnout_cooldown_duration_ms); + bt_ = std::make_shared(bt_path, rhi_ptr_, m_swerve_model_ptr, m_burnout_absolute_rpm_threshold, m_burnout_stall_duration_ms, m_burnout_cooldown_duration_ms, m_intake_setpoint); m_start_recorder_client = node_ptr_->create_client( "bag_recorder/start"); @@ -570,7 +573,7 @@ void SwerveRobotPlugin::teleop(double current_time){ } else{ intake_up = false; - intake_lift_target = 7.0; + intake_lift_target = m_intake_setpoint; } if(std::fabs(rhi_ptr_->getMotorPosition("intake_lift_motor") - intake_lift_target) < 0.05){ rhi_ptr_->setMotorCurrentLimitMilliAmps("intake_lift_motor", 0); @@ -640,26 +643,26 @@ void SwerveRobotPlugin::teleop(double current_time){ rhi_ptr_->setMotorVoltageCommandPercent("lift_r2", 0); } - bool tail_down = false; + // bool tail_down = false; // tail left and intake up - if(tail_mode){ - tail_down = true; - if(joy_data->btn_r2){ - rhi_ptr_->setMotorCurrentLimitMilliAmps("tail_motor", 2500); - rhi_ptr_->setMotorPositionCommand("tail_motor", m_stick_angle_kick); - } - else{ - rhi_ptr_->setMotorCurrentLimitMilliAmps("tail_motor", 2500); - rhi_ptr_->setMotorPositionCommand("tail_motor", m_stick_angle_start); - } - } - else{ - tail_down = false; - rhi_ptr_->setMotorCurrentLimitMilliAmps("tail_motor", 2500); - rhi_ptr_->setMotorPositionCommand("tail_motor", m_stick_angle_start); - } + // if(tail_mode){ + // tail_down = true; + // if(joy_data->btn_r2){ + // rhi_ptr_->setMotorCurrentLimitMilliAmps("tail_motor", 2500); + // rhi_ptr_->setMotorPositionCommand("tail_motor", m_stick_angle_kick); + // } + // else{ + // rhi_ptr_->setMotorCurrentLimitMilliAmps("tail_motor", 2500); + // rhi_ptr_->setMotorPositionCommand("tail_motor", m_stick_angle_start); + // } + // } + // else{ + // tail_down = false; + // rhi_ptr_->setMotorCurrentLimitMilliAmps("tail_motor", 2500); + // rhi_ptr_->setMotorPositionCommand("tail_motor", m_stick_angle_start); + // } - m_digital_io[m_digital_io_name_map["tail"]] = tail_down; + // m_digital_io[m_digital_io_name_map["tail"]] = tail_down; m_digital_io[m_digital_io_name_map["claw"]] = !m_claw_open; rhi_ptr_->setDigitalIO(m_digital_io); diff --git a/11_Robots/ghost_swerve/src/swerve_tree.cpp b/11_Robots/ghost_swerve/src/swerve_tree.cpp index eee8f4cc..bf3bfd47 100644 --- a/11_Robots/ghost_swerve/src/swerve_tree.cpp +++ b/11_Robots/ghost_swerve/src/swerve_tree.cpp @@ -13,7 +13,8 @@ SwerveTree::SwerveTree(std::string bt_path, std::shared_ptr swerve_ptr, double burnout_absolute_rpm_threshold, double burnout_stall_duration_ms, - double burnout_cooldown_duration_ms) : + double burnout_cooldown_duration_ms, + double lift_setpoint) : bt_path_(bt_path) { @@ -27,7 +28,8 @@ SwerveTree::SwerveTree(std::string bt_path, factory.registerNodeType("IntakeCmd", robot_hardware_interface_ptr, swerve_ptr, burnout_absolute_rpm_threshold, burnout_stall_duration_ms, - burnout_cooldown_duration_ms); + burnout_cooldown_duration_ms, + lift_setpoint); factory.registerNodeType("Climb", robot_hardware_interface_ptr, swerve_ptr); factory.registerNodeType("AutoDone", robot_hardware_interface_ptr, swerve_ptr); From 1345a0ab9813dc055029f5ddb758ce4b3f24848d Mon Sep 17 00:00:00 2001 From: Swerve Robot Date: Sat, 25 May 2024 22:19:32 -0500 Subject: [PATCH 2/4] fixes duplicate parameter runtime error --- 11_Robots/ghost_swerve/CMakeLists.txt | 2 +- 11_Robots/ghost_swerve/config/bt.xml | 2 +- .../include/ghost_swerve/bt_nodes/autoDone.hpp | 2 ++ .../ghost_swerve/include/ghost_swerve/swerve_tree.hpp | 4 ++-- 11_Robots/ghost_swerve/src/bt_nodes/autoDone.cpp | 3 ++- 11_Robots/ghost_swerve/src/bt_nodes/moveToPose.cpp | 8 +++++--- 11_Robots/ghost_swerve/src/swerve_motion_planner.cpp | 0 11_Robots/ghost_swerve/src/swerve_tree.cpp | 2 +- 8 files changed, 14 insertions(+), 9 deletions(-) delete mode 100644 11_Robots/ghost_swerve/src/swerve_motion_planner.cpp diff --git a/11_Robots/ghost_swerve/CMakeLists.txt b/11_Robots/ghost_swerve/CMakeLists.txt index a6c81a56..17c2a5b8 100644 --- a/11_Robots/ghost_swerve/CMakeLists.txt +++ b/11_Robots/ghost_swerve/CMakeLists.txt @@ -116,7 +116,7 @@ set(NODES swipeTail intakeCmd climb - # autoDone + autoDone ) foreach(NODE ${NODES}) diff --git a/11_Robots/ghost_swerve/config/bt.xml b/11_Robots/ghost_swerve/config/bt.xml index 082684c5..db81ef85 100644 --- a/11_Robots/ghost_swerve/config/bt.xml +++ b/11_Robots/ghost_swerve/config/bt.xml @@ -231,7 +231,7 @@ diff --git a/11_Robots/ghost_swerve/include/ghost_swerve/bt_nodes/autoDone.hpp b/11_Robots/ghost_swerve/include/ghost_swerve/bt_nodes/autoDone.hpp index 24c42369..11732d01 100644 --- a/11_Robots/ghost_swerve/include/ghost_swerve/bt_nodes/autoDone.hpp +++ b/11_Robots/ghost_swerve/include/ghost_swerve/bt_nodes/autoDone.hpp @@ -12,6 +12,7 @@ class AutoDone : public BT::SyncActionNode, public: AutoDone(const std::string& name, const BT::NodeConfig& config, + std::shared_ptr node_ptr, std::shared_ptr rhi_ptr, std::shared_ptr swerve_ptr); @@ -24,6 +25,7 @@ class AutoDone : public BT::SyncActionNode, template T get_input(std::string key); + std::shared_ptr node_ptr_; std::shared_ptr rhi_ptr_; std::shared_ptr swerve_ptr_; }; diff --git a/11_Robots/ghost_swerve/include/ghost_swerve/swerve_tree.hpp b/11_Robots/ghost_swerve/include/ghost_swerve/swerve_tree.hpp index 1cf73366..c4253624 100644 --- a/11_Robots/ghost_swerve/include/ghost_swerve/swerve_tree.hpp +++ b/11_Robots/ghost_swerve/include/ghost_swerve/swerve_tree.hpp @@ -28,7 +28,7 @@ #include "bt_nodes/loggingNode.hpp" #include "bt_nodes/moveToPose.hpp" #include "bt_nodes/swipeTail.hpp" -// #include "bt_nodes/autoDone.hpp" +#include "bt_nodes/autoDone.hpp" #include "bt_nodes/climb.hpp" #include "ghost_swerve/swerve_model.hpp" #include "ghost_v5_interfaces/robot_hardware_interface.hpp" @@ -53,7 +53,7 @@ class SwerveTree double burnout_absolute_rpm_threshold, double burnout_stall_duration_ms, double burnout_cooldown_duration_ms, - double lift_setpoint); + double lift_setpoint); void tick_tree(); private: diff --git a/11_Robots/ghost_swerve/src/bt_nodes/autoDone.cpp b/11_Robots/ghost_swerve/src/bt_nodes/autoDone.cpp index badd9971..17d29bfd 100644 --- a/11_Robots/ghost_swerve/src/bt_nodes/autoDone.cpp +++ b/11_Robots/ghost_swerve/src/bt_nodes/autoDone.cpp @@ -2,11 +2,12 @@ namespace ghost_swerve { -AutoDone::AutoDone(const std::string& name, const BT::NodeConfig& config, +AutoDone::AutoDone(const std::string& name, const BT::NodeConfig& config, std::shared_ptr node_ptr, std::shared_ptr rhi_ptr, std::shared_ptr swerve_ptr) : BT::SyncActionNode(name, config), rclcpp::Node("intake_cmd"), + node_ptr_(node_ptr), rhi_ptr_(rhi_ptr), swerve_ptr_(swerve_ptr){ } diff --git a/11_Robots/ghost_swerve/src/bt_nodes/moveToPose.cpp b/11_Robots/ghost_swerve/src/bt_nodes/moveToPose.cpp index 794f76d4..b4f99557 100644 --- a/11_Robots/ghost_swerve/src/bt_nodes/moveToPose.cpp +++ b/11_Robots/ghost_swerve/src/bt_nodes/moveToPose.cpp @@ -38,9 +38,11 @@ MoveToPose::MoveToPose( node_ptr_(node_ptr), swerve_ptr_(swerve_ptr) { - node_ptr_->declare_parameter( - "behavior_tree.cubic_planner_topic", - "/motion_planner/cubic_command"); + if(!node_ptr_->has_parameter("behavior_tree.cubic_planner_topic")){ + node_ptr_->declare_parameter( + "behavior_tree.cubic_planner_topic", + "/motion_planner/cubic_command"); + } std::string cubic_planner_topic = node_ptr_->get_parameter("behavior_tree.cubic_planner_topic").as_string(); diff --git a/11_Robots/ghost_swerve/src/swerve_motion_planner.cpp b/11_Robots/ghost_swerve/src/swerve_motion_planner.cpp deleted file mode 100644 index e69de29b..00000000 diff --git a/11_Robots/ghost_swerve/src/swerve_tree.cpp b/11_Robots/ghost_swerve/src/swerve_tree.cpp index cfe2014e..2ae48802 100644 --- a/11_Robots/ghost_swerve/src/swerve_tree.cpp +++ b/11_Robots/ghost_swerve/src/swerve_tree.cpp @@ -64,7 +64,7 @@ SwerveTree::SwerveTree( burnout_cooldown_duration_ms, lift_setpoint); factory.registerNodeType("Climb", node_ptr_, robot_hardware_interface_ptr, swerve_ptr); - // factory.registerNodeType("AutoDone", node_ptr_, robot_hardware_interface_ptr, swerve_ptr); + factory.registerNodeType("AutoDone", node_ptr_, robot_hardware_interface_ptr, swerve_ptr); tree_ = factory.createTreeFromFile(bt_path_); } From c238b42a3c645eead2a62f87795600aa09eaf7c9 Mon Sep 17 00:00:00 2001 From: Swerve Robot Date: Mon, 27 May 2024 19:35:13 -0500 Subject: [PATCH 3/4] fixes runtime error with unneeded rhi ptr --- .../ghost_motion_planner_core/src/motion_planner.cpp | 2 +- 11_Robots/ghost_over_under/launch/hardware.launch.py | 1 - 11_Robots/ghost_swerve/src/swerve_robot_plugin.cpp | 10 +++++----- 3 files changed, 6 insertions(+), 7 deletions(-) diff --git a/03_ROS/ghost_motion_planner_core/src/motion_planner.cpp b/03_ROS/ghost_motion_planner_core/src/motion_planner.cpp index c8d48366..96b0faba 100644 --- a/03_ROS/ghost_motion_planner_core/src/motion_planner.cpp +++ b/03_ROS/ghost_motion_planner_core/src/motion_planner.cpp @@ -79,7 +79,7 @@ void MotionPlanner::configure(std::string node_name) void MotionPlanner::sensorUpdateCallback(const ghost_msgs::msg::V5SensorUpdate::SharedPtr msg) { if (!planning_) { - fromROSMsg(*robot_hardware_interface_ptr_, *msg); + // fromROSMsg(*robot_hardware_interface_ptr_, *msg); // make sure it doesnt overwrite values during trajectory calculation } // update values for trajectory calculation diff --git a/11_Robots/ghost_over_under/launch/hardware.launch.py b/11_Robots/ghost_over_under/launch/hardware.launch.py index cbb6a8ff..0e9b7069 100644 --- a/11_Robots/ghost_over_under/launch/hardware.launch.py +++ b/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"] ) diff --git a/11_Robots/ghost_swerve/src/swerve_robot_plugin.cpp b/11_Robots/ghost_swerve/src/swerve_robot_plugin.cpp index a4765df8..ee7ca8c4 100644 --- a/11_Robots/ghost_swerve/src/swerve_robot_plugin.cpp +++ b/11_Robots/ghost_swerve/src/swerve_robot_plugin.cpp @@ -312,11 +312,11 @@ void SwerveRobotPlugin::initialize() 10); // resetPose(m_init_world_x, m_init_world_y, m_init_world_theta); - if (!m_recording) { - auto req = std::make_shared(); - m_start_recorder_client->async_send_request(req); - m_recording = true; - } + // if (!m_recording) { + // auto req = std::make_shared(); + // m_start_recorder_client->async_send_request(req); + // m_recording = true; + // } } void SwerveRobotPlugin::onNewSensorData() From 2c53901ed80d669a93ae4570fbf852b5922142cc Mon Sep 17 00:00:00 2001 From: Swerve Robot Date: Mon, 27 May 2024 19:42:49 -0500 Subject: [PATCH 4/4] removes rhi ptr from motion planner class --- .../ghost_motion_planner_core/motion_planner.hpp | 5 ----- .../src/motion_planner.cpp | 15 --------------- 2 files changed, 20 deletions(-) diff --git a/03_ROS/ghost_motion_planner_core/include/ghost_motion_planner_core/motion_planner.hpp b/03_ROS/ghost_motion_planner_core/include/ghost_motion_planner_core/motion_planner.hpp index 38d7de29..fdfddefe 100644 --- a/03_ROS/ghost_motion_planner_core/include/ghost_motion_planner_core/motion_planner.hpp +++ b/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 robot_hardware_interface_ptr_; rclcpp::Publisher::SharedPtr trajectory_pub_; std::shared_ptr 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::SharedPtr sensor_update_sub_; rclcpp::Subscription::SharedPtr pose_command_sub_; rclcpp::Subscription::SharedPtr odom_sub_; }; diff --git a/03_ROS/ghost_motion_planner_core/src/motion_planner.cpp b/03_ROS/ghost_motion_planner_core/src/motion_planner.cpp index 96b0faba..7f045f4e 100644 --- a/03_ROS/ghost_motion_planner_core/src/motion_planner.cpp +++ b/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( - sensor_update_topic, - 10, - std::bind(&MotionPlanner::sensorUpdateCallback, this, _1) - ); - pose_command_sub_ = node_ptr_->create_subscription( 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;