ROS in 5 Days (Python) Rosject Cannot stop robot


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
rospy.signal_shutdown(“We are done here!”)

def action_feeback_cb(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
print(“Just moving”)
vel.linear.x = 0.08
vel.angular.z = 0


Action client

action_client = actionlib.SimpleActionClient(‘/odom_record_as’, OdomRecordAction)
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())

pub = rospy.Publisher(‘/cmd_vel’, Twist, queue_size=1)
sub = rospy.Subscriber(‘/scan’, LaserScan, callback)
vel = Twist()

while not rospy.is_shutdown():

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.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:
    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):
    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)


    self.odom = [data.pose.pose.position.x, \
            data.pose.pose.position.y, \

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

        print("Odom> x: {:.2f} | y: {:.2f} | theta: {:.2f}". \
            format(self.odom[0], self.odom[1], self.odom[2]))

        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

        # 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:")

if name == ‘main’:
except rospy.ROSInterruptException:
rospy.signal_shutdown(“[record_odom] We are done here!”)

thanks in advance for the help.

Hi Varoma,

First of all, I suggest you to create your action client and server in two different nodes (and so, in two different files). It is a good practice to have, it makes the code clearer, easier to understand and more modular. Also, when you present your code, you can use tags for codes and clearly state which one is the action and server.

Regarding your case, if I am understanding well: Your action server receives and executes a goal well but you cannot receive any feedback from your client.
If this your problem, you can try debugging by printing the feedback topic of your action. The goal is to find where the issue comes from. You can try:

  • Printing the feedback in your action client → If you can’t see anything there is an issue in the client or in the server.
  • Echoing the feedback in the terminal → If you can’t see it there is an issue in your server.

However, I think the feedback works well. When you try to stop the robot, you are sending only one command (in other words, you are publishing the null velocity only one time: when the action is over), However, in you scan callback, you are always publishing a new speed. So, even if the robot receives the order to stop moving during an instant, the moment after it starts moving again. You can try to test the following condition first in callback: elif (action_client.get_state() == 3)

Let us know how it is going!

Kind regards,

This topic was automatically closed 5 days after the last reply. New replies are no longer allowed.