def Signal(self, data): if len(self.signal): print "It's still sending data" else: if self.state == 'Flying' or self.state == 'Hovering': self.ControlOff() self.signal = SignalResponse( tf = data.time, dt = data.dt, f = data.f, signal = data.signal, direction = data.direction ) self.timer['signal_timer'] = rospy.Timer( rospy.Duration(self.signal.dt), self.CmdSignal, oneshot = False ) return []
class ROS_Handler(Quadrotor, ROS_Object, object): """docstring for ROS_Handler: Subscribed to: ardrone/navdata -> reads state of the ardrone Publishes to: ardrone/land ardrone/takeoff ardrone/reset ardrone/controller/state -> communicates to the controller its state ardrone/trajectory -> Prints in an Odometry msg the desired trajectory It is called by a Timer trajectory data is taken from a self.quadrotor object instance """ def __init__(self, **kwargs): super(ROS_Handler, self).__init__() # self.quadrotor = Quadrotor() self.Ts = kwargs.get('Command_Time', IMU_Period) self.name = kwargs.get('name', "/goal") self.signal = [] self.publisher.update( land = rospy.Publisher('/ardrone/land', Empty, latch = True), takeoff = rospy.Publisher('/ardrone/takeoff', Empty, latch = True), reset = rospy.Publisher('/ardrone/reset', Empty, latch = True), controller_state = rospy.Publisher('/ardrone/controller/state', KeyValue, latch = True), cmd_vel = rospy.Publisher('/cmd_vel', Twist), trajectory = rospy.Publisher('/ardrone/trajectory', Odometry) ) self.subscriber.update( ardrone_state = rospy.Subscriber('/ardrone/navdata', Navdata, callback = self.RecieveNavdata), sonar_height = rospy.Subscriber('/sonar_height', Range, callback = self.ReceiveSonarHeight ) ) self.services.update( signal_service = rospy.Service('/ardrone_control/Signal', Signal, self.Signal) ) self.tf_broadcaster.update( goal_tf = tf.TransformBroadcaster() ) self.timer.update( trajectory_timer = rospy.Timer(rospy.Duration( self.Ts ), self.Talk, oneshot=False), signal_timer = None ) def RecieveNavdata( self, data ): self.set_state(data.state) def ReceiveSonarHeight(self, data): """ if data.max_range > data.range: self.landFlag = False if data.range < safe_min: pass """ pass def Signal(self, data): if len(self.signal): print "It's still sending data" else: if self.state == 'Flying' or self.state == 'Hovering': self.ControlOff() self.signal = SignalResponse( tf = data.time, dt = data.dt, f = data.f, signal = data.signal, direction = data.direction ) self.timer['signal_timer'] = rospy.Timer( rospy.Duration(self.signal.dt), self.CmdSignal, oneshot = False ) return [] def CmdSignal(self, time): msg = Twist() if len(self.signal): if self.signal.direction == 'yaw': msg.angular.z = self.signal.command() #set angular velocity else: setattr( msg.linear, self.signal.direction, self.signal.command() ) #set linear velocity else: print "Done sending Signal" self.SignalOff() self.publisher['cmd_vel'].publish( msg ) #always publish so last command makes it stop def SignalOff(self): try: self.timer['signal_timer'].shutdown( ) self.signal = [ ] except AttributeError: pass msg = Twist() self.publisher['cmd_vel'].publish( msg ) def Talk( self, time ): """ Publishes the Goal Point for the Controller """ msgs = Odometry( ) msgs.header.stamp = rospy.Time.now() msgs.header.frame_id = "/nav" msgs.child_frame_id = "/goal" # position msgs.pose.pose.position.x = self.position.x msgs.pose.pose.position.y = self.position.y msgs.pose.pose.position.z = self.position.z # orientation msgs.pose.pose.orientation.x = self.orientation.x msgs.pose.pose.orientation.y = self.orientation.y msgs.pose.pose.orientation.z = self.orientation.z msgs.pose.pose.orientation.w = self.orientation.w # linear velocity msgs.twist.twist.linear.x = self.velocity.x msgs.twist.twist.linear.y = self.velocity.y msgs.twist.twist.linear.z = self.velocity.z # angular velocity msgs.twist.twist.angular.x = self.velocity.roll msgs.twist.twist.angular.y = self.velocity.pitch msgs.twist.twist.angular.z = self.velocity.yaw self.publisher['trajectory'].publish(msgs) self.tf_broadcaster['goal_tf'].sendTransform( (msgs.pose.pose.position.x, msgs.pose.pose.position.y, msgs.pose.pose.position.z) , (msgs.pose.pose.orientation.x, msgs.pose.pose.orientation.y, msgs.pose.pose.orientation.z, msgs.pose.pose.orientation.w), msgs.header.stamp, msgs.child_frame_id, msgs.header.frame_id) def TakeOff(self, *args): # Send a takeoff message to the ardrone driver # Note we only send a takeoff message if the drone is landed - an unexpected takeoff is not good! print self.state if(self.state == 'Landed'): self.publisher['takeoff'].publish() print "Taking Off!" else: print "Drone is not landed" def Reset(self, *args): self.ControlOff() self.SignalOff() self.publisher['reset'].publish() print "Reset" def Land(self, *args): self.ControlOff() self.SignalOff() self.publisher['land'].publish() print "Landing" def X(self, scale): self.change_set_point('x', scale) def Y(self, scale): self.change_set_point('y', scale) def Z(self, scale): self.change_set_point('z', scale) def Yaw(self, scale): self.change_set_point('yaw', scale) # Order is important: first change set point in yaw, then update quaternions. if self.position.yaw >= pi : self.position.yaw -= 2 * pi; if self.position.yaw < -pi: self.position.yaw += 2 * pi; quaternion = tf.transformations.quaternion_from_euler(self.position.yaw, self.position.pitch, self.position.roll, 'rzyx') self.orientation.x = quaternion[0] self.orientation.y = quaternion[1] self.orientation.z = quaternion[2] self.orientation.w = quaternion[3] def ControlOff(self, *args): self.change_controller_state('Off') def ControlOn(self, *args): if self.state == 'Flying' or self.state == 'Hovering': self.change_controller_state('On') def default_chirp_data(self, direction): data = Signal() data.time = 10 data.dt = 0.01 data.f = 20 data.signal = 'chirp' data.direction = direction return data def ChirpX(self, *args): self.Signal( self.default_chirp_data('x') ) def ChirpY(self, *args): self.Signal( self.default_chirp_data('y') ) def ChirpZ(self, *args): self.Signal( self.default_chirp_data('z') ) def ChirpYaw(self, *args): self.Signal( self.default_chirp_data('yaw') ) def change_set_point(self, direction, scale): # it changes an attribute from the self.position by a fraction of a Step. setattr(self.position, direction, getattr(self.position, direction) + scale * Step[direction] ) def change_controller_state(self, state_new_name): new_state = KeyValue() if state_new_name in ControllerState.MAP: new_state.key = state_new_name else: new_state.key = 'Unknown' new_state.value = str( ControllerState.MAP.index( new_state.key ) ) self.publisher['controller_state'].publish(new_state) print "Controller New State is:", new_state.key