diff --git a/controls/src/autobrake_node.cpp b/controls/src/autobrake_node.cpp index bb3e1f6..10bed6f 100644 --- a/controls/src/autobrake_node.cpp +++ b/controls/src/autobrake_node.cpp @@ -15,23 +15,20 @@ using std::placeholders::_1; * * Angle from center to obstacle = tan(O_y / (O_x + R)) * Dist from center to obstacle = sqrt((O_x + R)^2 + O_y^2) - * + * * Circum dist to obstacle = R * angle from center to obstacle * Time to hit obstacle = circum dist to obstacle / velocity * * NOTE: LIDAR'S 0 is forward, and angles increment clockwise */ - -class AutobrakeNode : public rclcpp::Node -{ +class AutobrakeNode : public rclcpp::Node { public: - AutobrakeNode() : Node("autobrake") - { + AutobrakeNode(): Node("autobrake") { forward_brake_.data = MAX_VEL; - scan_sub_ = this->create_subscription( - "scan", 1, std::bind(&AutobrakeNode::checkCollision, this, _1)); + scan_sub_ = this->create_subscription("scan", 1, + std::bind(&AutobrakeNode::checkCollision, this, _1)); sensor_collect_sub_ = this->create_subscription( "sensor_collect", 1, std::bind(&AutobrakeNode::setVars, this, _1)); @@ -41,10 +38,8 @@ class AutobrakeNode : public rclcpp::Node forward_brake_pub_ = this->create_publisher("auto_max_vel", 1); - timer_ = this->create_wall_timer( - std::chrono::milliseconds(10), - std::bind(&AutobrakeNode::publishBrake, this) - ); + timer_ = this->create_wall_timer(std::chrono::milliseconds(10), + std::bind(&AutobrakeNode::publishBrake, this)); RCLCPP_INFO(this->get_logger(), "Autobrake node initialized."); } @@ -71,37 +66,32 @@ class AutobrakeNode : public rclcpp::Node rclcpp::TimerBase::SharedPtr timer_; // Calculate the maximum velocity based on the distance to an obstacle - float maxVelocity(float dist) - { - if (dist < 0) - return MAX_VEL; - if (dist < AUTOBRAKE_DISTANCE) - return 0; + float maxVelocity(float dist) { + if (dist < 0) return MAX_VEL; + if (dist < AUTOBRAKE_DISTANCE) return 0; return std::max(0.0f, -3.5f + std::sqrt(49 + 40 * dist) / 2 - 0.2f); } // LIDAR'S 0 is forward, and angles increment clockwise // Check for potential collisions based on laser scan data - void checkCollision(const sensor_msgs::msg::LaserScan::SharedPtr data) - { + void checkCollision(const sensor_msgs::msg::LaserScan::SharedPtr data) { int invert_flag = 1; float min_vel = MAX_VEL; - if (steering_angle_ < 0) - invert_flag = -1; + if (steering_angle_ < 0) invert_flag = -1; // Calculate turning radius (L / tan(steering_angle)), or infinity if driving straight - float turning_radius = std::abs(steering_angle_) > 0.01 ? VEHICLE_LENGTH / std::tan(invert_flag * steering_angle_) : std::numeric_limits::infinity(); - + float turning_radius = std::abs(steering_angle_) > 0.01 + ? VEHICLE_LENGTH / std::tan(invert_flag * steering_angle_) + : std::numeric_limits::infinity(); + // Calculate turning radii of inside and outside wheels float inner_wheel_radius = turning_radius - VEHICLE_WIDTH / 2; float outer_wheel_radius = turning_radius + VEHICLE_WIDTH / 2; - for (size_t i = 0; i < data->ranges.size(); ++i) - { - if (data->ranges[i] < data->range_min || data->ranges[i] > data->range_max) - continue; + for (size_t i = 0; i < data->ranges.size(); ++i) { + if (data->ranges[i] < data->range_min || data->ranges[i] > data->range_max) continue; float dist_to_obstacle = std::numeric_limits::infinity(); @@ -111,37 +101,33 @@ class AutobrakeNode : public rclcpp::Node float x = invert_flag * (r * std::sin(theta) + LIDAR_HORIZONTAL_OFFSET); float y = r * std::cos(theta); - if (turning_radius == std::numeric_limits::infinity()) - { + if (turning_radius == std::numeric_limits::infinity()) { // If driving straight, check if obstacle is within the width of the vehicle if (std::abs(x) < VEHICLE_WIDTH / 2) dist_to_obstacle = y; else continue; - } - else - { + } else { // For curving, calculate radius distance to obstacle from center of movement circle - float radius_dist_to_obstacle = std::sqrt((x + turning_radius) * (x + turning_radius) + y * y); + float radius_dist_to_obstacle = + std::sqrt((x + turning_radius) * (x + turning_radius) + y * y); - if (outer_wheel_radius > radius_dist_to_obstacle && radius_dist_to_obstacle > inner_wheel_radius) - { + if (outer_wheel_radius > radius_dist_to_obstacle + && radius_dist_to_obstacle > inner_wheel_radius) { // Calculate angle from center of movement circle to obstacle float angle_from_center_to_obstacle = std::atan2(y, turning_radius + x); if (angle_from_center_to_obstacle < 0) angle_from_center_to_obstacle += 2 * M_PI; angle_from_center_to_obstacle = fmod(angle_from_center_to_obstacle, 2 * M_PI); - + // Calculate circumferential distance to obstacle dist_to_obstacle = turning_radius * angle_from_center_to_obstacle; - } - else + } else continue; } // If the obstacle is within a valid distance, adjust the minimum velocity - if (dist_to_obstacle >= 0) - min_vel = std::min(min_vel, maxVelocity(dist_to_obstacle)); + if (dist_to_obstacle >= 0) min_vel = std::min(min_vel, maxVelocity(dist_to_obstacle)); } // Set the brake value to the minimum calculated velocity @@ -149,27 +135,23 @@ class AutobrakeNode : public rclcpp::Node } // Callback to set the velocity and steering angle from the sensor_collect topic - void setVars(const cev_msgs::msg::SensorCollect::SharedPtr data) - { + void setVars(const cev_msgs::msg::SensorCollect::SharedPtr data) { velocity_ = data->velocity; steering_angle_ = -data->steering_angle; // Invert the steering angle } // Callback to set the target velocity from the AckermannDrive topic - void setTargets(const ackermann_msgs::msg::AckermannDrive::SharedPtr data) - { + void setTargets(const ackermann_msgs::msg::AckermannDrive::SharedPtr data) { target_velocity_ = data->speed; } // Publish the brake value at a regular interval - void publishBrake() - { + void publishBrake() { forward_brake_pub_->publish(forward_brake_); } }; -int main(int argc, char *argv[]) -{ +int main(int argc, char* argv[]) { rclcpp::init(argc, argv); rclcpp::spin(std::make_shared()); rclcpp::shutdown(); diff --git a/controls/src/new_autobrake_node.cpp b/controls/src/new_autobrake_node.cpp index 5e499a2..42e0327 100644 --- a/controls/src/new_autobrake_node.cpp +++ b/controls/src/new_autobrake_node.cpp @@ -15,24 +15,21 @@ using std::placeholders::_1; * * Angle from center to obstacle = tan(O_y / (O_x + R)) * Dist from center to obstacle = sqrt((O_x + R)^2 + O_y^2) - * + * * Circum dist to obstacle = R * angle from center to obstacle * Time to hit obstacle = circum dist to obstacle / velocity * * NOTE: LIDAR'S 0 is forward, and angles increment clockwise */ - -class AutobrakeNode : public rclcpp::Node -{ +class AutobrakeNode : public rclcpp::Node { public: - AutobrakeNode() : Node("autobrake") - { + AutobrakeNode(): Node("autobrake") { forward_brake_.data = MAX_VEL; backward_brake_.data = MAX_VEL; - scan_sub_ = this->create_subscription( - "scan", 1, std::bind(&AutobrakeNode::checkCollision, this, _1)); + scan_sub_ = this->create_subscription("scan", 1, + std::bind(&AutobrakeNode::checkCollision, this, _1)); sensor_collect_sub_ = this->create_subscription( "sensor_collect", 1, std::bind(&AutobrakeNode::setVars, this, _1)); @@ -40,13 +37,13 @@ class AutobrakeNode : public rclcpp::Node rc_movement_sub_ = this->create_subscription( "rc_movement_msg", 1, std::bind(&AutobrakeNode::setTargets, this, _1)); - forward_brake_pub_ = this->create_publisher("auto_max_vel/forward", 1); - backward_brake_pub_ = this->create_publisher("auto_max_vel/backward", 1); + forward_brake_pub_ = this->create_publisher("auto_max_vel/forward", + 1); + backward_brake_pub_ = + this->create_publisher("auto_max_vel/backward", 1); - timer_ = this->create_wall_timer( - std::chrono::milliseconds(10), - std::bind(&AutobrakeNode::publishBrake, this) - ); + timer_ = this->create_wall_timer(std::chrono::milliseconds(10), + std::bind(&AutobrakeNode::publishBrake, this)); RCLCPP_INFO(this->get_logger(), "Autobrake node initialized."); } @@ -64,7 +61,7 @@ class AutobrakeNode : public rclcpp::Node float steering_angle_ = 0; float velocity_ = 0; float target_velocity_ = 0; - + std_msgs::msg::Float32 forward_brake_; rclcpp::Subscription::SharedPtr scan_sub_; @@ -74,37 +71,32 @@ class AutobrakeNode : public rclcpp::Node rclcpp::TimerBase::SharedPtr timer_; // Calculate the maximum velocity based on the distance to an obstacle - float maxVelocity(float dist) - { - if (dist < 0) - return MAX_VEL; - if (dist < AUTOBRAKE_DISTANCE) - return 0; + float maxVelocity(float dist) { + if (dist < 0) return MAX_VEL; + if (dist < AUTOBRAKE_DISTANCE) return 0; return std::max(0.0f, -3.5f + std::sqrt(49 + 40 * dist) / 2 - 0.2f); } // LIDAR'S 0 is forward, and angles increment clockwise // Check for potential collisions based on laser scan data - void checkCollision(const sensor_msgs::msg::LaserScan::SharedPtr data) - { + void checkCollision(const sensor_msgs::msg::LaserScan::SharedPtr data) { int invert_flag = 1; float min_vel = MAX_VEL; - if (steering_angle_ < 0) - invert_flag = -1; + if (steering_angle_ < 0) invert_flag = -1; // Calculate turning radius (L / tan(steering_angle)), or infinity if driving straight - float turning_radius = std::abs(steering_angle_) > 0.01 ? VEHICLE_LENGTH / std::tan(invert_flag * steering_angle_) : std::numeric_limits::infinity(); - + float turning_radius = std::abs(steering_angle_) > 0.01 + ? VEHICLE_LENGTH / std::tan(invert_flag * steering_angle_) + : std::numeric_limits::infinity(); + // Calculate turning radii of inside and outside wheels float inner_wheel_radius = turning_radius - VEHICLE_WIDTH / 2; float outer_wheel_radius = turning_radius + VEHICLE_WIDTH / 2; - for (size_t i = 0; i < data->ranges.size(); ++i) - { - if (data->ranges[i] < data->range_min || data->ranges[i] > data->range_max) - continue; + for (size_t i = 0; i < data->ranges.size(); ++i) { + if (data->ranges[i] < data->range_min || data->ranges[i] > data->range_max) continue; float dist_to_obstacle = std::numeric_limits::infinity(); @@ -114,37 +106,33 @@ class AutobrakeNode : public rclcpp::Node float x = invert_flag * (r * std::sin(theta) + LIDAR_HORIZONTAL_OFFSET); float y = r * std::cos(theta); - if (turning_radius == std::numeric_limits::infinity()) - { + if (turning_radius == std::numeric_limits::infinity()) { // If driving straight, check if obstacle is within the width of the vehicle if (std::abs(x) < VEHICLE_WIDTH / 2) dist_to_obstacle = y; else continue; - } - else - { + } else { // For curving, calculate radius distance to obstacle from center of movement circle - float radius_dist_to_obstacle = std::sqrt((x + turning_radius) * (x + turning_radius) + y * y); + float radius_dist_to_obstacle = + std::sqrt((x + turning_radius) * (x + turning_radius) + y * y); - if (outer_wheel_radius > radius_dist_to_obstacle && radius_dist_to_obstacle > inner_wheel_radius) - { + if (outer_wheel_radius > radius_dist_to_obstacle + && radius_dist_to_obstacle > inner_wheel_radius) { // Calculate angle from center of movement circle to obstacle float angle_from_center_to_obstacle = std::atan2(y, turning_radius + x); if (angle_from_center_to_obstacle < 0) angle_from_center_to_obstacle += 2 * M_PI; angle_from_center_to_obstacle = fmod(angle_from_center_to_obstacle, 2 * M_PI); - + // Calculate circumferential distance to obstacle dist_to_obstacle = turning_radius * angle_from_center_to_obstacle; - } - else + } else continue; } // If the obstacle is within a valid distance, adjust the minimum velocity - if (dist_to_obstacle >= 0) - min_vel = std::min(min_vel, maxVelocity(dist_to_obstacle)); + if (dist_to_obstacle >= 0) min_vel = std::min(min_vel, maxVelocity(dist_to_obstacle)); } // Set the brake value to the minimum calculated velocity @@ -152,27 +140,23 @@ class AutobrakeNode : public rclcpp::Node } // Callback to set the velocity and steering angle from the sensor_collect topic - void setVars(const cev_msgs::msg::SensorCollect::SharedPtr data) - { + void setVars(const cev_msgs::msg::SensorCollect::SharedPtr data) { velocity_ = data->velocity; steering_angle_ = -data->steering_angle; // Invert the steering angle } // Callback to set the target velocity from the AckermannDrive topic - void setTargets(const ackermann_msgs::msg::AckermannDrive::SharedPtr data) - { + void setTargets(const ackermann_msgs::msg::AckermannDrive::SharedPtr data) { target_velocity_ = data->speed; } // Publish the brake value at a regular interval - void publishBrake() - { + void publishBrake() { forward_brake_pub_->publish(forward_brake_); } }; -int main(int argc, char *argv[]) -{ +int main(int argc, char* argv[]) { rclcpp::init(argc, argv); rclcpp::spin(std::make_shared()); rclcpp::shutdown(); diff --git a/encoder_odometry/src/odometry.cpp b/encoder_odometry/src/odometry.cpp index e1c6002..c8cd5ec 100644 --- a/encoder_odometry/src/odometry.cpp +++ b/encoder_odometry/src/odometry.cpp @@ -8,22 +8,21 @@ using std::placeholders::_1; -class OdometryNode : public rclcpp::Node -{ +class OdometryNode : public rclcpp::Node { public: - OdometryNode() : Node("encoder_odometry") { + OdometryNode(): Node("encoder_odometry") { RCLCPP_INFO(this->get_logger(), "Initializing Ackermann Odometry Node"); sensor_collect_sub_ = this->create_subscription( "sensor_collect", 1, std::bind(&OdometryNode::updateOdometry, this, _1)); - + odom_publisher_ = this->create_publisher("/encoder_odometry", 1); } private: const float WHEELBASE = 0.185; - double x_ = 0.0; // Forward of base_link is x + double x_ = 0.0; // Forward of base_link is x double y_ = 0.0; double yaw_ = 0.0; @@ -37,14 +36,8 @@ class OdometryNode : public rclcpp::Node bool initialized = false; - double covariance_[36] = { - 0.01, 0, 0, 0, 0, 0, - 0, 0.01, 0, 0, 0, 0, - 0, 0, 0.01, 0, 0, 0, - 0, 0, 0, 0.01, 0, 0, - 0, 0, 0, 0, 0.01, 0, - 0, 0, 0, 0, 0, 0.01 - }; + double covariance_[36] = {0.01, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, + 0, 0.01, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, 0, 0.01}; rclcpp::Subscription::SharedPtr sensor_collect_sub_; rclcpp::Publisher::SharedPtr odom_publisher_; @@ -75,14 +68,14 @@ class OdometryNode : public rclcpp::Node y_ += delta_distance * std::sin(yaw_); publishOdometry(this->now(), data->velocity, delta_theta / dt); - + // Update carry vars old_steering_angle_ = new_steering_angle; prev_time_ = current_time; } - void publishOdometry(const rclcpp::Time ¤t_time, double linear_velocity, double angular_velocity) - { + void publishOdometry(const rclcpp::Time& current_time, double linear_velocity, + double angular_velocity) { /** * Publish odometry message using current pose/twist along with calculated velocities. */ @@ -104,8 +97,7 @@ class OdometryNode : public rclcpp::Node odom_msg.twist.twist.linear.x = linear_velocity; odom_msg.twist.twist.angular.z = angular_velocity; - for (int i = 0; i < 36; i++) - { + for (int i = 0; i < 36; i++) { odom_msg.pose.covariance[i] = covariance_[i]; } @@ -113,8 +105,7 @@ class OdometryNode : public rclcpp::Node } }; -int main(int argc, char *argv[]) -{ +int main(int argc, char* argv[]) { rclcpp::init(argc, argv); rclcpp::spin(std::make_shared()); rclcpp::shutdown(); diff --git a/serial_com/src/arduino_com.cpp b/serial_com/src/arduino_com.cpp index 71245ca..e7bc1d3 100644 --- a/serial_com/src/arduino_com.cpp +++ b/serial_com/src/arduino_com.cpp @@ -8,7 +8,8 @@ class SerialHandlerNode : public rclcpp::Node { public: - SerialHandlerNode() : Node("serial_handler_node"), steering_angle_(0.0), velocity_(0.0), max_velocity_(2.07) { + SerialHandlerNode() + : Node("serial_handler_node"), steering_angle_(0.0), velocity_(0.0), max_velocity_(2.07) { // Initialize serial port try { serial_port_.setPort("/dev/ttyACM0"); @@ -27,18 +28,20 @@ class SerialHandlerNode : public rclcpp::Node { } // Subscribers - rc_movement_sub_ = this->create_subscription( - "rc_movement_msg", 1, std::bind(&SerialHandlerNode::rcMovementCallback, this, std::placeholders::_1)); + rc_movement_sub_ = + this->create_subscription("rc_movement_msg", 1, + std::bind(&SerialHandlerNode::rcMovementCallback, this, std::placeholders::_1)); - auto_max_vel_sub_ = this->create_subscription( - "auto_max_vel", 1, std::bind(&SerialHandlerNode::maxVelocityCallback, this, std::placeholders::_1)); + auto_max_vel_sub_ = this->create_subscription("auto_max_vel", 1, + std::bind(&SerialHandlerNode::maxVelocityCallback, this, std::placeholders::_1)); // Publisher for sensor data - sensor_collect_pub_ = this->create_publisher("sensor_collect", 1); + sensor_collect_pub_ = this->create_publisher("sensor_collect", + 1); // Timer for reading serial data periodically - comm_timer_ = this->create_wall_timer( - std::chrono::milliseconds(20), std::bind(&SerialHandlerNode::commSerialData, this)); + comm_timer_ = this->create_wall_timer(std::chrono::milliseconds(20), + std::bind(&SerialHandlerNode::commSerialData, this)); } private: @@ -57,7 +60,7 @@ class SerialHandlerNode : public rclcpp::Node { // Subscribed topics to send over serial rclcpp::Subscription::SharedPtr rc_movement_sub_; rclcpp::Subscription::SharedPtr auto_max_vel_sub_; - + // Publisher for sensor data to SensorCollect message rclcpp::Publisher::SharedPtr sensor_collect_pub_; @@ -84,13 +87,15 @@ class SerialHandlerNode : public rclcpp::Node { // Parse the received message into reported sensor data if (parseMessage(message)) { - RCLCPP_DEBUG(this->get_logger(), "Parsed values - Int: %d, Float1: %f, Float2: %f", - reported_timestamp, reported_velocity, reported_steering_angle); + RCLCPP_DEBUG(this->get_logger(), + "Parsed values - Int: %d, Float1: %f, Float2: %f", reported_timestamp, + reported_velocity, reported_steering_angle); // Publish the parsed data publishSensorData(); } else { - RCLCPP_DEBUG(this->get_logger(), "Failed to parse message: '%s'", message.c_str()); + RCLCPP_DEBUG(this->get_logger(), "Failed to parse message: '%s'", + message.c_str()); } } } catch (const serial::IOException& e) { @@ -100,14 +105,13 @@ class SerialHandlerNode : public rclcpp::Node { try { // Use ostringstream to control the precision of the float values std::ostringstream serial_message; - serial_message << std::fixed << std::setprecision(2) - << velocity_ << " " - << steering_angle_ << " " - << max_velocity_ << "\n"; + serial_message << std::fixed << std::setprecision(2) << velocity_ << " " + << steering_angle_ << " " << max_velocity_ << "\n"; // Send the message over the serial port serial_port_.write(serial_message.str()); - RCLCPP_DEBUG(this->get_logger(), "Serial message sent: '%s'", serial_message.str().c_str()); + RCLCPP_DEBUG(this->get_logger(), "Serial message sent: '%s'", + serial_message.str().c_str()); } catch (const serial::IOException& e) { RCLCPP_ERROR(this->get_logger(), "Serial write error: %s", e.what()); @@ -122,16 +126,17 @@ class SerialHandlerNode : public rclcpp::Node { size_t second_space = message.find(' ', first_space + 1); if (first_space == std::string::npos || second_space == std::string::npos) { - return false; // Parsing failed + return false; // Parsing failed } try { reported_timestamp = std::stoi(message.substr(0, first_space)); - reported_velocity = std::stof(message.substr(first_space + 1, second_space - first_space - 1)); + reported_velocity = std::stof(message.substr(first_space + 1, + second_space - first_space - 1)); reported_steering_angle = std::stof(message.substr(second_space + 1)); return true; } catch (const std::exception& e) { - return false; // Parsing failed + return false; // Parsing failed } } @@ -148,13 +153,14 @@ class SerialHandlerNode : public rclcpp::Node { sensor_msg.velocity = reported_velocity; sensor_msg.steering_angle = reported_steering_angle; - sensor_collect_pub_->publish(sensor_msg); // Publish the message - RCLCPP_DEBUG(this->get_logger(), "Published sensor data: Timestamp: %f, Velocity: %f, Steering Angle: %f", - sensor_msg.timestamp, sensor_msg.velocity, sensor_msg.steering_angle); + sensor_collect_pub_->publish(sensor_msg); // Publish the message + RCLCPP_DEBUG(this->get_logger(), + "Published sensor data: Timestamp: %f, Velocity: %f, Steering Angle: %f", + sensor_msg.timestamp, sensor_msg.velocity, sensor_msg.steering_angle); } }; -int main(int argc, char *argv[]) { +int main(int argc, char* argv[]) { rclcpp::init(argc, argv); auto node = std::make_shared(); rclcpp::spin(node);