Rosject Part III Action Result is sending incorrect result

[Dkae]

Hello

I am currently on Part III of the Rosject.

I have created an Action Server which I am testing with the actionlib tool.

Now do I have the problem, that instead of having a result of a list with the different odometry recordings, I somehow just have a list where all the records have the same value, which is the last one of my recordings.

I move around with the robot with the keyboard, so I get different values in my local variable list_of_odoms.
So the values in the list self._result.list_of_odoms should be the same, but they are not. Instead all the entries have the same values, which is the last one of the list_of_odoms list.

So here is what the result is printing:

If I would scroll further down it will look the same.

And this are the values it should actually print:
list_of_odoms = [[0.0, 0.0, 0.0], [-0.6210482223171684, -0.6359845823398453, -0.033455432463252416], [-0.6210482223171684, -0.6359845823398453, -0.033455432463252416], [-0.6209075369941961, -0.6359625816968034, -0.03183212824509125], [-0.6207642441836297,-0.6359409826328215, -0.03018574664883157], [-0.6080471627239935, -0.6350874970981613, -0.04073843386715889], [-0.4342822982575167, -0.6088015342445245, -0.07701805111210773], [-0.24312986584196267, -0.5787489896288052, -0.07826747086152361], [-0.04552290681476177, -0.5472428917733237, -0.07945676182895253]]

So I don’t get, why it is not the same. Do I use wrong the append function with the point32 list?

Here my code:
#! /usr/bin/env python
import math
import rospy
import actionlib
from myrosject.msg import OdomRecordAction, OdomRecordResult, OdomRecordFeedback
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Point32

class Odometry_Record():

def __init__(self):
    #Create Action Server
    self._as = actionlib.SimpleActionServer("record_odom", OdomRecordAction, self.action_callback, False)
    self._as.start()
    rospy.loginfo('action server /record_odom is ready')
    self._feedback = OdomRecordFeedback()
    self._result = OdomRecordResult()
    #Create Subscriber
    self.sub_odom = rospy.Subscriber('/odom', Odometry, self.sub_odom_callback)
    rospy.loginfo("subscriber /odom is ready")
    #Init Values
    self.odom = Odometry()
    self.odom_record = Point32()
    self.rate = rospy.Rate(1)
    self.ctrl_c = False

def sub_odom_callback(self, msg):
    self.odom = msg

def action_callback(self, goal):
    #Init List of Odometry with zeros for distance calculation
    list_of_odoms = [[0.0,0.0,0.0]]
    #Result is empty at the beginning
    self._result.list_of_odoms = []
    
    success = True

    i = 0
    distance = 0.0

    while distance < 0.5: # 1 Round is approximately 6.5 Meters

        if self._as.is_preempt_requested():
            rospy.loginfo('The goal has been cancelled/preempted')
            self._as.set_preempted()
            success = False
            break

        #Add the next recording
        pos_x = self.odom.pose.pose.position.x
        pos_y = self.odom.pose.pose.position.y
        ori_z = self.odom.pose.pose.orientation.z
        list_of_odoms.append([pos_x,pos_y,ori_z])

        #Get point32() structure
        self.odom_record.x = pos_x
        self.odom_record.y = pos_y
        self.odom_record.z = ori_z
        self._result.list_of_odoms.append(self.odom_record)
        
        #Calculate distance traveled and give feedback
        i += 1

        if i > 1: 
            delta_distance = math.sqrt((math.pow(list_of_odoms[i][0] -list_of_odoms[i-1][0],2)) +
                (math.pow(list_of_odoms[i][1] -list_of_odoms[i-1][1],2)))
            distance = distance + delta_distance
            self._feedback.current_total = distance
            self._as.publish_feedback(self._feedback)

        self.rate.sleep()
    
    if success:
        rospy.loginfo("One lap done successfully")
        print(list_of_odoms)
        self._as.set_succeeded(self._result)

if name == ‘main’:
rospy.init_node(‘record_odom_node’)
Odometry_Record()
rospy.spin()

Thanks in advance for any help

[girishkumar.kannan]

Hi @Dkae ,

Could you please enclose your code in a proper code-block?
Refer code-block: Extended Syntax | Markdown Guide

Regards,
Girish

[girishkumar.kannan]

Hi @Dkae ,

So the problem you are having is what is referred to as shallow copy error.
Basically, you are appending the variable self.odom_record which is of type Point32().
When you append, Python internally makes a shallow copy, instead of a deep copy.
In a shallow copy, reference to the memory of the variable is copied, hence everytime you copy a variable, the most recent value will be present on all of the copies.
A deep copy, on the other hand, makes a completely independent copy of the variable, by copying the contents of the memory. Thus all the copies will be different if the variable changes overtime.

But going through your code, I feel that you are a bit confused.

@Dkae

list_of_odoms = [[0.0,0.0,0.0]]

Here, you are using a list of floats as one set of odometry values.

@Dkae

self._result.list_of_odoms.append(self.odom_record)

But you are appending a Point32() item to the list.

Either decide to use a list of Point32 or a list of list of floats. Don’t use both.

If you choose to use list of Point32:

1. Append a Point32 message directly to the list.
2. Then change the values of the last element of the list (recently appended item).

If you choose to use list of list of floats:

1. You can directly append a list of floats like list_of_odoms.append([val1, val2, val3]).

I used the second option as I was comfortable with it.

Let me know if you have fixed the problem.

Regards,
Girish

[Dkae]

@girishkumar.kannan

Point32

Hi @girishkumar.kannan

Thanks for your response and the explanation with the shadow and deep copies.

In my code list_of_odoms and self._result.list_of_odoms is not the same variable. This might be confusing because it has the same name, but I am using it for 2 different things.

I use the list_of_odoms just like you said as a list of floats for doing the distance calculation. So I can access values from the past.

Then for the result I am using the self._result.list_of_odoms as a list of Point32().
Because the ActionResult() is declared as a list of Point32(), I can not use the list of float.

With the list of float the append function works easy, you always have a new entry value. This would be option 2 of your suggestions.

But in my point of view I can not use the list of float for the result because in the action message the result is defined as a Point32() value.

When you do the append function with the point32() value, the shadow copy happens.

Therefore I will need your suggestions 1.

But here I quite don’t understand how to do it.
So in my code the values I want to add to the result is the “self.odom_record” which is Point32().
But self._result.list_of_odoms.append(self_odom_record) gives the shadow copies.

So what I actually want is that my self_result.list_of_odoms stays as it is and adds a new entry with the self.odom_record.

You mentioned that for this I have to append a Point32() value and then modifiey the last index with my current appending item.

But here my main question is:
When I append a Point32() msg, then I already overwrite everything that is in my self._result.list_of_odoms. So how am I supposed to append a Point32() msg without already overwriting everything?

[girishkumar.kannan]

Hi @Dkae ,

@girishkumar.kannan

I used the second option as I was comfortable with it.

I actually did not use this for result, but just to print it as an output upon result completion. I just checked my own program (after a long time). So, to be clear, I used this method to print out the values in the action server, but I did return a list of Point32 values for the list_of_odoms.

So the option 1 was what I did, but with modified values before appending to list:
This is my exact code that I had in my program script:

current_result = Point32()
current_result.x = self.pos_x
current_result.y = self.pos_y
current_result.z = self.theta_z
self.result.append(current_result)

I have this in the goal callback function.

@Dkae

gives the shadow copies.

Shallow, not shadow.

@Dkae

So in my code the values I want to add to the result is the “self.odom_record” which is Point32().
But self._result.list_of_odoms.append(self_odom_record) gives the shadow copies.

I believe this is because self.odom_record is a class variable. In my code I have used a local variable called current_result as Point32(). So my best suggestion for you will be to not use a class variable.

@Dkae

You mentioned that for this I have to append a Point32() value and then modifiey the last index with my current appending item.

Yes, you can append and modify, or, modify and append like I did.

@Dkae

When I append a Point32() msg, then I already overwrite everything that is in my self._result.list_of_odoms. So how am I supposed to append a Point32() msg without already overwriting everything?

That cannot happen when you append. You are not reassigning the value to the variable, you are just appending. Appending does not cause overwriting.

Here is your modified code:

#! /usr/bin/env python
import math
import rospy
import actionlib
from myrosject.msg import OdomRecordAction, OdomRecordResult, OdomRecordFeedback
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Point32

class Odometry_Record():

    def __init__(self):
        #Create Action Server
        self._as = actionlib.SimpleActionServer("record_odom", OdomRecordAction, self.action_callback, False)
        self._as.start()
        rospy.loginfo('action server /record_odom is ready')
        self._feedback = OdomRecordFeedback()
        self._result = OdomRecordResult()
        #Create Subscriber
        self.sub_odom = rospy.Subscriber('/odom', Odometry, self.sub_odom_callback)
        rospy.loginfo("subscriber /odom is ready")
        #Init Values
        self.odom = Odometry()
        # self.odom_record = Point32()   # do not use this
        self.rate = rospy.Rate(1)
        self.ctrl_c = False

    def sub_odom_callback(self, msg):
        self.odom = msg

    def action_callback(self, goal):
        #Init List of Odometry with zeros for distance calculation
        list_of_odoms = [[0.0,0.0,0.0]]
        #Result is empty at the beginning
        self._result.list_of_odoms = []
        
        success = True

        i = 0
        distance = 0.0

        while distance < 0.5: # 1 Round is approximately 6.5 Meters

            if self._as.is_preempt_requested():
                rospy.loginfo('The goal has been cancelled/preempted')
                self._as.set_preempted()
                success = False
                break

            #Add the next recording
            pos_x = self.odom.pose.pose.position.x
            pos_y = self.odom.pose.pose.position.y
            ori_z = self.odom.pose.pose.orientation.z
            list_of_odoms.append([pos_x,pos_y,ori_z])

            #Get point32() structure
            # self.odom_record.x = pos_x
            # self.odom_record.y = pos_y
            # self.odom_record.z = ori_z
            odom_record = Point32()
            odom_record.x = pos_x
            odom_record.y = pos_y
            odom_record.z = ori_z
            # self._result.list_of_odoms.append(self.odom_record)
            self._result.list_of_odoms.append(odom_record)
            
            #Calculate distance traveled and give feedback
            i += 1

            if i > 1: 
                delta_distance = math.sqrt((math.pow(list_of_odoms[i][0] -list_of_odoms[i-1][0],2)) +
                    (math.pow(list_of_odoms[i][1] -list_of_odoms[i-1][1],2)))
                distance = distance + delta_distance
                self._feedback.current_total = distance
                self._as.publish_feedback(self._feedback)

            self.rate.sleep()
        
        if success:
            rospy.loginfo("One lap done successfully")
            print(list_of_odoms)
            self._as.set_succeeded(self._result)

if __name__ == '__main__':
rospy.init_node('record_odom_node')
Odometry_Record()
rospy.spin()

Let me know if this works for you.

Regards,
Girish

[Dkae]

Hi @girishkumar.kannan

Yes this worked thanks!

It is intressting that the append has a different outcome when you append a self.variable instead of a local variable.

By using a local variable it worked. By using a self.variable it made shallow copies.