Hi,
I am at the actions section of the Rosject for the ROS in 5 Days (Python) class.
The problem is that it seems I am not able to stop the robot by using the callback function as described in the [actionlib API].(actionlib: actionlib.simple_action_client.SimpleActionClient Class Reference)
You can see how I implemented it in my code below:
#! /usr/bin/env python
import rospy
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan
from wall_follower_pkg.srv import FindWallRequest, FindWall
from std_srvs.srv import Empty
import actionlib
from wall_follower_pkg.msg import OdomRecordAction, OdomRecordGoal
#Callback that gets called on transitions to Done.
#The callback should take two parameters: the terminal state (as an integer from actionlib_msgs/GoalStatus) and the result.
def action_done_cb(goalStatus, result):
print("Goal status: " + str(goalStatus))
if (goalStatus == 3): # SUCCEEDED => 3
vel.linear.x = 0
vel.angular.z = 0
pub.publish(vel)
rospy.loginfo(result)
rospy.signal_shutdown(“We are done here!”)
def action_feeback_cb(feedback):
print(“Action_server_feedback_cb:”)
print(feedback)
def callback(laser):
print(‘{:.2f} | {:.2f} | {:.2f} |’
.format(laser.ranges[719], laser.ranges[360], laser.ranges[0]))
if (laser.ranges[0] < 0.3):
print(‘turn left: {:.2f}’.format(laser.ranges[0]))
vel.linear.x = 0.05
vel.angular.z = -0.4
elif (laser.ranges[0] < 0.2):
print(‘turn right: {:.2f}’.format(laser.ranges[0]))
vel.linear.x = 0.05
vel.angular.z = 0.4
elif (laser.ranges[0] > 0.2 and laser.ranges[0] < 0.3):
print(‘keep fwd: {:.2f}’.format(laser.ranges[0]))
vel.linear.x = 0.05
vel.angular.z = 0
elif (laser.ranges[360] < 0.5):
print(‘transition: {:.2f}’.format(laser.ranges[360]))
vel.linear.x = 0.05
vel.angular.z = 0.7
elif (action_client.get_state() == 3):
vel.linear.x = 0
vel.angular.z = 0
else:
print(“Just moving”)
vel.linear.x = 0.08
vel.angular.z = 0
rospy.init_node(‘wall_follower_client’)
Action client
action_client = actionlib.SimpleActionClient(‘/odom_record_as’, OdomRecordAction)
action_client.wait_for_server()
goal = OdomRecordGoal()
action_client.send_goal(goal, done_cb=action_done_cb, feedback_cb=action_feeback_cb)
Service client
rospy.wait_for_service(‘/find_wall’) # Wait for the service client /execute_trajectory to be running
wallfind_service_client = rospy.ServiceProxy(‘/find_wall’, FindWall) # Create the connection to the service
result = wallfind_service_client(FindWallRequest())
print(result)
pub = rospy.Publisher(‘/cmd_vel’, Twist, queue_size=1)
sub = rospy.Subscriber(‘/scan’, LaserScan, callback)
vel = Twist()
while not rospy.is_shutdown():
pub.publish(vel)
My problem is that the although the action server is working fine and setting the goal as completed, the action client does not seem to catch it in the callback function. The action server is shown below:
#! /usr/bin/env python
import rospy
import math
import time
import actionlib
from wall_follower_pkg.msg import OdomRecordFeedback, OdomRecordResult, OdomRecordAction
from nav_msgs.msg import Odometry
import numpy as np
class OdomRecorder():
_feedback = OdomRecordFeedback()
_result = OdomRecordResult()
def __init__(self):
self.sub = rospy.Subscriber('/odom', Odometry, self.odom_callback)
self._as = actionlib.SimpleActionServer("odom_record_as", OdomRecordAction, self.goal_callback, False)
self._as.start()
self.rate = rospy.Rate(10)
self.odom = []
self.dist_covered = 0
rospy.loginfo("Odometry started")
def calc_dist_covered(self, current, previous):
dist = 0
#print("Curr> x: {:.2f} | y: {:.2f} | theta: {:.2f}". \
# format(current[0], current[1], current[2]))
#print("Prev> x: {:.2f} | y: {:.2f} | theta: {:.2f}". \
# format(previous[0], previous[1], previous[2]))
dist_x = (current[0] - previous[0]) * math.sin(previous[2])
#print("dist_x: {:.4f} | {:.4f}".format(dist_x, math.sin(previous[2])))
dist_y = (current[1] - previous[1]) * math.cos(previous[2])
#print("dist_y: {:.4f} | {:.4f}".format(dist_y, math.cos(previous[2])))
dist = math.sqrt(math.pow(dist_x, 2) + math.pow(dist_y, 2))
#print("Current dist: {:.4f}".format(dist))
return dist
def euler_from_quaternion(self, quaternion):
"""
Converts quaternion (w in last place) to euler roll, pitch, yaw
quaternion = [x, y, z, w]
Bellow should be replaced when porting for ROS 2 Python tf_conversions is done.
taken from: https://get-help.robotigniteacademy.com/t/ros-2-in-5-days-python-part-3-topics-quiz-problem-with-rotate-function/19804
"""
x = quaternion[0]
y = quaternion[1]
z = quaternion[2]
w = quaternion[3]
sinr_cosp = 2 * (w * x + y * z)
cosr_cosp = 1 - 2 * (x * x + y * y)
roll = np.arctan2(sinr_cosp, cosr_cosp)
sinp = 2 * (w * y - z * x)
pitch = np.arcsin(sinp)
siny_cosp = 2 * (w * z + x * y)
cosy_cosp = 1 - 2 * (y * y + z * z)
yaw = np.arctan2(siny_cosp, cosy_cosp)
return roll, pitch, yaw
def odom_callback(self, data):
#print(data.pose.pose.position.x)
#print(data.pose.pose.position.y)
quat = [0.0, 0.0, 0.0, 0.0]
quat[0] = data.pose.pose.orientation.x
quat[1] = data.pose.pose.orientation.y
quat[2] = data.pose.pose.orientation.z
quat[3] = data.pose.pose.orientation.w
euler_rot = self.euler_from_quaternion(quat)
#print("{:.2f}\n".format(euler_rot[2]))
self.odom = [data.pose.pose.position.x, \
data.pose.pose.position.y, \
euler_rot[2]]
def goal_callback(self, goal):
rospy.loginfo("ActionGoal: " + str(goal))
r = rospy.Rate(1)
data_recorded = []
total_dist = 0
one_lap_finished = False
success = True
i = 0
while (one_lap_finished == False):
if self._as.is_preempt_requested():
rospy.loginfo("The goal has been cancelled/preempted")
success = False
one_lap_finished = True
self._as.set_preempted()
print("Odom> x: {:.2f} | y: {:.2f} | theta: {:.2f}". \
format(self.odom[0], self.odom[1], self.odom[2]))
data_recorded.append(self.odom)
#print(data_recorded)
r.sleep()
data_recorded.append(self.odom)
#print(data_recorded)
total_dist = total_dist + self.calc_dist_covered(data_recorded[-1], data_recorded[-2])
print("Total_dist: {:.2f}".format(total_dist))
self._feedback.current_total = total_dist
self._as.publish_feedback(self._feedback)
i+=1
# Compare if the latest recorded odometry is within 10% of the original position x,y
if (total_dist > 0.5 and
math.isclose(data_recorded[-1][0], data_recorded[0][0], abs_tol=0.1) and
math.isclose(data_recorded[-1][1], data_recorded[0][1], abs_tol=0.1)):
print("First> x: {:.2f} | First> y: {:.2f} | Last> x: {:.2f} | Last> y: {:.2f}". \
format(data_recorded[-1][0], data_recorded[0][0], \
data_recorded[-1][1], data_recorded[0][1]))
if success:
self._result.list_of_odoms = data_recorded
rospy.loginfo("Success! All odometry data are:")
#print(data_recorded)
self._as.set_succeeded(self._result)
if name == ‘main’:
try:
rospy.init_node(“odom_recorder”)
OdomRecorder()
rospy.spin()
except rospy.ROSInterruptException:
rospy.signal_shutdown(“[record_odom] We are done here!”)
thanks in advance for the help.