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 planning_scene_monitor sync when passed empty robot state #3187

Merged
merged 4 commits into from
Jan 7, 2025
Merged

Fix planning_scene_monitor sync when passed empty robot state #3187

Changes from 1 commit
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
06b171f
Fix planning_scene_monitor sync when passed empty robot state
MarqRazz Jan 2, 2025
9586c7d
address review
MarqRazz Jan 3, 2025
cb72982
check for null current_state_monitor_ and fall back to diff scene_ if…
MarqRazz Jan 3, 2025
2b56c75
Merge branch 'main' into fix_planning_scene_sync
sjahr Jan 7, 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
17 changes: 13 additions & 4 deletions moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -731,11 +731,20 @@ bool PlanningSceneMonitor::newPlanningSceneMessage(const moveit_msgs::msg::Plann

if (!scene.is_diff && parent_scene_)
{
// if the scene does not contain a robot state then use the latest value from the current_state_monitor_
// so that we are not updating the scene with stale values for the robot's state.
if (scene.robot_state.joint_state.name.empty() && current_state_monitor_->haveCompleteState())
// if the scene does not contain a robot state then use the latest known values for the robot state
// so that we are not updating the scene with stale values.
if (scene.robot_state.joint_state.name.empty())
{
parent_scene_->setCurrentState(*current_state_monitor_->getCurrentState());
// if we have a valid current_state_monitor_ with complete state use the latest data otherwise try
// to use the maintained (diff) scene_ which may fall back to the parent_scene_ data and not update.
if (current_state_monitor_ && current_state_monitor_->haveCompleteState())
{
parent_scene_->setCurrentState(*current_state_monitor_->getCurrentState());
}
else
{
parent_scene_->setCurrentState(scene_->getCurrentState());
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Why didn't you simply use this line only? All you wanted to achieve is to not loose RobotState information from existing planning scene diffs.

Copy link
Contributor Author

@MarqRazz MarqRazz Jan 7, 2025 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

When I was testing I found that the diff scene is not updated at a constant rate and wanted to ensure I used the most up to date information. I can make this a single line if you think making the diff scene update run at a fixed rate is worthwhile.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

If we go to the single line would it be better to push the diffs to the parent scene? Could there be other changes in the diff that might need to be applied before we replace the scene?

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

We need to deal with both issues separately.
This PR was about correctly maintaining the RobotState from the diffs. If the planning scene didn't have newer updates from the state monitor yet, we shouldn't care to pull them in early.

That the scene is not updated at a constant rate is another issue. Updates to the PSM's scene are only performed if 1) there are new updates received by CSM and 2) the throttling frequency is met. If both was true in your tests, then their is an issue with those updates, which should get fixed.
As far as I understood, you were observing issues with rviz' PlanningScene. This is updated from move_group's (PSM's) planning scene via a separate topic at a low rate of 2Hz:

moveit2/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp

Line 448 in 80c7f61

void PlanningSceneMonitor::scenePublishingThread()

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I suggest to cleanup this PR as I did for the MoveIt1 backport (moveit/moveit#3683).

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

We need to deal with both issues separately.

That is what I though too which is why I did not include any of those changes here. I will work on investigating the update rate and try to include a test to ensure it is working as expected.

As far as I understood, you were observing issues with rviz' PlanningScene. This is updated from move_group's (PSM's) planning scene via a separate topic at a low rate of 2Hz:

Yes this issues is visible in Rviz but is also effects future plans because the parent_scene contained stale robot state data after executing a move causing it to jump to the last known position on the next motion plan (shown in the plot below).
image

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Sure. This is fixed with the present PR. However, the other issue, i.e. the scene not being updated regularly, is a different story.

}
}
// clear maintained (diff) scene_ and set the full new scene in parent_scene_ instead
scene_->clearDiffs();
Expand Down
Loading