Hello,
I managed to get the odometry data, save them into a Point32 variable and send it as self._as.set_succeeded(self._result). My problem is that the client finishes always with a WARNING state_result instead of DONE.
The action server terminal is not giving me any error and if I print the result, seems fine for me, a Point32 variable.
Client terminal:
This is my client code:
#! /usr/bin/env python
import rospy
import time
import actionlib
from real_robot.msg import OdomRecordAction, OdomRecordResult, OdomRecordFeedback
# We create some constants with the corresponing vaules from the SimpleGoalState class
PENDING = 0
ACTIVE = 1
DONE = 2
WARN = 3
ERROR = 4
# definition of the feedback callback. This will be called when feedback
# is received from the action server
# it just prints a message indicating a new message has been received
def feedback_callback(feedback):
print("Total distance traveled is {}m".format(feedback))
# initializes the action client node
rospy.init_node('drone_action_client')
# create the connection to the action server
client = actionlib.SimpleActionClient("/record_odom", OdomRecordAction)
# waits until the action server is up and running
client.wait_for_server()
# creates a goal to send to the action server
goal = None
# sends the goal to the action server, specifying which feedback function
# to call when feedback received
client.send_goal(goal, feedback_cb=feedback_callback)
state_result = client.get_state()
rate = rospy.Rate(1)
rospy.loginfo("state_result: "+str(state_result))
while state_result < DONE:
rospy.loginfo("action in process")
rate.sleep()
state_result = client.get_state()
rospy.loginfo("state_result: "+str(state_result))
rospy.loginfo("[Result] State: "+str(state_result))
if state_result == ERROR:
rospy.logerr("Something went wrong in the Server Side")
if state_result == WARN:
rospy.logwarn("There is a warning in the Server Side")
print('[Result] State: %d'%(client.get_state()))
And this is my server 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.xOdom = data.pose.pose.position.x
self.yOdom = data.pose.pose.position.y
self.thOdom = data.pose.pose.orientation.z
"""
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.5
success = True
i=0
flag = True
for i in range(5):
if(self.first_run):
self.previous_x = self.xOdom
self.previous_y = self.yOdom
x = self.xOdom
y = self.yOdom
z = self.thOdom
d_increment = sqrt(pow((x - self.previous_x),2) + pow((y - self.previous_y),2))
self.total_distance = self.total_distance + d_increment
self.previous_x = self.xOdom
self.previous_y = self.yOdom
self.previous_z = self.thOdom
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()
i+=i
# build and publish the feedback message
self._feedback.current_total = self.total_distance
self._as.publish_feedback(self._feedback)
# 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()
Please, how can I debug this?
