Getting Result Pose of Action

[tobias-kausch]

How are you connecting to your Create 3?

USB Ethernet (ROS 2)

Computer(s) Model(s) and Operating System(s)

No response

Which version of ROS 2 is installed on your computer?

Humble

Which firmware version is installed on your robot?

G4.4

Which RMW is your robot running?

CycloneDDS

Does your robot have an assigned namespace? If so, please share.

No response

Is the robot connected to a network? If so, what is the network type?

No response

Are there multiple Create 3 robots connected to your network?

No

Is multicast enabled?

No

What is the Adapter Board's USB/BLE Toggle currently switched to?

USB

Describe your question.

Hello,

I am using python and rclpy to make the robot turn through the RotateAngle action client. I want to know once the robot has finished moving so that I can send it the next movement command. Using the .done() on the goal_handle does not work because it does not say when the robot is done moving but when it is done executing, which are not the same it seems.

https://github.com/iRobotEducation/irobot_create_msgs/blob/rolling/action/RotateAngle.action
Reading this ^ I see that somewhere there should be a result with the pose of the robot. My hope is that this pose is published once the robot has finished moving.
My issue is that I do not know how to get that result of the action. .result() does not seem to be this result, or I just don't know how to get it from that.

Anyone have any suggestions?

[alsora]

Hi @tobias-kausch,

Using the .done() on the goal_handle does not work because it does not say when the robot is done moving but when it is done executing, which are not the same it seems.

The robot will not move anymore after the action is concluded. From your message it's not clear how you are waiting for that to be completed, but I suspect that you are not waiting for the result correctly.

I see that somewhere there should be a result with the pose of the robot

Yes, the action result will contain the pose of the robot

My issue is that I do not know how to get that result of the action

See this example on how to use ROS 2 actions in python: https://github.com/ros2/examples/blob/rolling/rclpy/actions/minimal_action_client/examples_rclpy_minimal_action_client/client_asyncio.py#L54-L58

This example uses the following action https://github.com/ros2/example_interfaces/blob/rolling/action/Fibonacci.action
and you can see how it's accessing the sequence field contained in it https://github.com/ros2/examples/blob/rolling/rclpy/actions/minimal_action_client/examples_rclpy_minimal_action_client/client_asyncio.py#L58

[tobias-kausch]

Hey!
Thank you for your response.
When I try to use goal_handle.get_result_async(), I get the error that the Future object does not have the attribute .get_result_async() and asks if I meant _get_result_async. When I try that attribute it tells me I need a positional argument.

I get the goal handle variable by: goal_handle = my_action_client.send_goal_async(goal_msg)

Visual Studio also raises an error when I try to use the await statement in my code like in the example:
"await only allowed within async function"

What am I doing wrong?

Also, in the action I referenced, the result "pose" seems to be different from other attributes in that it says that it is through geometry_msgs:

#result
geometry_msgs/PoseStamped pose

Is the result perhaps being published to some topic using geometry_msgs/PoseStamped?

I'm obviously new to this and I appreciate your help!

[alsora]

Also, in the action I referenced, the result "pose" seems to be different from other attributes in that it says that it is through geometry_msgs:
#result
geometry_msgs/PoseStamped pose
Is the result perhaps being published to some topic using geometry_msgs/PoseStamped?

No, geometry_msgs/PoseStamped is just the type of the pose object.

What am I doing wrong?

Unfortunately I don't have enough context to help you.
I would just say that the ROS 2 action used by the create 3 is functionally identical to those that you can see in the ROS 2 examples, just with a different name (and different internal fields).
So, I would recommend to follow the official documentation step by step, trying to adapt the example to the Create 3 data types https://docs.ros.org/en/humble/Tutorials/Intermediate/Writing-an-Action-Server-Client/Py.html

IMPORTANT
I just noticed that you are using Humble on your laptop and G4.4 on your robot.
Those two versions are not compatible.
Even if you get the python script correctly, your current setup will lead to instabilities and potentially hard to debug errors.
It's required to use the same ROS 2 distribution (Humble vs Galactic) and RMW implementation (CycloneDDS vs Fast-DDS) on both machines (laptop and robot).
Note that different ROS 2 distributions also have a different default RMW (CycloneDDS is the default in Galactic and Fast-DDS is the default in Humble).

I would recommend to:

1. fix the inconsistency between versions
2. start following the examples I linked and, if you get stuck, post the full code that you are using and the error log

[brianabouchard]

@tobias-kausch in case it is also helpful to see an example for a Create 3 specific action, I have a python script that is a simple action client for the Create 3 dock action: https://github.com/brianabouchard/me35sp23_students/blob/main/Create3/Dock.py

[tobias-kausch]

Hey, thanks for your help!

I was able to use the documentation you shared to properly wait for an action to complete with DriveDistance and RotateAngle.
From this:
https://docs.ros.org/en/humble/Tutorials/Intermediate/Writing-an-Action-Server-Client/Py.html

However, I ran into a problem with the NavigateToPosition action, where I would get the pose result of the action before the robot was actually done moving. I pasted my code bellow. I am using the send_pose_goal() function to send a goal through the position_ac action client. Although, I am not actually using that function is this code. I would greatly appreciate your input on how to properly handle the action.

import rclpy
from rclpy.node import Node

from rclpy.parameter import Parameter

from std_msgs.msg import Float64MultiArray
from nav_msgs.msg import Odometry
from geometry_msgs.msg import PoseWithCovariance, PoseStamped

from rclpy.action import ActionClient
from irobot_create_msgs.action import DriveDistance, RotateAngle, NavigateToPosition
from irobot_create_msgs.msg import WheelVels

import math
from collections import deque
import itertools

qos_profile = rclpy.qos.QoSProfile(
        reliability=rclpy.qos.QoSReliabilityPolicy.BEST_EFFORT,
        history=rclpy.qos.QoSHistoryPolicy.KEEP_LAST,
        depth=10
    )

class IsMoving():
    driving = False
    rotating = False
    either = driving or rotating
    both = driving and rotating
    moving_que = deque(maxlen=21)
    
canceling = False

class ControllerNode(Node):
    def __init__(self):
        super().__init__("controller")
    
        #self.detection_subscriber = self.create_subscription(
        #         Float64MultiArray, 'person_XYZ', self.detection_callback, 10)
        # print('[Subsciptions Inilialized]')

        self.detection_subscriber = self.create_subscription(
                 Odometry, 'odom', self.odom_callback, qos_profile)
        print('[Subsciptions Inilialized]')

        self.drive_ac = ActionClient(self, DriveDistance, '/drive_distance')
        self.position_ac = ActionClient(self, NavigateToPosition, '/navigate_to_position')
        self.rotate_ac = ActionClient(self, RotateAngle, '/rotate_angle')
        print('[Action Clients Initialized]')
        
        self.declare_parameter('corner_num', rclpy.Parameter.Type.INTEGER)
        param_int = Parameter('corner_num', Parameter.Type.INTEGER, 0)
        self.set_parameters([param_int])
        print('[Parameters Initialized]')
        
        timer_period = 0.5  # seconds
        self.timer = self.create_timer(timer_period, self.corner_drive)
        self.i = 0
        
        self.next_step = 0
        self.cur_corner = 0
        self.cur_angle = 0.0

        
    IsMoving.moving_que
    
    def set_corner(self, num):
        assert num in [0,1,2,3]
        param_int = Parameter('corner_num', Parameter.Type.INTEGER, num)
        self.set_parameters([param_int])
    
    def send_drive_goal(self, goal_msg):
        self.drive_ac.wait_for_server()
        self.send_goal_future = self.drive_ac.send_goal_async(goal_msg)
        self.send_goal_future.add_done_callback(self.goal_response_callback)
        
    def send_pose_goal(self, goal_msg):
        self.position_ac.wait_for_server()
        self.send_goal_future = self.position_ac.send_goal_async(goal_msg)
        self.send_goal_future.add_done_callback(self.goal_response_callback)
        
    def send_rotate_goal(self, goal_msg):
        self.rotate_ac.wait_for_server()
        self.send_goal_future = self.rotate_ac.send_goal_async(goal_msg)
        self.send_goal_future.add_done_callback(self.goal_response_callback)
        
    def goal_done_callback(self, future):
        if future.done:
            IsMoving.driving = False
            IsMoving.rotating = False
            print('Action Done')
        
    def get_result_callback(self, future):
        self.result = future.result().result
        print(self.result)
        IsMoving.driving = False
        IsMoving.rotating = False
        print('Movement Completed')

    def goal_response_callback(self, future):
        goal_handle = future.result()
        if not goal_handle.accepted:
            print('Goal Rejected')
            return
        
        self.goal_handle = goal_handle
        self.get_result_future = goal_handle.get_result_async()
        self.get_result_future.add_done_callback(self.get_result_callback)
        print('Goal Accepted')
        
    def cancel_movement(self):
        canceling = True
        cancel_future = self.goal_handle.cancel_goal_async()
        cancel_future.add_done_callback(self.cancel_done)
        
    def cancel_done(self, future):
        print('Canceling Goal...')
        cancel_response = future.done
        if cancel_response == True:
            IsMoving.driving = False
            IsMoving.rotating = False
            canceling = False
           
           
    def box_drive(self):        
        drive_goal = DriveDistance.Goal()
        drive_goal.distance = 0.5
        rotate_goal = RotateAngle.Goal()
        rotate_goal.angle = -1.570796

        if self.next_step == 0 and not (IsMoving.driving or IsMoving.rotating):
            self.send_drive_goal(drive_goal)
            IsMoving.driving = True
            self.next_step = (self.next_step + 1) % 2
        if self.next_step == 1 and not (IsMoving.driving or IsMoving.rotating):
            self.send_rotate_goal(rotate_goal)
            IsMoving.rotating = True
            self.next_step = (self.next_step + 1) % 2
     
            
    def corner_drive(self):
        room = 0.5
        
        drive_goal = DriveDistance.Goal()
        drive_goal.distance = room
        
        rotate_goal = RotateAngle.Goal()

        corner = self.get_parameter('corner_num').value
        print(corner, self.cur_corner, self.cur_angle)
        
        if not (IsMoving.driving or IsMoving.rotating):
            if corner != self.cur_corner:
                if self.cur_corner == 0:
                    if corner in [1,2]:
                        angle_goal = 0.0
                        if angle_goal !=  self.cur_angle:
                            rotate_goal.angle = float(math.radians(angle_goal) - math.radians(self.cur_angle))
                            self.send_rotate_goal(rotate_goal)
                            IsMoving.rotating = True
                            self.cur_angle = angle_goal
                        else:
                            self.send_drive_goal(drive_goal)
                            IsMoving.driving = True
                            self.cur_corner = 1
                    if corner == 3:
                        angle_goal = -90.0
                        if angle_goal !=  self.cur_angle:
                            rotate_goal.angle = float(math.radians(angle_goal) - math.radians(self.cur_angle))
                            self.send_rotate_goal(rotate_goal)
                            IsMoving.rotating = True
                            self.cur_angle = angle_goal
                        else:
                            self.send_drive_goal(drive_goal)
                            IsMoving.driving = True
                            self.cur_corner = 3
                            
                elif self.cur_corner == 1:
                    if corner in [2,3]:
                        angle_goal = -90.0
                        if angle_goal !=  self.cur_angle:
                            rotate_goal.angle = float(math.radians(angle_goal) - math.radians(self.cur_angle))
                            self.send_rotate_goal(rotate_goal)
                            IsMoving.rotating = True
                            self.cur_angle = angle_goal
                        else:
                            self.send_drive_goal(drive_goal)
                            IsMoving.driving = True
                            self.cur_corner = 2
                            
                elif self.cur_corner == 2:
                    if corner == 1:
                        angle_goal = 90.0
                        if angle_goal !=  self.cur_angle:
                            rotate_goal.angle = float(math.radians(angle_goal) - math.radians(self.cur_angle))
                            self.send_rotate_goal(rotate_goal)
                            IsMoving.rotating = True
                            self.cur_angle = angle_goal
                        else:
                            self.send_drive_goal(drive_goal)
                            IsMoving.driving = True
                            self.cur_corner = 1
                    if corner == 3:
                        angle_goal = -180.0
                        if angle_goal !=  self.cur_angle:
                            rotate_goal.angle = float(math.radians(angle_goal) - math.radians(self.cur_angle))
                            print('angle goal:', rotate_goal.angle)
                            self.send_rotate_goal(rotate_goal)
                            IsMoving.rotating = True
                            self.cur_angle = angle_goal
                        else:
                            self.send_drive_goal(drive_goal)
                            IsMoving.driving = True
                            self.cur_corner = 3
                            
                elif self.cur_corner == 3:
                    if corner in [1,2]:
                        angle_goal = 0.0
                        if angle_goal !=  self.cur_angle:
                            rotate_goal.angle = float(math.radians(angle_goal) - math.radians(self.cur_angle))
                            self.send_rotate_goal(rotate_goal)
                            IsMoving.rotating = True
                            self.cur_angle = angle_goal
                        else:
                            self.send_drive_goal(drive_goal)
                            IsMoving.driving = True
                            self.cur_corner = 2
                    
                    
    #def timer_callback(self):
    #    self.box_drive()

    def odom_callback(self, msg: Odometry):
        pass
        i = 0
        pose = msg.pose.pose.position
        self.pose = [pose.x, pose.y, pose.z]
        if i == 0:
            self.origin = self.pose
            i += 1
        
        
def main(args=None):
    rclpy.init(args=args)
    node = ControllerNode()
    
    rclpy.spin(node)
    rclpy.shutdown()