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

Fixed dockerfile to upgrade packages after updating

Browse Source
This commit is contained in:
Pimpest
2026-04-16 03:14:34 +02:00
parent 18ef55a204
commit 12a83e876a
2 changed files with 7 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -127,17 +127,18 @@ public:
bool check_rival_collision(geometry_msgs::msg::Pose2D & pose)
{
if (!rivals_) {
if (!rivals_ || (this->clock_->now() - rivals_->header.stamp ) > rclcpp::Duration::from_seconds(1.0)) {
return false;
}
const double cosp = std::cos(pose.theta);
const double sinp = std::sin(pose.theta);
int i = 0;
for (const auto & rival : rivals_->point) {
geometry_msgs::msg::Point local_rival;
const double dx = rival.x - pose.x;
const double dy = rival.y - pose.y;
local_rival.x = dx * cosp + dy * sinp;
local_rival.y = -dx * sinp + dy * cosp;
local_rival.x = dx * cosp - dy * sinp;
local_rival.y = dx * sinp + dy * cosp;
const double qx = std::abs(local_rival.x) - robot_length_ / 2.0;
const double qy = std::abs(local_rival.y) - robot_width_ / 2.0;
@@ -148,6 +149,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_);
if (sdf < rival_radius_) {
return true;
}