diff --git a/headers/fan_publisher.hpp b/headers/fan_publisher.hpp deleted file mode 100644 index 33b7e98..0000000 --- a/headers/fan_publisher.hpp +++ /dev/null @@ -1,45 +0,0 @@ -// -// Created by Robbie on 2/12/25. -// - -#ifndef FAN_PUBLISHER_HPP -#define FAN_PUBLISHER_HPP - -#include "rclcpp/rclcpp.hpp" -#include "wheelchair_sensor_msgs/msg/fan_speed.hpp" - -struct FanSpeed -{ - int fan_percent_0; - int fan_percent_1; - int fan_percent_2; - int fan_percent_3; -}; - -class FanPublisher -{ -public: - FanPublisher(rclcpp::Node::SharedPtr node); - - void trigger_publish(FanSpeed fanSpeed); - -private: - rclcpp::Node::SharedPtr node_; - rclcpp::Publisher::SharedPtr publisher_; -}; - -/** -class FanPublisher : public rclcpp::Node -{ -public: - FanPublisher(); - -private: - void timer_callback(); - rclcpp::TimerBase::SharedPtr timer_; - rclcpp::Publisher::SharedPtr publisher_; - size_t count_; -}; -**/ - -#endif // FAN_PUBLISHER_HPP diff --git a/headers/fingerprint_subscriber.hpp b/headers/fingerprint_subscriber.hpp deleted file mode 100644 index 7a5e51c..0000000 --- a/headers/fingerprint_subscriber.hpp +++ /dev/null @@ -1,22 +0,0 @@ -// -// Created by Robbie on 2/14/25. -// - -#ifndef FINGERPRINT_SUBSCRIBER_HPP -#define FINGERPRINT_SUBSCRIBER_HPP - -#include "rclcpp/rclcpp.hpp" -#include "wheelchair_sensor_msgs/msg/fingerprint.hpp" - -class FingerprintSubscriber -{ -public: - FingerprintSubscriber(rclcpp::Node::SharedPtr node); - -private: - void topic_callback(const wheelchair_sensor_msgs::msg::Fingerprint::SharedPtr msg) const; - rclcpp::Subscription::SharedPtr subscription_; - rclcpp::Node::SharedPtr node_; -}; - -#endif //FINGERPRINT_SUBSCRIBER_HPP diff --git a/headers/heading.hpp b/headers/heading.hpp deleted file mode 100644 index e5a47c6..0000000 --- a/headers/heading.hpp +++ /dev/null @@ -1,6 +0,0 @@ -#ifndef HEADING_H -#define HEADING_H - -float heading(float mag_x, float mag_y); - -#endif //HEADING_H diff --git a/headers/light_publisher.hpp b/headers/light_publisher.hpp deleted file mode 100644 index aefd0a5..0000000 --- a/headers/light_publisher.hpp +++ /dev/null @@ -1,28 +0,0 @@ -// -// Created by leslier on 2/13/25. -// - -#ifndef LIGHTPUBLISHER_H -#define LIGHTPUBLISHER_H - -#include "rclcpp/rclcpp.hpp" -#include "wheelchair_sensor_msgs/msg/light.hpp" - -enum class LightStates{ - OFF = 0, - STEADY = 1, - FLASHING = 2, - }; - -class LightPublisher { -public: - LightPublisher(rclcpp::Node::SharedPtr node); - - void trigger_publish(LightStates light_state); - -private: - rclcpp::Node::SharedPtr node_; - rclcpp::Publisher::SharedPtr publisher_; -}; - -#endif //LIGHTPUBLISHER_H diff --git a/headers/obstacle_publisher.hpp b/headers/obstacle_publisher.hpp deleted file mode 100644 index 1f088f9..0000000 --- a/headers/obstacle_publisher.hpp +++ /dev/null @@ -1,28 +0,0 @@ -#pragma once -#include -#include -#include -#include - -class ObstaclePublisher : public rclcpp::Node -{ -public: - ObstaclePublisher(); - -private: - /* subscriber */ - rclcpp::Subscription::SharedPtr cloud_sub_; - - /* publishers */ - rclcpp::Publisher::SharedPtr front_clear_pub_; - rclcpp::Publisher::SharedPtr back_clear_pub_; - rclcpp::Publisher::SharedPtr left_turn_clear_pub_; - rclcpp::Publisher::SharedPtr right_turn_clear_pub_; - rclcpp::Publisher::SharedPtr obstacle_flag_pub_; - - /* diagnostics */ - rclcpp::Publisher::SharedPtr obstacles_cloud_pub_; - rclcpp::Publisher::SharedPtr markers_pub_; - - void cloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg); -}; diff --git a/headers/obstacle_publisher_heading.hpp b/headers/obstacle_publisher_heading.hpp deleted file mode 100644 index 6f0a014..0000000 --- a/headers/obstacle_publisher_heading.hpp +++ /dev/null @@ -1,31 +0,0 @@ -#pragma once -#include -#include -#include -#include - -class ObstaclePublisher : public rclcpp::Node -{ -public: - ObstaclePublisher(); - -private: - /* subscriber */ - rclcpp::Subscription::SharedPtr cloud_sub_; - - /* publishers */ - rclcpp::Publisher::SharedPtr obstacle_flag_pub_; - - // clear/blocked flags used by the motion controller - rclcpp::Publisher::SharedPtr front_clear_pub_; - rclcpp::Publisher::SharedPtr back_clear_pub_; - rclcpp::Publisher::SharedPtr left_turn_clear_pub_; - rclcpp::Publisher::SharedPtr right_turn_clear_pub_; - - // diagnostic visuals - rclcpp::Publisher::SharedPtr obstacles_cloud_pub_; - rclcpp::Publisher::SharedPtr markers_pub_; - - - void cloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg); -}; diff --git a/headers/sensors_subscriber.h b/headers/sensors_subscriber.h deleted file mode 100644 index 6578d66..0000000 --- a/headers/sensors_subscriber.h +++ /dev/null @@ -1,51 +0,0 @@ -// -// Created by Robbie on 2/12/25. -// - -#ifndef SENSOR_SUBSCRIBER_HPP -#define SENSOR_SUBSCRIBER_HPP - -#include "rclcpp/rclcpp.hpp" -#include "wheelchair_sensor_msgs/msg/sensors.hpp" - -struct SensorData { - int left_speed; - int right_speed; - int ultrasonic_front_0; - int ultrasonic_front_1; - int ultrasonic_back; - int ultrasonic_left; - int ultrasonic_right; - bool pir_front; - bool pir_back; - bool pir_left; - bool pir_right; - int fan_speed_0; - int fan_speed_1; - int fan_speed_2; - int fan_speed_3; - float linear_acceleration_x; - float linear_acceleration_y; - float linear_acceleration_z; - float angular_velocity_x; - float angular_velocity_y; - float angular_velocity_z; - float magnetic_field_x; - float magnetic_field_y; - float magnetic_field_z; -}; - -class SensorsSubscriber -{ -public: - SensorsSubscriber(rclcpp::Node::SharedPtr node); - SensorData get_latest_sensor_data() const; - -private: - void topic_callback(const wheelchair_sensor_msgs::msg::Sensors::SharedPtr msg) const; - rclcpp::Subscription::SharedPtr subscription_; - rclcpp::Node::SharedPtr node_; - mutable SensorData latest_sensor_data_; -}; - -#endif // SENSOR_SUBSCRIBER_HPP diff --git a/headers/speed_publisher.hpp b/headers/speed_publisher.hpp deleted file mode 100644 index 33fd3f0..0000000 --- a/headers/speed_publisher.hpp +++ /dev/null @@ -1,47 +0,0 @@ -// -// Created by Angela on 4/18/25. -// - - -#pragma once -#include -#include -#include -#include "sensors_subscriber.h" - -namespace wheelchair -{ - -class SpeedPublisher : public rclcpp::Node -{ -public: - explicit SpeedPublisher(const std::string& out_topic = "speed_cmd", - const rclcpp::QoS& qos = rclcpp::QoS(10)); - -private: - static constexpr float SPEED_MAX = 3.0f; - static constexpr float SPEED_CAUTION = 2.0f; - static constexpr float SPEED_STOP = 0.0f; - static constexpr float ACC_LOW_THR = 7.0f; - static constexpr float ACC_HIGH_THR = 12.0f; - static constexpr float MAX_ACCEL = 0.5f; // m / s² - static constexpr float DT = 0.5f; // timer period - - void timerCb(); // main FSM timer - void obstacleCb(const std_msgs::msg::Bool::SharedPtr); - - float fuseSpeeds(float ultrasonic, - float acc_mag, - bool pir_hit, - bool obstacle_close) const; - - std::shared_ptr sensor_sub_; - rclcpp::Subscription::SharedPtr obs_sub_; - rclcpp::Publisher::SharedPtr speed_pub_; - rclcpp::TimerBase::SharedPtr timer_; - - bool obstacle_flag_{false}; - float prev_speed_{0.0f}; -}; - -} diff --git a/headers/temp_monitor.hpp b/headers/temp_monitor.hpp deleted file mode 100644 index 8b6826e..0000000 --- a/headers/temp_monitor.hpp +++ /dev/null @@ -1,14 +0,0 @@ -// -// Created by Robbie on 4/21/25. -// - -#ifndef WHEELCHAIR_CODE_MODULE_TEMP_MONITOR_H -#define WHEELCHAIR_CODE_MODULE_TEMP_MONITOR_H - -#include - -std::string findCpuThermalZone(); - -double readTemperature(const std::string& tempPath); - -#endif //WHEELCHAIR_CODE_MODULE_TEMP_MONITOR_H diff --git a/headers/uwb_subscriber.hpp b/headers/uwb_subscriber.hpp deleted file mode 100644 index af2c1cd..0000000 --- a/headers/uwb_subscriber.hpp +++ /dev/null @@ -1,29 +0,0 @@ -#pragma once - -#include -#include -#include "rclcpp/rclcpp.hpp" -#include "wheelchair_sensor_msgs/msg/uwb.hpp" - -class UWBSubscriber -{ -public: - UWBSubscriber(rclcpp::Node::SharedPtr node); - - - - float dist1() const noexcept { return dist1_.load(); } - float dist2() const noexcept { return dist2_.load(); } - float dist3() const noexcept { return dist3_.load(); } - float dist4() const noexcept { return dist4_.load(); } - float dist5() const noexcept { return dist5_.load(); } - float dist6() const noexcept { return dist6_.load(); } - -private: - rclcpp::Node::SharedPtr node_; - void msgCallback(const wheelchair_sensor_msgs::msg::UWB::SharedPtr msg); - rclcpp::Subscription::SharedPtr sub_; - - std::atomic dist1_{0.f}, dist2_{0.f}, dist3_{0.f}, - dist4_{0.f}, dist5_{0.f}, dist6_{0.f}; -}; diff --git a/headers/wheelchair_pid_controller.cpp b/headers/wheelchair_pid_controller.cpp deleted file mode 100644 index 150956c..0000000 --- a/headers/wheelchair_pid_controller.cpp +++ /dev/null @@ -1,103 +0,0 @@ -#include -#include -#include -#include // for std::min and std::max -#include -#include - -// Helper function to put values within a range. -double clamp(double value, double min_val, double max_val) { - return std::max(min_val, std::min(value, max_val)); -} - -// Normalize an angle to the range [-180, 180] degrees. -double normalizeAngle(double angle) { - while (angle > 180) angle -= 360; - while (angle < -180) angle += 360; - return angle; -} - -// Replace this with the actual sensor reading. -double getDesiredHeading() { - // For example, return 0.0 if we want to face north. - return 0.0; -} - -// Replace this with the actual sensor reading. -double getCurrentHeading() { - // Example: current heading is measured as 10.0 degrees. - return 10.0; -} - -// Replace this with the logic to map overall speed to a base PWM percentage. -double getBasePWM() { - // For instance, 3 mph might map to 100% PWM; here, we use 70%. - return 70.0; -} - -// Replace this with the actual motor control code. -void setMotorPWM(const std::string &motor, double pwm) { - std::cout << "Motor " << motor << " PWM: " << pwm << "%" << std::endl; -} - -int main() { - // PID tuning parameters - const double Kp = 1.0; - const double Ki = 0.1; - const double Kd = 0.05; - - // Anti-windup limits for the integral term - const double max_integral = 100.0; - const double min_integral = -100.0; - - // PID state variables - double previous_error = 0.0; - double integral = 0.0; - - // Control loop timing: 20 ms cycle (i.e., 50 Hz) - const double dt = 0.02; // in seconds - - while (true) { - // 1. Get the desired and current headings - double desired_heading = getDesiredHeading(); - double current_heading = getCurrentHeading(); - - // 2. Compute the error and normalize it - double error = normalizeAngle(desired_heading - current_heading); - - // 3. Compute the proportional term - double P = Kp * error; - - // 4. Compute the integral term with anti-windup clamping - integral += error * dt; - integral = clamp(integral, min_integral, max_integral); - double I = Ki * integral; - - // 5. Compute the derivative term - double derivative = (error - previous_error) / dt; - double D = Kd * derivative; - - // Save the error for the next cycle - previous_error = error; - - // 6. Total PID output is the turn offset - double turn_offset = P + I + D; - - // 7. Get the base PWM for forward speed - double base_pwm = getBasePWM(); - - // 8. Adjust left and right motor PWM values based on the turn offset. - // For a positive turn offset, left wheel gets more power and right less. - double pwm_left = clamp(base_pwm + turn_offset, 0.0, 100.0); - double pwm_right = clamp(base_pwm - turn_offset, 0.0, 100.0); - - // 9. Output motor commands - setMotorPWM("LEFT_MOTOR", pwm_left); - setMotorPWM("RIGHT_MOTOR", pwm_right); - - // 10. Wait for the next cycle - std::this_thread::sleep_for(std::chrono::milliseconds(static_cast(dt * 1000))); - } - - return 0; -} diff --git a/src/fan_publisher.cpp b/src/fan_publisher.cpp deleted file mode 100644 index be13914..0000000 --- a/src/fan_publisher.cpp +++ /dev/null @@ -1,47 +0,0 @@ -// -// Created by aec117-fkmj9r3 on 2/12/25. -// - -#include "fan_publisher.hpp" - -using namespace std::chrono_literals; - -FanPublisher::FanPublisher(rclcpp::Node::SharedPtr node) -: node_(node) -{ - publisher_ = node_->create_publisher("fan_duty_cycles", 10); -} - -void FanPublisher::trigger_publish(FanSpeed fanSpeed) -{ - auto message = wheelchair_sensor_msgs::msg::FanSpeed(); - message.fan_percent_0 = fanSpeed.fan_percent_0; - message.fan_percent_1 = fanSpeed.fan_percent_1; - message.fan_percent_2 = fanSpeed.fan_percent_2; - message.fan_percent_3 = fanSpeed.fan_percent_3; - RCLCPP_INFO(node_->get_logger(), "Publishing: fan_percent_0=%d, fan_percent_1=%d, fan_percent_2=%d, fan_percent_3=%d", - message.fan_percent_0, message.fan_percent_1, message.fan_percent_2, message.fan_percent_3); - publisher_->publish(message); -} - -/** -FanPublisher::FanPublisher() -: Node("fan_publisher"), count_(0) -{ - publisher_ = this->create_publisher("fan_duty_cycles", 10); - timer_ = this->create_wall_timer( - 500ms, std::bind(&FanPublisher::timer_callback, this)); -} - -void FanPublisher::timer_callback() -{ - auto message = wheelchair_sensor_msgs::msg::FanSpeed(); - message.fan_percent_0 = 0; - message.fan_percent_1 = 0; - message.fan_percent_2 = 0; - message.fan_percent_3 = 0; - RCLCPP_INFO(this->get_logger(), "Publishing: fan_percent_0=%d, fan_percent_1=%d, fan_percent_2=%d, fan_percent_3=%d", - message.fan_percent_0, message.fan_percent_1, message.fan_percent_2, message.fan_percent_3); - publisher_->publish(message); -} -**/ diff --git a/src/fingerprint_subscriber.cpp b/src/fingerprint_subscriber.cpp deleted file mode 100644 index a5b0b91..0000000 --- a/src/fingerprint_subscriber.cpp +++ /dev/null @@ -1,17 +0,0 @@ -// -// Created by Robbie on 2/14/25. -// - -#include "fingerprint_subscriber.hpp" - -FingerprintSubscriber::FingerprintSubscriber(rclcpp::Node::SharedPtr node) -: node_(node) -{ - subscription_ = node_->create_subscription( - "fingerprint", rclcpp::QoS(10).best_effort(), std::bind(&FingerprintSubscriber::topic_callback, this, std::placeholders::_1)); -} - -void FingerprintSubscriber::topic_callback(const wheelchair_sensor_msgs::msg::Fingerprint::SharedPtr msg) const -{ - RCLCPP_INFO(node_->get_logger(), "I heard: fingerprint=%d", msg->user_id); -} \ No newline at end of file diff --git a/src/gyro_turn.cpp b/src/gyro_turn.cpp deleted file mode 100644 index 6aac7d6..0000000 --- a/src/gyro_turn.cpp +++ /dev/null @@ -1,178 +0,0 @@ -/* main.cpp – left-pivot obstacle avoidance - drive_mode: 0=manual, 1=joystick pass-through, 2=autonomous */ - -#include "main.h" -#include "rclcpp/rclcpp.hpp" -#include "sensors_subscriber.hpp" -#include "obstacle_subscriber.hpp" -#include "ref_speed_publisher.hpp" -#include "heading.hpp" -#include -#include -#include -#include - -/* ── config ─────────────────────────────────────────────── */ -constexpr int SPEED = 15; -constexpr int INNER_SPEED = SPEED / 4; // 3 -constexpr double PIVOT_DURATION = 2.5; // s -constexpr double RETURN_PIVOT_DURATION = 2.5; // s - -/* ── shared flags ───────────────────────────────────────── */ -std::atomic_bool front_clear{true}; -std::atomic_bool back_clear{true}; -std::atomic_bool left_turn_clear{true}; // ← we *only* use this one -std::atomic_bool right_turn_clear{true}; // kept for wiring; ignored -std::atomic_int drive_mode{0}; -std::atomic_int room{0}; - -/* ── FSM ────────────────────────────────────────────────── */ -enum class Mode { STRAIGHT, PIVOT, RETURN_PIVOT, STOP }; -Mode mode = Mode::STRAIGHT; -double timer = 0.0; -int pivot_dir = -1; // −1 = left (right never used) - -/* ───────────────────────────────────────────────────────── */ -int main(int argc, char *argv[]) -{ - rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("wheelchair_code_module"); - - auto sensors_sub = std::make_shared(node); - auto obstacle_sub = std::make_shared( - node, front_clear, back_clear, left_turn_clear, right_turn_clear); - auto ref_pub = std::make_shared(node); - - auto electron_sub = node->create_subscription( - "electron_selfdrive", 10, - [node](std_msgs::msg::Int32::SharedPtr m){ - drive_mode.store(m->data, std::memory_order_relaxed); - RCLCPP_INFO(node->get_logger(), "[RX] drive-mode=%d", m->data); - }); - - auto electron_room = node->create_subscription( - "electron_room", 10, - [node](std_msgs::msg::Int32::SharedPtr m) - { - room.store(m->data, std::memory_order_relaxed); - //RCLCPP_INFO(node->get_logger(), "[RX] drive-mode=%d", m->data); - }); - - rclcpp::executors::SingleThreadedExecutor exec; - exec.add_node(node); - std::thread spin_thread([&]{ exec.spin(); }); - - rclcpp::Rate loop(30); - - while (rclcpp::ok()) - { - RefSpeed cmd{SPEED, SPEED}; - - /* ─ mode 0: joystick passthrough ─ */ - if (drive_mode.load() == 0) { - auto s = sensors_sub->get_latest_sensor_data(); - cmd.leftSpeed = s.left_speed; - cmd.rightSpeed = s.right_speed; - - //temp code for the heading - //auto heading_value = heading(s.magnetic_field_x, s.magnetic_field_y); - //RCLCPP_INFO(node->get_logger(), "Heading: %.2f degrees", heading_value); - - ref_pub->trigger_publish(cmd); - loop.sleep(); - continue; - } - /* ─ mode 2: autonomous ─ */ - if (drive_mode.load() == 2) { - static float target_angle = -90.0; - static float curr_angle = 0; - static float prev_error = 0.0f; - static bool target_reached = false; - static auto prevTime = std::chrono::steady_clock::now(); // initilization - RefSpeed cmd{0, 0}; - - - //used to set the angle during testing - int room_val = room.load(); - if(room_val == 419 && !target_reached){ - target_angle = 90; - } else if (room_val == 400 && !target_reached){ - target_angle = -90; - } - - auto sensorValue = sensors_sub->get_latest_sensor_data(); - float gyro_vel_z = sensorValue.angular_velocity_z * (180.0f / M_PI); - auto currentTime = std::chrono::steady_clock::now(); - std::chrono::duration elapsed = currentTime - prevTime; - double dt = elapsed.count(); // seconds - - curr_angle += gyro_vel_z * dt; - - if(target_reached){ - RCLCPP_INFO(node->get_logger(), "Target angle reached"); - curr_angle = 0; - cmd= RefSpeed{0, 0}; - ref_pub->trigger_publish(cmd); - loop.sleep(); - continue; - } - - //PD controller - float error = target_angle - curr_angle; - float derivative = (error - prev_error) / dt; - - const float Kp = 1.0f; - const float Kd = 0.2f; - - float output = abs(Kp * error + Kd * derivative); - - const float maxSpeed = SPEED; - output = std::clamp(output, -maxSpeed, maxSpeed); - - if(std::abs(error) < 0){ - target_reached = true; - cmd = RefSpeed{0, 0}; - ref_pub->trigger_publish(cmd); - loop.sleep(); - continue; - } - - if(target_angle < 0){ //right is negative angle - cmd = RefSpeed{static_cast(output), 0}; - } else if(target_angle > 0){ //left is positive angle - cmd = RefSpeed{0, static_cast(output)}; - } - - ref_pub->trigger_publish(cmd); - - RCLCPP_INFO(node->get_logger(), "Turning… angle = %.3f deg / %.3f deg", curr_angle, target_angle); - RCLCPP_INFO(node->get_logger(), "Error = %.3f ", std::abs(error)); - //RCLCPP_INFO(node->get_logger(), "Angle = %.2f deg, Output speed = %.2f", curr_angle, output); - - prev_error = error; - - // if(abs(curr_angle) < target_angle){ - // RefSpeed cmd{0, output}; - // ref_pub->trigger_publish(cmd); - // RCLCPP_INFO(node->get_logger(), "Turning… angle = %.3f deg / %.3f deg", curr_angle, target_angle); - // } else { - // RefSpeed cmd{0, 0}; - // ref_pub->trigger_publish(cmd); - // RCLCPP_INFO(node->get_logger(), "Reached target angle"); - // } - - prevTime = currentTime; - - loop.sleep(); - continue; - } - - /* mode extra*/ - loop.sleep(); - } - - exec.cancel(); - spin_thread.join(); - rclcpp::shutdown(); - return 0; -} diff --git a/src/heading.cpp b/src/heading.cpp deleted file mode 100644 index 04cde49..0000000 --- a/src/heading.cpp +++ /dev/null @@ -1,14 +0,0 @@ -// Used to determine the heading -#include - -const float PI = 3.14159265f; - -float heading(float mag_y, float mag_x){ - float heading = atan2f(mag_y, -mag_x) * 180 / PI; - if (heading < 0){ - heading += 360.0; - } - - //TODO could use the declination to get it in terms of true north - return heading; -} diff --git a/src/light_publisher.cpp b/src/light_publisher.cpp deleted file mode 100644 index 5e3b0cd..0000000 --- a/src/light_publisher.cpp +++ /dev/null @@ -1,18 +0,0 @@ -// -// Created by leslier on 2/13/25. -// - -#include "light_publisher.hpp" - - -LightPublisher::LightPublisher(rclcpp::Node::SharedPtr node) -: node_(node) -{ - publisher_ = node_->create_publisher("light", 10); -} - -void LightPublisher::trigger_publish(LightStates light_state) { - auto message = wheelchair_sensor_msgs::msg::Light(); - message.state = static_cast(light_state); - this->publisher_->publish(message); -} \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index 784ea08..aa6ddb1 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,4 +1,4 @@ -/* main.cpp – d6 = hard stop, d2 = 90° spin then pivot-FSM +/* main.cpp * * drive_mode * 0 : raw joystick @@ -7,7 +7,6 @@ */ #include "main.h" -#include "uwb_subscriber.hpp" #include "rclcpp/rclcpp.hpp" #include "sensors_subscriber.hpp" #include "obstacle_subscriber.hpp" @@ -20,13 +19,6 @@ /* ── constants ─────────────────────────────────────────── */ constexpr int SPEED = 30; constexpr int INNER_SPEED = 0; -constexpr double PIVOT_TIME = 2.5; -constexpr double RETURN_TIME = 2.5; -constexpr float UWB_STOP_RANGE = 4.5; // d6 ≤ 2.15 m → hard stop -constexpr float UWB_TURN_RANGE = 3.0; // d2 ≤ 3.0 m → spin + FSM -constexpr double TURN90_TIME = 3.2; -constexpr double UWB_STOPPING_FIRST = 1.9; - /* ── shared flags set by subscribers ───────────────────── */ std::atomic_bool front_clear{true}; @@ -36,17 +28,6 @@ std::atomic_bool right_turn_clear{true}; std::atomic_int drive_mode{0}; std::atomic_int room{0}; -/* ── FSM state for complex part ────────────────────────── */ -enum class Mode { STRAIGHT, PIVOT, RETURN, STOP }; -Mode mode = Mode::STRAIGHT; -double timer = 0.0; -int pivot_dir = -1; // −1 = first swing left - -/* ── helper for one-shot 90° turn on d2 trigger ────────── */ -bool turning90 = false; -double turn_t = 0.0; -bool after_spin = false; // ensures we enter FSM right after spin - /* ─────────────────────────────────────────────────────── */ int main(int argc, char* argv[]) { @@ -69,19 +50,6 @@ int main(int argc, char* argv[]) }); - auto electron_room = node->create_subscription( - "electron_room", 10, - [node](std_msgs::msg::Int32::SharedPtr m) - { - room.store(m->data, std::memory_order_relaxed); - RCLCPP_INFO(node->get_logger(), "[RX] drive-mode=%d", m->data); - }); - - - /* receive message from UWB sensors */ - auto uwb_sub = std::make_shared(node); - - /* spin ROS callbacks in a background thread */ rclcpp::executors::SingleThreadedExecutor exec; exec.add_node(node); @@ -133,163 +101,29 @@ int main(int argc, char* argv[]) bool f_ok = front_clear.load(); bool b_ok = !back_clear.load(std::memory_order_relaxed); bool l_ok = left_turn_clear.load(); - //float d6 = 0.0; // front beacon - float d2 = uwb_sub->dist2(); // left-flank sensor - float d3 = uwb_sub->dist3(); // left-flank sensor - - RCLCPP_INFO(node->get_logger(), - "UWB2: %f", d2); - RCLCPP_INFO(node->get_logger(), - "UWB3: %f", d3); - - - if (room_val == 419 && d3 > UWB_STOPPING_FIRST && turning90 == false){ - RefSpeed cmd{SPEED, SPEED}; - if (!f_ok && !b_ok) { // both ways blocked - cmd = {0, 0}; - } else if (!f_ok) { // stop forward - if (cmd.leftSpeed > 0) cmd.leftSpeed = 0; - if (cmd.rightSpeed > 0) cmd.rightSpeed = 0; - } else if (!b_ok) { // stop reverse - if (cmd.leftSpeed < 0) cmd.leftSpeed = 0; - if (cmd.rightSpeed < 0) cmd.rightSpeed = 0; - } - ref_pub->trigger_publish(cmd); - loop.sleep(); - continue; - } - - if (room_val == 419 && d3 > 0 && d3 < UWB_STOPPING_FIRST && turning90 == false) - { - RefSpeed cmd{0, 0}; - ref_pub->trigger_publish(cmd); - loop.sleep(); - continue; - - } - - /* d2 trigger → one 90° spin (left) then hand off to FSM */ - if (room_val == 400 && d2 > 0 && d2 < UWB_TURN_RANGE && turning90 == false) - { - turning90 = true; - spin_start = steady_clock.now(); - turn_t = 0.0; - RCLCPP_INFO(node->get_logger(), "Starting turn"); - loop.sleep(); - continue; - - } - - if (room_val == 400 && turning90 == false) { - - bool front_ok = front_clear.load(); - bool back_ok = !back_clear.load(std::memory_order_relaxed); - RefSpeed cmd{SPEED, SPEED}; - if (!front_ok && !back_ok) { // both ways blocked - cmd = {0, 0}; - } else if (!front_ok) { // stop forward - if (cmd.leftSpeed > 0) cmd.leftSpeed = 0; - if (cmd.rightSpeed > 0) cmd.rightSpeed = 0; - } else if (!back_ok) { // stop reverse - if (cmd.leftSpeed < 0) cmd.leftSpeed = 0; - if (cmd.rightSpeed < 0) cmd.rightSpeed = 0; - } - ref_pub->trigger_publish(cmd); - loop.sleep(); - continue; + RefSpeed cmd{SPEED, SPEED}; + if (!f_ok && !b_ok) { // both ways blocked + cmd = {0, 0}; + } else if (!f_ok) { // stop forward + if (cmd.leftSpeed > 0) cmd.leftSpeed = 0; + if (cmd.rightSpeed > 0) cmd.rightSpeed = 0; + } else if (!b_ok) { // stop reverse + if (cmd.leftSpeed < 0) cmd.leftSpeed = 0; + if (cmd.rightSpeed < 0) cmd.rightSpeed = 0; } - if(turning90 == true && !after_spin){ - double elapsed = (steady_clock.now() - spin_start).seconds(); - RCLCPP_INFO(node->get_logger(), "Turning"); - if (elapsed < TURN90_TIME) { - RefSpeed cmd{0, SPEED}; - ref_pub->trigger_publish(cmd); - RCLCPP_INFO(node->get_logger(), - "Turning… elapsed = %.3f s / %.3f s", - elapsed, TURN90_TIME); - loop.sleep(); - continue; - } - /* spin finished */ - //turning90 = false; - after_spin = true; - cmd = {0,0}; - ref_pub->trigger_publish(cmd); loop.sleep(); continue; - - } - - - /* After the spin, immediately run the complex FSM */ - if (after_spin) - { - float d6 = uwb_sub->dist6(); - - /* d6 beacon → hard stop */ - if (d6 > 0.f && d6 < UWB_STOP_RANGE) - { - cmd = {0,0}; - ref_pub->trigger_publish(cmd); loop.sleep(); continue; - } - - bool front_ok = front_clear.load(); - bool left_ok = left_turn_clear.load(); - switch (mode) - { - case Mode::STRAIGHT: - if (!front_ok) { - if (!left_ok) mode = Mode::STOP; - else { pivot_dir = -1; timer = 0.0; mode = Mode::PIVOT; } - } - break; - - case Mode::PIVOT: - - if (!left_ok) { mode = Mode::STOP; break; } - timer += DT; - cmd.leftSpeed = (pivot_dir == +1) ? SPEED : INNER_SPEED; - cmd.rightSpeed = (pivot_dir == +1) ? INNER_SPEED : SPEED; - if (timer >= PIVOT_TIME) { - pivot_dir = -pivot_dir; - timer = 0; mode = Mode::RETURN; - } - break; - - case Mode::RETURN: - - if (!left_ok) { mode = Mode::STOP; break; } - - timer += DT; - cmd.leftSpeed = (pivot_dir == +1) ? SPEED : INNER_SPEED; - cmd.rightSpeed = (pivot_dir == +1) ? INNER_SPEED : SPEED; - if (timer >= RETURN_TIME) { - - timer = 0; mode = front_ok ? Mode::STRAIGHT : Mode::STOP; - - } - break; - - case Mode::STOP: - cmd = {0,0}; - - if (front_ok) mode = Mode::STRAIGHT; - - break; - } - - ref_pub->trigger_publish(cmd); loop.sleep(); continue; - } - - /* If none of the above, just keep going straight */ - ref_pub->trigger_publish(cmd); loop.sleep(); continue; + ref_pub->trigger_publish(cmd); + loop.sleep(); + continue; } loop.sleep(); // unknown drive_mode } - exec.cancel(); spin_thread.join(); + exec.cancel(); + spin_thread.join(); rclcpp::shutdown(); return 0; } diff --git a/src/main_1.cpp b/src/main_1.cpp deleted file mode 100644 index 5fcef7a..0000000 --- a/src/main_1.cpp +++ /dev/null @@ -1,203 +0,0 @@ -/* main.cpp – left-pivot obstacle avoidance - drive_mode: 0 = manual / joystick pass-through - 1 = joystick + safety - 2 = autonomous */ - -#include "main.h" -#include "uwb_subscriber.hpp" -#include "rclcpp/rclcpp.hpp" -#include "sensors_subscriber.hpp" -#include "obstacle_subscriber.hpp" -#include "ref_speed_publisher.hpp" -#include -#include -#include - -/* ── config ─────────────────────────────────────────────── */ -constexpr int SPEED = 100; -constexpr int INNER_SPEED = SPEED / 4; // 3 -constexpr double PIVOT_DURATION = 2.5; // s -constexpr double RETURN_PIVOT_DURATION = 2.5; // s -constexpr float UWB_STOP_RANGE = 2.15; -constexpr float UWB_TURN_RANGE = 3; //change accordingly - -/* ── shared flags ───────────────────────────────────────── */ -std::atomic_bool front_clear{true}; -std::atomic_bool back_clear{true}; -std::atomic_bool left_turn_clear{true}; // we only pivot left -std::atomic_bool right_turn_clear{true}; // kept for wiring -std::atomic_int drive_mode{0}; -std::atomic uwb_dist6{0.0f}; - -/* ── FSM for autonomous mode ────────────────────────────── */ -enum class Mode { STRAIGHT, PIVOT, RETURN_PIVOT, STOP }; -Mode mode = Mode::STRAIGHT; -double timer = 0.0; -int pivot_dir = -1; // −1 = left (right never used) - -/* ───────────────────────────────────────────────────────── */ -int main(int argc, char *argv[]) -{ - rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("wheelchair_code_module"); - - /* pubs / subs */ - auto sensors_sub = std::make_shared(node); - auto obstacle_sub = std::make_shared( - node, front_clear, back_clear, left_turn_clear, right_turn_clear); - auto ref_pub = std::make_shared(node); - - /* receive drive-mode commands from Electron GUI */ - auto electron_sub = node->create_subscription( - "electron_selfdrive", 10, - [node](std_msgs::msg::Int32::SharedPtr m) - { - drive_mode.store(m->data, std::memory_order_relaxed); - RCLCPP_INFO(node->get_logger(), "[RX] drive-mode=%d", m->data); - }); - - /* receive message from UWB sensors */ - auto uwb_sub = std::make_shared(node); - - /* spin ROS callbacks in a background thread */ - rclcpp::executors::SingleThreadedExecutor exec; - exec.add_node(node); - std::thread spin_thread([&]{ exec.spin(); }); - - /* main control loop – 30 Hz */ - rclcpp::Rate loop(30); - const double DT = 1.0 / 30.0; - - while (rclcpp::ok()) - { - /* Default command: straight at SPEED */ - RefSpeed cmd{SPEED, SPEED}; - - /* ─────────── MODE 0 : manual / pure joystick ─────────── */ - if (drive_mode.load() == 0) - { - auto s = sensors_sub->get_latest_sensor_data(); - cmd.leftSpeed = s.left_speed; - cmd.rightSpeed = s.right_speed; - - ref_pub->trigger_publish(cmd); - loop.sleep(); - continue; // skip rest of loop - } - - /* ─────────── MODE 1 : joystick + obstacle safety ─────── */ - else if (drive_mode.load() == 1) - { - /* grab latest joystick input */ - auto s = sensors_sub->get_latest_sensor_data(); - cmd.leftSpeed = s.left_speed; - cmd.rightSpeed = s.right_speed; - - bool front_ok = front_clear.load(); // true = path clear - bool back_ok = back_clear.load(); - - RefSpeed ref_speed; - - if (!front_ok && !back_ok) // both directions blocked - { - ref_speed = {0, 0}; - } - else - { - /* block forward if front is NOT clear */ - if (!front_ok) - { - if (cmd.leftSpeed > 0) cmd.leftSpeed = 0; - if (cmd.rightSpeed > 0) cmd.rightSpeed = 0; - } - - /* block reverse if back is NOT clear */ - if (!back_ok) - { - if (cmd.leftSpeed < 0) cmd.leftSpeed = 0; - if (cmd.rightSpeed < 0) cmd.rightSpeed = 0; - } - - ref_speed = cmd; // safe command - } - - ref_pub->trigger_publish(ref_speed); - loop.sleep(); - continue; - } - - /* ─────────── MODE 2 : autonomous left-pivot logic ────── */ - else if (drive_mode.load() == 2) - { - bool front_ok = front_clear.load(); - bool left_ok = left_turn_clear.load(); - float d6 = uwb_sub->dist6(); - bool uwb_clear = d6 > UWB_STOP_RANGE; - bool uwb_turn_clear = (d2 > 0.0f) && (d2 > UWB_TURN_RANGE); - bool uwb_blocked = (d6 > 0.0f) && (d6 <= UWB_STOP_RANGE); - if (uwb_blocked) { - mode = Mode::STOP; - } - - switch (mode) - { - /* driving straight until obstacle */ - case Mode::STRAIGHT: - if (!front_ok) // front blocked - { - if (!left_ok || !uwb_clear) - { mode = Mode::STOP; } // nowhere to go - else - { pivot_dir = -1; // always pivot left - timer = 0.0; - mode = Mode::PIVOT; - } - } - break; - - /* pivot in place to the left */ - case Mode::PIVOT: - if (!left_ok) { mode = Mode::STOP; break; } - timer += DT; - cmd.leftSpeed = (pivot_dir == +1) ? SPEED : INNER_SPEED; - cmd.rightSpeed = (pivot_dir == +1) ? INNER_SPEED : SPEED; - if (timer >= PIVOT_DURATION) { - pivot_dir = -pivot_dir; // swing back right - timer = 0; - mode = Mode::RETURN_PIVOT; - } - break; - - /* pivot back slightly to realign */ - case Mode::RETURN_PIVOT: - if (!left_ok) { mode = Mode::STOP; break; } - timer += DT; - cmd.leftSpeed = (pivot_dir == +1) ? SPEED : INNER_SPEED; - cmd.rightSpeed = (pivot_dir == +1) ? INNER_SPEED : SPEED; - if (timer >= RETURN_PIVOT_DURATION) { - timer = 0; - mode = front_ok ? Mode::STRAIGHT : Mode::STOP; - } - break; - - /* emergency stop */ - case Mode::STOP: - cmd.leftSpeed = cmd.rightSpeed = 0; - if (front_ok) mode = Mode::STRAIGHT; - break; - } - - ref_pub->trigger_publish(cmd); - loop.sleep(); - continue; - } - - loop.sleep(); - } - - /* clean shutdown */ - exec.cancel(); - spin_thread.join(); - rclcpp::shutdown(); - return 0; -} diff --git a/src/main_2.cpp b/src/main_2.cpp deleted file mode 100644 index 9e9ba69..0000000 --- a/src/main_2.cpp +++ /dev/null @@ -1,130 +0,0 @@ -#include "rclcpp/rclcpp.hpp" -#include "sensors_subscriber.h" - -#include -#include -#include -#include -#include - -using namespace std::chrono_literals; - -// Speed constants -const float SPEED_MAX = 3.0f; // Full speed -const float SPEED_CAUTION = 2.0f; // Reduced speed -const float SPEED_STOP = 0.0f; // Stop - -// IMU thresholds (example values) -const float ACC_LOW_THRESHOLD = 7.0f; // Below normal acceleration threshold -const float ACC_HIGH_THRESHOLD = 12.0f; // Above normal acceleration threshold - -// Finite State Machine (FSM) states -enum class RobotState { - NORMAL, // Full speed - CAUTION, // Reduced speed - STOP // Full stop -}; - -class StateMachine { -public: - StateMachine() : current_state_(RobotState::NORMAL) {} - - // Update the FSM state based on the computed speed limit - void update(float speed_limit) { - if (speed_limit == SPEED_STOP) { - current_state_ = RobotState::STOP; - } else if (speed_limit == SPEED_CAUTION) { - current_state_ = RobotState::CAUTION; - } else { - current_state_ = RobotState::NORMAL; - } - } - - std::string getStateName() const { - switch (current_state_) { - case RobotState::NORMAL: return "NORMAL"; - case RobotState::CAUTION: return "CAUTION"; - case RobotState::STOP: return "STOP"; - default: return "UNKNOWN"; - } - } - -private: - RobotState current_state_; -}; - -class WheelchairNode : public rclcpp::Node { -public: - WheelchairNode() : Node("wheelchair_node") { - // Create a timer to periodically update the FSM using live sensor data (every 500ms) - timer_ = this->create_wall_timer(500ms, std::bind(&WheelchairNode::updateFSM, this)); - RCLCPP_INFO(this->get_logger(), "Wheelchair node started."); - } - - // Call this after the node is fully constructed - void initialize() { - // Create the sensor subscriber. - // SensorsSubscriber is assumed to accept a rclcpp::Node::SharedPtr. - sensor_subscriber_ = std::make_shared(this->shared_from_this()); - } - -private: - // This method polls the latest sensor data, computes a decision, and updates the FSM. - void updateFSM() { - // Retrieve the latest sensor data. - auto sensor_data = sensor_subscriber_->get_latest_sensor_data(); - - // Ultrasonic Sensor Logic - float ultrasonic_distance = sensor_data.ultrasonic_front_0; - float ultrasonic_speed_limit = SPEED_MAX; - if (ultrasonic_distance >= 1.0f) { - ultrasonic_speed_limit = SPEED_MAX; - } else if (ultrasonic_distance >= 0.5f) { - ultrasonic_speed_limit = SPEED_CAUTION; - } else { - ultrasonic_speed_limit = SPEED_STOP; - } - - // IMU Sensor Logic - // Compute acceleration magnitude from the IMU data. - float ax = sensor_data.linear_acceleration_x; - float ay = sensor_data.linear_acceleration_y; - float az = sensor_data.linear_acceleration_z; - float accMag = std::sqrt(ax * ax + ay * ay + az * az); - float imu_speed_limit = SPEED_MAX; - // If acceleration is too low or too high, assume a dangerous condition and require stop. - if (accMag < ACC_LOW_THRESHOLD || accMag > ACC_HIGH_THRESHOLD) { - imu_speed_limit = SPEED_STOP; - } - - // Combine Decisions - float final_speed_limit = std::min(ultrasonic_speed_limit, imu_speed_limit); - - // Update the FSM with the final computed speed limit. - fsm_.update(final_speed_limit); - - // Log the sensor readings, computed limits, and current FSM state. - RCLCPP_INFO(this->get_logger(), - "Ultrasonic: %.2f m, IMU acc: %.2f, Final Speed: %.2f, FSM State: %s", - ultrasonic_distance, accMag, final_speed_limit, fsm_.getStateName().c_str()); - } - - rclcpp::TimerBase::SharedPtr timer_; - std::shared_ptr sensor_subscriber_; - StateMachine fsm_; -}; - -int main(int argc, char * argv[]) { - rclcpp::init(argc, argv); - - // Create the node using std::make_shared to allow shared_from_this() - auto node = std::make_shared(); - // Initialize the sensor subscriber now that the node is fully constructed. - node->initialize(); - - // Spin to process callbacks (both for the sensor subscriber and the timer) - rclcpp::spin(node); - - rclcpp::shutdown(); - return 0; -} diff --git a/src/main_3.cpp b/src/main_3.cpp deleted file mode 100644 index 923bdcd..0000000 --- a/src/main_3.cpp +++ /dev/null @@ -1,458 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "sensors_subscriber.h" - -// Speed constants -static constexpr float SPEED_MAX = 3.0f; -static constexpr float SPEED_CAUTION = 2.0f; -static constexpr float SPEED_STOP = 0.0f; - -// IMU thresholds -static constexpr float ACC_LOW_THRESHOLD = 7.0f; -static constexpr float ACC_HIGH_THRESHOLD = 12.0f; - -// Robot states -enum class RobotState { - NORMAL, - CAUTION, - STOP -}; - -class StateMachine { -public: - StateMachine() : current_state_(RobotState::NORMAL) {} - - void update(float speed_limit) { - if (speed_limit == SPEED_STOP) { - current_state_ = RobotState::STOP; - } else if (speed_limit == SPEED_CAUTION) { - current_state_ = RobotState::CAUTION; - } else { - current_state_ = RobotState::NORMAL; - } - } - - std::string getStateName() const { - switch (current_state_) { - case RobotState::NORMAL: return "NORMAL"; - case RobotState::CAUTION: return "CAUTION"; - case RobotState::STOP: return "STOP"; - default: return "UNKNOWN"; - } - } - -private: - RobotState current_state_; -}; - -// A small struct for bounding boxes -struct BoundingBox { - int cluster_id; - Eigen::Vector3f min_pt; - Eigen::Vector3f max_pt; - float distance; // from LiDAR origin -}; - -class WheelchairNode : public rclcpp::Node -{ -public: - WheelchairNode() - : Node("wheelchair_node") - { - // LiDAR subscription - lidar_sub_ = create_subscription( - "/livox/lidar", - 10, - std::bind(&WheelchairNode::cloudCallback, this, std::placeholders::_1) - ); - RCLCPP_INFO(get_logger(), "Subscribed to /livox/lidar."); - - // PCL Visualizer - viewer_ = std::make_shared("Refined LiDAR Clusters"); - viewer_->setBackgroundColor(0, 0, 0); - - // Timer to update the PCL viewer ~20 Hz - viewer_timer_ = create_wall_timer( - std::chrono::milliseconds(50), - std::bind(&WheelchairNode::updateViewer, this) - ); - - // FSM Timer (0.5 s) - fsm_timer_ = create_wall_timer( - std::chrono::milliseconds(500), - std::bind(&WheelchairNode::updateFSM, this) - ); - - RCLCPP_INFO(get_logger(), "WheelchairNode constructor done."); - } - - // Call this after node is constructed via std::make_shared - void initialize() - { - sensor_subscriber_ = std::make_shared(this->shared_from_this()); - RCLCPP_INFO(get_logger(), "SensorsSubscriber created in initialize()."); - } - -private: - // LiDAR Callback - void cloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) - { - pcl::PointCloud::Ptr cloud_in(new pcl::PointCloud); - pcl::fromROSMsg(*msg, *cloud_in); - - if (cloud_in->empty()) { - RCLCPP_WARN(get_logger(), "Received an empty cloud. Skipping processing."); - return; - } - - // Downsample - pcl::PointCloud::Ptr down(new pcl::PointCloud); - { - pcl::VoxelGrid voxel; - voxel.setInputCloud(cloud_in); - voxel.setLeafSize(0.1f, 0.1f, 0.1f); - voxel.filter(*down); - } - - // Segment ground - pcl::SACSegmentation seg; - seg.setOptimizeCoefficients(true); - seg.setModelType(pcl::SACMODEL_PLANE); - seg.setMethodType(pcl::SAC_RANSAC); - seg.setDistanceThreshold(0.2f); - - pcl::PointIndices::Ptr inliers(new pcl::PointIndices); - pcl::ModelCoefficients::Ptr coeffs(new pcl::ModelCoefficients); - seg.setInputCloud(down); - seg.segment(*inliers, *coeffs); - - pcl::PointCloud::Ptr ground(new pcl::PointCloud); - pcl::PointCloud::Ptr obstacles(new pcl::PointCloud); - - if (!inliers->indices.empty()) { - pcl::ExtractIndices extract; - extract.setInputCloud(down); - extract.setIndices(inliers); - extract.setNegative(false); - extract.filter(*ground); - - extract.setNegative(true); - extract.filter(*obstacles); - } else { - obstacles = down; // No plane => everything is obstacles - } - - // Remove outliers - { - pcl::StatisticalOutlierRemoval sor; - sor.setInputCloud(obstacles); - sor.setMeanK(20); - sor.setStddevMulThresh(2.0); - sor.filter(*obstacles); - } - - // Cluster - std::vector cluster_indices; - { - pcl::search::KdTree::Ptr tree(new pcl::search::KdTree); - tree->setInputCloud(obstacles); - - pcl::EuclideanClusterExtraction ec; - ec.setClusterTolerance(0.2f); - ec.setMinClusterSize(30); - ec.setSearchMethod(tree); - ec.setInputCloud(obstacles); - ec.extract(cluster_indices); - } - - // Compute bounding boxes - std::vector boxes; - boxes.reserve(cluster_indices.size()); - int cluster_id = 0; - - for (auto & group : cluster_indices) { - Eigen::Vector3f min_pt( - std::numeric_limits::max(), - std::numeric_limits::max(), - std::numeric_limits::max() - ); - Eigen::Vector3f max_pt( - -std::numeric_limits::max(), - -std::numeric_limits::max(), - -std::numeric_limits::max() - ); - - for (int idx : group.indices) { - auto & pt = (*obstacles)[idx]; - min_pt.x() = std::min(min_pt.x(), pt.x); - min_pt.y() = std::min(min_pt.y(), pt.y); - min_pt.z() = std::min(min_pt.z(), pt.z); - max_pt.x() = std::max(max_pt.x(), pt.x); - max_pt.y() = std::max(max_pt.y(), pt.y); - max_pt.z() = std::max(max_pt.z(), pt.z); - } - - Eigen::Vector3f center = (min_pt + max_pt)*0.5f; - float distance = center.norm(); - - BoundingBox bb; - bb.cluster_id = cluster_id++; - bb.min_pt = min_pt; - bb.max_pt = max_pt; - bb.distance = distance; - - boxes.push_back(bb); - } - - // Filter by bounding box size - std::vector final_bboxes; - for (auto & b : boxes) { - Eigen::Vector3f size = b.max_pt - b.min_pt; - float dx = size.x(), dy = size.y(), dz = size.z(); - bool pass_max = (dx < 3.0f && dy < 3.0f && dz < 3.0f); - bool pass_min = (dx > 0.1f || dy > 0.1f || dz > 0.1f); - if (pass_max && pass_min) { - final_bboxes.push_back(b); - } - } - - // Store for FSM & viewer - { - std::lock_guard lock(data_mutex_); - ground_cloud_ = ground; - obstacles_cloud_ = obstacles; - cluster_indices_ = cluster_indices; - final_bboxes_ = final_bboxes; - } - - // Debug prints - RCLCPP_INFO(get_logger(), - "LiDAR => Down:%zu, Ground:%zu, Obs:%zu, Clusters:%zu, BBoxes:%zu", - down->size(), - ground->size(), - obstacles->size(), - cluster_indices.size(), - final_bboxes.size() - ); - for (auto & b : final_bboxes) { - RCLCPP_INFO(get_logger(), - " -> Cluster %d: distance= %.2f m", b.cluster_id, b.distance); - } - } - - // PCL Viewer ~20 Hz - void updateViewer() - { - pcl::PointCloud::Ptr ground, obstacles; - std::vector clusters; - std::vector bboxes; - - { - std::lock_guard lock(data_mutex_); - ground = ground_cloud_; - obstacles = obstacles_cloud_; - clusters = cluster_indices_; - bboxes = final_bboxes_; - } - - if (!ground || !obstacles) { - return; - } - - viewer_->removeAllPointClouds(); - viewer_->removeAllShapes(); - - // Ground in green - { - pcl::visualization::PointCloudColorHandlerCustom green(ground, 0, 255, 0); - viewer_->addPointCloud(ground, green, "ground_cloud"); - } - - // Obstacles in white - { - pcl::visualization::PointCloudColorHandlerCustom white(obstacles, 255, 255, 255); - viewer_->addPointCloud(obstacles, white, "obstacles_cloud"); - } - - // Each cluster in random color + bounding boxes - int i = 0; - for (auto & group : clusters) { - pcl::PointCloud::Ptr ccloud(new pcl::PointCloud); - for (int idx : group.indices) { - ccloud->push_back((*obstacles)[idx]); - } - - uint8_t r = std::rand() % 256; - uint8_t g = std::rand() % 256; - uint8_t b = std::rand() % 256; - pcl::visualization::PointCloudColorHandlerCustom color(ccloud, r, g, b); - - std::string cname = "cluster_" + std::to_string(i); - viewer_->addPointCloud(ccloud, color, cname); - - // Matching bounding box - for (auto & box : bboxes) { - if (box.cluster_id == i) { - viewer_->addCube( - box.min_pt.x(), box.max_pt.x(), - box.min_pt.y(), box.max_pt.y(), - box.min_pt.z(), box.max_pt.z(), - static_cast(r)/255.0, - static_cast(g)/255.0, - static_cast(b)/255.0, - "bbox_" + std::to_string(i) - ); - viewer_->setShapeRenderingProperties( - pcl::visualization::PCL_VISUALIZER_REPRESENTATION, - pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME, - "bbox_" + std::to_string(i) - ); - - // Distance text - Eigen::Vector3f c = (box.min_pt + box.max_pt)*0.5f; - char dist[64]; - std::snprintf(dist, 64, "Dist: %.2f m", box.distance); - viewer_->addText3D( - dist, - pcl::PointXYZ(c.x(), c.y(), box.max_pt.z() + 0.3f), - 0.2f, - 1.0, 1.0, 1.0, - "dist_text_" + std::to_string(i) - ); - break; - } - } - - i++; - } - - viewer_->spinOnce(10); - } - - // Timer: FSM logic ~0.5 s - void updateFSM() - { - // 1) Copy bounding boxes - std::vector bboxes_copy; - { - std::lock_guard lock(data_mutex_); - bboxes_copy = final_bboxes_; - } - - // 2) Retrieve sensor data - auto sensor_data = sensor_subscriber_->get_latest_sensor_data(); - - // Print the raw sensor data before final speed calc - RCLCPP_INFO(get_logger(), - "[SENSORS] Ultras=%.2f, IMU=(%.2f, %.2f, %.2f), PIR=(%s,%s,%s,%s)", - sensor_data.ultrasonic_front_0, - sensor_data.linear_acceleration_x, - sensor_data.linear_acceleration_y, - sensor_data.linear_acceleration_z, - (sensor_data.pir_front ? "true" : "false"), - (sensor_data.pir_back ? "true" : "false"), - (sensor_data.pir_left ? "true" : "false"), - (sensor_data.pir_right ? "true" : "false") - ); - - // Ultrasonic => speed limit - float ultrasonic = sensor_data.ultrasonic_front_0; - float speed_ultr = (ultrasonic >= 1.0f) ? SPEED_MAX : - (ultrasonic >= 0.5f) ? SPEED_CAUTION : SPEED_STOP; - - // IMU => speed limit - float ax = sensor_data.linear_acceleration_x; - float ay = sensor_data.linear_acceleration_y; - float az = sensor_data.linear_acceleration_z; - float accMag = std::sqrt(ax*ax + ay*ay + az*az); - - float speed_imu = SPEED_MAX; - if ((accMag < ACC_LOW_THRESHOLD) || (accMag > ACC_HIGH_THRESHOLD)) { - speed_imu = SPEED_STOP; - } - - float final_speed = std::min(speed_ultr, speed_imu); - - // PIR => any triggered => STOP - bool pir_triggered = (sensor_data.pir_front || - sensor_data.pir_back || - sensor_data.pir_left || - sensor_data.pir_right); - if (pir_triggered) { - final_speed = std::min(final_speed, SPEED_STOP); - } - - // LiDAR => if cluster <1.0 m => STOP - for (auto & box : bboxes_copy) { - if (box.distance < 1.0f) { - final_speed = std::min(final_speed, SPEED_STOP); - } - } - - // Update FSM - fsm_.update(final_speed); - - // Print final speed/state - RCLCPP_INFO(get_logger(), - "[FSM] Ultras=%.2f, IMU=%.2f, PIR=%s, Clusters=%zu => Speed=%.2f, State=%s", - ultrasonic, - accMag, - (pir_triggered ? "true" : "false"), - bboxes_copy.size(), - final_speed, - fsm_.getStateName().c_str() - ); - } - - // Member Variables - rclcpp::Subscription::SharedPtr lidar_sub_; - - // Timers - rclcpp::TimerBase::SharedPtr viewer_timer_; - rclcpp::TimerBase::SharedPtr fsm_timer_; - - // LiDAR data (protected by mutex) - std::mutex data_mutex_; - pcl::PointCloud::Ptr ground_cloud_{nullptr}; - pcl::PointCloud::Ptr obstacles_cloud_{nullptr}; - std::vector cluster_indices_; - std::vector final_bboxes_; - - // PCL Visualizer - pcl::visualization::PCLVisualizer::Ptr viewer_; - - // Sensors + FSM - std::shared_ptr sensor_subscriber_; - StateMachine fsm_; -}; - -// Standard ROS 2 main -int main(int argc, char** argv) -{ - rclcpp::init(argc, argv); - - auto node = std::make_shared(); - node->initialize(); // Now safe to call shared_from_this() inside the node - - rclcpp::spin(node); - rclcpp::shutdown(); - return 0; -} diff --git a/src/main_4.cpp b/src/main_4.cpp deleted file mode 100644 index e9e5361..0000000 --- a/src/main_4.cpp +++ /dev/null @@ -1,458 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "sensors_subscriber.h" - -// Speed constants -static constexpr float SPEED_MAX = 3.0f; -static constexpr float SPEED_CAUTION = 2.0f; -static constexpr float SPEED_STOP = 0.0f; - -// IMU thresholds -static constexpr float ACC_LOW_THRESHOLD = 7.0f; -static constexpr float ACC_HIGH_THRESHOLD = 12.0f; - -// Robot states -enum class RobotState { - NORMAL, - CAUTION, - STOP -}; - -class StateMachine { -public: - StateMachine() : current_state_(RobotState::NORMAL) {} - - void update(float speed_limit) { - if (speed_limit == SPEED_STOP) { - current_state_ = RobotState::STOP; - } else if (speed_limit == SPEED_CAUTION) { - current_state_ = RobotState::CAUTION; - } else { - current_state_ = RobotState::NORMAL; - } - } - - std::string getStateName() const { - switch (current_state_) { - case RobotState::NORMAL: return "NORMAL"; - case RobotState::CAUTION: return "CAUTION"; - case RobotState::STOP: return "STOP"; - default: return "UNKNOWN"; - } - } - -private: - RobotState current_state_; -}; - -// A small struct for bounding boxes -struct BoundingBox { - int cluster_id; - Eigen::Vector3f min_pt; - Eigen::Vector3f max_pt; - float distance; // from LiDAR origin -}; - -class WheelchairNode : public rclcpp::Node -{ -public: - WheelchairNode() - : Node("wheelchair_node") - { - // LiDAR subscription - lidar_sub_ = create_subscription( - "/livox/lidar", - 10, - std::bind(&WheelchairNode::cloudCallback, this, std::placeholders::_1) - ); - RCLCPP_INFO(get_logger(), "Subscribed to /livox/lidar."); - - // PCL Visualizer - viewer_ = std::make_shared("Refined LiDAR Clusters"); - viewer_->setBackgroundColor(0, 0, 0); - - // Timer to update the PCL viewer ~20 Hz - viewer_timer_ = create_wall_timer( - std::chrono::milliseconds(50), - std::bind(&WheelchairNode::updateViewer, this) - ); - - // FSM Timer (0.5 s) - fsm_timer_ = create_wall_timer( - std::chrono::milliseconds(500), - std::bind(&WheelchairNode::updateFSM, this) - ); - - RCLCPP_INFO(get_logger(), "WheelchairNode constructor done."); - } - - // Call this after node is constructed via std::make_shared - void initialize() - { - sensor_subscriber_ = std::make_shared(this->shared_from_this()); - RCLCPP_INFO(get_logger(), "SensorsSubscriber created in initialize()."); - } - -private: - // LiDAR Callback - void cloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) - { - pcl::PointCloud::Ptr cloud_in(new pcl::PointCloud); - pcl::fromROSMsg(*msg, *cloud_in); - - if (cloud_in->empty()) { - RCLCPP_WARN(get_logger(), "Received an empty cloud. Skipping processing."); - return; - } - - // Downsample - pcl::PointCloud::Ptr down(new pcl::PointCloud); - { - pcl::VoxelGrid voxel; - voxel.setInputCloud(cloud_in); - voxel.setLeafSize(0.1f, 0.1f, 0.1f); - voxel.filter(*down); - } - - // Segment ground - pcl::SACSegmentation seg; - seg.setOptimizeCoefficients(true); - seg.setModelType(pcl::SACMODEL_PLANE); - seg.setMethodType(pcl::SAC_RANSAC); - seg.setDistanceThreshold(0.2f); - - pcl::PointIndices::Ptr inliers(new pcl::PointIndices); - pcl::ModelCoefficients::Ptr coeffs(new pcl::ModelCoefficients); - seg.setInputCloud(down); - seg.segment(*inliers, *coeffs); - - pcl::PointCloud::Ptr ground(new pcl::PointCloud); - pcl::PointCloud::Ptr obstacles(new pcl::PointCloud); - - if (!inliers->indices.empty()) { - pcl::ExtractIndices extract; - extract.setInputCloud(down); - extract.setIndices(inliers); - extract.setNegative(false); - extract.filter(*ground); - - extract.setNegative(true); - extract.filter(*obstacles); - } else { - obstacles = down; // No plane => everything is obstacles - } - - // Remove outliers - { - pcl::StatisticalOutlierRemoval sor; - sor.setInputCloud(obstacles); - sor.setMeanK(20); - sor.setStddevMulThresh(2.0); - sor.filter(*obstacles); - } - - // Cluster - std::vector cluster_indices; - { - pcl::search::KdTree::Ptr tree(new pcl::search::KdTree); - tree->setInputCloud(obstacles); - - pcl::EuclideanClusterExtraction ec; - ec.setClusterTolerance(0.2f); - ec.setMinClusterSize(30); - ec.setSearchMethod(tree); - ec.setInputCloud(obstacles); - ec.extract(cluster_indices); - } - - // Compute bounding boxes - std::vector boxes; - boxes.reserve(cluster_indices.size()); - int cluster_id = 0; - - for (auto & group : cluster_indices) { - Eigen::Vector3f min_pt( - std::numeric_limits::max(), - std::numeric_limits::max(), - std::numeric_limits::max() - ); - Eigen::Vector3f max_pt( - -std::numeric_limits::max(), - -std::numeric_limits::max(), - -std::numeric_limits::max() - ); - - for (int idx : group.indices) { - auto & pt = (*obstacles)[idx]; - min_pt.x() = std::min(min_pt.x(), pt.x); - min_pt.y() = std::min(min_pt.y(), pt.y); - min_pt.z() = std::min(min_pt.z(), pt.z); - max_pt.x() = std::max(max_pt.x(), pt.x); - max_pt.y() = std::max(max_pt.y(), pt.y); - max_pt.z() = std::max(max_pt.z(), pt.z); - } - - Eigen::Vector3f center = (min_pt + max_pt)*0.5f; - float distance = center.norm(); - - BoundingBox bb; - bb.cluster_id = cluster_id++; - bb.min_pt = min_pt; - bb.max_pt = max_pt; - bb.distance = distance; - - boxes.push_back(bb); - } - - // Filter by bounding box size - std::vector final_bboxes; - for (auto & b : boxes) { - Eigen::Vector3f size = b.max_pt - b.min_pt; - float dx = size.x(), dy = size.y(), dz = size.z(); - bool pass_max = (dx < 3.0f && dy < 3.0f && dz < 3.0f); - bool pass_min = (dx > 0.1f || dy > 0.1f || dz > 0.1f); - if (pass_max && pass_min) { - final_bboxes.push_back(b); - } - } - - // Store for FSM & viewer - { - std::lock_guard lock(data_mutex_); - ground_cloud_ = ground; - obstacles_cloud_ = obstacles; - cluster_indices_ = cluster_indices; - final_bboxes_ = final_bboxes; - } - - // Debug prints - RCLCPP_INFO(get_logger(), - "LiDAR => Down:%zu, Ground:%zu, Obs:%zu, Clusters:%zu, BBoxes:%zu", - down->size(), - ground->size(), - obstacles->size(), - cluster_indices.size(), - final_bboxes.size() - ); - for (auto & b : final_bboxes) { - RCLCPP_INFO(get_logger(), - " -> Cluster %d: distance= %.2f m", b.cluster_id, b.distance); - } - } - - // PCL Viewer ~20 Hz - void updateViewer() - { - pcl::PointCloud::Ptr ground, obstacles; - std::vector clusters; - std::vector bboxes; - - { - std::lock_guard lock(data_mutex_); - ground = ground_cloud_; - obstacles = obstacles_cloud_; - clusters = cluster_indices_; - bboxes = final_bboxes_; - } - - if (!ground || !obstacles) { - return; - } - - viewer_->removeAllPointClouds(); - viewer_->removeAllShapes(); - - // Ground in green - { - pcl::visualization::PointCloudColorHandlerCustom green(ground, 0, 255, 0); - viewer_->addPointCloud(ground, green, "ground_cloud"); - } - - // Obstacles in white - { - pcl::visualization::PointCloudColorHandlerCustom white(obstacles, 255, 255, 255); - viewer_->addPointCloud(obstacles, white, "obstacles_cloud"); - } - - // Each cluster in random color + bounding boxes - int i = 0; - for (auto & group : clusters) { - pcl::PointCloud::Ptr ccloud(new pcl::PointCloud); - for (int idx : group.indices) { - ccloud->push_back((*obstacles)[idx]); - } - - uint8_t r = std::rand() % 256; - uint8_t g = std::rand() % 256; - uint8_t b = std::rand() % 256; - pcl::visualization::PointCloudColorHandlerCustom color(ccloud, r, g, b); - - std::string cname = "cluster_" + std::to_string(i); - viewer_->addPointCloud(ccloud, color, cname); - - // Matching bounding box - for (auto & box : bboxes) { - if (box.cluster_id == i) { - viewer_->addCube( - box.min_pt.x(), box.max_pt.x(), - box.min_pt.y(), box.max_pt.y(), - box.min_pt.z(), box.max_pt.z(), - static_cast(r)/255.0, - static_cast(g)/255.0, - static_cast(b)/255.0, - "bbox_" + std::to_string(i) - ); - viewer_->setShapeRenderingProperties( - pcl::visualization::PCL_VISUALIZER_REPRESENTATION, - pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME, - "bbox_" + std::to_string(i) - ); - - // Distance text - Eigen::Vector3f c = (box.min_pt + box.max_pt)*0.5f; - char dist[64]; - std::snprintf(dist, 64, "Dist: %.2f m", box.distance); - viewer_->addText3D( - dist, - pcl::PointXYZ(c.x(), c.y(), box.max_pt.z() + 0.3f), - 0.2f, - 1.0, 1.0, 1.0, - "dist_text_" + std::to_string(i) - ); - break; - } - } - - i++; - } - - viewer_->spinOnce(10); - } - - // Timer: FSM logic ~0.5 s - void updateFSM() - { - // 1) Copy bounding boxes - std::vector bboxes_copy; - { - std::lock_guard lock(data_mutex_); - bboxes_copy = final_bboxes_; - } - - // 2) Retrieve sensor data - auto sensor_data = sensor_subscriber_->get_latest_sensor_data(); - - // Print the raw sensor data before final speed calc - RCLCPP_INFO(get_logger(), - "[SENSORS] Ultras=%.2f, IMU=(%.2f, %.2f, %.2f), PIR=(%s,%s,%s,%s)", - sensor_data.ultrasonic_front_0, - sensor_data.linear_acceleration_x, - sensor_data.linear_acceleration_y, - sensor_data.linear_acceleration_z, - (sensor_data.pir_front ? "true" : "false"), - (sensor_data.pir_back ? "true" : "false"), - (sensor_data.pir_left ? "true" : "false"), - (sensor_data.pir_right ? "true" : "false") - ); - - // Ultrasonic => speed limit - float ultrasonic = sensor_data.ultrasonic_front_0; - float speed_ultr = (ultrasonic >= 1.0f) ? SPEED_MAX : - (ultrasonic >= 0.5f) ? SPEED_CAUTION : SPEED_STOP; - - // IMU => speed limit - float ax = sensor_data.linear_acceleration_x; - float ay = sensor_data.linear_acceleration_y; - float az = sensor_data.linear_acceleration_z; - float accMag = std::sqrt(ax*ax + ay*ay + az*az); - - float speed_imu = SPEED_MAX; - if ((accMag < ACC_LOW_THRESHOLD) || (accMag > ACC_HIGH_THRESHOLD)) { - speed_imu = SPEED_STOP; - } - - float final_speed = std::min(speed_ultr, speed_imu); - - // PIR => any triggered => STOP - bool pir_triggered = (sensor_data.pir_front || - sensor_data.pir_back || - sensor_data.pir_left || - sensor_data.pir_right); - if (pir_triggered) { - final_speed = std::min(final_speed, SPEED_STOP); - } - - // LiDAR => if cluster <1.0 m => STOP - for (auto & box : bboxes_copy) { - if (box.distance < 1.0f) { - final_speed = std::min(final_speed, SPEED_STOP); - } - } - - // Update FSM - fsm_.update(final_speed); - - // Print final speed/state - RCLCPP_INFO(get_logger(), - "[FSM] Ultras=%.2f, IMU=%.2f, PIR=%s, Clusters=%zu => Speed=%.2f, State=%s", - ultrasonic, - accMag, - (pir_triggered ? "true" : "false"), - bboxes_copy.size(), - final_speed, - fsm_.getStateName().c_str() - ); - } - - // Member Variables - rclcpp::Subscription::SharedPtr lidar_sub_; - - // Timers - rclcpp::TimerBase::SharedPtr viewer_timer_; - rclcpp::TimerBase::SharedPtr fsm_timer_; - - // LiDAR data (protected by mutex) - std::mutex data_mutex_; - pcl::PointCloud::Ptr ground_cloud_{nullptr}; - pcl::PointCloud::Ptr obstacles_cloud_{nullptr}; - std::vector cluster_indices_; - std::vector final_bboxes_; - - // PCL Visualizer - pcl::visualization::PCLVisualizer::Ptr viewer_; - - // Sensors + FSM - std::shared_ptr sensor_subscriber_; - StateMachine fsm_; -}; - -// Standard ROS 2 main -int main(int argc, char** argv) -{ - rclcpp::init(argc, argv); - - auto node = std::make_shared(); - node->initialize(); - - rclcpp::spin(node); - rclcpp::shutdown(); - return 0; -} diff --git a/src/main_rounding.cpp b/src/main_rounding.cpp deleted file mode 100644 index ebff8f1..0000000 --- a/src/main_rounding.cpp +++ /dev/null @@ -1,106 +0,0 @@ -// main.cpp – 2025-05-08 pivot-enhanced -#include "main.h" -#include "rclcpp/rclcpp.hpp" -#include "fan_publisher.hpp" -#include "light_publisher.hpp" -#include "sensors_subscriber.hpp" -#include "obstacle_subscriber.hpp" -#include "fingerprint_subscriber.hpp" -#include "ref_speed_publisher.hpp" - -#include -#include -#include -#include -#include - -constexpr int PIVOT_SPEED = 25; - - -std::atomic_bool front_clear{true}; -std::atomic_bool back_clear{true}; -std::atomic_bool left_turn_clear{true}; -std::atomic_bool right_turn_clear{true}; - -int main(int argc, char *argv[]) -{ - rclcpp::init(argc, argv); - auto node = std::make_shared("wheelchair_code_module"); - - - auto sensors_subscriber = std::make_shared(node); - auto obstacle_subscriber = std::make_shared(node, front_clear, back_clear); - auto fan_publisher = std::make_shared(node); - auto light_publisher = std::make_shared(node); - auto fingerprint_subscriber = std::make_shared(node); - auto ref_speed_publisher = std::make_shared(node); - - /* cone-clear subscriptions (fire-and-forget lambdas) */ - auto left_sub = node->create_subscription( - "left_turn_clear", 10, - [](std_msgs::msg::Bool::SharedPtr m){ - left_turn_clear.store(m->data, std::memory_order_relaxed); - }); - auto right_sub = node->create_subscription( - "right_turn_clear", 10, - [](std_msgs::msg::Bool::SharedPtr m){ - right_turn_clear.store(m->data, std::memory_order_relaxed); - }); - - std::thread spin_thread([&]() { rclcpp::spin(node); }); - RCLCPP_INFO(node->get_logger(), "Wheelchair node has started."); - - rclcpp::Rate rate(30); - while (rclcpp::ok()) - { - auto sd = sensors_subscriber->get_latest_sensor_data(); - - /* joystick requests */ - RefSpeed ref_speed; - ref_speed.leftSpeed = sd.left_speed; - ref_speed.rightSpeed = sd.right_speed; - - bool front_is_blocked = !front_clear.load(std::memory_order_relaxed); - bool back_is_blocked = !back_clear .load(std::memory_order_relaxed); - - if (front_is_blocked && back_is_blocked) { - ref_speed.leftSpeed = ref_speed.rightSpeed = 0; - } - else if (front_is_blocked) { - /* pivot around obstacle */ - bool right_ok = right_turn_clear.load(std::memory_order_relaxed); - bool left_ok = left_turn_clear .load(std::memory_order_relaxed); - - if (right_ok) { // clockwise turn - ref_speed.leftSpeed = 0; - ref_speed.rightSpeed = PIVOT_SPEED; - } - else if (left_ok) { // counter-clockwise turn - ref_speed.leftSpeed = PIVOT_SPEED; - ref_speed.rightSpeed = 0; - } - else { // cones both blocked → stop forward - if (ref_speed.leftSpeed > 0) ref_speed.leftSpeed = 0; - if (ref_speed.rightSpeed > 0) ref_speed.rightSpeed = 0; - } - } - else if (back_is_blocked) { - if (ref_speed.leftSpeed < 0) ref_speed.leftSpeed = 0; - if (ref_speed.rightSpeed < 0) ref_speed.rightSpeed = 0; - } - - RCLCPP_INFO(node->get_logger(), - "[CMD] Joy L=%d R=%d | front=%d back=%d | coneL=%d coneR=%d", - ref_speed.leftSpeed, ref_speed.rightSpeed, - front_clear.load(), back_clear.load(), - left_turn_clear.load(), right_turn_clear.load()); - - ref_speed_publisher->trigger_publish(ref_speed); - rate.sleep(); - } - - rclcpp::shutdown(); - spin_thread.join(); - RCLCPP_INFO(node->get_logger(), "Shutting down."); - return 0; -} diff --git a/src/main_stopping.cpp b/src/main_stopping.cpp deleted file mode 100644 index adec341..0000000 --- a/src/main_stopping.cpp +++ /dev/null @@ -1,69 +0,0 @@ -/* main.cpp – fixed to match ObstacleSubscriber(node, front, back, left, right) */ - -#include "main.h" -#include "rclcpp/rclcpp.hpp" -#include "fan_publisher.hpp" -#include "light_publisher.hpp" -#include "sensors_subscriber.hpp" -#include "obstacle_subscriber.hpp" -#include "fingerprint_subscriber.hpp" -#include "ref_speed_publisher.hpp" - -#include -#include -#include -#include - -/* obstacle flags: true = clear, false = blocked */ -std::atomic_bool front_clear{true}; -std::atomic_bool back_clear{true}; -std::atomic_bool left_turn_clear{true}; -std::atomic_bool right_turn_clear{true}; - -int main(int argc, char *argv[]) -{ - rclcpp::init(argc, argv); - auto node = std::make_shared("wheelchair_code_module"); - - auto sensors_subscriber = std::make_shared(node); - auto obstacle_subscriber = std::make_shared( - node, - front_clear, - back_clear, - left_turn_clear, - right_turn_clear); // ← now passes all required flags - auto fan_publisher = std::make_shared(node); - auto light_publisher = std::make_shared(node); - auto fingerprint_subscriber = std::make_shared(node); - auto ref_speed_publisher = std::make_shared(node); - - std::thread spin_thread([&]{ rclcpp::spin(node); }); - RCLCPP_INFO(node->get_logger(), "Wheelchair node has started."); - - rclcpp::Rate rate(30); - while (rclcpp::ok()) - { - /* base forward command */ - RefSpeed ref_speed{15, 15}; - - bool front_blocked = !front_clear.load(std::memory_order_relaxed); - bool back_blocked = !back_clear.load(std::memory_order_relaxed); - - if (front_blocked && back_blocked) { // both ways blocked - ref_speed = {0, 0}; - } else if (front_blocked) { // stop forward - if (ref_speed.leftSpeed > 0) ref_speed.leftSpeed = 0; - if (ref_speed.rightSpeed > 0) ref_speed.rightSpeed = 0; - } else if (back_blocked) { // stop reverse - if (ref_speed.leftSpeed < 0) ref_speed.leftSpeed = 0; - if (ref_speed.rightSpeed < 0) ref_speed.rightSpeed = 0; - } - - ref_speed_publisher->trigger_publish(ref_speed); - rate.sleep(); - } - - rclcpp::shutdown(); - spin_thread.join(); - return 0; -} diff --git a/src/main_turning.cpp b/src/main_turning.cpp deleted file mode 100644 index bcfbd7a..0000000 --- a/src/main_turning.cpp +++ /dev/null @@ -1,133 +0,0 @@ -/* main.cpp – left-pivot obstacle avoidance - drive_mode: 0=manual, 1=joystick pass-through, 2=autonomous */ - -#include "main.h" -#include "rclcpp/rclcpp.hpp" -#include "sensors_subscriber.hpp" -#include "obstacle_subscriber.hpp" -#include "ref_speed_publisher.hpp" -#include -#include -#include - -/* ── config ─────────────────────────────────────────────── */ -constexpr int SPEED = 15; -constexpr int INNER_SPEED = SPEED / 4; // 3 -constexpr double PIVOT_DURATION = 2.5; // s -constexpr double RETURN_PIVOT_DURATION = 2.5; // s - -/* ── shared flags ───────────────────────────────────────── */ -std::atomic_bool front_clear{true}; -std::atomic_bool back_clear{true}; -std::atomic_bool left_turn_clear{true}; // ← we *only* use this one -std::atomic_bool right_turn_clear{true}; // kept for wiring; ignored -std::atomic_int drive_mode{0}; - -/* ── FSM ────────────────────────────────────────────────── */ -enum class Mode { STRAIGHT, PIVOT, RETURN_PIVOT, STOP }; -Mode mode = Mode::STRAIGHT; -double timer = 0.0; -int pivot_dir = -1; // −1 = left (right never used) - -/* ───────────────────────────────────────────────────────── */ -int main(int argc, char *argv[]) -{ - rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("wheelchair_code_module"); - - auto sensors_sub = std::make_shared(node); - auto obstacle_sub = std::make_shared( - node, front_clear, back_clear, left_turn_clear, right_turn_clear); - auto ref_pub = std::make_shared(node); - - auto electron_sub = node->create_subscription( - "electron_selfdrive", 10, - [node](std_msgs::msg::Int32::SharedPtr m){ - drive_mode.store(m->data, std::memory_order_relaxed); - RCLCPP_INFO(node->get_logger(), "[RX] drive-mode=%d", m->data); - }); - - rclcpp::executors::SingleThreadedExecutor exec; - exec.add_node(node); - std::thread spin_thread([&]{ exec.spin(); }); - - rclcpp::Rate loop(30); - const double DT = 1.0 / 30.0; - - while (rclcpp::ok()) - { - RefSpeed cmd{SPEED, SPEED}; - - /* ─ mode 1: joystick passthrough ─ */ - if (drive_mode.load() == 0) { - auto s = sensors_sub->get_latest_sensor_data(); - cmd.leftSpeed = s.left_speed; - cmd.rightSpeed = s.right_speed; - ref_pub->trigger_publish(cmd); - loop.sleep(); - continue; - } - - /* ─ mode 2: autonomous ─ */ - if (drive_mode.load() == 2) { - bool front = front_clear.load(); - bool left_ok = left_turn_clear.load(); - - switch (mode) - { - case Mode::STRAIGHT: - if (!front) { - if (!left_ok) { - mode = Mode::STOP; // nowhere to go - } else { - pivot_dir = -1; // always left - timer = 0; - mode = Mode::PIVOT; - } - } - break; - - case Mode::PIVOT: - if (!left_ok) { mode = Mode::STOP; break; } - timer += DT; - cmd.leftSpeed = (pivot_dir == +1) ? SPEED : INNER_SPEED; - cmd.rightSpeed = (pivot_dir == +1) ? INNER_SPEED : SPEED; - if (timer >= PIVOT_DURATION) { - pivot_dir = -pivot_dir; // swing back right - timer = 0; - mode = Mode::RETURN_PIVOT; - } - break; - - case Mode::RETURN_PIVOT: - if (!left_ok) { mode = Mode::STOP; break; } - timer += DT; - cmd.leftSpeed = (pivot_dir == +1) ? SPEED : INNER_SPEED; - cmd.rightSpeed = (pivot_dir == +1) ? INNER_SPEED : SPEED; - if (timer >= RETURN_PIVOT_DURATION) { - timer = 0; - mode = front ? Mode::STRAIGHT : Mode::STOP; - } - break; - - case Mode::STOP: - cmd.leftSpeed = cmd.rightSpeed = 0; - if (front) mode = Mode::STRAIGHT; - break; - } - - ref_pub->trigger_publish(cmd); - loop.sleep(); - continue; - } - - /* mode 0: manual/off */ - loop.sleep(); - } - - exec.cancel(); - spin_thread.join(); - rclcpp::shutdown(); - return 0; -} - diff --git a/src/obstacle_publisher.cpp b/src/obstacle_publisher.cpp deleted file mode 100644 index d57964d..0000000 --- a/src/obstacle_publisher.cpp +++ /dev/null @@ -1,191 +0,0 @@ -// obstacle_publisher.cpp -// 2025‑05‑08 – ultra‑conservative revision 2025‑05‑16c -// The chair was still initiating pivots in tight spots. Dial the side‑sector -// test up another notch so it will only turn when there is *lots* of space: -// • Cone half‑angle shrunk to ±40° (was 50°) -// • Clear distance pushed to 2.0 m (was 1.5 m) -// • Everything else (5‑/3‑frame debounce, “blocked if ANY point” rule) -// unchanged. -// Publishes four Bool topics: -// front_clear, back_clear, left_turn_clear, right_turn_clear - -#include "obstacle_publisher.hpp" - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include -#include -#include - -namespace { - -constexpr float Z_MIN = 0.05f; -constexpr float Z_MAX = 1.80f; - -/* stop‑box dimensions (m) ---------------------------------------------- */ -constexpr float F_BOX_X_MIN = 0.0f, F_BOX_X_MAX = 0.60f, F_BOX_Y_HALF = 0.75f; -constexpr float B_BOX_X_MIN = -0.40f, B_BOX_X_MAX = -0.05f, B_BOX_Y_HALF = 0.75f; - -/* tunnel “wall warn” thresholds --------------------------------------- */ -constexpr float F_TUNNEL_Y_HALF = 0.50f, F_WALL_WARN = 2.50f; -constexpr float B_TUNNEL_Y_HALF = 0.50f, B_WALL_WARN = -2.00f; - -/* *** side‑cone parameters *** --------------------- */ -constexpr float CONE_HALF_ANGLE = static_cast(M_PI) * 35.0f / 180.0f; // ±35° -constexpr float CLEAR_DIST = 1.60f; // ≥1.60 m of open space required - -/* debounce helper ------------------------------------------------------ */ -inline void debounce(bool hit, int& h, int& c, bool& latched) -{ - hit ? (++h, c = 0) : (++c, h = 0); - if (!latched && h >= 4) latched = true; - if ( latched && c >= 2) latched = false; -} - -} // anonymous namespace - -/* ─────────────────────────────────────────────────────────────────────── */ -ObstaclePublisher::ObstaclePublisher() : Node("obstacle_publisher") -{ - cloud_sub_ = create_subscription( - "/livox/lidar", 10, - std::bind(&ObstaclePublisher::cloudCallback, this, std::placeholders::_1)); - - front_clear_pub_ = create_publisher("front_clear", 10); - back_clear_pub_ = create_publisher("back_clear", 10); - left_turn_clear_pub_ = create_publisher("left_turn_clear", 10); - right_turn_clear_pub_ = create_publisher("right_turn_clear", 10); - - obstacles_cloud_pub_ = create_publisher("obstacles_cloud", 10); - markers_pub_ = create_publisher("obstacle_markers", 10); - - RCLCPP_INFO(get_logger(), "ObstaclePublisher node started."); -} - -/* ─────────────────────────────────────────────────────────────────────── */ -void ObstaclePublisher::cloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) -{ - pcl::PointCloud::Ptr raw(new pcl::PointCloud); - pcl::fromROSMsg(*msg, *raw); - if (raw->empty()) return; - - /* 1 ─ voxel grid (10 cm) -------------------------------------------- */ - pcl::VoxelGrid vg; - vg.setInputCloud(raw); - vg.setLeafSize(0.1f, 0.1f, 0.1f); - pcl::PointCloud::Ptr cloud(new pcl::PointCloud); - vg.filter(*cloud); - - /* 2 ─ cut torso & chair footprint ----------------------------------- */ - { - pcl::CropBox cb; - cb.setMin(Eigen::Vector4f(-0.45f,-0.45f,-0.70f,1)); - cb.setMax(Eigen::Vector4f( 0.50f, 0.45f, 0.45f,1)); - cb.setNegative(true); - cb.setInputCloud(cloud); - cb.filter(*cloud); - } - - /* 3 ─ ground removal ------------------------------------------------- */ - pcl::PointCloud::Ptr obs(new pcl::PointCloud); - { - pcl::SACSegmentation seg; - seg.setOptimizeCoefficients(true); - seg.setModelType(pcl::SACMODEL_PLANE); - seg.setMethodType(pcl::SAC_RANSAC); - seg.setDistanceThreshold(0.2f); - - pcl::ModelCoefficients::Ptr coeff(new pcl::ModelCoefficients); - pcl::PointIndices::Ptr ground(new pcl::PointIndices); - seg.setInputCloud(cloud); - seg.segment(*ground, *coeff); - - bool is_floor = false; - if (!coeff->values.empty()) { - Eigen::Vector3f n(coeff->values[0], coeff->values[1], coeff->values[2]); - n.normalize(); - is_floor = std::acos(std::fabs(n.z())) * 180.0f / M_PI < 15.0f; - } - - if (is_floor && !ground->indices.empty()) { - pcl::ExtractIndices ex; - ex.setInputCloud(cloud); - ex.setIndices(ground); - ex.setNegative(true); - ex.filter(*obs); - } else { - obs = cloud; - } - } - - /* 4 ─ statistical outlier ------------------------------------------- */ - pcl::StatisticalOutlierRemoval sor; - sor.setInputCloud(obs); - sor.setMeanK(20); - sor.setStddevMulThresh(2.0); - sor.filter(*obs); - if (obs->empty()) return; - - /* 5 ─ obstacle checks ----------------------------------------------- */ - bool hit_f=false, hit_b=false, hit_l=false, hit_r=false; - float min_front = std::numeric_limits::max(); - float max_back = -std::numeric_limits::max(); - - for (const auto& p : obs->points) { - if (p.z < Z_MIN || p.z > Z_MAX) continue; - - /* stop boxes */ - if (p.x>F_BOX_X_MIN && p.xB_BOX_X_MIN && p.x0.25f) hit_b=true; - - /* tunnel warn */ - if (std::fabs(p.y)0) min_front=std::min(min_front,p.x); - if (std::fabs(p.y)0) { - float ang = std::atan2(p.y, p.x); - float rng = std::hypot(p.x, p.y); - if (rng <= CLEAR_DIST) { - if (ang >=-CONE_HALF_ANGLE && ang<=0.0f) hit_r = true; - if (ang > 0.0f && ang<=CONE_HALF_ANGLE) hit_l = true; - } - } - } - - if (min_front < F_WALL_WARN) hit_f = true; - if (max_back > B_WALL_WARN) hit_b = true; - - /* 6 ─ debounce ------------------------------------------------------- */ - static int hf=0,cf=0,hb=0,cb=0; static bool lat_f=false,lat_b=false; - debounce(hit_f, hf, cf, lat_f); - debounce(hit_b, hb, cb, lat_b); - - static int hl=0,cl=0, hr=0,cr=0; static bool lat_l=false,lat_r=false; - debounce(hit_l, hl, cl, lat_l); - debounce(hit_r, hr, cr, lat_r); - - /* 7 ─ publish -------------------------------------------------------- */ - auto pub_bool = [&](auto& pub, bool clear){ std_msgs::msg::Bool m; m.data = clear; pub->publish(m); }; - pub_bool(front_clear_pub_, !lat_f); - pub_bool(back_clear_pub_, !lat_b); - pub_bool(left_turn_clear_pub_, !lat_l); - pub_bool(right_turn_clear_pub_, !lat_r); - - /* 8 ─ diagnostics cloud --------------------------------------------- */ - sensor_msgs::msg::PointCloud2 pc; - pcl::toROSMsg(*obs, pc); - pc.header = msg->header; - obstacles_cloud_pub_->publish(pc); -} diff --git a/src/obstacle_publisher_node.cpp b/src/obstacle_publisher_node.cpp deleted file mode 100644 index 96d99e3..0000000 --- a/src/obstacle_publisher_node.cpp +++ /dev/null @@ -1,12 +0,0 @@ -// Created by Angela B. on 4/21/25. - -#include "obstacle_publisher.hpp" -#include - -int main(int argc, char* argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/src/obstacle_publisher_rounding.cpp b/src/obstacle_publisher_rounding.cpp deleted file mode 100644 index b10b6c7..0000000 --- a/src/obstacle_publisher_rounding.cpp +++ /dev/null @@ -1,234 +0,0 @@ -// obstacle_publisher.cpp -// 2025-05-08 — adds distance-based cone test so /left_turn_clear and -#include "obstacle_publisher.hpp" - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include -#include -#include - -namespace { - -constexpr float Z_MIN = 0.05f; -constexpr float Z_MAX = 1.80f; - -/* stop-box dimensions (m) */ -constexpr float F_BOX_X_MIN = 0.0f, F_BOX_X_MAX = 0.60f, F_BOX_Y_HALF = 0.75f; -constexpr float B_BOX_X_MIN = -0.40f, B_BOX_X_MAX = -0.05f, B_BOX_Y_HALF = 0.75f; - -/* tunnel “wall warn” thresholds */ -constexpr float F_TUNNEL_Y_HALF = 0.50f, F_WALL_WARN = 1.10f; -constexpr float B_TUNNEL_Y_HALF = 0.50f, B_WALL_WARN = -1.10f; - -/* cone fan parameters */ -constexpr float CONE_HALF_ANGLE = static_cast(M_PI) * 75.0f / 180.0f; // ±75° -constexpr float CLEAR_DIST = 0.80f; // ≥80 cm ahead in the cone ⇒ clear - -/* debouncer helper */ -inline void debounce(bool hit, int& h, int& c, bool& latched) -{ - hit ? (++h, c = 0) : (++c, h = 0); - if (!latched && h >= 5) latched = true; - if ( latched && c >= 3) latched = false; -} - -} // anonymous namespace - -/* ───────────────────────────────────────────────────────────────────────── */ -ObstaclePublisher::ObstaclePublisher() : Node("obstacle_publisher") -{ - /* LiDAR input */ - cloud_sub_ = create_subscription( - "/livox/lidar", 10, - std::bind(&ObstaclePublisher::cloudCallback, this, std::placeholders::_1)); - - /* Boolean flags */ - front_clear_pub_ = create_publisher("front_clear", 10); - back_clear_pub_ = create_publisher("back_clear", 10); - left_turn_clear_pub_ = create_publisher("left_turn_clear", 10); - right_turn_clear_pub_ = create_publisher("right_turn_clear", 10); - - /* Diagnostics */ - obstacles_cloud_pub_ = create_publisher("obstacles_cloud", 10); - markers_pub_ = create_publisher("obstacle_markers", 10); - - RCLCPP_INFO(get_logger(), "ObstaclePublisher node started."); -} - -void ObstaclePublisher::cloudCallback( - const sensor_msgs::msg::PointCloud2::SharedPtr msg) -{ - /* convert to PCL */ - pcl::PointCloud::Ptr raw(new pcl::PointCloud); - pcl::fromROSMsg(*msg, *raw); - if (raw->empty()) return; - - /* voxel down-sample (10 cm) */ - pcl::VoxelGrid vg; - vg.setInputCloud(raw); - vg.setLeafSize(0.1f, 0.1f, 0.1f); - pcl::PointCloud::Ptr cloud(new pcl::PointCloud); - vg.filter(*cloud); - - /* remove torso box */ - { - pcl::CropBox cb; - cb.setMin(Eigen::Vector4f(-0.35f,-0.35f,-0.70f,1)); - cb.setMax(Eigen::Vector4f( 0.35f, 0.35f, 0.20f,1)); - cb.setNegative(true); - cb.setInputCloud(cloud); - cb.filter(*cloud); - } - - /* remove chair footprint */ - { - pcl::CropBox cb; - cb.setMin(Eigen::Vector4f(-0.45f,-0.45f,-0.10f,1)); - cb.setMax(Eigen::Vector4f( 0.50f, 0.45f, 0.45f,1)); - cb.setNegative(true); - cb.setInputCloud(cloud); - cb.filter(*cloud); - } - - /* RANSAC ground removal (≤15° tilt) */ - pcl::PointCloud::Ptr obs(new pcl::PointCloud); - { - pcl::SACSegmentation seg; - seg.setOptimizeCoefficients(true); - seg.setModelType(pcl::SACMODEL_PLANE); - seg.setMethodType(pcl::SAC_RANSAC); - seg.setDistanceThreshold(0.2f); - - pcl::ModelCoefficients::Ptr coeff(new pcl::ModelCoefficients); - pcl::PointIndices::Ptr ground(new pcl::PointIndices); - seg.setInputCloud(cloud); - seg.segment(*ground, *coeff); - - bool is_floor = false; - if (!coeff->values.empty()) { - Eigen::Vector3f n(coeff->values[0], coeff->values[1], coeff->values[2]); - n.normalize(); - is_floor = std::acos(std::fabs(n.z())) * 180.0f / M_PI < 15.0f; - } - - if (is_floor && !ground->indices.empty()) { - pcl::ExtractIndices ex; - ex.setInputCloud(cloud); - ex.setIndices(ground); - ex.setNegative(true); - ex.filter(*obs); - } else { - obs = cloud; - } - } - - /* statistical outlier filter */ - pcl::StatisticalOutlierRemoval sor; - sor.setInputCloud(obs); - sor.setMeanK(20); - sor.setStddevMulThresh(2.0); - sor.filter(*obs); - if (obs->empty()) return; - - /* obstacle + cone logic */ - bool hit_front=false, hit_back=false; - float min_front = std::numeric_limits::max(); - float max_back = -std::numeric_limits::max(); - - /* smallest range inside each cone */ - float min_r = std::numeric_limits::max(); // ahead-right - float min_l = std::numeric_limits::max(); // ahead-left - - for (const auto& p : obs->points) { - if (p.z < Z_MIN || p.z > Z_MAX) continue; - - /* stop boxes */ - if (p.x>F_BOX_X_MIN && p.xB_BOX_X_MIN && p.x0.25f) hit_back=true; - - /* tunnel-wall warn */ - if (std::fabs(p.y)0) min_front=std::min(min_front,p.x); - if (std::fabs(p.y)0) { // only look forward - float ang = std::atan2(p.y, p.x); // +y left, –y right - float rng = std::hypot(p.x, p.y); - if (ang >=-CONE_HALF_ANGLE && ang<=0.0f) min_r = std::min(min_r, rng); - if (ang > 0.0f && ang<=CONE_HALF_ANGLE) min_l = std::min(min_l, rng); - } - } - - if (min_front < F_WALL_WARN) hit_front = true; - if (max_back > B_WALL_WARN) hit_back = true; - - bool right_sector_blocked = (min_r < CLEAR_DIST); - bool left_sector_blocked = (min_l < CLEAR_DIST); - - /* debounce front/back flags */ - static int hf=0,cf=0,hb=0,cb=0; static bool lat_f=false,lat_b=false; - debounce(hit_front,hf,cf,lat_f); - debounce(hit_back ,hb,cb,lat_b); - - /* publish clear/blocked flags */ - auto pub_bool = [&](auto pub, bool clear) { - std_msgs::msg::Bool m; m.data = clear; pub->publish(m); - }; - pub_bool(front_clear_pub_ , !lat_f); - pub_bool(back_clear_pub_ , !lat_b); - pub_bool(left_turn_clear_pub_ , !left_sector_blocked); - pub_bool(right_turn_clear_pub_, !right_sector_blocked); - - /* diagnostics cloud */ - sensor_msgs::msg::PointCloud2 pc; - pcl::toROSMsg(*obs, pc); - pc.header = msg->header; - obstacles_cloud_pub_->publish(pc); - - /* cluster BBoxes */ - std::vector clusters; - pcl::search::KdTree::Ptr tree(new pcl::search::KdTree); - tree->setInputCloud(obs); - pcl::EuclideanClusterExtraction ec; - ec.setInputCloud(obs); - ec.setSearchMethod(tree); - ec.setClusterTolerance(0.2f); - ec.setMinClusterSize(10); - ec.extract(clusters); - - visualization_msgs::msg::MarkerArray marr; - int id=0; - for(const auto& cl:clusters){ - Eigen::Vector3f mn(FLT_MAX,FLT_MAX,FLT_MAX), mx(-FLT_MAX,-FLT_MAX,-FLT_MAX); - for(int idx:cl.indices){ - const auto& p=obs->at(idx); - Eigen::Vector3f v(p.x,p.y,p.z); - mn=mn.cwiseMin(v); mx=mx.cwiseMax(v); - } - visualization_msgs::msg::Marker m; - m.header=msg->header; m.ns="obstacle_boxes"; m.id=id++; - m.type=m.CUBE; m.action=m.ADD; m.pose.orientation.w=1.0; - m.pose.position.x=0.5f*(mn.x()+mx.x()); - m.pose.position.y=0.5f*(mn.y()+mx.y()); - m.pose.position.z=0.5f*(mn.z()+mx.z()); - m.scale.x=(mx.x()-mn.x())+0.10f; - m.scale.y=(mx.y()-mn.y())+0.10f; - m.scale.z=(mx.z()-mn.z())+0.10f; - m.color.r=1.0f; m.color.g=0.6f; m.color.b=0.0f; m.color.a=0.8f; - m.lifetime=rclcpp::Duration::from_seconds(0.5); - marr.markers.push_back(m); - } - markers_pub_->publish(marr); -} diff --git a/src/speed_publisher.cpp b/src/speed_publisher.cpp deleted file mode 100644 index d4f6ce4..0000000 --- a/src/speed_publisher.cpp +++ /dev/null @@ -1,88 +0,0 @@ -#include "speed_publisher.hpp" -#include -#include - -using std_msgs::msg::Bool; -using std_msgs::msg::Float32; - -namespace wh = wheelchair; - -wh::SpeedPublisher::SpeedPublisher(const std::string& out_topic, - const rclcpp::QoS& qos) -: rclcpp::Node("speed_publisher") -{ - speed_pub_ = create_publisher(out_topic, qos); - - obs_sub_ = create_subscription( - "obstacle_detected", 10, - std::bind(&SpeedPublisher::obstacleCb, this, std::placeholders::_1)); - - sensor_sub_ = std::make_shared(this->shared_from_this()); - - timer_ = create_wall_timer( - std::chrono::milliseconds(static_cast(DT * 1e3)), - std::bind(&SpeedPublisher::timerCb, this)); - - RCLCPP_INFO(get_logger(), - "SpeedPublisher started — listening to obstacle_detected and sensors."); -} - - -void wh::SpeedPublisher::timerCb() -{ - - auto s = sensor_sub_->get_latest_sensor_data(); - float ultrasonic = s.ultrasonic_front_0; - float acc_mag = std::hypot(s.linear_acceleration_x, - std::hypot(s.linear_acceleration_y, - s.linear_acceleration_z)); - bool pir_hit = s.pir_front || s.pir_back || - s.pir_left || s.pir_right; - - - float raw = fuseSpeeds(ultrasonic, acc_mag, pir_hit, obstacle_flag_); - - - float max_delta = MAX_ACCEL * DT; - float cmd = std::clamp(raw, - prev_speed_ - max_delta, - prev_speed_ + max_delta); - prev_speed_ = cmd; - - - Float32 out; - out.data = cmd; - speed_pub_->publish(out); - - RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000, - "[SPD] U=%.2f A=%.2f PIR=%s OBS=%s => %.2f m/s", - ultrasonic, acc_mag, - (pir_hit?"T":"F"), (obstacle_flag_?"T":"F"), cmd); -} - - -float wh::SpeedPublisher::fuseSpeeds(float ultrasonic, - float acc_mag, - bool pir_hit, - bool obstacle_close) const -{ - float limit_ultr = (ultrasonic >= 1.0f) ? SPEED_MAX : - (ultrasonic >= 0.5f) ? SPEED_CAUTION - : SPEED_STOP; - - float limit_imu = (acc_mag < ACC_LOW_THR || acc_mag > ACC_HIGH_THR) - ? SPEED_STOP : SPEED_MAX; - - float limit_pir = pir_hit ? SPEED_STOP : SPEED_MAX; - - float limit_lid = obstacle_close ? SPEED_STOP : SPEED_MAX; - - return std::min({limit_ultr, limit_imu, limit_pir, limit_lid}); -} - - - -void wh::SpeedPublisher::obstacleCb(const Bool::SharedPtr msg) -{ - obstacle_flag_ = msg->data; -} diff --git a/src/temp_monitor.cpp b/src/temp_monitor.cpp deleted file mode 100644 index 28d891a..0000000 --- a/src/temp_monitor.cpp +++ /dev/null @@ -1,148 +0,0 @@ -// -// Created by Robbie on 4/21/25. -// - -#include "temp_monitor.hpp" -#include "fan_publisher.hpp" -#include -#include -#include -#include -#include -#include -#include -#include - -std::atomic_bool running(true); - -void handle_sigint(int) -{ - running = false; // causes main loop to exit -} - -namespace fs = std::filesystem; - -std::string findCpuThermalZone() { - const std::string basePath = "/sys/class/thermal/"; - - for (const auto& entry : fs::directory_iterator(basePath)) { - if (entry.is_directory() && entry.path().filename().string().find("thermal_zone") == 0) { - std::ifstream typeFile(entry.path() / "type"); - std::string type; - if (typeFile.is_open()) { - std::getline(typeFile, type); - typeFile.close(); - - if (type.find("CPU") != std::string::npos || type.find("cpu") != std::string::npos) { - return (entry.path() / "temp").string(); - } - } - } - } - return ""; -} - -double readTemperature(const std::string& tempPath) { - std::ifstream tempFile(tempPath); - int tempMilliC = 0; - if (tempFile.is_open()) { - tempFile >> tempMilliC; - tempFile.close(); - return tempMilliC / 1000.0; - } - return -1.0; -} - -int main(int argc, char** argv) { - std::string tempPath = findCpuThermalZone(); - if (tempPath.empty()) { - std::cerr << "CPU thermal zone not found." << std::endl; - return 1; - } - - // Initialize the ROS 2 node and setting up subscribers and publishers - rclcpp::init(argc, argv); - auto node = std::make_shared("temperature_monitor"); - auto fan_publisher = std::make_shared(node); - - std::signal(SIGINT, handle_sigint); - - auto executor = std::make_shared(); - executor->add_node(node); - - // Thread that runs the executor manually - std::thread spin_thread([&]() { - while (rclcpp::ok() && running) { - executor->spin_some(); - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - } - }); - - // Set the rate to 1 Hz - rclcpp::Rate rate(1); - - RCLCPP_INFO(node->get_logger(), "Monitoring CPU temperature (press Ctrl+C to stop):"); - while (rclcpp::ok() && running) { - double tempC = readTemperature(tempPath); - if (tempC < 0) { - RCLCPP_INFO(node->get_logger(), "Failed to read temperature!"); - } else { - RCLCPP_INFO(node->get_logger(), "CPU Temperature: %f °C", tempC); - - if (tempC < 30.0) { - // Handle case for temperatures below 30°C - FanSpeed fanSpeed; - fanSpeed.fan_percent_0 = 0; - fanSpeed.fan_percent_1 = 0; - fanSpeed.fan_percent_2 = 0; - fanSpeed.fan_percent_3 = 0; - fan_publisher->trigger_publish(fanSpeed); - } else if (tempC >= 30.0 && tempC < 40.0) { - // Handle case for temperatures between 30°C and 40°C - FanSpeed fanSpeed; - fanSpeed.fan_percent_0 = 25; - fanSpeed.fan_percent_1 = 25; - fanSpeed.fan_percent_2 = 25; - fanSpeed.fan_percent_3 = 25; - fan_publisher->trigger_publish(fanSpeed); - } else if (tempC >= 40.0 && tempC < 50) { - // Handle case for temperatures between 40 and 50 - FanSpeed fanSpeed; - fanSpeed.fan_percent_0 = 50; - fanSpeed.fan_percent_1 = 50; - fanSpeed.fan_percent_2 = 50; - fanSpeed.fan_percent_3 = 50; - fan_publisher->trigger_publish(fanSpeed); - } else if (tempC >= 50.0){ - FanSpeed fanSpeed; - fanSpeed.fan_percent_0 = 75; - fanSpeed.fan_percent_1 = 75; - fanSpeed.fan_percent_2 = 75; - fanSpeed.fan_percent_3 = 75; - fan_publisher->trigger_publish(fanSpeed); - } - } - - // sleep to maintain the rate - rate.sleep(); - } - - FanSpeed off; - off.fan_percent_0 = 0; - off.fan_percent_1 = 0; - off.fan_percent_2 = 0; - off.fan_percent_3 = 0; - fan_publisher->trigger_publish(off); - RCLCPP_INFO(rclcpp::get_logger("shutdown"), "Shutdown: Turning off fans."); - - // Give time to publish and flush - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - - // Cleanup - executor->cancel(); // This ensures executor thread stops - rclcpp::shutdown(); - spin_thread.join(); - - - return 0; -} diff --git a/src/uwb_subscriber.cpp b/src/uwb_subscriber.cpp deleted file mode 100644 index 4d128d8..0000000 --- a/src/uwb_subscriber.cpp +++ /dev/null @@ -1,25 +0,0 @@ -// -// Created by Angela on 5/18/25. -// - -#include "uwb_subscriber.hpp" - -UWBSubscriber::UWBSubscriber(rclcpp::Node::SharedPtr node) -: node_(node) -{ - sub_ = node_->create_subscription( - "uwb_data", rclcpp::QoS(10), std::bind(&UWBSubscriber::msgCallback, this, std::placeholders::_1)); -} - - -void UWBSubscriber::msgCallback( - const wheelchair_sensor_msgs::msg::UWB::SharedPtr msg) -{ - dist1_.store(msg->dist1, std::memory_order_relaxed); - dist2_.store(msg->dist2, std::memory_order_relaxed); - dist3_.store(msg->dist3, std::memory_order_relaxed); - dist4_.store(msg->dist4, std::memory_order_relaxed); - dist5_.store(msg->dist5, std::memory_order_relaxed); - dist6_.store(msg->dist6, std::memory_order_relaxed); - -}