Skip to content

Cherry-pick reversibility fixes to jazzy #152

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

Merged
mxgrey merged 3 commits into from
Jun 24, 2025
Merged

Cherry-pick reversibility fixes to jazzy #152

Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
c137a8b
Rotate directly towards the immediate target (#149)
mxgrey Jun 11, 2025
f8229b7
Fix reverse drive for slotcar (#150)
mxgrey Jun 18, 2025
f695392
Fix style
mxgrey Jun 19, 2025
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
57 changes: 25 additions & 32 deletions rmf_robot_sim_common/src/slotcar_common.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -542,28 +542,14 @@ SlotcarCommon::UpdateResult SlotcarCommon::update_diff_drive(

if (rotate_towards_next_target)
{
if (_traj_wp_idx+1 < trajectory.size())
{
const auto dpos_next =
compute_dpos(trajectory.at(_traj_wp_idx+1).pose, _pose);

const auto goal_heading =
compute_heading(trajectory.at(_traj_wp_idx+1).pose);
const auto goal_heading =
compute_heading(trajectory.at(_traj_wp_idx).pose);

double dir = 1.0;
result.w = compute_change_in_rotation(
current_heading, dpos_next, &goal_heading, &dir);
result.w = compute_change_in_rotation(
current_heading,
Eigen::Vector3d::Zero(),
&goal_heading);

if (dir < 0.0)
current_heading *= -1.0;
}
else
{
const auto goal_heading =
compute_heading(trajectory.at(_traj_wp_idx).pose);
result.w = compute_change_in_rotation(
current_heading, goal_heading);
}
result.target_linear_speed_now = 0.0;
_current_mode.mode = rmf_fleet_msgs::msg::RobotMode::MODE_PAUSED;
}
Expand Down Expand Up @@ -591,12 +577,9 @@ SlotcarCommon::UpdateResult SlotcarCommon::update_diff_drive(
if (!rotate_towards_next_target && _traj_wp_idx < trajectory.size())
{
const double d_yaw_tolerance = 5.0 * M_PI / 180.0;
auto goal_heading = compute_heading(trajectory.at(_traj_wp_idx).pose);
double dir = 1.0;
result.w =
compute_change_in_rotation(current_heading, dpos, &goal_heading, &dir);
if (dir < 0.0)
current_heading *= -1.0;
result.w = compute_change_in_rotation(
current_heading, dpos, nullptr, &dir);

// If d_yaw is less than a certain tolerance (i.e. we don't need to spin
// too much), then we'll include the forward velocity. Otherwise, we will
Expand All @@ -615,7 +598,8 @@ SlotcarCommon::UpdateResult SlotcarCommon::update_diff_drive(
const auto goal_heading = compute_heading(trajectory.back().pose);
result.w = compute_change_in_rotation(
current_heading,
goal_heading);
Eigen::Vector3d::Zero(),
&goal_heading);

result.v = 0.0;
}
Expand Down Expand Up @@ -893,7 +877,7 @@ std::string SlotcarCommon::get_level_name(const double z)
double SlotcarCommon::compute_change_in_rotation(
const Eigen::Vector3d& heading_vec,
const Eigen::Vector3d& dpos,
const Eigen::Vector3d* traj_vec,
const Eigen::Vector3d* requested_heading,
double* const dir) const
{
if (dpos.norm() < 1e-3)
Expand All @@ -904,12 +888,21 @@ double SlotcarCommon::compute_change_in_rotation(
}

Eigen::Vector3d target = dpos;
// If a traj_vec is provided and slotcar is reversible, of the two possible
// headings (dpos/-dpos), choose the one closest to traj_vec
if (traj_vec && _reversible)
if (requested_heading)
{
// If requested_heading was provided, use that instead of dpos because requested_heading means
// we are already close enough to the target
target = *requested_heading;
}
else if (_reversible)
{
const double dot = traj_vec->dot(dpos);
target = dot < 0 ? -dpos : dpos;
// If no requested_heading is given then the robot needs to turn to face the
// direction that it needs to move towards, i.e. dpos. If the robot is
// reversible, then this dot product tells us if it should actually move
// towards the goal in reverse.
const auto dot = heading_vec.dot(target);

target = dot < 0 ? -target : target;
// dir is negative if slotcar will need to reverse to go towards target
if (dir)
{
Expand Down