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