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

Changed approach acorn to not use goal angle as target

Browse Source
This commit is contained in:
Pimpest
2026-04-15 15:39:30 +02:00
parent 980598718d
commit b154b5e719
2 changed files with 11 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
toid_behaviors/include/toid_behaviors/approach_acorns.hpp
View File

@@ -60,6 +60,7 @@ protected:
double new_target_angle_; double new_target_angle_;
geometry_msgs::msg::Pose target_pose_; geometry_msgs::msg::Pose target_pose_;
geometry_msgs::msg::Pose initial_pose_;
double target_angle_; double target_angle_;
double target_sign_; double target_sign_;
bool backwards_; bool backwards_;

13
toid_behaviors/src/approach_acorns.cpp
View File

@@ -112,11 +112,16 @@ void ApproachAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg)
pose_global.pose.position.x += std::cos(yaw) * -0.35 - std::sin(yaw) * 0.010; pose_global.pose.position.x += std::cos(yaw) * -0.35 - std::sin(yaw) * 0.010;
pose_global.pose.position.y += std::sin(yaw) * -0.35 + std::cos(yaw) * 0.010; pose_global.pose.position.y += std::sin(yaw) * -0.35 + std::cos(yaw) * 0.010;
tf2::convert(tf2::Quaternion::createFromRPY(0, 0, yaw), pose_global.pose.orientation);
yaw = angles::normalize_angle(yaw);
std::lock_guard _lock(mutex_); std::lock_guard _lock(mutex_);
const double dx = initial_pose_.position.x - pose_global.pose.position.x;
const double dy = initial_pose_.position.y - pose_global.pose.position.y;
double yaw_to_goal = std::atan2(dy,dx);
tf2::convert(tf2::Quaternion::createFromRPY(0, 0, yaw_to_goal), pose_global.pose.orientation);
yaw = yaw_to_goal;
auto [a, b, c] = avg_.push(pose_global.pose); auto [a, b, c] = avg_.push(pose_global.pose);
distance_ = x * x + y * y; distance_ = x * x + y * y;
new_target_pose_.position.x = a; new_target_pose_.position.x = a;
@@ -142,14 +147,16 @@ void ApproachAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg)
} }
ResultStatus ApproachAcorns::onStart( ResultStatus ApproachAcorns::onStart(
const std::shared_ptr<const MoveAction::Goal> command, const geometry_msgs::msg::Pose &, const std::shared_ptr<const MoveAction::Goal> command, const geometry_msgs::msg::Pose & pose,
const geometry_msgs::msg::Twist & vel) const geometry_msgs::msg::Twist & vel)
{ {
std::lock_guard _lock(mutex_); std::lock_guard _lock(mutex_);
distance_ = 1.0; distance_ = 1.0;
new_target_pose_.position.x = command->x; new_target_pose_.position.x = command->x;
new_target_pose_.position.y = command->y; new_target_pose_.position.y = command->y;
backwards_ = command->backwards; backwards_ = command->backwards;
initial_pose_ = pose;
new_target_pose_.orientation = tf2::toMsg(tf2::Quaternion::createFromRPY(0, 0, command->theta)); new_target_pose_.orientation = tf2::toMsg(tf2::Quaternion::createFromRPY(0, 0, command->theta));
new_target_angle_ = command->theta; new_target_angle_ = command->theta;