class CustomActionClass(object):
_feedback = CustomActionMsgFeedback()
_result = CustomActionMsgResult()
_pubTakoff = rospy.Publisher("/drone/takeoff", Empty, queue_size=10)
_pubLand = rospy.Publisher("/drone/land", Empty, queue_size=10)
def __init__(self):
self._as = actionlib.SimpleActionServer("/action_custom_msg_as",
CustomActionMsgAction,
self.goal_callback, False)
self._as.start()
def goal_callback(self, goal):
success = True
# publish info to the console for the user
if (goal.goal == 'TAKEOFF'):
rospy.loginfo("Taking off!")
self._pubTakoff.publish(Empty())
self._feedback.feedback = "TAKEOFF"
elif (goal.goal == 'LAND'):
rospy.loginfo("Landing!")
self._pubLand.publish(Empty())
self._feedback.feedback = "LAND"
else:
rospy.loginfo('The goal has been cancelled/preempted')
self._as.set_preempted()
success = False
self._as.publish_feedback(self._feedback)
if success:
rospy.loginfo('Success!')
self._as.set_succeeded(self._feedback)
def goal_callback(self, goal):
# this callback is called when the action server is called.
# helper variable
success = True
action = goal.goal
# publish info to the console for the user
rospy.loginfo('Goal: ' + action)
# check that preempt (cancelation) has not been requested by the action client
if self._as.is_preempt_requested():
rospy.loginfo('The goal has been cancelled/preempted')
# the following line, sets the client in preempted state (goal cancelled)
self._as.set_preempted()
success = False
elif action == 'TAKEOFF':
self.take_off()
elif action == 'LAND':
self.land()
else:
rospy.loginfo('Invalid goal')
success = False
# at this point, either the goal has been achieved (success==true)
# or the client preempted the goal (success==false)
if success:
rospy.loginfo('Action succeeded')
self._as.set_succeeded(CustomActionMsgResult())
class Take_Off_Land_Class(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()
def goal_callback(self, goal):
# this callback is called when the action server is called.
# this is the function that computes the Fibonacci sequence
# and returns the sequence to the node that called the action server
# helper variables
r = rospy.Rate(1)
success = True
launchPub = rospy.Publisher('/drone/takeoff', Empty, queue_size=1)
landPub = rospy.Publisher('/drone/land', Empty, queue_size=1)
# publish info to the console for the user
rospy.loginfo("Checking For Instruction")
goalCommand = goal.goal
time.sleep(1)
if self._as.is_preempt_requested():
rospy.loginfo('The goal has been cancelled/preempted')
# the following line, sets the client in preempted state (goal cancelled)
self._as.set_preempted()
success = False
# we end the calculation of the Fibonacci sequence
if (goalCommand == "TAKEOFF"):
self._feedback.feedback = "TAKEOFF"
launchPub.publish(Empty())
if (goalCommand == "LAND"):
self._feedback.feedback = "LAND"
landPub.publish(Empty())
self._as.publish_feedback(self._feedback)
# at this point, either the goal has been achieved (success==true)
# or the client preempted the goal (success==false)
# If success, then we publish the final result
# If not success, we do not publish anything in the result
#if success:
# self._result.sequence = self._feedback.sequence
# rospy.loginfo('Succeeded calculating the Fibonacci of order %i' % fibonacciOrder )
self._as.set_succeeded(self._result)
class CustomActionMsgClass(object):
_feedback = CustomActionMsgFeedback()
_result = CustomActionMsgResult()
def __init__(self):
self._as = actionlib.SimpleActionServer("action_custom_msg_as",
CustomActionMsgAction,
self.goal_callback, False)
self._as.start()
def goal_callback(self, goal):
success = True
r = rospy.Rate(1)
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()
takeoff_or_land = goal.goal
i = 0
for i in xrange(0, 4):
if self._as.is_preempt_requested():
rospy.loginfo('The goal has been cancelled/preempted')
self._as.set_preempted()
success = False
break
if takeoff_or_land == 'TAKEOFF':
self._pub_takeoff.publish(self._takeoff_msg)
self._feedback.feedback = 'Taking Off...'
self._as.publish_feedback(self._feedback)
if takeoff_or_land == 'LAND':
self._pub_land.publish(self._land_msg)
self._feedback.feedback = 'Landing...'
self._as.publish_feedback(self._feedback)
r.sleep()
if success:
self._result = Empty()
self._as.set_succeeded(self._result)
class Actions_quiz(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()
def goal_callback(self, goal):
# this callback is called when the action server is called.
# this is the function that computes the Fibonacci sequence
# and returns the sequence to the node that called the action server
# helper variables
success = True
_goal = goal.goal
success = True
r = rospy.Rate(1)
launch = rospy.Publisher('/drone/takeoff', Empty, queue_size=1)
land = rospy.Publisher('/drone/land', Empty, queue_size=1)
rospy.loginfo('Instruction : %s' % _goal)
time.sleep(1)
# check that preempt (cancelation) has not been requested by the action client
if self._as.is_preempt_requested():
rospy.loginfo('The goal has been cancelled/preempted')
# the following line, sets the client in preempted state (goal cancelled)
self._as.set_preempted()
success = False
if (_goal == "TAKEOFF"):
launch.publish(Empty())
self._feedback.feedback = "TAKEOFF"
elif (_goal == "LAND"):
land.publish(Empty())
self._feedback.feedback = "LAND"
self._as.publish_feedback(self._feedback)
# at this point, either the goal has been achieved (success==true)
# or the client preempted the goal (success==false)
# If success, then we publish the final result
# If not success, we do not publish anything in the result
#if success:
# rospy.loginfo('Successfully done command : %s' % goal.goal )
self._as.set_succeeded(self._result)
class CustomActionMsgClass(object):
# Message assignment
feedback = CustomActionMsgFeedback()
result = CustomActionMsgResult()
def __init__(self):
# Constructor for action server
self.action_server = actionlib.SimpleActionServer("action_custom_msg_as", CustomActionMsgAction, self.goal_callback, False)
self.action_server.start()
def goal_callback(self, goal):
# Publishers Assignment
self.takeoff_pub = rospy.Publisher('/drone/takeoff', Empty, queue_size=1)
self.land_pub = rospy.Publisher('/drone/land', Empty, queue_size=1)
goal_word = goal.goal
r = rospy.Rate(1)
success = True
for i in range(4):
# Check for preempt and set if required
if self.action_server.is_preempt_requested():
rospy.loginfo('The goal has been preempted')
self.action_server.set_preempted()
success = False
break
# Check for TAKEOFF goal word
if goal_word == 'TAKEOFF':
self.takeoff_pub.publish(Empty())
self.feedback.feedback = 'Taking Off...'
rospy.loginfo('Taking Off...')
self.action_server.publish_feedback(self.feedback)
# Check for LAND goal word
if goal_word == 'LAND':
self.land_pub.publish(Empty())
self.feedback.feedback = 'Landing...'
rospy.loginfo('Landing...')
self.action_server.publish_feedback(self.feedback)
# Send feedback once per second
r.sleep()
# Returning Empty result
if success == True:
self.result = Empty()
self.action_server.set_succeeded(self.result)
def goal_callback(self, _goal):
feedback = CustomActionMsgFeedback()
feedback.feedback = _goal.goal
if _goal.goal == 'TAKEOFF ':
activity = self.takeoff_drone
elif _goal.goal == 'LAND':
activity = self.land_drone
else:
assert 0
action = self._as_
rate = rospy.Rate(1)
activity()
while not rospy.is_shutdown():
action.publish_feedback(feedback)
rate.sleep()
result = CustomActionMsgResult()
action.set_succeeded(result)
class CustomActionMsgClass(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()
def goal_callback(self, goal):
# this callback is called when the action server is called.
# helper variables
success = True
r = rospy.Rate(1)
# 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()
# Get the goal word: UP or DOWN
takeoff_or_land = goal.goal
i = 0
for i in xrange(0, 4):
# check that preempt (cancelation) has not been requested by the action client
if self._as.is_preempt_requested():
rospy.loginfo('The goal has been cancelled/preempted')
# the following line, sets the client in preempted state (goal cancelled)
self._as.set_preempted()
success = False
break
# Logic that makes the robot move UP or DOWN
if takeoff_or_land == 'TAKEOFF':
self._pub_takeoff.publish(self._takeoff_msg)
self._feedback.feedback = 'Taking Off...'
self._as.publish_feedback(self._feedback)
if takeoff_or_land == 'LAND':
self._pub_land.publish(self._land_msg)
self._feedback.feedback = 'Landing...'
self._as.publish_feedback(self._feedback)
# the sequence is computed at 1 Hz frequency
r.sleep()
# at this point, either the goal has been achieved (success==true)
# or the client preempted the goal (success==false)
# If success, then we publish the final result
# If not success, we do not publish anything in the result
if success:
self._result = Empty()
self._as.set_succeeded(self._result)
class MoveSquareClass(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 publish_once_in_cmd_vel(self, cmd):
"""
This is because publishing in topics sometimes fails teh first time you publish.
In continuos publishing systems there is no big deal but in systems that publish only
once it IS very important.
"""
while not self.ctrl_c:
connections = self._pub_cmd_vel.get_num_connections()
if connections > 0:
self._pub_cmd_vel.publish(cmd)
rospy.loginfo("Publish in cmd_vel...")
break
else:
self.rate.sleep()
def take_off(self):
# make the drone takeoff
i = 0
while not i == 5:
self._pub_takeoff.publish(self._takeoff_msg)
rospy.loginfo('Taking off...')
time.sleep(1)
i += 1
rospy.loginfo('Finished Takeoff')
self._as.set_succeeded(self._result)
def land(self):
# make the drone takeoff
i = 0
while not i == 5:
self._pub_land.publish(self._land_msg)
rospy.loginfo('Landing...')
time.sleep(1)
i += 1
rospy.loginfo('Finished Landing')
self._as.set_succeeded(self._result)
def goal_callback(self, goal):
# this callback is called when the action server is called.
# this is the function that computes the Fibonacci sequence
# and returns the sequence to the node that called the action server
# helper variables
r = rospy.Rate(1)
success = True
# define the different publishers and messages that will be used
self._pub_cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
self._move_msg = Twist()
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()
# get the command from goal object
command = goal.goal
# check that preempt (cancelation) has not been requested by the action client
if self._as.is_preempt_requested():
rospy.loginfo('The goal has been cancelled/preempted')
# the following line, sets the client in preempted state (goal cancelled)
self._as.set_preempted()
success = False
elif command == "TAKEOFF":
self.take_off()
self._feedback.feedback = "TAKEOFF"
elif command == "LAND":
self.land()
self._feedback.feedback = "LAND"
# build and publish the feedback message
self._as.publish_feedback(self._feedback)
# the sequence is computed at 1 Hz frequency
r.sleep()
# at this point, either the goal has been achieved (success==true)
# or the client preempted the goal (success==false)
# If success, then we publish the final result
# If not success, we do not publish anything in the result
if success:
rospy.loginfo('Finished the mission, you may type another command')