Robot status topic via controller #141

axelschroth · 2020-03-24T15:30:55Z

Implements issue #32

Notes and limitations:

controller publishes state messages as long as ExternalControl is running or paused on UR. Otherwise rostopic echo /robot_status does not receive further messages.
Tested with UR simulation software -> no tests regarding emergency stops (since this is not possible with ursim)
Criteria of published content is based on ur_modern_driver implementation (see Publish industrial_msgs::RobotStatus Zagitta/ur_modern_driver#5)

Update from upstream repo

gavanderhoorn · 2020-03-24T15:54:06Z

Does anyone have an idea, why no further state messages are published by industrial_robot_status_controller as soon as I stop the robot program (with ExternalControl Cap, see PR)?

The controller will only publish something if its update(..) is called.

You may have to add it to the consistent_controllers list here so it doesn't get stopped.

fixes problem that no messages are published if robot program is not running

gavanderhoorn · 2020-03-24T17:46:00Z

ur_robot_driver/package.xml

@@ -38,6 +38,7 @@
  <depend>ur_dashboard_msgs</depend>
  <depend>ur_msgs</depend>
  <depend>std_srvs</depend>
+  <depend>industrial_robot_status_interface</depend>


Should this be a build_depend only?

It's a header-only library, there is nothing at runtime there.

Yes, you're right

gavanderhoorn · 2020-03-24T17:48:00Z

Would you not need to add industrial_robot_status_interface to the COMPONENTS of Catkin in the CMakeLists.txt?

axelschroth · 2020-03-26T15:35:12Z

Would you not need to add industrial_robot_status_interface to the COMPONENTS of Catkin in the CMakeLists.txt?

Done with last commit. Thanks! Any further suggestions?

gavanderhoorn · 2020-03-26T15:36:50Z

Not right now.

Seeing as CI is 'down' at the moment (#144) and I'm not an maintainer here we'll have to wait for @fmauch to process the PR :)

But thanks for implementing this. 👍

gavanderhoorn · 2020-03-27T16:49:04Z

ur_robot_driver/src/ros/hardware_interface.cpp

+void HardwareInterface::extractRobotStatus()
+{
+  // robot status bit 2: Is teach button pressed
+  robot_status_resource_.mode = robot_status_bits_[2] ? RobotMode::MANUAL : RobotMode::AUTO;


I guess I do have an additional comment: please use some constants here for the bit indices instead of magic nrs.

Is this really the right check? The robot can be in manual mode without the teach button being pressed, right?

I am aware, that usually on industrial robots manual and automatic modes have a different meaning than the one used on UR robots. But boiling it down to the teach button pressed feels misleading to me, as well.

@fmauch: this was ported from ur_modern_driver I believe.

Would you happen to have a suggestion on how to better determine whether the controller is in AUTO or MANUAL?

I don't even believe URs really have that concept.

When they're being "freedrived" though they are definitely not in AUTO (or whatever we want to call the equivalent on URs).

I don't even believe URs really have that concept.

They do (on e-Series, on CB3 the same concept was called "running mode" and "programming mode"), but that concept is different (or a little less restrictive). In AUTO mode (which you can activate after setting a password) one can only load and execute predefined programs. (See section "Operational Mode Selection" in the manual)

However, I am currently not aware of a method receiving this information. (which, of course, doesn't necessarily mean, that it doesn't exist)

When they're being "freedrived" though they are definitely not in AUTO (or whatever we want to call the equivalent on URs).

Yes, but it feels as if we would be misusing the field from the message for something semantically different. Which, as you reminded me often, not the best thing to do :-)

Would you happen to have a suggestion on how to better determine whether the controller is in AUTO or MANUAL?

Forgot to answer that... To be honest - I don't know. I think, using what UR calls manual and automatic operational mode comes closest to what other industrial robots semantically state with this.

I agree with your points, but as you said @gavanderhoorn , this implementation is based on the interpretation of the ur_modern_driver.
I also thought about the issue when porting to the RTDE interface, but I didn't find any possibility to read this information about the Operational Mode over the RTDE interface (see RTDE Guide )...

Another point to think of: What would we do with the CB-series? I think in the old Polyscope versions, you have to use a configurable input to change to running mode. Yet we could use that as "robot mode" if we have the information.

What about limiting this PR to porting the ur_modern_driver implementation, then using a potential follow-up PR after/if we get more information on what @fmauch mentions?

It would be a nice enhancement over the partial implementation from ur_modern_driver.

@axelschroth: if you could address my comment about magic nrs which started this thread, I'd vote for merging it then.

I'd be happy about that, I will commit the changes about the magic nrs tomorrow.

What about limiting this PR to porting the ur_modern_driver implementation, then using a potential follow-up PR after/if we get more information on what @fmauch mentions?

I'd be fine with that.

I addressed your suggestion using an enum in the rtde client, because this belongs to the rtde specification. Are you okay with that?

gavanderhoorn · 2020-03-31T09:07:47Z

ur_robot_driver/src/ros/hardware_interface.cpp

+  // note that e-stop is handled by a seperate variable
+  robot_status_resource_.in_error =
+      safety_status_bits_[UrRtdeSafetyStatusBits::IS_PROTECTIVE_STOPPED] ? TriState::TRUE : TriState::FALSE;


One thing I believe we could include in this PR: in_error should be the union of anything which makes the controller go into an error state. Is IS_EMERGENCY_STOPPED also a cause of IS_PROTECTIVE_STOPPED? If not, then it would need to be || here I believe.

Any other causes we could add to in_error?

Sure, I can add this. Unfortunately I cannot test emergency stops, since I don't have a real robot here, just ursim. I don't know the behaviour of these bits, therefore I cannot tell which bits provide additional information regarding the error state.

I think all of these should be used for that:

IS_PROTECTIVE_STOPPED = 2, IS_ROBOT_EMERGENCY_STOPPED = 6, IS_EMERGENCY_STOPPED = 7, IS_VIOLATION = 8, IS_FAULT = 9, IS_STOPPED_DUE_TO_SAFETY = 10

I could make a short trip to the lab these days to test the behavior.

fmauch · 2020-03-31T11:42:45Z

ur_robot_driver/include/ur_robot_driver/rtde/rtde_client.h

@@ -48,6 +48,29 @@ namespace ur_driver
 {
 namespace rtde_interface
 {
+enum UrRtdeRobotStatusBits


Please make this scoped enums as we use this all over the driver.

Ok, then I'd rather switch to constants like

static const uint8_t ROBOT_STATUS_BIT_IS_POWER_ON = 0;

and so on, because accessing the bitsets with scoped enums gets quite ugly..

I believe @fmauch already added a std::underlying_type<> somewhere?

fmauch · 2020-03-31T11:43:01Z

ur_robot_driver/include/ur_robot_driver/rtde/rtde_client.h

+  IS_POWER_BUTTON_PRESSED = 3
+};
+
+enum UrRtdeSafetyStatusBits


Scoped enums, same as above

fmauch · 2020-03-31T11:45:38Z

ur_robot_driver/config/ur3e_controllers.yaml

+
+robot_status_controller:
+   type: industrial_robot_status_controller/IndustrialRobotStatusController
+   handle_name: industrial_robot_status_handle
+   publish_rate: 10


This should be added for the other controller files, as well.

Good point! Thanks for reviewing

gavanderhoorn · 2020-03-31T13:59:08Z

Thanks for iterating @axelschroth 👍

gavanderhoorn · 2020-03-31T14:08:15Z

ur_robot_driver/src/ros/hardware_interface.cpp

+  if (safety_status_bits_[toUnderlying(UrRtdeSafetyStatusBits::IS_PROTECTIVE_STOPPED)] ||
+      safety_status_bits_[toUnderlying(UrRtdeSafetyStatusBits::IS_ROBOT_EMERGENCY_STOPPED)] ||
+      safety_status_bits_[toUnderlying(UrRtdeSafetyStatusBits::IS_EMERGENCY_STOPPED)] ||
+      safety_status_bits_[toUnderlying(UrRtdeSafetyStatusBits::IS_VIOLATION)] ||
+      safety_status_bits_[toUnderlying(UrRtdeSafetyStatusBits::IS_FAULT)] ||
+      safety_status_bits_[toUnderlying(UrRtdeSafetyStatusBits::IS_STOPPED_DUE_TO_SAFETY)])


This may be taking it too far, but: seeing as this set never changes, what about keeping a "mask" bitset around, initialised once and then used as:

if ((safety_status_bits_ | in_error_bitset_).any()) ...

std::bitset supports the necessary operators.

I'll leave that up to @axelschroth It is fine like that from my side.

We could ignore it in this PR, but seeing as this is run @ the controller's update rate (ie: 125 or 500 Hz), it's 7*500 vs 1*500 operations per second. I'm not sure the compiler will be able to coalesce all the bitset::test(..) invocations into a single masking operation.

true... And it should be fairly quick to implement :-)

fmauch · 2020-04-01T09:10:12Z

Thanks again @axelschroth I well test that in the lab ASAP

axelschroth · 2020-04-01T09:49:52Z

Thank you guys for reviewing @gavanderhoorn and @fmauch!

fmauch · 2020-04-03T07:26:54Z

Testing results:

Robot running:

mode:
  val: 2
e_stopped:
  val: 0
drives_powered:
  val: 1
motion_possible:
  val: 1
in_motion:
  val: -1
in_error:
  val: 1
error_code: 0

EM-Stop hit:

mode:
  val: 2
e_stopped:
  val: 1
drives_powered:
  val: 1
motion_possible:
  val: 0
in_motion:
  val: -1
in_error:
  val: 1
error_code: 0

EM-Stop released:

mode:
  val: 2
e_stopped:
  val: 0
drives_powered:
  val: 1
motion_possible:
  val: 0
in_motion:
  val: -1
in_error:
  val: 1
error_code: 0

Protective stop:

mode:
  val: 2
e_stopped:
  val: 0
drives_powered:
  val: 1
motion_possible:
  val: 1
in_motion:
  val: -1
in_error:
  val: 1
error_code: 0

After Bootup:

mode:
  val: 2
e_stopped:
  val: 0
drives_powered:
  val: 0
motion_possible:
  val: 0
in_motion:
  val: -1
in_error:
  val: 1
error_code: 0

Switched on after bootup (not yet started):

mode:
  val: 2
e_stopped:
  val: 0
drives_powered:
  val: 1
motion_possible:
  val: 0
in_motion:
  val: -1
in_error:
  val: 1
error_code: 0

in_error seems to be wrong, I will look into that, later.

ur_robot_driver/src/ros/hardware_interface.cpp

fmauch · 2020-04-03T07:44:42Z

motion_possible in protective stop seems wrong, right?

ur_robot_driver/src/ros/hardware_interface.cpp

Co-Authored-By: Felix Exner <felix_mauch@web.de>

fmauch · 2020-04-03T08:49:40Z

ur_robot_driver/src/ros/hardware_interface.cpp

+  robot_status_resource_.drives_powered =
+      robot_status_bits_[toUnderlying(UrRtdeRobotStatusBits::IS_POWER_ON)] ? TriState::TRUE : TriState::FALSE;
+
+  robot_status_resource_.motion_possible =


I think a union would be good here, as well. Best achieved by combination with the following safety status bits:

IS_PROTECTIVE_STOPPED = 2, IS_RECOVERY_MODE = 3, IS_SAFEGUARD_STOPPED = 4, IS_SYSTEM_EMERGENCY_STOPPED = 5, IS_ROBOT_EMERGENCY_STOPPED = 6, IS_EMERGENCY_STOPPED = 7, IS_VIOLATION = 8, IS_FAULT = 9, IS_STOPPED_DUE_TO_SAFETY = 10

Sorry for coming with that this late, it just came up while testing.

Can't we check in_error? Are there any bits in this list which are not in the other?

I did not propose IS_SAFEGUARD_STOPPED for in_error as I wouldn't consider that an error status. However, motion will not be possible, while this is active...

Hm. Other industrial robots will report this as an error I believe.

If we don't, then we can run into the situation where in_error is false and motors_on is true, but motion_possible is false. That would at least be confusing.

I thought, this would be exactly the purpose of this field. Otherwise it would be redundant, right?

But if that's the way this is usually being handled, I am absolutely fine with it.

Then motion_possible should be (pseudo):

motion_possible = not (in_error or (IS_SAFEGUARD_STOPPED in robot_status_bits_))

I believe.

Or if we want to be nice: it should also include the status of the external control program.

Alright, I will modify the expression for motion_possible shortly. However I don't have time to include the status of the external control program, this condition may be added later if that's okay.

Yes, absolutely!

motion_possible = in_error or (IS_SAFEGUARD_STOPPED in robot_status_bits_)

I guess you mean the negated version of this expression. Motion is not possible if controller is in error or safeguard stop

Yes, of course.

Updated.

…mode

fmauch · 2020-04-14T08:50:23Z

I just tested this PR again and noticed one more issue: robot_motion always states true when there is no error and no safeguard stop active, even if the robot hardware is powered down completely. I made a PR to your branch @axelschroth with how I think the behavior is more correct.

set motion_possible to true only of robot can be actually moved

axelschroth · 2020-04-20T12:23:52Z

I just tested this PR again and noticed one more issue: robot_motion always states true when there is no error and no safeguard stop active, even if the robot hardware is powered down completely. I made a PR to your branch @axelschroth with how I think the behavior is more correct.

Great, thank you! I merged your PR.

gavanderhoorn

This looks like it's OK now.

👍 @axelschroth and thanks again for iterating.

Adding a check on the status of the control program can be done in a future PR.

fmauch · 2020-04-20T16:08:03Z

Perfect, I'll give it another HW test tomorrow and then merge it in if everything is fine. Thanks again for your continuous effors @axelschroth

gavanderhoorn · 2020-04-21T12:44:51Z

Nice 🎉

axelschroth added 2 commits March 20, 2020 12:47

Merge pull request #1 from UniversalRobots/master

9f506df

Update from upstream repo

Added robot_status topic via industrial robot status controller

e388e54

axelschroth added 2 commits March 24, 2020 17:04

Added robot_status_controller to consistent_controllers

a8381ef

fixes problem that no messages are published if robot program is not running

clang formatting

a6c752d

gavanderhoorn reviewed Mar 24, 2020

View reviewed changes

Adjusted dependencies and formatting

121789f

gavanderhoorn reviewed Mar 27, 2020

View reviewed changes

Robot/safety status bits: Replaced comments by enums

df4bf93

gavanderhoorn reviewed Mar 31, 2020

View reviewed changes

Robot status: in_error considers emergency stopped flag

2e8445d

fmauch suggested changes Mar 31, 2020

View reviewed changes

axelschroth added 3 commits March 31, 2020 15:52

Added robot status controller to all configs

1822563

Use scoped enums

0ef07d5

Robot status: in_error considers several error bits

a3df292

gavanderhoorn reviewed Mar 31, 2020

View reviewed changes

fmauch approved these changes Mar 31, 2020

View reviewed changes

Reduce bitset tests for in_error state

f6596c2

fmauch reviewed Apr 3, 2020

View reviewed changes

ur_robot_driver/src/ros/hardware_interface.cpp Outdated Show resolved Hide resolved

fmauch reviewed Apr 3, 2020

View reviewed changes

ur_robot_driver/src/ros/hardware_interface.cpp Outdated Show resolved Hide resolved

fmauch reviewed Apr 3, 2020

View reviewed changes

ur_robot_driver/src/ros/hardware_interface.cpp Outdated Show resolved Hide resolved

Robot status: fixed in_error state

19c41e6

Co-Authored-By: Felix Exner <felix_mauch@web.de>

fmauch reviewed Apr 3, 2020

View reviewed changes

axelschroth and others added 3 commits April 9, 2020 11:59

Robot status: motion possible depends on error bits instead of robot …

44a9db2

…mode

Merge remote-tracking branch 'origin/robot_status' into robot_status

0a3121b

set motion_possible to true only of robot can be actually moved

de535e1

fmauch and others added 3 commits April 14, 2020 11:10

Code formatting

6915b0e

Only reflect RobotMode::RUNNING in motion_possible

b593c18

Merge pull request #2 from fmauch/robot_status

e03bb3f

set motion_possible to true only of robot can be actually moved

gavanderhoorn approved these changes Apr 20, 2020

View reviewed changes

fmauch merged commit ad3cb7c into UniversalRobots:master Apr 21, 2020

gavanderhoorn mentioned this pull request Apr 21, 2020

Publish industrial_msgs/RobotStatus messages #32

Closed

axelschroth deleted the robot_status branch April 21, 2020 16:56

fmauch mentioned this pull request May 12, 2020

diagnostics concept #170

Open

gavanderhoorn mentioned this pull request Sep 29, 2021

Ability to detect program pause on teach pendant via ROS topics #478

Open

Robot status topic via controller #141

Robot status topic via controller #141

Conversation

axelschroth commented Mar 24, 2020

gavanderhoorn commented Mar 24, 2020

Choose a reason for hiding this comment

Choose a reason for hiding this comment

gavanderhoorn commented Mar 24, 2020

axelschroth commented Mar 26, 2020

gavanderhoorn commented Mar 26, 2020

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

fmauch Mar 30, 2020 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

gavanderhoorn commented Mar 31, 2020

gavanderhoorn Mar 31, 2020 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

gavanderhoorn Mar 31, 2020 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

fmauch commented Apr 1, 2020

axelschroth commented Apr 1, 2020

fmauch commented Apr 3, 2020

fmauch commented Apr 3, 2020

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

gavanderhoorn Apr 7, 2020 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

fmauch commented Apr 14, 2020

axelschroth commented Apr 20, 2020

gavanderhoorn left a comment

Choose a reason for hiding this comment

fmauch commented Apr 20, 2020

gavanderhoorn commented Apr 21, 2020

fmauch Mar 30, 2020 •

edited

Loading

gavanderhoorn Mar 31, 2020 •

edited

Loading

gavanderhoorn Mar 31, 2020 •

edited

Loading

gavanderhoorn Apr 7, 2020 •

edited

Loading