From 166642207bc6a08fddb26586cbbd39acd1a6e20e Mon Sep 17 00:00:00 2001 From: pateman16 <patrickkarlsson16@gmail.com> Date: Mon, 23 Apr 2018 17:20:56 +0200 Subject: [PATCH] minor --- action/takeoff_curr.action | 8 +++++ src/launch/Untitled Document | 2 ++ src/scripts/takeoff_curr_server.py | 57 ++++++++++++++++++++++++++++++ 3 files changed, 67 insertions(+) create mode 100644 action/takeoff_curr.action create mode 100644 src/launch/Untitled Document create mode 100755 src/scripts/takeoff_curr_server.py diff --git a/action/takeoff_curr.action b/action/takeoff_curr.action new file mode 100644 index 0000000..cacbd29 --- /dev/null +++ b/action/takeoff_curr.action @@ -0,0 +1,8 @@ +#goal definition +geometry_msgs/PoseStamped destination +--- +#result definition +std_msgs/String data +--- +#feedback +geometry_msgs/PoseStamped current_position diff --git a/src/launch/Untitled Document b/src/launch/Untitled Document new file mode 100644 index 0000000..2623144 --- /dev/null +++ b/src/launch/Untitled Document @@ -0,0 +1,2 @@ + +2 meter = barro 0.2 diff --git a/src/scripts/takeoff_curr_server.py b/src/scripts/takeoff_curr_server.py new file mode 100755 index 0000000..c474f76 --- /dev/null +++ b/src/scripts/takeoff_curr_server.py @@ -0,0 +1,57 @@ +#!/usr/bin/env python +import rospy +import actionlib +import autonomous_offboard.msg +from std_msgs.msg import Bool, String + +from geometry_msgs.msg import PoseStamped +class takeoff_curr_server(): + def __init__(self): + + #variables + self.target_reached = False + #publishers + self.pose_control = rospy.Publisher('/position_control/set_position', PoseStamped, queue_size=10) + self.mode_control = rospy.Publisher('/position_control/set_mode', String, queue_size=10) + + #subscribers + rospy.Subscriber('/position_control/distance', Bool, self.distance_reached_cb) + self.local_pose = PoseStamped() + self.take_off_pose = PoseStamped() + rospy.Subscriber('/mavros/local_position/pose', PoseStamped, self._local_pose_callback) + + self.rate = rospy.Rate(20) + #self.result = simulation_control.msg.goto_positionResult() + self.action_server = actionlib.SimpleActionServer('takeoff_curr', autonomous_offboard.msg.takeoff_currAction, + execute_cb=self.execute_cb, auto_start=False) + self.action_server.start() + + + def _local_pose_callback(self, data): + self.local_pose = data + + def execute_cb(self, goal): + self.take_off_pose.pose.position.x = self.local_pose.position.pose.x + self.take_off_pose.pose.position.y = self.local_pose.position.pose.y + self.take_off_pose.pose.position.z = goal.destination.pose.position.z + self.mode_control.publish('posctr') + self.pose_control.publish(self.take_off_pose) + rospy.sleep(0.1) + print(self.target_reached) + while not self.target_reached: + self.rate.sleep() + + rospy.loginfo("Destination reached") + self.action_server.set_succeeded() + + + def distance_reached_cb(self, data): + self.target_reached = data.data + +if __name__ == '__main__': + try: + + rospy.init_node('takeoff_curr_server') + takeoff_curr_server() + except rospy.ROSInterruptException: + pass -- GitLab