Stopping robot during planned moves fails occasionally

[riv-david]

I have been working on a problem where the robot needs to stop and then plan a different move based on the status of a sensor.

Expected sequence:

• robot starts at point A
• robot moves to point B (planned path, then executed)
• Sensor triggers
• Robot stop issued (before getting to B)
• robot moves to point A (planned path, then executed)
• sensor trigger
• robot stop issued (before getting to A)
• robot moves to point A (planned path, then executed)
• robot gets to point A

my python code commanding this sequence completes as expected however when running the sequence for 10 cycles in a row for example it will trigger an error (see below) on some of the stop commands but not all 20 (2 stops per cycle). The robot stops at the point expected (visually checked, over 1cm before the target position) when the sensor is triggered. The next move is also planned fine and the whole sequence continues regardless of the error.

I am using the code below to achieve the stops. moveit is our own python module for interacting with moveit. Within that is a standard movegroup which is defined like this move_group = moveit_commander.MoveGroupCommander(self.planning_group, ns=f"/{self.ns}")

self.moveit.move_group.stop()
self.moveit.move_group.set_start_state_to_current_state()
self.moveit.move_group.clear_pose_targets()
time.sleep(0.5)

Here is a Wireshark dump of the communication and the package numbers of interest;

• 61040 (control message that worked)
• 61563 (reply saying it worked)
• 67677 (control message that failed)
• 67736 (reply saying it failed)

wiresharkdump.zip

[gavanderhoorn]

Could you locally merge #389 and see whether that prevents the error?

[riv-david]

We will give it a go today

[riv-david]

@gavanderhoorn we merged that PR branch into our local version and tested it this afternoon and were able to replicate the same issue. It appeared within 10 of the cycles described above.

Can I ask if we are best continuing with that PR merged in anyway/wondering why it has not been merged yet?

[EricMarcil]

@David-Mason-1992 When this occurs and you get the error, does the motion still stops? If it continues, do it reach its fully reach its target position?
Basically on the MotoRos driver, there is a stop flag that is raise to signal different motion working thread to stop. It then waits for the confirmation that those threads stop with a 20 ms timeout. Then, the Queue of buffered points is cleared out. The return value is based on both stopping the processing and clearing the queue. I'm wondering which one would be failing. If you are able to telnet in the controller to see console feedback, check if you see a message: WARNING: Message processing not stopped before clearing queue. For instruction on telnet to the controller please refer to the following manual, section 5.4 Viewing Output on PC Console Window. https://www.motoman.com/getmedia/0b44600a-ea48-4174-b81a-89e3cbd90dcd/190844-1CD.pdf.aspx?ext=.pdf

[riv-david]

Hi @EricMarcil.

When we get this error the robot still stops as expected - no visible difference from when we do not get console errors.

We will have a look. Should we do that test on the branch where we merged in #389 or our original one?

[gavanderhoorn]

I have a suspicion there might (also) be a mismatch between the values reported by MotoROS and the values (in the enum) motoman_driver uses to detect success or failure.

I remember a similar case earlier this year.

Haven't had time to check this specific occurrence though.

Can I ask if we are best continuing with that PR merged in anyway/wondering why it has not been merged yet?

Well, if you're using motoman_driver in a normal way, then you wouldn't really run into issues without it I believe.

And the reason it hasn't been merged is, as you can see from the comments on that PR, I requested a review by other maintainers but that hasn't happened yet. If you can comment on whether you've seen any issues from using the changes in that PR on the PR that would be appreciated.

[EricMarcil]

@David-Mason-1992 If the issue is in the MotoRos driver and you are seeing it with either branch, then I don't think it matters on which on you test.

[riv-david]

Ok thank you both we will take a look :)

[EricMarcil]

@David-Mason-1992 Looking at the WireShark you sent, it looks like the problem is occuring when the internal motion queue is full. You can see that you start receiving some busy signal on new trajectory points (which is normal, ROS will resend the request and it will eventually get accepted when the queue empties out).
I can see that the thread we are trying to stop is then in a subloop to add points to the queue which prevents it to react to the stop signal. We just need to add a condition to the subloop. I'll make the change and send you a new version to test shortly.

[EricMarcil]

@David-Mason-1992 Here's a zipped output folder with new version of the MotoRos driver. Select the one from the folder that matches your controller. (Make sure to delete the current version from your controller.) (zipped file was removed. Please use files from PR #426)

Note: I just made the change, I'm not currently setup to test it. So I haven't created a PR for it yet. Let me know if it fixes the problem. Or if you want me to test it here, I'll can try to do it tomorrow or early next week.

[gavanderhoorn]

@EricMarcil: might actually be nice to push your changes and open the PR.

Then we can discuss the change -- and @David-Mason-1992 can report whether it fixes his issue -- there.

[EricMarcil]

PR #426 I only included the code change for now. I'll update the version and MotoRos binary once we have confirmation that the issue is fixed.

[riv-robot]

I have been running the same code as @David-Mason-1992 and haven't been able to replicate the issue. PR #426 has fixed the issue.