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
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