Hi experts.
Could some friend please help me in making my code works? It is not working…I have seen the solution and there is some way of thinking similar. But I really would like to use the structure of my code and not just apply the solution. Is it possible to “repair” this code in order to work without change the structure a lot? If yes, please, how can I perform that?
The code is below:
#! /usr/bin/env python
import rospy
import time
import actionlib
from ardrone_as.msg import ArdroneAction, ArdroneGoal, ArdroneResult, ArdroneFeedback
from geometry_msgs.msg import Twist
from std_msgs.msg import Empty
# We create some constants with the corresponding values from the SimpleGoalState class
PENDING = 0
ACTIVE = 1
DONE = 2
WARN = 3
ERROR = 4
pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
#fly = rospy.Publisher('/drone/takeoff', Empty, queue_size=1)
#land = rospy.Publisher('/drone/land', Empty, queue_size=1)
def drone_up():
move_uav_up = Twist()
global time
time = 0
while time <= 5:
move_uav_up.linear.z = 0.15
time +=1
move_uav_up.linear.z = 0
global pub
pub.publish(move_uav_up)
def drone_down():
move_uav_down = Twist()
global end
end = 0
while end < (goal.nseconds-5):
rate.sleep()
end +=1
move_uav_down.linear.z = -0.10
pub.publish(move_uav_down)
def stop_drone():
stop_uav = Twist()
if time > end:
stop_uav.linear.x = 0.0
stop_uav.linear.y = 0.0
stop_uav.linear.z = 0.0
pub.publish(stop_uav)
def move_drone():
move_uav = Twist()
while time > 5 and time <= end:
move_uav.linear.x = 0.2
move_uav.linear.y = 0.2
move_uav.linear.z = 0.2
pub.publish(move_uav)
def initialize()
drone_up()
move_drone()
def finish()
drone_down()
stop_drone()
nImage = 1
# definition of the feedback callback. This will be called when feedback
# is received from the action server
# it just prints a message indicating a new message has been received
def feedback_callback(feedback):
global nImage
print('[Feedback] image n.%d received'%nImage)
nImage += 1
# initializes the action client node
rospy.init_node('drone_action_client')
# create the connection to the action server
Action_server_topic_name = '/ardrone_action_server'
client = actionlib.SimpleActionClient('/ardrone_action_server', ArdroneAction)
# waits until the action server is up and running
rospy.loginfo('Waiting for action Server' +Action_server_topic_name)
client.wait_for_server()
rospy.loginfo('Action Server Found...'+Action_server_topic_name)
# creates a goal to send to the action server
goal = ArdroneGoal()
goal.nseconds = 20 # indicates, take pictures along 20 seconds
# sends the goal to the action server, specifying which feedback function
# to call when feedback received
client.send_goal(goal, feedback_cb=feedback_callback)
rate = rospy.Rate(1)
# Uncomment these lines to test goal preemption:
#time.sleep(3.0)
#client.cancel_goal() # would cancel the goal 3 seconds after starting
# wait until the result is obtained
# you can do other stuff here instead of waiting
# and check for status from time to time
status = client.get_state()
# check the client API link below for more info
rospy.loginfo("Let's move the UAV while it finishes taking pictures task")
#client.wait_for_result()
while status < DONE:
rospy.loginfo("Moving the UAV around while waiting the server finishes taking pictures")
#drone_up()
#move_drone()
initialize()
rate.sleep()
status = client.get_state() # this line must be repeated here to update the status of the task
rospy.loginfo("state_result: " +str(status))
#stop_drone()
#drone_down()
finish()
if status == ERROR:
rospy.logerr("Something is wrong in the server ")
if status == WARN:
rospy.logwarn("There is a warning in the Server, Pay Attention!")
#print('[Result] State: %d'%(client.get_state()))
Thanks in advance
