Hello,
Im finishing the rosject in the ros basic in five days python version, I miss only to send the result after a lap is done. I think that my code is done for that but I somehow when the total distance >= lap condition is met, the while loop keeps running and the result isnt sent to the client. Please could someone help me?
so far, the client gets the feedback and I can see that the distance is ok, so more or less a lap is 6,5 meters. But when the while loop meets the condition, keeps running
Here is my code:
#! /usr/bin/env python
from math import degrees, sqrt
import rospy
import actionlib
import numpy as np
from real_robot.msg import OdomRecordAction, OdomRecordResult, OdomRecordFeedback
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Point32
from tf.transformations import euler_from_quaternion
class move_turtle(object):
# create messages that are used to publish feedback and results
_feedback = OdomRecordFeedback()
_result = OdomRecordResult()
xOdom=0
yOdom=0
thOdom=0
def __init__(self):
# creates the action server
self._as = actionlib.SimpleActionServer("record_odom", OdomRecordAction, self.callback, False)
self._as.start()
self.ctrl_c = False
self.rate = rospy.Rate(1)
self.first_run = True
self.total_distance = 0.
self.previous_x = 0
self.previous_y = 0
self.previous_z = 0
self.odomData=Point32()
def call_record_odom(self,data):
self.odomData.x = data.pose.pose.position.x
self.odomData.y = data.pose.pose.position.y
self.odomData.z = data.pose.pose.orientation.z
def callback(self,goal):
rospy.loginfo("executing odometry record")
self.subodom = rospy.Subscriber('/odom', Odometry, self.call_record_odom)
total_distance=0
lap = 6
while total_distance < lap:
if(self.first_run):
self.previous_x = abs(self.odomData.x)
self.previous_y = abs(self.odomData.y)
x = abs(self.odomData.x)
y = abs(self.odomData.y)
z = self.odomData.z
d_increment = sqrt(pow((abs(x) - abs(self.previous_x)),2) + pow((abs(y) - abs(self.previous_y)),2))
self.total_distance = self.total_distance + d_increment
self.previous_x = self.odomData.x
self.previous_y = self.odomData.y
self.previous_z = self.odomData.z
self.first_run = False
# check that preempt (cancelation) has not been requested by the action client
if self._as.is_preempt_requested():
rospy.loginfo('The goal has been cancelled/preempted')
# the following line, sets the client in preempted state (goal cancelled)
self._as.set_preempted()
success = False
# we end the calculation of the Fibonacci sequence
break
# build and publish the feedback message
self._feedback.current_total = self.total_distance
self._as.publish_feedback(self._feedback)
#succes true
success = True
# the sequence is computed at 1 Hz frequency
self.rate.sleep()
if success:
#append odometry readings for results
self._result.list_of_odoms.append(self.odomData)
list(self._result.list_of_odoms)
rospy.loginfo("odometry data: {}".format(list(self._result.list_of_odoms)))
self._as.set_succeeded(self._result)
rospy.loginfo("Returning odometry values")
if __name__ == '__main__':
rospy.init_node('Odom_record_node')
move_turtle()
rospy.spin()