Rosject - ros in 5 days - Custom Action Message

edvart · October 2, 2021, 1:09pm

Hello. I’m doing the ros in 5 days rosject - and am at the Actions part.

Im trying to test the list_of_odoms, but my code isnt running. it returns an error when i set the result as succeeded. i tried to subscribe to the odometry and append it to the result/list of odoms.

redord_odom.py

#! /usr/bin/env python

import rospy
from turtlebot3_move.msg import OdomRecordAction, OdomRecordFeedback, OdomRecordResult
from turtlebot3_move.msg import OdomRecordAction, OdomRecordActionFeedback, OdomRecordActionResult
import actionlib
from nav_msgs.msg import Odometry
import time

class Robot(object):

    _feedback = OdomRecordFeedback()

    def __init__(self):
        self._as = actionlib.SimpleActionServer('/record_odom', OdomRecordAction, self.execute_callback, False)
        self._as.start()
        self.ctrl_c = False
        self.rospy = rospy
        self._result = OdomRecordResult()

    def sub_cb(self, msg):
        return msg.pose.pose

    def execute_callback(self, goal):
        
        success = True
        print('action server called')
        
        self.sub = rospy.Subscriber('/odom', Odometry, self.sub_cb, queue_size = 1)

        for i in range(0, 10):

            self._result.list_of_odoms.append(self.sub_cb)
            time.sleep(1)

        if success:
            rospy.loginfo('succeeded')
            self._as.set_succeeded(self._result)
        

if __name__ == '__main__':
    rospy.init_node('record_odom')
    Robot() 
    rospy.spin()

shell:

^Cuser:~$ rosrun turtlebot3_move record_odom.py
action server called
[INFO] [1633178800.041950, 1743.076000]: succeeded
[ERROR] [1633178800.241541, 1743.181000]: Exception in your execute callback: 'function' object has no attribute 'x'
Traceback (most recent call last):
  File "/home/simulations/public_sim_ws/src/all/actionlib/actionlib/src/actionlib/simple_action_server.py", line 289, in executeLoop
    self.execute_callback(goal)
  File "/home/user/catkin_ws/src/turtlebot3_move/scripts/record_odom.py", line 37, in execute_callback
    self._as.set_succeeded(self._result)
  File "/home/simulations/public_sim_ws/src/all/actionlib/actionlib/src/actionlib/simple_action_server.py", line 162, in set_succeeded
    self.current_goal.set_succeeded(result, text)
  File "/home/simulations/public_sim_ws/src/all/actionlib/actionlib/src/actionlib/server_goal_handle.py", line 195, in set_succeeded
    self.action_server.publish_result(self.status_tracker.status, result)
  File "/home/simulations/public_sim_ws/src/all/actionlib/actionlib/src/actionlib/action_server.py", line 182, in publish_result
    self.result_pub.publish(ar)
  File "/opt/ros/noetic/lib/python3/dist-packages/rospy/topics.py", line 882, in publish
    self.impl.publish(data)
  File "/opt/ros/noetic/lib/python3/dist-packages/rospy/topics.py", line 1066, in publish
    serialize_message(b, self.seq, message)
  File "/opt/ros/noetic/lib/python3/dist-packages/rospy/msg.py", line 152, in serialize_message
    msg.serialize(b)
  File "/home/user/catkin_ws/devel/lib/python3/dist-packages/turtlebot3_move/msg/_OdomRecordActionResult.py", line 163, in serialize
    buff.write(_get_struct_3d().pack(_x.x, _x.y, _x.z))
AttributeError: 'function' object has no attribute 'x'

[ERROR] [1633178800.248160, 1743.189000]: To transition to an aborted state, the goal must be in a preempting or active state, it is currently in state: 3

bayodesegun · October 4, 2021, 8:41am

You are appending a function here, instead of a value.

I see that you are trying to appending the pose from the subscriber. You can do that by changing this line to

self._result.list_of_odoms.append(self.pose)

and changing def sub_cb(self, msg):

def sub_cb(self, msg):
        self.pose = msg.pose.pose
        return self.pose

edvart · October 5, 2021, 2:19pm

ok, i tried that here. But i get the error that Robot has no attribute pose (which makes sense to me, as self.pose is just what it returns when sub_cb is called, so i cant just use it like that right?)

#! /usr/bin/env python

import rospy
from turtlebot3_move.msg import OdomRecordAction, OdomRecordFeedback, OdomRecordResult
from turtlebot3_move.msg import OdomRecordAction, OdomRecordActionFeedback, OdomRecordActionResult
import actionlib
from nav_msgs.msg import Odometry
import time

class Robot(object):

    _feedback = OdomRecordFeedback()

    def __init__(self):
        self._as = actionlib.SimpleActionServer('/record_odom', OdomRecordAction, self.execute_callback, False)
        self._as.start()
        self.ctrl_c = False
        self.rospy = rospy
        self._result = OdomRecordResult()

    def sub_cb(self, msg):
        self.pose = msg.pose.pose
        return self.pose

    def execute_callback(self, goal):
        
        success = True
        print('action server called')
        
        self.sub = rospy.Subscriber('/odom', Odometry, self.sub_cb, queue_size = 1)

        for i in range(0, 10):

            self._result.list_of_odoms.append(self.pose)
            time.sleep(1)

        if success:
            rospy.loginfo('succeeded')
            self._as.set_succeeded(self._result)
        

if __name__ == '__main__':
    rospy.init_node('record_odom')
    Robot() 
    rospy.spin()

or did i misunderstand something. i thought this self.pose could not be used like a global variable.

error code:

AttributeError: 'Robot' object has no attribute 'pose'

bayodesegun · October 6, 2021, 8:32am

This is because self.pose does not exist until the subscriber callback runs for the first time. You could rewrite your logic to check if “self has attribute pose” or…

You could:

Define self.pose in __init__ and set its value to a Pose object (similar to the way it was done for self._result). You need to import the Pose class.
- TODO: figure out how to import the Pose class.
Move the definition of self.sub to __init__ as well, so that the subscriber becomes ready as soon as possible.

edvart · October 6, 2021, 6:20pm

Thank you!!! extremely helpful tips. i complete the whole rosject from there!

my action server ended up looking like this:

#! /usr/bin/env python

import rospy
from turtlebot3_move.msg import OdomRecordAction, OdomRecordFeedback, OdomRecordResult
import actionlib
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Pose, Point
import time
import numpy as np

class Robot(object):

    _feedback = OdomRecordFeedback()

    def __init__(self):
        self._as = actionlib.SimpleActionServer('/record_odom', OdomRecordAction, self.execute_callback, False)
        self.sub = rospy.Subscriber('/odom', Odometry, self.sub_cb, queue_size = 1)
        self._result = OdomRecordResult()
        self.pose = Point()
        self._as.start()
        self.ctrl_c = False
        self.rospy = rospy

    def sub_cb(self, msg):
        self.pose = msg.pose.pose.position
        return self.pose

    def distance(self, x_start, y_start, x_current, y_current):
        return np.sqrt((x_current-x_start)**2 + (y_current-y_start)**2)

    def execute_callback(self, goal):

        success = True

        self.x_start = self.pose.x
        self.y_start = self.pose.y
        self.travelled = 0

        for i in range(0, 15):
            
            self._result.list_of_odoms.append(self.pose) #legger til current pose i result-listen
            self.rospy.loginfo('x = '+ str(self.pose.x) + '  y = '+ str(self.pose.y))

            self.travelled += self.distance(self.x_start, self.y_start, self.pose.x, self.pose.y)
            self._feedback.current_total = self.travelled

            self._as.publish_feedback(self._feedback)

            self.x_start = self.pose.x
            self.y_start = self.pose.y
            time.sleep(1)

        if success:
            rospy.loginfo('succeeded')
            self._as.set_succeeded(self._result)
        
if __name__ == '__main__':
    rospy.init_node('record_odom')
    Robot() 
    rospy.spin()