search

iRobotEducation / create3_docs

Documentation for the iRobot® Create®3 Educational Robot
BSD 3-Clause "New" or "Revised" License
49 stars 16 forks source link

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

Open Jacob-Friedberg opened 1 year ago

Jacob-Friedberg commented 1 year ago

Discussed in https://github.com/iRobotEducation/create3_docs/discussions/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](url) ``` # 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](https://github.com/iRobotEducation/create3_docs/files/10938188/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](https://user-images.githubusercontent.com/7202146/224217765-1c62cb4c-8191-4c0d-916c-65298f214fc7.png) Full Dump of the create3 logs: [messages.txt](https://github.com/iRobotEducation/create3_docs/files/10938363/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 commented 1 year ago

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 commented 1 year ago

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 commented 1 year ago

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 commented 1 year ago

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.