Skip to content

Commit

Permalink
Merge pull request #12 from cornellev/fix
Browse files Browse the repository at this point in the history
  • Loading branch information
@ethanuppal
ethanuppal authored Dec 11, 2024
2 parents 3887c95 + f5d2928 commit 16f3f4e
Show file tree
Hide file tree
Showing 4 changed files with 108 additions and 145 deletions.
80 changes: 31 additions & 49 deletions controls/src/autobrake_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<sensor_msgs::msg::LaserScan>(
"scan", 1, std::bind(&AutobrakeNode::checkCollision, this, _1));
scan_sub_ = this->create_subscription<sensor_msgs::msg::LaserScan>("scan", 1,
std::bind(&AutobrakeNode::checkCollision, this, _1));

sensor_collect_sub_ = this->create_subscription<cev_msgs::msg::SensorCollect>(
"sensor_collect", 1, std::bind(&AutobrakeNode::setVars, this, _1));
Expand All @@ -41,10 +38,8 @@ class AutobrakeNode : public rclcpp::Node

forward_brake_pub_ = this->create_publisher<std_msgs::msg::Float32>("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.");
}
Expand All @@ -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<float>::infinity();

float turning_radius = std::abs(steering_angle_) > 0.01
? VEHICLE_LENGTH / std::tan(invert_flag * steering_angle_)
: std::numeric_limits<float>::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<float>::infinity();

Expand All @@ -111,65 +101,57 @@ 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<float>::infinity())
{
if (turning_radius == std::numeric_limits<float>::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
forward_brake_.data = min_vel;
}

// 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<AutobrakeNode>());
rclcpp::shutdown();
Expand Down
88 changes: 36 additions & 52 deletions controls/src/new_autobrake_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,38 +15,35 @@ 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<sensor_msgs::msg::LaserScan>(
"scan", 1, std::bind(&AutobrakeNode::checkCollision, this, _1));
scan_sub_ = this->create_subscription<sensor_msgs::msg::LaserScan>("scan", 1,
std::bind(&AutobrakeNode::checkCollision, this, _1));

sensor_collect_sub_ = this->create_subscription<cev_msgs::msg::SensorCollect>(
"sensor_collect", 1, std::bind(&AutobrakeNode::setVars, this, _1));

rc_movement_sub_ = this->create_subscription<ackermann_msgs::msg::AckermannDrive>(
"rc_movement_msg", 1, std::bind(&AutobrakeNode::setTargets, this, _1));

forward_brake_pub_ = this->create_publisher<std_msgs::msg::Float32>("auto_max_vel/forward", 1);
backward_brake_pub_ = this->create_publisher<std_msgs::msg::Float32>("auto_max_vel/backward", 1);
forward_brake_pub_ = this->create_publisher<std_msgs::msg::Float32>("auto_max_vel/forward",
1);
backward_brake_pub_ =
this->create_publisher<std_msgs::msg::Float32>("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.");
}
Expand All @@ -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<sensor_msgs::msg::LaserScan>::SharedPtr scan_sub_;
Expand All @@ -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<float>::infinity();

float turning_radius = std::abs(steering_angle_) > 0.01
? VEHICLE_LENGTH / std::tan(invert_flag * steering_angle_)
: std::numeric_limits<float>::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<float>::infinity();

Expand All @@ -114,65 +106,57 @@ 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<float>::infinity())
{
if (turning_radius == std::numeric_limits<float>::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
forward_brake_.data = min_vel;
}

// 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<AutobrakeNode>());
rclcpp::shutdown();
Expand Down
Loading

0 comments on commit 16f3f4e

Please sign in to comment.