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

Jump to bottom

Create3 Fails to run consecutive rotate goals with RCLPY ROS2 #351

Open
Jacob-Friedberg opened this issue Mar 10, 2023 Discussed in #350 · 4 comments
Open

Create3 Fails to run consecutive rotate goals with RCLPY ROS2 #351

Jacob-Friedberg opened this issue Mar 10, 2023 Discussed in #350 · 4 comments

Comments

@Jacob-Friedberg
Copy link

Discussed in #350

Originally posted by Jacob-Friedberg March 9, 2023
ATTENTION: To run this code you MUST be on XORG/X11 the pynput library DOES NOT WORK on Wayland

System Info
Operating System: Kubuntu 22.04
Kernel Version: 5.15.0-67-generic (64-bit)
Graphics Platform: X11
Python Version: 3.10.6 (64-bit)

ROS Info
ROS_DISTRO=Humble
RMW_IMPLEMENTATION=rmw_cyclonedds_cpp

Create3 Info
Firmware Version: H.0.0
ROS 2 Domain ID: 0
RMW_IMPLEMENTATION:rmw_cyclonedds_cpp
Application Ros2 Parameters file:

create3_05F8/motion_control:
  ros__parameters:
    reflexes_enabled: false

Hello,

I am running into some issues sending repeated rotate goal commands to the create 3 using RCLPY. The code that I am running is pasted below:

import rclpy
from rclpy.node import Node
from rclpy.action.client import ActionClient
from rclpy.qos import qos_profile_sensor_data
from rclpy.callback_groups import MutuallyExclusiveCallbackGroup,ReentrantCallbackGroup
from action_msgs.msg._goal_status import GoalStatus

from irobot_create_msgs.action import DriveDistance, Undock,RotateAngle
from irobot_create_msgs.msg import HazardDetectionVector

from pynput.keyboard import KeyCode
from key_commander import KeyCommander
from threading import Lock
from rclpy.executors import MultiThreadedExecutor
from time import sleep,time

# To help with Multithreading
lock = Lock()

class Slash(Node):
    """
    Class to coordinate actions and subscriptions
    """

    def __init__(self, namespace):
        super().__init__('slasher')

        # 2 Seperate Callback Groups for handling the bumper Subscription and Action Clients
        cb_Subscripion = MutuallyExclusiveCallbackGroup()
        #cb_Action = cb_Subscripion
        cb_Action =MutuallyExclusiveCallbackGroup()

        # Subscription to Hazards, the callback function attached only looks for bumper hits
        self.subscription = self.create_subscription(
            HazardDetectionVector, f'/{namespace}/hazard_detection', self.listener_callback, qos_profile_sensor_data,callback_group=cb_Subscripion)

        # Action clients for movements
        self._undock_ac = ActionClient(self, Undock, f'/{namespace}/undock',callback_group=cb_Action)
        self._drive_ac = ActionClient(self, DriveDistance, f'/{namespace}/drive_distance',callback_group=cb_Action)
        self._rotate_ac = ActionClient(self, RotateAngle, f'/{namespace}/rotate_angle',callback_group=cb_Action)


        # Variables
        self._goal_uuid = None



    def listener_callback(self, msg):
        '''
        This function is called every time self.subscription gets a message
        from the Robot. Here it parses the message from the Robot and if its
        a 'bump' message, cancel the current action. 

        For this to work, make sure you have:
        ros__parameters:
            reflexes_enabled: false
        in your Application Configuration Parameters File!!!
        '''

        # If it wasn't doing anything, there's nothing to cancel
        if self._goal_uuid is None:
            return

        # msg.detections is an array of HazardDetection from HazardDetectionVectors.
        # Other types can be gotten from HazardDetection.msg
        for detection in msg.detections:
            if detection.type == 1:   #If it is a bump
                self.get_logger().warning('HAZARD DETECTED')
                
                with lock: # Make this the only thing happening
                    self.get_logger().warning('CANCELING GOAL')           
                    self._goal_uuid.cancel_goal_async()
                    # Loop until the goal status returns canceled
                    while self._goal_uuid.status is not GoalStatus.STATUS_CANCELED:
                        pass    
                    print('Goal canceled.')


#--------------------Async send goal calls-----------------------------
    def sendRotateGoal(self,goal):
        """
        Sends a rotate goal asynchronously and 'blocks' until the goal is complete
        """
        
        with lock:
            self._rotate_ac.wait_for_server()

            goal_future = self._rotate_ac.send_goal_async(goal)
            while not goal_future.done():
                pass # Wait for Action Server to accept goal
            
            # Hold ID in case we need to cancel it
            self._goal_uuid = goal_future.result() 

        while self._goal_uuid.status == GoalStatus.STATUS_UNKNOWN:
            pass # Wait until a Status has been assigned
        
        #initial timestamp for checking if we are stuck in the loop
        start = time()
        
        # After getting goalID, Loop while the goal is currently running
        while self._goal_uuid.status is not GoalStatus.STATUS_SUCCEEDED:
            elapsed = time()

            #If we are looping for more than 5 seconds we know we got stuck, and should print out the current goal status
            if (elapsed - start) > 5:
                self.get_logger().warning('Stuck in rotate goal. Current Goal Status is:' + str(self._goal_uuid.status))
                start = time()


            if self._goal_uuid.status is GoalStatus.STATUS_CANCELED:
                break # If the goal was canceled, stop looping otherwise loop till finished
            
        
        with lock:
            # Reset the goal ID, nothing should be running
            self._goal_uuid = None 




    def sendDriveGoal(self,goal):
        """
        Sends a drive goal asynchronously and 'blocks' until the goal is complete
        """
        
        with lock:
            self._drive_ac.wait_for_server()

            goal_future = self._drive_ac.send_goal_async(goal)
            while not goal_future.done():
                pass # Wait for Action Server to accept goal
            
            # Hold ID in case we need to cancel it
            self._goal_uuid = goal_future.result() 

        while self._goal_uuid.status == GoalStatus.STATUS_UNKNOWN:
            pass # Wait until a Status has been assigned
        
        #initial timestamp for checking if we are stuck in the loop
        start = time()

        # After getting goalID, Loop while the goal is currently running
        while self._goal_uuid.status is not GoalStatus.STATUS_SUCCEEDED:
            
            elapsed = time()

            #If we are looping for more than 5 seconds we know we got stuck, and should print out the current goal status
            if (elapsed - start) > 5:
                self.get_logger().warning('Stuck in drive goal. Current Goal Status is:' + str(self._goal_uuid.status))
                start = time()
            
            if self._goal_uuid.status is GoalStatus.STATUS_CANCELED:
                break # If the goal was canceled, stop looping otherwise loop till finished
            pass
        
        with lock:
            # Reset the goal ID, nothing should be running
            self._goal_uuid = None 

#----------------------------------------------------------------------


    def drive_away(self):
        """
        Undocks robot and drives out a meter asynchronously
        """
        # Freshly started, undock
        self.get_logger().warning('WAITING FOR SERVER')
    # wait until the robot server is found and ready to receive a new goal
        self._undock_ac.wait_for_server()
        self.get_logger().warning('SERVER AVAILABLE')
        self.get_logger().warning('UNDOCKING')

    # create new Undock goal object to send to server
        undock_goal = Undock.Goal()

        self._undock_ac.send_goal(undock_goal)
        self.get_logger().warning('UNDOCKED')

    # create goal object and specify distance to drive
        drive_goal = DriveDistance.Goal()
        drive_goal.distance = 1.0
    # send goal to async function
        self.get_logger().warning('DRIVING!')
        self.sendDriveGoal(drive_goal)
        
        for i in range(0,3):
            self.get_logger().warning('LOOP Iteration:' + str(i))
            
            rotate_goal = RotateAngle.Goal()
            rotate_goal.angle = 3.14
            self.get_logger().warning('ROTATING Iter:' + str(i))
            self.sendRotateGoal(rotate_goal)

            #If a drive goal is added the problem goes away
            # drive_goal = DriveDistance.Goal()
            # drive_goal.distance = 0.1

            # self.get_logger().warning('DRIVING Iter:' + str(i))
            # self.sendDriveGoal(drive_goal)

        self.get_logger().warning('ALL DONE!')




if __name__ == '__main__':
    rclpy.init()

    namespace = 'create3_05F8'
    s = Slash(namespace)

    # 1 thread for the Subscription, another for the Action Clients
    exec = MultiThreadedExecutor(2)
    exec.add_node(s)

    keycom = KeyCommander([
        (KeyCode(char='r'), s.drive_away),
        ])

    print("r: Start drive_away")
    try:
        exec.spin() # execute slash callbacks until shutdown or destroy is called
    except KeyboardInterrupt:
        print('KeyboardInterrupt, shutting down.')
        print("Shutting down executor")
        exec.shutdown()
        print("Destroying Node")
        s.destroy_node()
        print("Shutting down RCLPY")
        rclpy.try_shutdown()

I am creating a multi threaded executor with 2 threads and adding a node i created called slash which contains a subscriber for the hazard vector and several action clients for sending goals. both the subscriber and action client are created using separate mutually exclusive groups so they should be able to run concurrently with each other(action client doing stuff while the hazard vector subscriber is being serviced) . The subscriber for the hazard vector is used to cancel the current goal if a bump is detected.

I am using a lock to gain mutually exclusive access to the goal_uuid and to prevent the sendRotateGoal and sendDriveGoal functions from executing at different points in their functions if the subscriber for the hazard vector detects a bump and is currently cancelling the goal. Mainly its to prevent sending a goal while cancelling or messing with the goal_uuid until after we are finished cancelling the current goal.

I am using the goal status enum to check the goal_uuid.status of running and canceling goals as defined here:
https://docs.ros2.org/foxy/api/action_msgs/msg/GoalStatus.html

# Indicates status has not been properly set.
int8 STATUS_UNKNOWN = 0

# The goal has been accepted and is awaiting execution.
int8 STATUS_ACCEPTED = 1

# The goal is currently being executed by the action server.
int8 STATUS_EXECUTING = 2

# The client has requested that the goal be canceled and the action server has accepted the cancel request.
int8 STATUS_CANCELING = 3

# The goal was achieved successfully by the action server.
int8 STATUS_SUCCEEDED = 4

# The goal was canceled after an external request from an action client.
int8 STATUS_CANCELED = 5

# The goal was terminated by the action server without an external request.
int8 STATUS_ABORTED = 6

The sequencer for actions being sent to the Create3 is in the drive_away function in the slash Node. This function is called from the KeyCommander object that exists in a different thread than Main or the executor for RCLPY and uses pynput to read keyboard inputs to more easily run functions from the same terminal RCLPY runs in. Both of these src files are attached below:
Rotate_issue_src.zip

When the 'r' key is pressed the function begins as expected. The Create 3 undocks, then moves 1 meter away.

# create new Undock goal object to send to server
        undock_goal = Undock.Goal()

        self._undock_ac.send_goal(undock_goal)
        self.get_logger().warning('UNDOCKED')

    # create goal object and specify distance to drive
        drive_goal = DriveDistance.Goal()
        drive_goal.distance = 1.0
    # send goal to async function
        self.get_logger().warning('DRIVING!')
        self.sendDriveGoal(drive_goal)

After that we enter a loop to create a new RotateAngle goal and setting the angle to be 3.14 and send the rotate goal.

for i in range(0,3):
            self.get_logger().warning('LOOP Iteration:' + str(i))
            
            rotate_goal = RotateAngle.Goal()
            rotate_goal.angle = 3.14
            self.get_logger().warning('ROTATING Iter:' + str(i))
            self.sendRotateGoal(rotate_goal)

This is where the issues start. The first time we send the create3 a rotate goal it will successfully rotate. The second time a rotate goal is sent usually the create3 will just sit there and do nothing(sometimes on a fresh restart application it will do 2 in a row but will fail on the third). I can see from a printout of the goal_uuid.status that occurs every 5 seconds if the goal is taking to long to finish that the status is STATUS_EXECUTING. The robot will remain stuck like this unless I cancel the current goal by tapping the bumper. When i tap the bumper the goal is cancelled as expected and the goal_uuid.status updates properly to be STATUS_CANCELED. The loop will continue, and the Create3 will do 1 rotate iteration just fine but the next one will fail in the exact same way as mentioned prior. For me this issue is repeatable as can be seen in the screenshot below:
image

Full Dump of the create3 logs:
messages.txt

Partial dump of the test run:

Mar 10 03:40:27 iRobot-05F8BBBCD0F04402B8575581F4953FC0 user.notice create-platform: [INFO] [1678419627.443730506] [create3_05F8.motion_control]: Received new undock goal
Mar 10 03:40:27 iRobot-05F8BBBCD0F04402B8575581F4953FC0 user.notice create-platform: [INFO] [1678419627.465663100] [create3_05F8.motion_control]: digital_sensor_msi_listener: m_is_config_active 1
Mar 10 03:40:27 iRobot-05F8BBBCD0F04402B8575581F4953FC0 user.notice create-platform: 26,26 meas_vel:0,0
Mar 10 03:40:27 iRobot-05F8BBBCD0F04402B8575581F4953FC0 user.notice create-platform: [WAR] Packet handler running too long exec_time: 114  id: 64
Mar 10 03:40:27 iRobot-05F8BBBCD0F04402B8575581F4953FC0 user.notice create-platform: [DBG] [SE]: !wheels_stopped - odom:31842,9218
Mar 10 03:40:27 iRobot-05F8BBBCD0F04402B8575581F4953FC0 user.notice create-platform: [DBG] [SE]: !velocity_stopped - cmd_vel:30,-30 meas_vel:2,0
Mar 10 03:40:27 iRobot-05F8BBBCD0F04402B8575581F4953FC0 user.notice create-platform: [WAR] Packet handler running too long exec_time: 4  id: 25
Mar 10 03:40:27 iRobot-05F8BBBCD0F04402B8575581F4953FC0 user.notice create-platform: [WAR] Packet handler running too long exec_time: 3  id: 25
Mar 10 03:40:27 iRobot-05F8BBBCD0F04402B8575581F4953FC0 user.notice create-platform: [WAR] Packet handler running too long exec_time: 3  id: 25
Mar 10 03:40:27 iRobot-05F8BBBCD0F04402B8575581F4953FC0 user.notice create-platform: [WAR] Packet handler running too long exec_time: 4  id: 25
Mar 10 03:40:27 iRobot-05F8BBBCD0F04402B8575581F4953FC0 user.notice create-platform: [DBG] [SE]: !wheels_stopped - odom:32668,8390
Mar 10 03:40:27 iRobot-05F8BBBCD0F04402B8575581F4953FC0 user.notice create-platform: [DBG] [SE]: !wheels_stopped - odom:32667,8390
Mar 10 03:40:27 iRobot-05F8BBBCD0F04402B8575581F4953FC0 user.notice create-platform: [DBG] [SE]: !wheels_stopped - odom:32666,8388
Mar 10 03:40:27 iRobot-05F8BBBCD0F04402B8575581F4953FC0 user.notice create-platform: [DBG] [SE]: !wheels_stopped - odom:32666,8387
Mar 10 03:40:27 iRobot-05F8BBBCD0F04402B8575581F4953FC0 user.notice create-platform: [DBG] [SE]: !wheels_stopped - odom:32665,8386
Mar 10 03:40:27 iRobot-05F8BBBCD0F04402B8575581F4953FC0 user.notice create-platform: [DBG] [SE]: !wheels_stopped - odom:32664,8386
Mar 10 03:40:27 iRobot-05F8BBBCD0F04402B8575581F4953FC0 user.notice create-platform: [WAR] Packet handler running too long exec_time: 3  id: 25
Mar 10 03:40:27 iRobot-05F8BBBCD0F04402B8575581F4953FC0 user.notice create-platform: [WAR] Packet handler running too long exec_time: 3  id: 25
Mar 10 03:40:27 iRobot-05F8BBBCD0F04402B8575581F4953FC0 user.notice create-platform: [DBG] RobotDock constructor
Mar 10 03:40:27 iRobot-05F8BBBCD0F04402B8575581F4953FC0 user.notice create-platform: [DBG] undock_statemachine: sent docking params packet initial pass
Mar 10 03:40:27 iRobot-05F8BBBCD0F04402B8575581F4953FC0 user.notice create-platform: [DBG] robot_undock_statemachine: DOCK_PROTO_SEND_CONFIG -->> DOCK_PROTO_WAIT_CONFIG
Mar 10 03:40:32 iRobot-05F8BBBCD0F04402B8575581F4953FC0 user.notice create-platform: [DBG] robot_dock: got config received[INFO] [1678419632.992892234] [create3_05F8.motion_control]: Undock Goal Succeeded
Mar 10 03:40:33 iRobot-05F8BBBCD0F04402B8575581F4953FC0 user.notice create-platform: [INFO] [1678419633.000412877] [create3_05F8.motion_control]: Received new drive_distance goal
Mar 10 03:40:33 iRobot-05F8BBBCD0F04402B8575581F4953FC0 user.notice create-platform: [INFO] [1678419633.007975416] [create3_05F8.motion_control]: drive_distance goal with distance 1.000000, max_speed 0.300000
Mar 10 03:40:33 iRobot-05F8BBBCD0F04402B8575581F4953FC0 user.notice create-platform: [INFO] [1678419633.050076572] [create3_05F8.motion_control]: digital_sensor_msi_listener: m_is_config_active 0
Mar 10 03:40:36 iRobot-05F8BBBCD0F04402B8575581F4953FC0 kern.warn kernel: [11902.728836] SYNC - AP changed B/G protection to 0
Mar 10 03:40:37 iRobot-05F8BBBCD0F04402B8575581F4953FC0 kern.warn kernel: [11903.342228] SYNC - AP changed B/G protection to 1
Mar 10 03:40:37 iRobot-05F8BBBCD0F04402B8575581F4953FC0 user.notice create-platform: [INFO] [1678419637.439752266] [create3_05F8.motion_control]: drive_distance goal succeeded traveled commanded distance
Mar 10 03:40:37 iRobot-05F8BBBCD0F04402B8575581F4953FC0 user.notice create-platform: [INFO] [1678419637.455943086] [create3_05F8.motion_control]: Received new rotate_angle goal
Mar 10 03:40:37 iRobot-05F8BBBCD0F04402B8575581F4953FC0 user.notice create-platform: [INFO] [1678419637.457970992] [create3_05F8.motion_control]: rotate_angle goal with angle 3.140000, max_speed 1.900000, per wheel 221
Mar 10 03:40:39 iRobot-05F8BBBCD0F04402B8575581F4953FC0 user.notice create-platform: [INFO] [1678419639.468321428] [create3_05F8.mobility_monitor]: Received mobility notification: ROBOT_NOTIFY_GYRO_ROTATION_TEST_DONE (56) with arg 0
Mar 10 03:40:39 iRobot-05F8BBBCD0F04402B8575581F4953FC0 user.notice create-platform: [WARN] [1678419639.488494807] [create3_05F8.motion_control]: rotate_angle goal succeeded: rotated commanded angle
Mar 10 03:40:39 iRobot-05F8BBBCD0F04402B8575581F4953FC0 user.notice create-platform: [INFO] [1678419639.506305461] [create3_05F8.motion_control]: Received new rotate_angle goal
Mar 10 03:40:39 iRobot-05F8BBBCD0F04402B8575581F4953FC0 user.notice create-platform: [INFO] [1678419639.508453953] [create3_05F8.motion_control]: rotate_angle goal with angle 3.140000, max_speed 1.900000, per wheel 221
Mar 10 03:40:48 iRobot-05F8BBBCD0F04402B8575581F4953FC0 user.notice create-platform: [INFO] [1678419648.607102533] [create3_05F8.root_ble_interface]: New bump state 192
Mar 10 03:40:48 iRobot-05F8BBBCD0F04402B8575581F4953FC0 user.notice create-platform: [INFO] [1678419648.767116255] [create3_05F8.root_ble_interface]: New bump state 0
Mar 10 03:40:48 iRobot-05F8BBBCD0F04402B8575581F4953FC0 user.notice create-platform: [INFO] [1678419648.984572126] [create3_05F8.motion_control]: Received request to cancel rotate_angle goal
Mar 10 03:40:49 iRobot-05F8BBBCD0F04402B8575581F4953FC0 user.notice create-platform: [INFO] [1678419648.990297771] [create3_05F8.motion_control]: rotate_angle canceled by client
Mar 10 03:40:49 iRobot-05F8BBBCD0F04402B8575581F4953FC0 user.notice create-platform: [INFO] [1678419649.540486441] [create3_05F8.motion_control]: Received new rotate_angle goal
Mar 10 03:40:49 iRobot-05F8BBBCD0F04402B8575581F4953FC0 user.notice create-platform: [INFO] [1678419649.542648290] [create3_05F8.motion_control]: rotate_angle goal with angle 3.140000, max_speed 1.900000, per wheel 221
Mar 10 03:40:51 iRobot-05F8BBBCD0F04402B8575581F4953FC0 user.notice create-platform: [INFO] [1678419651.516878651] [create3_05F8.mobility_monitor]: Received mobility notification: ROBOT_NOTIFY_GYRO_ROTATION_TEST_DONE (56) with arg 0
Mar 10 03:40:51 iRobot-05F8BBBCD0F04402B8575581F4953FC0 user.notice create-platform: [WARN] [1678419651.538601288] [create3_05F8.motion_control]: rotate_angle goal succeeded: rotated commanded angle
Mar 10 03:40:51 iRobot-05F8BBBCD0F04402B8575581F4953FC0 user.notice create-platform: [INFO] [1678419651.546163256] [create3_05F8.motion_control]: Received new rotate_angle goal
Mar 10 03:40:51 iRobot-05F8BBBCD0F04402B8575581F4953FC0 user.notice create-platform: [INFO] [1678419651.555103733] [create3_05F8.motion_control]: rotate_angle goal with angle 3.140000, max_speed 1.900000, per wheel 221
Mar 10 03:40:57 iRobot-05F8BBBCD0F04402B8575581F4953FC0 user.notice create-platform: [INFO] [1678419657.317706045] [create3_05F8.system_health]: CPU usage: max 72 [%] mean 57 [%] RAM usage: 31/59 [MB]
Mar 10 03:40:59 iRobot-05F8BBBCD0F04402B8575581F4953FC0 user.notice create-platform: [INFO] [1678419659.712580060] [create3_05F8.root_ble_interface]: New bump state 64
Mar 10 03:40:59 iRobot-05F8BBBCD0F04402B8575581F4953FC0 user.notice create-platform: [INFO] [1678419659.725953253] [create3_05F8.root_ble_interface]: New bump state 192
Mar 10 03:40:59 iRobot-05F8BBBCD0F04402B8575581F4953FC0 user.notice create-platform: [INFO] [1678419659.885883462] [create3_05F8.root_ble_interface]: New bump state 0
Mar 10 03:41:00 iRobot-05F8BBBCD0F04402B8575581F4953FC0 user.notice create-platform: [INFO] [1678419660.114653181] [create3_05F8.motion_control]: Received request to cancel rotate_angle goal
Mar 10 03:41:00 iRobot-05F8BBBCD0F04402B8575581F4953FC0 user.notice create-platform: [INFO] [1678419660.138670860] [create3_05F8.motion_control]: rotate_angle canceled by client
Mar 10 03:41:00 iRobot-05F8BBBCD0F04402B8575581F4953FC0 user.notice create-platform: [INFO] [1678419660.297981423] [create3_05F8.motion_control]: Received new rotate_angle goal
Mar 10 03:41:00 iRobot-05F8BBBCD0F04402B8575581F4953FC0 user.notice create-platform: [INFO] [1678419660.305632574] [create3_05F8.motion_control]: rotate_angle goal with angle 3.140000, max_speed 1.900000, per wheel 221
Mar 10 03:41:02 iRobot-05F8BBBCD0F04402B8575581F4953FC0 user.notice create-platform: [INFO] [1678419662.254640775] [create3_05F8.mobility_monitor]: Received mobility notification: ROBOT_NOTIFY_GYRO_ROTATION_TEST_DONE (56) with arg 0
Mar 10 03:41:02 iRobot-05F8BBBCD0F04402B8575581F4953FC0 user.notice create-platform: [WARN] [1678419662.289145606] [create3_05F8.motion_control]: rotate_angle goal succeeded: rotated commanded angle
Mar 10 03:41:02 iRobot-05F8BBBCD0F04402B8575581F4953FC0 user.notice create-platform: [INFO] [1678419662.297815214] [create3_05F8.motion_control]: Received new rotate_angle goal
Mar 10 03:41:02 iRobot-05F8BBBCD0F04402B8575581F4953FC0 user.notice create-platform: [INFO] [1678419662.306011426] [create3_05F8.motion_control]: rotate_angle goal with angle 3.140000, max_speed 1.900000, per wheel 221
Mar 10 03:41:07 iRobot-05F8BBBCD0F04402B8575581F4953FC0 kern.warn kernel: [11933.852074] SYNC - AP changed B/G protection to 0
Mar 10 03:41:10 iRobot-05F8BBBCD0F04402B8575581F4953FC0 kern.warn kernel: [11936.197388] wcid 1 rx bmc[0] 0 exp > 0
Mar 10 03:41:18 iRobot-05F8BBBCD0F04402B8575581F4953FC0 kern.warn kernel: [11944.604071] SYNC - AP changed B/G protection to 1
Mar 10 03:41:23 iRobot-05F8BBBCD0F04402B8575581F4953FC0 kern.warn kernel: [11949.932647] SYNC - AP changed B/G protection to 0
Mar 10 03:41:25 iRobot-05F8BBBCD0F04402B8575581F4953FC0 kern.warn kernel: [11951.160495] SYNC - AP changed B/G protection to 1
Mar 10 03:41:30 iRobot-05F8BBBCD0F04402B8575581F4953FC0 kern.warn kernel: [11956.994451] SYNC - AP changed B/G protection to 0
Mar 10 03:41:49 iRobot-05F8BBBCD0F04402B8575581F4953FC0 kern.warn kernel: [11975.426541] SYNC - AP changed B/G protection to 1
Mar 10 03:41:56 iRobot-05F8BBBCD0F04402B8575581F4953FC0 kern.warn kernel: [11982.082449] SYNC - AP changed B/G protection to 0
Mar 10 03:41:57 iRobot-05F8BBBCD0F04402B8575581F4953FC0 kern.warn kernel: [11983.212660] SYNC - AP changed B/G protection to 1
Mar 10 03:41:57 iRobot-05F8BBBCD0F04402B8575581F4953FC0 user.notice create-platform: [INFO] [1678419717.319764056] [create3_05F8.system_health]: CPU usage: max 67 [%] mean 53 [%] RAM usage: 30/59 [MB]

Looking at the logs for the create3 for these actions we can see that the rotate goals are received

[create3_05F8.motion_control]: Received new rotate_angle goal

but get stuck on this log message:

[create3_05F8.motion_control]: rotate_angle goal with angle 3.140000, max_speed 1.900000, per wheel 221

The Rotate goals that succeed have these following messages:

[create3_05F8.motion_control]: Received new rotate_angle goal
[create3_05F8.motion_control]: rotate_angle goal with angle 3.140000, max_speed 1.900000, per wheel 221
[create3_05F8.mobility_monitor]: Received mobility notification: ROBOT_NOTIFY_GYRO_ROTATION_TEST_DONE (56) with arg 0
[create3_05F8.motion_control]: rotate_angle goal succeeded: rotated commanded angle

Interestingly, in my testing i found that this issue occurs so far with just multiple rotate goals in a row. If you have a loop containing a bunch of drive goals they work as expected and all execute for every iteration. I also found that if you rotate once, then perform a drive goal the create3 will drive and rotate for all iterations of the loop as normal.

All of this leads me to believe there may be an issue with the RotateAngle action server on the create3. Somewhere I think things are getting locked up on the create3 for consecutive rotate goals. The goal_uuid_status for the consecutive rotate goal seems to be set properly as executing, but the mobility monitor message never appears in the log and the create3 just sits idle until the goal is cancelled.

Let me know if you need more info from me to try and fix this issue.

I have not tested this issue yet on the new H.1.0 firmware, but will and report back the results.
@Jacob-Friedberg
Copy link
Author

Jacob-Friedberg commented Mar 10, 2023 •
edited
Loading

I have updated my Create3 with the newest H.1.0 firmware and the issue remains. Same behavior as before.

I also tested it with a synchronous send goal call and it stops working just like before with the same log messages.

@alsora alsora closed this as completed Mar 11, 2023
@alsora alsora reopened this Mar 11, 2023
@alsora
Copy link
Collaborator

alsora commented Mar 11, 2023

Hi @Jacob-Friedberg,
We are aware of the problem and we are going to push a fix soon.

As a temporary solution, we recommend you to change RMW from Fast-rtps to cyclonedds.
This should be done both on the robot (via the webserver) as well as on the laptop.

@Jacob-Friedberg
Copy link
Author

Jacob-Friedberg commented Mar 11, 2023 •
edited
Loading

Great! I'm glad it's being worked on. However, i would like to mention that the RMW that I'm using on both the create3 and my laptop is cyclonedds already. Did you mean i should swap to fast-rtps?

If not then this issue exists on cyclonedds as well

@alsora
Copy link
Collaborator

alsora commented Mar 11, 2023

Interesting.
We found a bug in fast-rtps that results in the same exact symptoms when sending action or service requests in a loop.
This can happen with all ROS 2 actions or services and is usually triggers after ~10 iterations of the requests loop.
This bug comes from the rmw_fastrtps code and we couldn't reproduce the problem with cyclonedds.

There may be an additional bug specific to the rotate goal action.
We will look into it and keep you posted!
Thank you for reporting this.

@shamlian shamlian modified the milestone: H.1.1 Mar 21, 2023
@shamlian shamlian mentioned this issue Jan 9, 2024
Merged
shamlian added a commit that referenced this issue Jan 19, 2024
@shamlian
Merge pull request #505 from iRobotEducation/shamlian/release_G.5.4_G…
a4091bb 
….2.4

Release G.5.4 / H.2.4

# Changelog (from G.5.3/H.2.3)
## Core Robot
### Webserver
* Add beta feature to disconnect from wlan0 and forget the SSID (#110)
### Power Management
* Robot will now change its light ring to "spinning red" when the battery level dips below 3%, and will explicitly call the /robot_power service when it falls below 2%.
## ROS 2
### Actions
* The /rotate_angle action no longer accepts overriding goals; an ongoing goal must be completed (successfully or unsuccessfully) before a new goal will be accepted. (#351)
* Improve reliability of robot docking and undocking.
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Assignees
No one assigned
Labels
None yet
Projects
None yet
Milestone
No milestone
Development

No branches or pull requests
3 participants
@shamlian @Jacob-Friedberg @alsora