Hello,
I have a problem in actions quiz server.
I want to calculate the distance by taking the last location and the current location.
how do I get these values? I have created a subscriber to /odom topic but I get the location values just outside the loop of the movement. How do I get these values constantly? Can I call the “get_odom” method in my loop? how?
this is my code now:
import rclpy
from rclpy.action import ActionServer
from rclpy.node import Node
from actions_quiz_msg.action import Distance
from geometry_msgs.msg import Twist
from custom_interfaces.msg import Dist
from nav_msgs.msg import Odometry
from rclpy.qos import ReliabilityPolicy, QoSProfile
import time
from math import sqrt
class MyActionServer(Node):
def __init__(self):
super().__init__('actions_quiz_server')
self._action_server = ActionServer(
self, Distance, 'distance_as', self.execute_callback)
self.cmd = Twist() # Creating Twist parameter
self.dst = Dist() # Creating Dist parameter
self.publisherMove_ = self.create_publisher(
Twist, 'cmd_vel', 10) # Create publisher for movement
self.publisherDist_ = self.create_publisher(
Dist, 'total_distance', 10) # Create publisher for distance
self.subscriber_ = self.create_subscription(
Odometry, "/odom", self.get_odom, QoSProfile(depth=10, reliability=ReliabilityPolicy.BEST_EFFORT)) # Create subscriber to odometry
# Save initial values
self.msg = Odometry()
self.TraveledDistance = 0
# This method will run every time thats /odom topic get new position from the robot
def get_odom(self, msg):
# Save the current odometry of position and orientation
self.current_odom_x = msg.pose.pose.position.x
self.current_odom_y = msg.pose.pose.position.y
def execute_callback(self, goal_handle):
self.get_logger().info('Executing goal...') # First printing - server terminal
feedback_msg = Distance.Feedback() # Creating feedback message
self.last_x = self.current_odom_x
self.last_y = self.current_odom_y
for i in range(1, goal_handle.request.seconds):
# Calculate current distance traveled: (Calculated as straight lines)
dx = (self.current_odom_x-self.last_x)
dy = (self.current_odom_y-self.last_y)
self.TraveledDistance = self.TraveledDistance + \
sqrt((dx) ** 2+(dy) ** 2)
self.last_x = self.current_odom_x
self.last_y = self.current_odom_y
# Insert calculated distance to variables:
self.dst.dist = self.TraveledDistance
# Insert value into feedback message
feedback_msg.current_dist = self.TraveledDistance
self.get_logger().info('current_dist: ' + str(self.TraveledDistance))
# Print feedback
self.get_logger().info('Feedback: ' + str(feedback_msg.current_dist))
# Publish the feedback message
goal_handle.publish_feedback(feedback_msg)
# publish to /total_distance topic
self.publisherDist_.publish(self.dst)
# Publish feedback message
# Set desired velocity and publish data
self.cmd.linear.x = 0.2
self.cmd.angular.z = 0.0
self.publisherMove_.publish(self.cmd)
time.sleep(1)
goal_handle.succeed() # Set Goal status as succeed
# Stop the Robot
self.cmd.linear.x = 0.0
self.cmd.angular.z = 0.0
self.publisherMove_.publish(self.cmd)
result = Distance.Result() # Creating result message
result.total_dist = self.TraveledDistance
result.status = True
return result
def main(args=None):
rclpy.init(args=args)
actions_quiz_server = MyActionServer()
rclpy.spin(actions_quiz_server)
if __name__ == '__main__':
main()
Thanks