Part III of ROS Basics in 5 Days for Python course - Recording Odometry readings

Hi guys,

I’m trying to solve the part III of ROS Basics in 5 Days for Python course. In this exercise we need to create a new ROS node that contains an action server named “record_odom”. The server uses a “OdomRecord.action” message, which I defined as follows

"

geometry_msgs/Point list_of_odoms

float32 current_total
"

I believe that most of my code is done. However I’m getting an error when trying to save the odometry readings on the list_of_odoms atribute. The server callback code is´

# Server callback
    def goal_callback(self, goal):
        rate = rospy.Rate(1)
        dist = 0  # Travelled distance
        success = True
        i = 0
        vec_result_x = zeros(40)
        vec_result_y = zeros(40)
        vec_result_z = zeros(40)

        while dist <= self._dist_one_lap:
            # Check if the goal is cancelled
            if self._as.is_preempt_requested():
                success = False
                self._as.set_preempted()
                break
            
            # Saving odometry readins
            vec_result_x[i] = self.position_x
            vec_result_y[i] = self.position_y
            vec_result_z[i] = self.orientation_theta
            
            # Travelled distance
            dist += sqrt( ( vec_result_x[i] - vec_result_x[i-1] )**2 + ( vec_result_y[i] - vec_result_y[i-1] )**2 ) 
            self._feedback.current_total = dist
            self._as.publish_feedback(self._feedback)

            # loop variables
            i += 1
            rate.sleep()

When the success variable is set to Tue, the following error is returned

"
[ERROR] [1620687943.209565, 2469.565000]: Exception in your execute callback: ‘list’ object has no attribute ‘x’
Traceback (most recent call last):
File “/opt/ros/noetic/lib/python3/dist-packages/actionlib/simple_action_server.py”, line 289, in executeLoop
self.execute_callback(goal)
File “/home/user/catkin_ws/src/my_project/scripts/partII_actions_server.py”, line 67, in goal_callback
self._result.result.list_of_odoms.x = vec_result_x
AttributeError: ‘list’ object has no attribute ‘x’
"

Can someone help me? I’m stuck on this.

Hi @pedroaugusto.feis you can send me your full package to u1802520@unimilitar.edu.co so i can have a better look. your message deffinition, i think i can help you.

I can’t tell how you’ve defined list_of_odoms, since that’s not in your code example.

Also can’t tell - did you import zeros from numpy? That’s not a regular python function. And how did you decide that 40 items would be enough? Why not just make list variables and then append items to them as you go? No need to create a bunch of list items with 0. You can actually store each odom reading in a single list, and then access the components as needed if need be.
/K

Hello @comm

The syntax below is strange for me

( vec_result_x[i] - vec_result_x[i-1] )**2

Could you try replacing by the syntax below?

pow( vec_result_x[i] - vec_result_x[i-1], 2 )

Mostly to help me debugging that code, because I couldn’t reproduce the error on my side

Please, let me know if that helps you

Regards

Hello,

Thanks for the help.

I did import the zeros from numpy. However, as you pointed out, it is better to just append the new items on the list. I’ve already changed this on my code. Anyway, this do not solve the problem.

The list_of_odoms ins defined inside the “OdomRecord.action” message which is a file inside the action folder with the following content :

---
geometry_msgs/Point[] list_of_odoms
---
float32 current_total

Hello,

Unfortunately, this did not solve my problem.

In the following is the full code of my action server file. There are some identations problem which appeared after pasting the code here.

#!/usr/bin/env python

import rospy
import actionlib

from my_project.msg import OdomRecordAction, OdomRecordFeedback, OdomRecordResult
from nav_msgs.msg import Odometry
from numpy import sqrt

class RecordOdometry(object):
    # Creating some initial class parameters
    _feedback = OdomRecordFeedback()
    _result = OdomRecordResult()
    position_x = 0.0
    position_y = 0.0
    orientation_theta = 0.0
    _dist_one_lap = 1.0


    # Constructor
    def __init__(self):
        # Creating some object parameters
        rospy.loginfo("Initializing the action server.")
        # Action server
    self._as = actionlib.SimpleActionServer("record_odometry_server", OdomRecordAction, execute_cb=self.goal_callback, auto_start=False)
        self._as.start()
        rospy.loginfo("Action server initalized.")

        # Subscriber
        rospy.loginfo("Creating /odom subscriber...")
        _sub_odom = rospy.Subscriber("/odom", Odometry, self.odom_callback)
        while _sub_odom.get_num_connections() < 1:
            pass

        rospy.loginfo("/odom subscriber created.")


    # Server callback
    def goal_callback(self, goal):
        rate = rospy.Rate(1)
        dist = 0  # Travelled distance
        success = True
        i = 0

        vec_result_x = []
        vec_result_y = []
        vec_result_z = []

        while dist <= self._dist_one_lap:
            # Check if the goal is cancelled
            if self._as.is_preempt_requested():
                success = False
                self._as.set_preempted()
                break
            
            # Saving odometry readins
            vec_result_x.append(self.position_x)
            vec_result_y.append(self.position_y)
            vec_result_z.append(self.orientation_theta)

            # Travelled distance
            dist += sqrt( pow( vec_result_x[i] - vec_result_x[i-1], 2) + pow( vec_result_y[i] - vec_result_y[i-1], 2) ) 

            self._feedback.current_total = dist
            self._as.publish_feedback(self._feedback)
            # loop variables
            i += 1
            rate.sleep()

        if success:
            print(vec_result_x)
            self._result.list_of_odoms.x = vec_result_x
            self._result.list_of_odoms.y = vec_result_y
            self._result.list_of_odoms.z = vec_result_z
            self._as.set_succeeded(self._result.result.list_of_odoms)
            rospy.loginfo("Finishing the action server.")


    # Odometry callback
    def 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

if __name__ == '__main__':
    rospy.init_node("record_odometry_server_node")
    RecordOdometry()
    rospy.spin()

Right - so list_of_odoms is really a list of Point messages. You need to specify which message in the list you’re taking the x value of. And you could be appending new odom points to that list as you take readings. Then you can just use the list and get the x,y,z components as needed rather than creating new lists.

/K

Ok. So I created a Point message

odom_readings = Point()

Then, set the odometry values to its atributes

odom_readings.x = self.position_x
odom_readings.y = self.position_y
odom_readings.z = self.orientation_theta

Finally, appended it to the result object

result.list_of_odoms.append(odom_readings)

However, as a result of the append command, all the result list is changing. In the following, it is shown the result.list_of_odoms list between two iterations

iteration 1 –
[x: -0.8449477296600538
y: 0.4248536140629088
z: -0.9208840466609143]

iteration 2 –
[x: -0.8411427633752305
y: 0.4248859734212179
z: -0.8943785437249375, x: -0.8411427633752305
y: 0.4248859734212179
z: -0.8943785437249375]

Note that the first elements are changed during the second iteration. What is happening? The full service code is shown below.

# Server callback
def goal_callback(self, goal):
    result = OdomRecordResult()
    odom_readings = Point()
    my_list = []
    rate = rospy.Rate(1)
    dist = 0  # Travelled distance
    success = True
    i = 0
    a = []
    
    while dist <= self._dist_one_lap:
        rospy.loginfo("Saving odometry readings.")
        # Check if the goal is cancelled
        if self._as.is_preempt_requested():
            success = False
            self._as.set_preempted()
            break
        
        # Saving odometry readins
        odom_readings.x = self.position_x
        odom_readings.y = self.position_y
        odom_readings.z = self.orientation_theta
        
        result.list_of_odoms.append(odom_readings)
        print(result.list_of_odoms)
        
        # Travelled distance
        #dist += sqrt( pow( self._result.list_of_odoms[i].[0]] - self._result.list_of_odoms[i-1].[0]], 2) + pow( self._result.list_of_odoms[i].y - self._result.list_of_odoms[i-1].y, 2) ) 
        dist += 0.25  # Just to evolve the distance

        self._feedback.current_total = dist
        self._as.publish_feedback(self._feedback)

        # loop variables
        i += 1
        rate.sleep()

    if success:
        self._as.set_succeeded(result)
        rospy.loginfo("Finishing the action server.")

Ok, i have checked your package, seems there is a problem of referencing, to correct it you just have to make a new instance of Point, everytime you want to append a new one, so the new lines for saving the odometry readings become:

            # Saving odometry readins
            self._odom_readings = Point()
            self._odom_readings.x = self.position_x
            self._odom_readings.y = self.position_y
            self._odom_readings.z = self.orientation_theta

check this post. for more information. it is python stuff but not big deal.

3 Likes

Yes, it worked! Many thanks.

Best,

Pedro Assis"

2 Likes

@u1802520 is correct. Look up the topic of Python assigning by value or reference. The reason you were getting copies of the same numbers is because you were changing the values the different names pointed to (I don’t know that I’m explaining it well).

I’m also still not sure why you’re bothering with the whole odom.readings.x etc. You seem to be making this harder than it needs to be. With your odom_callback just capture the odometry message. Append that to the list of odometry messages, and also get the data you want from it as needed.

/K

1 Like