Monitored planning scene sync issues #3174
Comments
|
Is #3019 possibly related? (not sure why this would be more of an issue in Jazzy than Iron, but there is a noted PR in MoveIt 1 that has yet to be ported to ROS 2) |
|
I'm not using the Octomap so I don't think that is related, but thanks for taking a look @mikeferguson! To verify I tried to disable and enable the scene and it still has out of date information. I have also double checked the topic |
|
So I have started a debug branch here that includes additional print messages as the planning scene is updated. Here is what I have found/debugged so far...
Then joint_states continue to update till the arm_controller reports Once the trajectory has completed MTC applies changes to the planning scene to delete the attached collision object. These steps are reported by the prints |
|
Okay I kind of have a reproducible example with the Panda robot and the MTC demo. The issue is not as bad when running the Panda robot but you can see below that the The results very from run to run and you can execute the solutions multiple times without restarting the demo by selecting solutions and clicking the |
|
So when I run my custom robot I noticed that performPendingStateUpdate is not periodic and randomly runs every 1 to 3 seconds. When I test this on the Panda robot example it runs every 30-300mSec. I can see that this if statement logic goes all the way back to a ROS1 bug but don't get why |
Description
Previously I have been running my robot on Iron binaries and only see this issue when running MoveIt from source on the
mainbranch. As I have been debugging this I updated everything to Jazzy with MoveItmain(commitf91a650) and MTCros2(commit8b4aa43) from source just to make sure I was not fighting some underlying compatibility issue.The problem I noticed is that sometimes the PlanningScene display in Rviz will sometimes fail to update the robots pose at the end of trajectory execution. When this this update fails the objects that I am attaching will have an offset because the stored start state for the next move is out of date. I also noticed that on the next trajectory execution the command will jump to the old joint angles stored in the
planning_scene_monitorand start moving from there as seen in the plot below around the 3.5 second mark.From my understanding the
planning_scene_monitorruns at 2Hz and only updates it's internal copy of the robots state when it is in motion. My question is how does the planning_scene_monitor get updated at the end of trajectory execution if it happens to fall between the 2Hz update cycle?I have been trying to debug the issue but am falling short, so I'm hoping someone here can help point me in the right direction. I have changed several of the DEBUG messages in the
planning_scene_monitorand below is the output when the trajectory executes as expected and the planning scene finishes with the same state as the values reported on the/joint_statestopic. Notice the final print where therobot stamp: 0.648387has a recent value compared to the scene update time. You can also see when it printsscene update / robot stampthat it does it once from theplanning_scene_monitoras it publishes and a second time when Rviz receives the update.Here is the output when the planning scene fails to update at the end of trajectory execution.
ROS Distro
Jazzy
OS and version
Ubuntu 22.04 with Jazzy in Docker
Source or binary build?
Source
If binary, which release version?
No response
If source, which branch?
main
Which RMW are you using?
CycloneDDS
Steps to Reproduce
I can't post my code and can't reproduce the issue with the Panda/MTC demo.
Expected behavior
Planning scene updates at the end of trajectory execution
Actual behavior
The robot state fails to update and gives incorrect start state for the next motion.
Backtrace or Console output
No response
The text was updated successfully, but these errors were encountered: