Skip to content
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

Jump to bottom

Determine slotcar target heading by proximity to the commanded heading in its trajectory #254

Merged
merged 1 commit into from
Oct 30, 2020
Merged

Determine slotcar target heading by proximity to the commanded heading in its trajectory #254

Show file tree
Hide file tree
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
6 changes: 4 additions & 2 deletions building_sim_plugins/building_plugins_common/include/building_sim_common/slotcar_common.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -244,9 +244,11 @@ class SlotcarCommon

std::string get_level_name(const double z) const;

double compute_change_in_rotation(Eigen::Vector3d heading_vec,
double compute_change_in_rotation(
const Eigen::Vector3d& heading_vec,
const Eigen::Vector3d& dpos,
double* permissive = nullptr);
const Eigen::Vector3d* traj_vec = nullptr,
double* const dir = nullptr) const;

void publish_tf2(const rclcpp::Time& t);

Expand Down
34 changes: 18 additions & 16 deletions building_sim_plugins/building_plugins_common/src/slotcar_common.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -369,10 +369,10 @@ std::pair<double, double> SlotcarCommon::update(const Eigen::Isometry3d& pose,
if (!rotate_towards_next_target)
{
const double d_yaw_tolerance = 5.0 * M_PI / 180.0;

auto goal_heading = compute_heading(trajectory[_traj_wp_idx]);
double dir = 1.0;
velocities.second =
compute_change_in_rotation(current_heading, dpos, &dir);
compute_change_in_rotation(current_heading, dpos, &goal_heading, &dir);
if (dir < 0.0)
current_heading *= -1.0;
// If d_yaw is less than a certain tolerance (i.e. we don't need to spin
Expand Down Expand Up @@ -465,9 +465,10 @@ std::string SlotcarCommon::get_level_name(const double z) const
}

double SlotcarCommon::compute_change_in_rotation(
Eigen::Vector3d heading_vec,
const Eigen::Vector3d& heading_vec,
const Eigen::Vector3d& dpos,
double* permissive)
const Eigen::Vector3d* traj_vec,
double* const dir) const
{
if (dpos.norm() < 1e-3)
{
Expand All @@ -476,22 +477,23 @@ double SlotcarCommon::compute_change_in_rotation(
return 0.0;
}

// Flip the heading vector if the dot product is less than zero. That way,
// the robot will turn towards the heading that's closer.
const double dot = heading_vec.dot(dpos);
if (permissive && dot < 0.0)
{
heading_vec = -1.0 * heading_vec;
*permissive = -1.0;
}
else if (permissive)
Eigen::Vector3d target = dpos;
// If a traj_vec is provided, of the two possible headings (dpos/-dpos),
// choose the one closest to traj_vec
if (traj_vec)
{
*permissive = 1.0;
const double dot = traj_vec->dot(dpos);
target = dot < 0 ? -dpos : dpos;
// dir is negative if slotcar will need to reverse to go towards target
if (dir)
{
*dir = dot < 0 ? -1.0 : 1.0;
}
}

const auto cross = heading_vec.cross(dpos);
const auto cross = heading_vec.cross(target);
const double direction = cross(2) < 0.0 ? -1.0 : 1.0;
const double denom = heading_vec.norm() * dpos.norm();
const double denom = heading_vec.norm() * target.norm();
const double d_yaw = direction * std::asin(cross.norm() / denom);

return d_yaw;
Expand Down