From c137a8be96216b296ad5926ed3f4db2a0e95a0ef Mon Sep 17 00:00:00 2001 From: Grey Date: Wed, 11 Jun 2025 18:24:23 +0800 Subject: [PATCH 1/3] Rotate directly towards the immediate target (#149) Signed-off-by: Michael X. Grey Co-authored-by: Luca Della Vedova --- rmf_robot_sim_common/src/slotcar_common.cpp | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/rmf_robot_sim_common/src/slotcar_common.cpp b/rmf_robot_sim_common/src/slotcar_common.cpp index 301da85..98c158e 100644 --- a/rmf_robot_sim_common/src/slotcar_common.cpp +++ b/rmf_robot_sim_common/src/slotcar_common.cpp @@ -593,10 +593,7 @@ SlotcarCommon::UpdateResult SlotcarCommon::update_diff_drive( 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); // 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 From f8229b7a9cda294c148bddc8d4a27dec4a3e6bf8 Mon Sep 17 00:00:00 2001 From: Grey Date: Thu, 19 Jun 2025 06:32:17 +0800 Subject: [PATCH 2/3] Fix reverse drive for slotcar (#150) * Fix reverse drive for slotcar Signed-off-by: Michael X. Grey * Fix logic for choosing between driving toward target vs facing requested direction Signed-off-by: Michael X. Grey --------- Signed-off-by: Michael X. Grey --- rmf_robot_sim_common/src/slotcar_common.cpp | 53 ++++++++++----------- 1 file changed, 24 insertions(+), 29 deletions(-) diff --git a/rmf_robot_sim_common/src/slotcar_common.cpp b/rmf_robot_sim_common/src/slotcar_common.cpp index 98c158e..1c7b07e 100644 --- a/rmf_robot_sim_common/src/slotcar_common.cpp +++ b/rmf_robot_sim_common/src/slotcar_common.cpp @@ -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; } @@ -591,9 +577,8 @@ 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); + 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 @@ -612,7 +597,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; } @@ -890,7 +876,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) @@ -901,12 +887,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) { From f695392f734545eaf10fc0e7a519aa9fa542be25 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Thu, 19 Jun 2025 07:58:59 +0000 Subject: [PATCH 3/3] Fix style Signed-off-by: Michael X. Grey --- rmf_robot_sim_common/src/slotcar_common.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/rmf_robot_sim_common/src/slotcar_common.cpp b/rmf_robot_sim_common/src/slotcar_common.cpp index 1c7b07e..a17bbed 100644 --- a/rmf_robot_sim_common/src/slotcar_common.cpp +++ b/rmf_robot_sim_common/src/slotcar_common.cpp @@ -578,7 +578,8 @@ SlotcarCommon::UpdateResult SlotcarCommon::update_diff_drive( { const double d_yaw_tolerance = 5.0 * M_PI / 180.0; double dir = 1.0; - result.w = compute_change_in_rotation(current_heading, dpos, nullptr, &dir); + 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