Actions Quiz (Chapter 10) - Drone not moving

[roduralde93]

Hello!
So I got past the action terminal status warning of the goal, but the drone doesn’t move at all when calling the action server through the topics. As before, I have my server running and I can see the topics associated with it, so I call it:

rostopic pub /action_custom_msg_as/goal actions_quiz/CustomActionMsgActionGoal "header:
  seq: 0
  stamp:
    secs: 0
    nsecs: 0
  frame_id: ''
goal_id:
  stamp:
    secs: 0
    nsecs: 0
  id: ''
goal:
  goal: 'Take-off'"

And nothing happens. Given the structure of my code, if the callback is called, it should at least print an “Incorrect Goal!” message, but even that doesn’t happen:

#! /usr/bin/env python
import rospy
import time
import actionlib

from actions_quiz.msg import CustomActionMsgGoal, CustomActionMsgFeedback, CustomActionMsgAction
from std_msgs.msg import Empty

class MoveSquareClass(object):
    
  # create messages that are used to publish feedback/result
    _feedback = CustomActionMsgFeedback()
    _result   = CustomActionMsgGoal()

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

        # define the different publishers and messages that will be used
        self._pub_takeoff = rospy.Publisher('/drone/takeoff', Empty, queue_size=1)
        self._takeoff_msg = Empty()
        self._pub_land = rospy.Publisher('/drone/land', Empty, queue_size=1)
        self._land_msg = Empty()

        if goal == 'Take-off':
            self.take_off(self._takeoff_msg)
            rospy.loginfo("Taking off...")         

        elif goal == 'Land':
            self.land(self._land_msg)
            rospy.loginfo("Landing...")

        else:
            rospy.loginfo("Incorrect Goal!")

        self._as.set_succeeded(self._result)


    def take_off(self):
        # make the drone takeoff
        i=0
        while not i == 3:
            self._pub_takeoff.publish(self._takeoff_msg)
            rospy.loginfo('Taking off...')
            time.sleep(1)
            i += 1

    def land(self):
        while not i == 3:
            self._pub_land.publish(self._land_msg)
            rospy.loginfo('Landing...')
            time.sleep(1)
            i += 1
        
      
if __name__ == '__main__':
  rospy.init_node('action_custom_msg_as')
  MoveSquareClass()
  rospy.spin()

I can echo the topic and it is indeed receiving the command:

rostopic echo /action_custom_msg_as/goal
header:
  seq: 3
  stamp:
    secs: 0
    nsecs:         0
  frame_id: ''
goal_id:
  stamp:
    secs: 0
    nsecs:         0
  id: ''
goal:
  goal: "Take-off"
---

So I think the goal_callback is not getting activated somehow?
I tried putting print in the goal_callback just to see if it was entering that block and it’s not (as nothing is showed on any terminal)

 def goal_callback(self, goal):

        rospy.loginfo("Goal_callback was activated!") #Test print
        # define the different publishers and messages that will be used
        self._pub_takeoff = rospy.Publisher('/drone/takeoff', Empty, queue_size=1)
        self._takeoff_msg = Empty()
        self._pub_land = rospy.Publisher('/drone/land', Empty, queue_size=1)
        self._land_msg = Empty()

        if goal == 'Take-off':
            self.take_off(self._takeoff_msg)
            rospy.loginfo("Taking off...")         

        elif goal == 'Land':
            self.land(self._land_msg)
            rospy.loginfo("Landing...")

        else:
            rospy.loginfo("Incorrect Goal!")

        self._as.set_succeeded(self._result)

Any help would again be appreciated. Thanks!

[albertoezquerro]

Hello @roduralde93 ,

Several things here. Let’s go point by point.

1. The callback is indeed being called. You can’t see the prints of your program because you don’t have the argument output="screen" in your launch file. This argument indicates that all log messages are printed to the Shell.

2. If you add this argument to your launch file, you will see that your program prints “Incorrect Goal!”. This is because you are evaluating the wrong thing in your callback:
   if goal == 'Take-off':
   Here, your goal variable contains the WHOLE goal message (with the header, goal_id, etc…). If you want to evaluate only the goal string, you need to check the variable goal.goal. This is the field that contains the string (‘Take-off’, ‘Land’).

3. After fixing this, your program will correctly enter the take_off sequence. It will still fail because you are not calling the take_off() method correctly, but I will leave that for you to figure out and fix.

[roduralde93]

Man does it feel obvious when pointed out… Ha! Thanks, got it working now.