Fix heading check in compute_change_in_rotation for slotcar (#162)

kjchee · web-flow · commit dd32b1a7b850 · 2026-05-01T22:17:05.000+08:00
Signed-off-by: kjchee &lt;keai_jiang_chee@cgh.com.sg&gt;
diff --git a/rmf_robot_sim_common/src/slotcar_common.cpp b/rmf_robot_sim_common/src/slotcar_common.cpp
@@ -876,7 +876,7 @@ double SlotcarCommon::compute_change_in_rotation(
   const Eigen::Vector3d* requested_heading,
   double* const dir) const
 {
-  if (dpos.norm() < 1e-3)
+  if (dpos.norm() < 1e-3 && !requested_heading)
   {
     // We're right next to the waypoint, so we don't really need any heading
     // to reach it.

Original file line number	Diff line number	Diff line change
`@@ -876,7 +876,7 @@ double SlotcarCommon::compute_change_in_rotation(`
`876`	`876`	`const Eigen::Vector3d* requested_heading,`
`877`	`877`	`double* const dir) const`
`878`	`878`	`{`
`879`		`- if (dpos.norm() < 1e-3)`
	`879`	`+ if (dpos.norm() < 1e-3 && !requested_heading)`
`880`	`880`	`{`
`881`	`881`	`// We're right next to the waypoint, so we don't really need any heading`
`882`	`882`	`// to reach it.`