ROS2 Basics in 5 Days for Python

Hello,

I am facing problem in action server of real robot project for ROS2. The problem is in returning the result value of callback function of action server. Whenever I append “result.list_of_odoms.append(self.odom_readings)” then it gives an error of “geometry_msgs__msg__point__convert_from_py: Assertion `PyFloat_Check(field)’ failed.” Although displays and stores the values of list of odoms perfectly. This error is eliminated by “return (result,)”. But with this “get_result_callback” function is not called upon completion on the client side.

When I write “return result” in action server callback function without appending the lists then it goes into “get_result_callback” function upon completion on the client side.

Can anyone please help. Thank you.

class RecordOdometry (Node):
    def __init__ (self):
        super().__init__('action_node')
        self.sub_odom=self.create_subscription(Odometry, '/odom',self.sub_odom_callback,QoSProfile(depth=10,reliability=ReliabilityPolicy.BEST_EFFORT))
        self.action_server=ActionServer(self,OdomRecord,'record_odom',self.callback_action_server)
        self.odom_readings=Point()
        self.odom_readings._x=[]
        self.odom_readings._y=[]
        self.odom_readings._z=[]
        self.distance=0.0
        self.pos_st_x=0.0
        self.pos_st_y=0.0
        
    
    def sub_odom_callback (self,msg):
        self.position_x=msg.pose.pose.position.x
        self.position_y=msg.pose.pose.position.y
        self.orientation_theta=msg.pose.pose.orientation.z
        

    def callback_action_server (self,goal_handle):
        self.get_logger().info('Executing goal... Please Hold On...!!!!! Wait for a while!!!')
        feedback_msg=OdomRecord.Feedback()
        self.distance=0.0
        i=0
        self.pos_init_x=self.position_x
        self.pos_init_y=self.position_y
        
        result=OdomRecord.Result()
        #result.list_of_odoms=[]
        while (self.distance<0.7):
            self.odom_readings.x.append(float(self.position_x))
            self.odom_readings.y.append(float(self.position_y))
            self.odom_readings.z.append(float(self.orientation_theta))
            self.distance=math.sqrt((self.position_x-self.pos_init_x)**2 + (self.position_y-self.pos_init_y)**2) + self.distance    
            if i==0:
                self.pos_st_x=self.position_x
                self.pos_st_y=self.position_y
            #self.distance=+self.distance
            self.pos_init_x=self.position_x
            self.pos_init_y=self.position_y
            i=+1
            feedback_msg.current_total=self.distance
            self.get_logger().info('{0}'.format(feedback_msg.current_total))
            goal_handle.publish_feedback(feedback_msg)
            
            if self.distance>0.5 and self.position_x<self.pos_st_x+0.2 and self.position_y<self.pos_st_y+0.2 and self.position_x>self.pos_st_x-0.2 and self.position_y>self.pos_st_y-0.2:        
                break
            time.sleep(1)
        goal_handle.succeed()
        result.list_of_odoms.append(self.odom_readings)
        self.get_logger().info('One Lap Completed...')
        self.get_logger().info('{0}'.format(result.list_of_odoms))
        return result

Hi Talha, what do you mean by returning the result, the

“get_result_callback” function is not called upon completion on the client side.

Can you see the action succeed when the client calls it?

Dear Roalgoal, thank you for your reply. Returning the result means “to return the result in ‘callback_action_server’ function” in action server node. Let me elaborate it case wise:

  • Case 1: Whenever I “return result” in callback_action_server (in server node) and do NOT append the stored values into result.list_of_odoms then only after completion the function get_result_callback gets called in client node
  • Case 2: Whenever I “return result” in callback_action_server (in server node) and append the stored values into result.list_of_odoms then after completion the function get_result_callback does NOT gets called in client node (& in this case “geometry_msgs__msg__point__convert_from_py: Assertion PyFloat_Check(field) failed.” is displayed although values are stored in result.list_of_odoms perfectly.
  • Case 3: Wheneve I “return (result,)” in this format in callback_action_server (in server node) and append the stored values into result.list_of_odoms then after completion the function get_result_callback does NOT gets called in client node (& in this case no error is generated)

Please feel free to clear any ambiguities in question. Thank you.

Hi Talha,

You can try creating a timer function where you can append the list of odometry points every second that depends on the action server status (the callback result) something like this:

def __init__(self):
        # timer
        self.timer = self.create_timer(timer_period_sec=1.0, callback=self.callback_timer, callback_group=self.parameter["cbg"])
    def callback_timer(self):
        if self.action_server_status:
            self.record_odometry.list_of_odoms.append(self.point)
            #self.get_logger().info(f"Timer: {self.action_server_seconds}! self.record_odometry: {self.record_odometry.list_of_odoms}")
            self.calculate_distance()
            self.action_server_seconds += 1.0


where cbg is ReentrantCallbackGroup(), so a Multithreader executor is needed.

Then, the server callback would be something like this:

    def callback_record_odometry(self, goal_handle):
        self.get_logger().info("Executing goal...")
        self.action_server_status = True
        current_time = self.action_server_seconds
        
        self.get_logger().info(f"Start recording odometry!")
        while self.action_server_status:
            if current_time == self.action_server_seconds:
                pass
            else:
                #self.get_logger().info(f"time: {current_time}, {self.distance}")
                goal_handle.publish_feedback(self.distance)
                current_time = self.action_server_seconds
                
        goal_handle.succeed()
        self.action_server_status = False
        return self.record_odometry

Dear Roalgoal,
Sorry to disturb again. But I tried this time_callback method and everything works fine (publishing feedback topic after every minute and appends record_odom) but return self.record_odometry of callback_action_server gives the following error:

and due to this get_result_callback does NOT gets called in client node…

I would be grateful if you could please help me out. Thank you.
[/quote]

I suspect this is an issue with the type of message you are storing in the variable. Have you defined self.record_odometry = [action].Result(), where [action] is the custom interface you created?

For example, you can create a message Point.msg (NOT action) with the fields

float64 x
float64 y
float64 theta

and then, in the action, you can use that as the feedback, and just return a total distance:

---
custom_interface/Point[] list_of_odoms
---
float64 current_total

Start with returning something you know will not give you an error, and maybe that way you can debug your issue. It is really hard to see what is wrong without seeing your whole setup working.

Dear roalgoal,

Thank you for your reply. It worked when I replaced:

goal_handle.succeed()

with

goal_handle.set_succeeded()

in action server… Now it is going in the callback funtion get_result_callback in action client node… Thank you…
[/quote]