#! /usr/bin/env python
import rospy
import actionlib
from geometry_msgs.msg import Twist
from actionlib.msg import TestFeedback , TestAction , TestResult
from std_msgs.msg import Empty
class movesquare(object):
_feedback = TestFeedback()
_result = TestResult()
def __init__(self):
self.takeoffpub = rospy.Publisher('/drone/takeoff',Empty,queue_size=1)
self.takemsg = Empty()
self.landpub = rospy.Publisher('/drone/land',Empty,queue_size=1)
self.landmsg = Empty()
self.ctr_c = False
self.actionserv = actionlib.SimpleActionServer('move_drone_in_square',TestAction,self.feedback_callback,False)
self.actionserv.start()
self.movepub = rospy.Publisher('/cmd_vel',Twist,queue_size = 1)
self.cmd = Twist()
self.rate = rospy.Rate(10)
rospy.on_shutdown(self.hook)
def hook(self):
self.ctr_c=True
self.stop_drone()
rospy.loginfo('turning off')
def publish_once_cmd(self,mov):
while not self.ctr_c:
numcon = self.movepub.get_num_connections()
if numcon > 0:
self.movepub.publish(mov)
else:
self.rate.sleep()
def stop_drone(self):
self.cmd.linear.x =0
self.cmd.angular.z=0
self.publish_once_cmd(self.cmd)
def turn_drone(self):
self.cmd.linear.x =0
self.cmd.angular.z=1
self.publish_once_cmd(self.cmd)
rospy.loginfo('turn')
def forward_drone(self):
self.cmd.linear.x =1
self.cmd.angular.z=0
self.publish_once_cmd(self.cmd)
rospy.loginfo('forward')
def feedback_callback(self,goal):
success = True
rate=rospy.Rate(1)
i=0
while i<3:
self.takeoffpub.publish(self.takemsg)
i=i+1
rate.sleep()
dura = goal.goal
turndura =1.8
i=0
for i in xrange(0,4):
if self.actionserv.is_preempt_requested():
self.actionserv.set_preempted()
success = False
break
self.forward_drone()
rospy.sleep(dura)
self.turn_drone()
rospy.sleep(turndura)
self._feedback.feedback = i
self.actionserv.publish_feedback(self._feedback)
rate.sleep()
i=0
if success:
self._result.result = 4*(dura+turn_dura)
self.actionserv.set_succeeded(self._result)
while i<3:
self.landpub.publish(self.landmsg)
i=i+1
rate.sleep()
if __name__=='__main__':
rospy.init_node('dronesquarenode')
movesquare()
rospy.spin()
These are 3 problems i am facing
->The rospy.on_shutdown() is not calling hook function which has function to stop UAV , even after pressing ctr_c the UAV keeps moving
->despite adding rospy.loginfo() the logs are not showing during execution , however all logs are simultaneously showing once i press ctr_c
->The program seems to be stuck as self.forward_drone() function in feedback_callback function.