hello I am writing the cod for the action server to record odometry data, when I run the python file it does not give me error but it also does not generate 5 related topics of my action server. can someone please help me out with the reason and the problem of this code?
#!/usr/bin/env python
import rospy
import time
import actionlib
from turtlebot3_move_test.msg import OdomRecordMsgAction, OdomRecordMsgFeedback, OdomRecordMsgResult
from geometry_msgs.msg import Twist, Pose, Point, PoseWithCovariance, Quaternion
from std_msgs.msg import Empty
from nav_msgs.msg import Odometry
import math
class RoboRecord():
_feed = OdomRecordMsgFeedback()
_res = OdomRecordMsgResult()
def init_node(self):
rospy.loginfo("initializing...")
self._as = actionlib.SimpleActionServer("record_odom_server", OdomRecordMsgAction, self.goal_callback, False)
self._as.start()
rospy.loginfo("action server initalized...")
self.ctrl_c = False
self.rate = rospy.Rate(10)
def callback(self,msg):
self.position_x = msg.pose.pose.position.x
self.position_y = msg.pose.pose.position.y
self.orientation_theta = msg.pose.pose.orientation.z
def goal_callback(self,goal):
i=0
r=rospy.Rate(1)
success = True
dis=0
self._res.list_of_odoms=[]
self.sub = rospy.Subscriber('/odom', Odometry, self.callback)
while dis < 999999:
if self._as.is_preempt_requested():
rospy.loginfo('the goal has been cancelled/preempted...')
self._as.set_preempted()
success = False
break
self._odom_readings = Point()
self._odom_readings.x = self.position_x
self._odom_readings.y = self.position_y
self._odom_readings.z = self.orientation_theta
if i!=0:
distance=math.sqrt((math.pow(self._odom_readings.x[i]-self._odom_readings.x[i-1],2)) +
(math.pow(self._odom_readings.y[i]-self._odom_readings.x[i-1],2)))
else:
distance=0
dis += distance
self._feed.current_total= dis
self._as.publish_feedback(self._feed)
self._res.list_of_odoms.append(self._odom_readings)
i+=1
r.sleep()
if success:
rospy.loginfo('publishing result... ')
self._as.set_succeeded(self._res)
print(self._res.list_of_odoms)
rospy.loginfo("finishing action server...")
if __name__ == "__main__":
rospy.init_node('action_node')
RoboRecord()
rospy.spin()
You need to define these attributes in the class before accessing or assigning values to them. You can do this in the __init__ method like you have done for other attributes.
@marco.nc.arruda I got that thank you, just a very last question, I changed my code to the following code and I suppose it mainly should be correct but when I launch it and publish the topic of the action, then checking the feedback, it shows a increasing some numbers for total distance in publishing the feedback prior to moving the robot, do you know what is wrong with it ?
#!/usr/bin/env python
import rospy
import time
import actionlib
from turtlebot3_move_test.msg import OdomRecordMsgAction, OdomRecordMsgFeedback, OdomRecordMsgResult
from geometry_msgs.msg import Twist, Pose, Point, PoseWithCovariance, Quaternion
from std_msgs.msg import Empty
from nav_msgs.msg import Odometry
import math
from sensor_msgs.msg import LaserScan
class RoboRecord():
_feed = OdomRecordMsgFeedback()
_res = OdomRecordMsgResult()
def __init__(self):
rospy.loginfo("initializing...")
self._as = actionlib.SimpleActionServer("record_odom_server", OdomRecordMsgAction, self.goal_callback, False)
self._as.start()
rospy.loginfo("action server initalized...")
self.sub1= rospy.Subscriber('/scan', LaserScan, self.laser_callback)
self.sub = rospy.Subscriber('/odom', Odometry, self.callback)
self.rate = rospy.Rate(10)
self.ctrl_c = False
def laser_callback(self, msg):
#print(len(msg.ranges))
#self.msg=msg
d=int((len(msg.ranges))/2)
self.distance_to_wall=msg.ranges[d]
return self.distance_to_wall
def callback(self,msg):
self.position_x = msg.pose.pose.position.x
self.position_y = msg.pose.pose.position.y
self.orientation_theta = msg.pose.pose.orientation.z
return self.position_x, self.position_y, self.orientation_theta
def goal_callback(self,goal):
i=0
r=rospy.Rate(1)
success = True
dis=0
_odom_readings = Point()
_odom_readings.x= []
_odom_readings.y=[]
_odom_readings.z=[]
self._res.list_of_odoms=[]
while self.distance_to_wall>0.2 :
if self._as.is_preempt_requested():
rospy.loginfo('the goal has been cancelled/preempted...')
self._as.set_preempted()
success = False
break
_odom_readings.x.append(self.position_x)
_odom_readings.y.append(self.position_y)
_odom_readings.z.append(self.orientation_theta)
if i!=0:
distance=math.sqrt((math.pow(_odom_readings.x[i]-_odom_readings.x[i-1],2)) +
(math.pow(_odom_readings.y[i]-_odom_readings.x[i-1],2)))
else:
distance=0
dis += distance
self._feed.current_total= dis
self._as.publish_feedback(self._feed)
self._res.list_of_odoms.append(_odom_readings)
r.sleep()
i+=1
if success:
rospy.loginfo('publishing result... ')
self._as.set_succeeded(self._res)
print(self._res.list_of_odoms)
rospy.loginfo("finishing action server...")
rospy.loginfo("action server finished...")
if __name__ == "__main__":
rospy.init_node('action_node')
RoboRecord()
rospy.spin()
###########################3
'''
#! /usr/bin/env python
import rospy
import time
import actionlib
from actions_quiz.msg import CustomActionMsgFeedback, CustomActionMsgResult, CustomActionMsgAction
from geometry_msgs.msg import Twist
from std_msgs.msg import Empty
class MoveClass(object):
# create messages that are used to publish feedback/result
_feedback = CustomActionMsgFeedback()
_result = CustomActionMsgResult()
def __init__(self):
# creates the action server
self._as = actionlib.SimpleActionServer("/action_custom_msg_as", CustomActionMsgAction, self.goal_callback, False)
self._as.start()
#self.ctrl_c = False
self.rate = rospy.Rate(10)
def goal_callback(self, goal):
c=goal.goal
success = True
self._pub_takeoff = rospy.Publisher('/drone/takeoff', Empty, queue_size=1)
self._takeoff_msg = Empty()
time.sleep(1)
self._pub_land = rospy.Publisher('/drone/land', Empty, queue_size=1)
self._land_msg = Empty()
time.sleep(1)
if c == "TAKEOFF":
self._pub_takeoff.publish(self._takeoff_msg)
rospy.loginfo('Taking off...')
self._as.publish_feedback(self._feedback)
if c == "LAND":
self._pub_land.publish(self._land_msg)
rospy.loginfo('Landing...')
self._as.publish_feedback(self._feedback)
if success:
self._result = Empty()
self._as.set_succeeded(self._result)
rospy.init_node('action_custom_msg')
MoveClass()
rospy.spin()
'''
Do you mean that the action feedback shows the odometry changing even if the robot isn’t moving? This sometimes happens on a very small scale (check the e-10 or similar at the end of the message), which does not affect your program