Robotics StackExchange | Archived questions

One node that contains an ActionServer and a subscriber

Hi, I'm quite new to ROS, and trying to work on a project in ROS2. I want to create a node which acts as an action server. In order for the action server to know the right feedback to give to the client, I need to save information that I can get from another node publishing it on a topic. I tried just

lass MissionExecuter(Node):
def __init__(self):
    super().__init__('SantaMissionExecuter') #type: ignore

    self.current_position = Pose()
    self.move_up_action_server = ActionServer(self, MoveUpAction, 'move_up', self.move_up_callback)
    self.setpoint_pos_local_publisher = self.create_publisher(PoseStamped, '/drone1/setpoint_position/local', 0)
    self.local_pos_subscriber = self.create_subscription(Odometry,
                                                         '/drone1/global_position/local',
                                                         self.get_current_position_callback, 10)

def move_up_callback(self, goal_handle):
    self.get_logger().info(f'Accepted Goal of going up {goal_handle.request.distance} meters.')

    initial_height = self.current_position.position.z
    goal_height = initial_height + goal_handle.request.distance

    self.move_up_publish_move_local(goal_handle.request.distance)

    feedback_msg = MoveUpAction.Feedback()

    while (self.current_position.position.z < goal_height):
        feedback_msg.relative_height = self.current_position.position.z - initial_height
        self.get_logger().info(f'Sending Feedback: {feedback_msg.relative_height}')
        goal_handle.publish_feedback(feedback_msg)
        time.sleep(0.5)

    goal_handle.succeed()

    result = MoveUpAction.Result()
    result.final_height_above_takeoff = self.current_position.position.z
    return result

def move_up_publish_move_local(self, distance: float) -> None:
    self.get_logger().info('Publishing move local request')
    msg = PoseStamped()
    msg.pose.position.z = distance
    self.setpoint_pos_local_publisher.publish(msg)

def get_current_position_callback(self, msg: Odometry):
    self.get_logger().info(f'Got position data: {msg.pose.pose.position.z}')
    self.current_position = msg.pose.pose


def main(args=None):
    spin_and_catch_errors_with_multi_thread(MissionExecuter, args)

However, I suspect that the ActionServer disables the topic subscriber callback, even though It runs multi threaded (see appendix) Any help? Thank you

def spin_and_catch_errors_with_multi_thread(node_class, args=None) -> None:
rclpy.init(args=args)
node = node_class()
executor = MultiThreadedExecutorCatchExceptions(num_threads=5)
while rclpy.ok():
    try:
        rclpy.spin_once(node, executor=executor)
    except Exception as e:
        node.get_logger().error(f"Error of type: {type(e)} \n Error content: {traceback.format_exc()}")
node.destroy_node()
rclpy.shutdown()

enter code here

Asked by Erel on 2023-05-16 09:38:59 UTC

Comments

Answers

I think you're on the right track. But I wouldn't say that the action server "disables" the subscription, as much as it "blocks" the subscription. I believe the issue is that you must have the action server and odom subscription in separate Callback Groups. Having everything in the same same (default mutually exclusive) callback group is the same as a single-threaded executor, regardless of executor or number of threads type.

You'll need to create at least 2 callback groups (or one re-entrant cb group) and use one cb group for the action server, and the other for the subscription. Then your multi-threaded executor should behave as you expect.

Asked by ChuiV on 2023-05-16 13:25:25 UTC

Comments