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