Skip to content

Instantly share code, notes, and snippets.

@RDaneelOlivav
Created January 22, 2020 17:54
Show Gist options
  • Save RDaneelOlivav/b0a040b32f8d2b8717117f0f45f94f2c to your computer and use it in GitHub Desktop.
Save RDaneelOlivav/b0a040b32f8d2b8717117f0f45f94f2c to your computer and use it in GitHub Desktop.
#! /usr/bin/env python
import rospy
import time
import actionlib
from my_custom_action_msg_pkg.msg import CustomActionMsgFeedback, CustomActionMsgResult, CustomActionMsgAction
from std_msgs.msg import Empty
class MoveUpDownClass(object):
def __init__(self):
rospy.loginfo("Start Class")
# creates the action server
self._feedback = CustomActionMsgFeedback()
self._result = CustomActionMsgResult()
self._as = actionlib.SimpleActionServer("/action_custom_msg_as", CustomActionMsgAction, self.goal_callback, False)
self._as.start()
self.ctrl_c = False
self.rate = rospy.Rate(10)
rospy.loginfo("Start Class---END")
def publish_once_in_cmd_vel(self, cmd):
"""
This is because publishing in topics sometimes fails teh first time you publish.
In continuos publishing systems there is no big deal but in systems that publish only
once it IS very important.
"""
while not self.ctrl_c:
connections = self._pub_cmd_vel.get_num_connections()
if connections > 0:
self._pub_cmd_vel.publish(cmd)
rospy.loginfo("Publish in cmd_vel")
break
else:
self.rate.sleep()
def up_drone(self):
i = 0
while not i == 3:
self._pub_takeoff.publish(self._takeoff_msg)
rospy.loginfo("Taking off")
time.sleep(1)
i += 1
def down_drone(self):
i = 0
while not i == 3:
self._pub_land.publish(self._land_msg)
rospy.loginfo("Landing")
time.sleep(1)
i += 1
def goal_callback(self, goal):
self._pub_takeoff = rospy.Publisher("/drone/takeoff", Empty, queue_size=1)
self._takeoff_msg = Empty()
self._pub_land = rospy.Publisher("/drone/land", Empty, queue_size=1)
self._land_msg = Empty()
word = goal.goal
if word == "TAKEOFF":
self.up_drone()
rospy.loginfo("ca va monter")
if word == "LAND":
self.down_drone()
rospy.loginfo('ca va descendre')
if __name__ == "__main__":
rospy.init_node("move_drone_up_down", log_level=rospy.DEBUG)
MoveUpDownClass()
rospy.spin()
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment