def execute_cb(self, goal):
xyVel = Float32()
xyVel.data = 1
self.set_xy_vel.publish(xyVel)
goto_position_goal = goto_position_velGoal()
goto_position_goal.destination.pose.orientation.w = 0.707
goto_position_goal.destination.pose.orientation.z = 0.707
self.goto_position_client.wait_for_server()
#self.detect_object_client.wait_for_server()
self.destination = goal.destination
self.calculate_waypoints2()
idx = 0
while (self.detectedPos.pose.position.x == float("inf")):
goto_position_goal.destination.pose.position.x = self.waypoints.item(
(idx, 0))
goto_position_goal.destination.pose.position.y = self.waypoints.item(
(idx, 1))
goto_position_goal.destination.pose.position.z = self.waypoints.item(
(idx, 2))
self.position_publisher.publish(self.cur_pose)
self.goto_position_client.send_goal(goto_position_goal)
self.goto_position_client.wait_for_result()
idx = idx + 1
if idx == self.waypoints.shape[
0] or self.detectedPos.pose.position.x != float("inf"):
idx = 0
self.result.detected_object_pos = self.detectedPos
self.result.detected_object_pos.pose.position.z = self.searchHeight
self.action_server.set_succeeded(self.result)
#detect_object_goal = detect_objectGoal()
#self.detect_object_client.send_goal(detect_object_goal)
#self.detect_object_client.wait_for_result()
#self.detectedPos = self.detect_object_client.get_result().detected_position
time.sleep(0.1)
#roslib.load_manifest('autonomous_offboard')
import rospy
import actionlib
import mavros_state
import time
from autonomous_offboard.msg import center_on_objectAction, center_on_objectGoal, descend_on_objectAction, descend_on_objectGoal, detect_objectAction, detect_objectGoal,goto_position_velAction, goto_position_velGoal, goto_positionAction, goto_positionGoal, short_grippersAction, short_grippersGoal, long_grippersAction, long_grippersGoal, coverageAction, coverageGoal
from std_msgs.msg import Float32
from geometry_msgs.msg import PoseStamped
if __name__ == '__main__':
rospy.init_node('action_controller_takeoff_test')
goto_position_vel_client = actionlib.SimpleActionClient('goto_position_vel', goto_position_velAction)
goto_position_vel_client.wait_for_server()
goto_position_vel_goal = goto_position_velGoal()
mv_state = mavros_state.mavros_state()
print 'Setting offboard'
mv_state.set_mode('OFFBOARD')
print 'Arming vehicle'
print 'Takeoff'
mv_state.arm(True)
print 'Sleeping 20 sec'
time.sleep(20)
rospy.loginfo("change true height position")
goto_position_vel_goal.destination.pose.position.x = 0
goto_position_vel_goal.destination.pose.position.y = 5
goto_position_vel_goal.destination.pose.position.z = 5
goto_position_vel_client.send_goal(goto_position_vel_goal)
goto_position_vel_client.wait_for_result()