Pimpest/toid
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Switched to using serial ports by id for control

Browse Source
This commit is contained in:
Pimpest
2026-04-16 04:30:21 +02:00
parent 12a83e876a
commit 733a774c37
4 changed files with 91 additions and 75 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
toid_behaviors/include/toid_behaviors/simple_move.hpp
View File

@@ -140,6 +140,8 @@ public:
local_rival.x = dx * cosp - dy * sinp;
local_rival.y = dx * sinp + dy * cosp;
local_rival.x -= 0.105;
const double qx = std::abs(local_rival.x) - robot_length_ / 2.0;
const double qy = std::abs(local_rival.y) - robot_width_ / 2.0;
@@ -149,7 +151,7 @@ public:
double length = std::sqrt(mqx * mqx + mqy * mqy);
double sdf = length + std::min(std::max(qx, qy), 0.0);
RCLCPP_INFO(this->logger_, "Distance to rival %d: %lf", i++, sdf-rival_radius_);
RCLCPP_DEBUG(this->logger_, "Distance to rival %d: %lf", i++, sdf-rival_radius_);
if (sdf < rival_radius_) {
return true;
}
@@ -168,7 +170,7 @@ protected:
double robot_width_ = 0.30;
double robot_length_ = 0.30;
double rival_radius_ = 0.30;
double rival_radius_ = 0.22;
Rival::SharedPtr rivals_;
rclcpp::Subscription<Rival>::SharedPtr rivals_sub_;