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

Fix logic in CheckStartStateBounds adapter #3143

Merged
merged 8 commits into from
Dec 4, 2024
+220 −43
Merged

Fix logic in CheckStartStateBounds adapter #3143

Changes from 2 commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
1c85808
Fix logic in CheckStartStateBoundsAdapter
sea-bass Nov 30, 2024
6517687
Explicit fallthrough
sea-bass Nov 30, 2024
9968f5e
Fix switch statement and add basic test scaffolding
sea-bass Nov 30, 2024
e0d57ac
Remove extra variable
sea-bass Nov 30, 2024
6c87d3d
Fix logic and add tests for continuous joints
sea-bass Dec 1, 2024
11d14a1
Validate changed start state
sea-bass Dec 1, 2024
bdd6494
Appeasing our Lord and Savior clang-tidy
sea-bass Dec 1, 2024
42ff99e
Update status message
sea-bass Dec 4, 2024
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
7 changes: 4 additions & 3 deletions moveit_ros/planning/planning_request_adapter_plugins/src/check_start_state_bounds.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -101,7 +101,7 @@ class CheckStartStateBounds : public planning_interface::PlanningRequestAdapter
// it is within the model's declared bounds (usually -Pi, Pi), since the values wrap around.
// It is possible that the encoder maintains values outside the range [-Pi, Pi], to inform
// how many times the joint was wrapped. Because of this, we remember the offsets for continuous
// joints, and we un-do them when the plan comes from the planner
// joints, and we undo them when the plan comes from the planner.
switch (jmodel->getType())
{
case moveit::core::JointModel::REVOLUTE:
Expand All @@ -115,8 +115,9 @@ class CheckStartStateBounds : public planning_interface::PlanningRequestAdapter
{
changed_req = true;
}
break;
}
break;
[[fallthrough]]; // If the revolute joint is non-continuous, we want to enter default case.
sea-bass marked this conversation as resolved.
Show resolved Hide resolved
}
// Normalize yaw; no offset needs to be remembered
case moveit::core::JointModel::PLANAR:
Expand Down Expand Up @@ -170,7 +171,7 @@ class CheckStartStateBounds : public planning_interface::PlanningRequestAdapter
}
}

// If we made any changes, consider using them them
// If we made any changes, consider using them.
if (params.fix_start_state && changed_req)
{
RCLCPP_WARN(logger_, "Changing start state.");
Expand Down
Loading