Hi,
I am doing rosject of ROS Baiscs in 5 Days. I meet with a bug which I can totally not understand.
In the action part, we need to record odometries into action result, which is a list containing geometry_msgs/Point32[]:
My method is to append new pose into the list_of_odoms. However, it seems this list only records the latest data, and it will replace all appended data with this latest data. For example, the original list is [1,2], after append with 3, it should be [1,2,3], but it turns out to be [3,3,3]. I cannot quite understand why append will cause such problem, which I have never seen before.
I thought it may be the problem of action result, so I create just a normal list which is python built-in type list, and plan to assign this list into action result at the end. The problem remains. So I think this should be problem of rospy itself. But why?? I have completely no idea what happened here…
Please help
Jialei Li
The nly thing I can think of is that in python if you do append of an object, if that object is updated with another value, all the places where that object was placed get updated ( like a pointer object would in C++). Normally this is fixed by eahc time you append , you firts create NEW object and then append. Or doing deepcopy of that object first in case its a data input
Hi,
thanks for your advice. But would you please explain it more clearly about the solution? I don’t really understand what does it mean to ‘‘Normally this is fixed by eahc time you append , you firts create NEW object and then append’’.
Thanks for your patience!
Jialei Li
Hi,
thanks for debugging for me. Here is the code of action server:
#! /usr/bin/env python
import rospy
import actionlib
import time
import math
import os
from section_action.msg import OdomRecordResult, OdomRecordFeedback, OdomRecordAction
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Point32
from datetime import datetime as dt
class OdomRecordClass():
def __init__(self):
# preparation
self.seconds = 0
# define action server of 'odom record'
self.odom_record_action_server = actionlib.SimpleActionServer(
"odom_record_action_server", OdomRecordAction, self.action_goal_callback, False)
self.odom_record_action_server.start()
rospy.loginfo(
'the action /odom_record_action_server of odom recording is ready......')
# define subscriber of '/odom'
self.odom_sub = rospy.Subscriber('/odom', Odometry, self.sub_callback)
# in order to get infomation in '/odom'
self.odom_data = Odometry()
# in order to restore the info (x,y,theta) from '/dom'
self.current_pose = Point32()
self.previous_pose = Point32()
self.result = OdomRecordResult()
# in order to feedback the current total distance that robot has moved so far
self.feedback = OdomRecordFeedback()
# robot doesn't move any distance at the beginning
self.feedback.current_total = 0.0
# records has nothing at the beginning
self.result.list_of_odoms = []
# wait for /odom for 1 second
time.sleep(1)
def sub_callback(self, msg):
# get pose info from '/odom'
self.odom_data = msg
def action_goal_callback(self, goal):
# get initial pose
self.current_pose.x = self.odom_data.pose.pose.position.x
self.current_pose.y = self.odom_data.pose.pose.position.y
self.current_pose.z = self.odom_data.pose.pose.orientation.z
self.interval_dist = 0
# if the robot hasn't finished one lap, then keep moving
while True:
# send pose into previous_pose for distance calculation
self.previous_pose.x = self.current_pose.x
self.previous_pose.y = self.current_pose.y
self.previous_pose.z = self.current_pose.z
# update pose info
self.current_pose.x = self.odom_data.pose.pose.position.x
self.current_pose.y = self.odom_data.pose.pose.position.y
self.current_pose.z = self.odom_data.pose.pose.orientation.z
# calculate and publish current total distance
interval_x = self.current_pose.x - self.previous_pose.x
interval_y = self.current_pose.y - self.previous_pose.y
self.interval_dist = math.sqrt(interval_x ** 2 + interval_y ** 2)
self.feedback.current_total += self.interval_dist
rospy.loginfo(f'current pose is: {self.current_pose}')
self.odom_record_action_server.publish_feedback(
self.feedback)
# record pose every 1 second
self.result.list_of_odoms.append(self.current_pose)
time.sleep(1)
self.seconds += 1
# if the robto stops, i.e. distance between current and previous time epoch is nearly 0
# then stop recording
if self.seconds >= 50 and self.interval_dist <= 0.001:
break
# return the records with all pose info
self.odom_record_action_server.set_succeeded(self.result)
rospy.loginfo(f'the action result is: {self.result}')
rospy.signal_shutdown('action is over')
def main(self):
rospy.loginfo('call action......')
rospy.spin()
if __name__ == '__main__':
rospy.init_node("odom_record_1_node")
odom_record = OdomRecordClass()
odom_record.main()
This is because, with append, list_of_odoms is storing a reference to self.current_pose, not the actual values, so all the elements of list_of_odoms change whenever self.current_pose changes.
Instead of this, try:
# short form
self.result.list_of_odoms += [self.current_pose]
# long form
self.result.list_of_odoms = self.result.list_of_odoms + [self.current_pose]
thanks for your advice. Unfortunately it still doesn’t work, it still updates all elements in list.
I have found another solution, although a little bit stupid but at least it can realize the function:
save poses in .txt every 1 second, then read and assign them into instance of ActionResult when the wall following is finished.
Of course I am still open for other possible solutions
