Skip to content

Complete rotation in first half of path before finishing translation #546

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
KhaleelA19 wants to merge 1 commit into
base: main
Choose a base branch
from
Open

Complete rotation in first half of path before finishing translation #546

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
15 changes: 12 additions & 3 deletions src/subjugator/gnc/subjugator_path_planner/src/PathPlanner.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -49,7 +49,7 @@ std::vector<geometry_msgs::msg::Pose> PathPlanner::slerp(geometry_msgs::msg::Pos

// get vector from current to goal with magnitude segment_legnth, add this to A one too few times, then final path
// is B
// TODO rotations??

double const segment_legnth = 0.5;
double const x_err = goal_pose.position.x - last_odom_.pose.pose.position.x;
double const y_err = goal_pose.position.y - last_odom_.pose.pose.position.y;
Expand All @@ -72,13 +72,22 @@ std::vector<geometry_msgs::msg::Pose> PathPlanner::slerp(geometry_msgs::msg::Pos
segment_count_ = (int)(err_magnitude / segment_legnth);
for (int i = 1; i < segment_count_ + 1; ++i)
{
// orientation rides along with translation so short paths stay short
// orientation completes in the first half of the path, then holds
// at the goal rotation while translation finishes
double t = static_cast<double>(i) / (segment_count_);
double t2 = t * 2.0;
geometry_msgs::msg::Pose pose;
if (t2 < 1.0)
{
pose.orientation = slerp.at(t2).quat_msg();
}
else
{
pose.orientation = slerp.at(1.0).quat_msg();
}
pose.position.x = last_odom_.pose.pose.position.x + i * delta_x;
pose.position.y = last_odom_.pose.pose.position.y + i * delta_y;
pose.position.z = last_odom_.pose.pose.position.z + i * delta_z;
pose.orientation = slerp.at(t).quat_msg();
path.push_back(pose);
}

Expand Down
Loading