Is it possible to completely disable backup safety?

[siqiyan]

Hi, I am currently wrote a simple node that use the IR sensor to allow the Create 3 robot to follow its front obstacle but keep a fixed distance (around 10 cm). If the obstacle moves forward, the robot follows, and if the obstacle moves backward the robot backup accordingly. I am using a PID controller to generate continues motion command and send it to cmd_vel topic, right now the forward movement works perfectly, but the problem is that due to safety constraints the robot does not backup and the LED turns to yellow. I am aware that the robot has backup limit by default, so I went to the web interface and change safety_override to full and restart the application, but even doing this does not give me full backup control, everytime a negative linear motion is generated the robot LED still turns to yellow and it stops moving.
After some testing I found that if the negative motion (say linear.x=-0.5) is send only once the robot can backup for around half meter, then if the message is send again the robot will continue backup for another half meter and so on; but if the same motion command is send repeatedly at 10Hz then the robot will refuse to backup. So I think there is still some restrictions that prevent the robot from backup even if the safety_override is set to full. Is that possible to completely disable this constraint? Thanks!

[alsora]

This is not the expected behavior.
Setting safety_override to full should disable all safety mechanisms, as described in the Safety Configuration docs.

In particular, note that you can change safety settings using a ROS 2 parameter service call, rather than the webserver.

ros2 param set /motion_control safety_override full

Similarly, you can query the current safety value from a running robot using

ros2 param get /motion_control safety_override

Would you mind trying again using these calls and verifying that the parameter is set as expected?
It would also be helpful to see the logs on /rosout topic while you do these operations and then when you drive backward.

[siqiyan]

Hi, thanks for the reply! I tried to set the safety_override using the service call, and here is the output for the query:

String value is: full

Unfurtunately the robot still refuse to backup and the LED turns to yellow as before. Here is the output for ros2 topic echo /rosout when the robot stops:

stamp:
  sec: 1660427954
  nanosec: 349679318
level: 30
name: motion_control
msg: Reached backup limit! Stop Driving robot backward or disable from safety_override parameter!
file: ../../apps/create-platform/nodes/src/motion-control-node.cpp
function: control_robot
line: 294

[alsora]

Ok, thanks for the information.
Is your robot running the latest firmware version? This should be G.2.2, you can check the version from the webserver.

In any case, this seems to be a software bug.
I will run some test to investigate.

[siqiyan]

Yes, the firmware version is G.2.2

[eschembor-irobot]

Can you try setting the safety_override using the service call to "none" and then back to "full" and see if that allows the robot to move backwards.
Note that if the robot has already exhausted its backup budget before enabling the safety_override then you may still see the yellow LED and that "Reached backup limit!" message, although it should still allow the robot to backup.

[siqiyan]

Can you try setting the safety_override using the service call to "none" and then back to "full" and see if that allows the robot to move backwards. Note that if the robot has already exhausted its backup budget before enabling the safety_override then you may still see the yellow LED and that "Reached backup limit!" message, although it should still allow the robot to backup.

Hi, I set the safety_override to "none" and then to "full" using the service call but it does not work, the LED still turns to yellow and the robot does not backup.

[maddiepero]

I have found success if I set the safety override parameter to full when I spin the node. Here is the link to an example code I wrote that does that. The specific function is set_params() and then I call that function as soon as I initialize the node in the main() function. https://github.com/tuftsceeo/Tufts_Create3_Examples/blob/main/Projects/Penalty_Shootout/penalty_kick.py

[siqiyan]

Hi, thanks for the reply! I followed your example code and set the safety_override parameter to 'full' in my Python script, but my robot still stuck when it supposed to backup using cmd_vel. However, I tried to use ActionClient and set a DriveDistance to -0.25 according to your code and the robot can backup normally. So I modify my script to make the robot first backup using ActionClient and then start to following obstacles using IR sensor, but the robot still stuck after finish executing the DriveDistance goal. Here is the /rosout output:

stamp:
  sec: 1660431077
  nanosec: 815497952
level: 20
name: motion_control
msg: Setting safety override to "full"
file: ../../apps/create-platform/nodes/src/motion-control-node.cpp
function: set_safety_mode
line: 470
---
stamp:
  sec: 1660431077
  nanosec: 847559056
level: 20
name: motion_control
msg: Received new drive_distance goal
file: ../../apps/create-platform/behaviors/include/create-platform/behaviors/drive-goal-behaviors.hpp
function: handle_drive_goal_goal
line: 98
---
stamp:
  sec: 1660431077
  nanosec: 848353737
level: 20
name: motion_control
msg: drive_distance goal with distance -0.250000, max_speed 0.306000
file: ../../apps/create-platform/behaviors/src/drive-goal-behaviors.cpp
function: initialize_goal
line: 218
---
stamp:
  sec: 1660431077
  nanosec: 849824937
level: 30
name: motion_control
msg: Reached backup limit! Stop Driving robot backward or disable from safety_override parameter!
file: ../../apps/create-platform/nodes/src/motion-control-node.cpp
function: control_robot
line: 294
---
stamp:
  sec: 1660431079
  nanosec: 609223972
level: 20
name: motion_control
msg: 'drive_distance goal succeeded: traveled commanded distance'
file: ../../apps/create-platform/behaviors/src/drive-goal-behaviors.cpp
function: iterate_on_goal
line: 236
---
stamp:
  sec: 1660431081
  nanosec: 149782830
level: 30
name: motion_control
msg: Reached backup limit! Stop Driving robot backward or disable from safety_override parameter!
file: ../../apps/create-platform/nodes/src/motion-control-node.cpp
function: control_robot
line: 294
---

It seems that the robot is able to backup with ActionClient even if it is complaining with reached backup limit, but it won't backup using the cmd_vel topic.

[shamlian]

Would you be willing to provide your python file which trips this problem? We're actively trying to reproduce it, now.

[siqiyan]

Sure, here is my Python script:

import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy
from irobot_create_msgs.msg import IrIntensityVector
from geometry_msgs.msg import Twist

class IR_Follower(Node):
    def __init__(self):
        super().__init__('ir_follower')
        self.ir_intensity_front_center_left = 0
        self.ir_intensity_front_center_right = 0

        self.publisher_ = self.create_publisher(Twist, '/cmd_vel', 10)
        timer_period = 0.1  # seconds
        self.timer = self.create_timer(timer_period, self.cmd_callback)

        qos_profile = QoSProfile(
            reliability=QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT,
            history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST,
            depth=1
        )
        self.subscription = self.create_subscription(
            IrIntensityVector,
            '/ir_intensity',
            self.ir_callback,
            qos_profile=qos_profile
        )

    def ir_callback(self, msg):
        for data in msg.readings:
            if data.header.frame_id == 'ir_intensity_front_center_right':
                self.ir_intensity_front_center_right = data.value
            if data.header.frame_id == 'ir_intensity_front_center_left':
                self.ir_intensity_front_center_left = data.value

    def cmd_callback(self):
        ir_val = (self.ir_intensity_front_center_left + self.ir_intensity_front_center_right) * 0.5
        cmd_twist = Twist()
        if ir_val > 10:
            err = 100 - ir_val
            print('err:', err)
            cmd_twist.linear.x = err * 0.001
            cmd_twist.linear.x = min(0.5, cmd_twist.linear.x)
            cmd_twist.linear.x = max(-0.5, cmd_twist.linear.x)
            self.publisher_.publish(cmd_twist)
            print('cmd_twist published:', cmd_twist)

def main(args=None):
    rclpy.init(args=args)
    ir_follower = IR_Follower()
    rclpy.spin(ir_follower)
    ir_follower.destroy_node()
    rclpy.shutdown()

[shamlian]

Thanks for reporting this, especially with such detail. We are looking into it right now.

[shamlian]

Did some more testing and it looks like when the reflexes are enabled, and one is triggered (like, say, proximity slowdown,) the robot re-enables safety (that is, it temporarily sets the safety-override to none). I think your script can be made to work if you disable reflexes.REFLEX_PROXIMITY_SLOWDOWN (ros2 param set /motion_control reflexes.REFLEX_PROXIMITY_SLOWDOWN false), or just disable all the reflexes altogether (ros2 param set /motion_control reflexes_enabled false)

[siqiyan]

Did some more testing and it looks like when the reflexes are enabled, and one is triggered (like, say, proximity slowdown,) the robot re-enables safety (that is, it temporarily sets the safety-override to none). I think your script can be made to work if you disable reflexes.REFLEX_PROXIMITY_SLOWDOWN (ros2 param set /motion_control reflexes.REFLEX_PROXIMITY_SLOWDOWN false), or just disable all the reflexes altogether (ros2 param set /motion_control reflexes_enabled false)

This is working! I set the reflexes_enabled to false and this time the robot can backup as expected, thank you very much for the help!

[shamlian]

Excellent! On our next release, we will be changing the policy of the robot to not re-enable safety on the proximity slowdown reflex in particular. In the mean time, I'm glad you have a workaround. Sorry it took so long for us to figure it out.

[shamlian]

When the reflexes are enabled, and one is triggered (like, say, proximity slowdown,) the robot re-enables safety (that is, it temporarily sets the safety-override to none). I think your script can be made to work if you disable reflexes.REFLEX_PROXIMITY_SLOWDOWN (ros2 param set /motion_control reflexes.REFLEX_PROXIMITY_SLOWDOWN false), or just disable all the reflexes altogether (ros2 param set /motion_control reflexes_enabled false). It is our plan for a subsequent release to not re-enable safety with the proximity slowdown in particular.