125 lines
4.1 KiB
Python
125 lines
4.1 KiB
Python
|
#!/usr/bin/env python
|
||
|
|
|
||
|
|
# -*- coding: utf-8 -*-
|
||
|
|
"""
|
||
|
|
Created on Mon Feb 10 12:10:34 2020
|
||
|
|
|
||
|
|
@author: Maxi
|
||
|
|
"""
|
||
|
|
|
||
|
|
import re
|
||
|
|
import roslib
|
||
|
|
import rospy
|
||
|
|
import sys
|
||
|
|
import time
|
||
|
|
|
||
|
|
import std_msgs.msg as msg
|
||
|
|
from sensor_msgs.msg import JointState
|
||
|
|
|
||
|
|
import pigpio
|
||
|
|
import Pin
|
||
|
|
|
||
|
|
# Header header
|
||
|
|
# uint seq
|
||
|
|
# time stamp
|
||
|
|
# string frame_id
|
||
|
|
# string name
|
||
|
|
# float position
|
||
|
|
# float velocity
|
||
|
|
# float effort
|
||
|
|
|
||
|
|
class Commander:
|
||
|
|
def __init__(self):
|
||
|
|
self.test = False
|
||
|
|
""" constants """
|
||
|
|
self.rotate_old = self.rotate = 0 # rad
|
||
|
|
self.elevate_old = self.elevate = 0 # rad
|
||
|
|
self.servo_old = self.servo = 0 # rad
|
||
|
|
|
||
|
|
""" status """
|
||
|
|
self.rotate_enable = False
|
||
|
|
self.elevate_enable = True
|
||
|
|
self.servo_enable = False
|
||
|
|
|
||
|
|
""" setup ROS """
|
||
|
|
rospy.init_node('Commander')
|
||
|
|
self.pub = rospy.Publisher('command',
|
||
|
|
msg.String,
|
||
|
|
queue_size=10)
|
||
|
|
self.pub_gs = rospy.Publisher('groundstation_monitoring',
|
||
|
|
JointState,
|
||
|
|
queue_size=10)
|
||
|
|
|
||
|
|
self.listener_monitor = rospy.Subscriber('monitor',
|
||
|
|
msg.String,
|
||
|
|
self.callback)
|
||
|
|
self.listener_interrupt = rospy.Subscriber('interrupt',
|
||
|
|
msg.String,
|
||
|
|
self.callback)
|
||
|
|
self.ground_control = rospy.Subscriber('groundstation_command',
|
||
|
|
JointState,
|
||
|
|
self.callback_cmd)
|
||
|
|
|
||
|
|
""" keep node running """
|
||
|
|
while not rospy.is_shutdown():
|
||
|
|
rospy.spin()
|
||
|
|
|
||
|
|
def callback_cmd(self, cmd):
|
||
|
|
self.old_cmd()
|
||
|
|
rospy.loginfo(rospy.get_caller_id() + 'I heard the command \n\t Sequence: %s \n\t FrameID: %s \n\t Joint: %s \n\t Position: %s \n\t Velocity: %s \n\t Effort: %s',
|
||
|
|
cmd.header.seq, cmd.header.frame_id, cmd.name, cmd.position, cmd.velocity, cmd.effort)
|
||
|
|
# self.pub_gs.publish(cmd)
|
||
|
|
self.rotate = cmd.position[0]
|
||
|
|
self.elevate = cmd.position[1]
|
||
|
|
self.servo = cmd.position[2]
|
||
|
|
self.convert_command()
|
||
|
|
|
||
|
|
def old_cmd(self):
|
||
|
|
self.rotate_old = self.rotate
|
||
|
|
self.elevate_old = self.elevate
|
||
|
|
self.servo_old = self.servo
|
||
|
|
|
||
|
|
def convert_command(self):
|
||
|
|
if not self.rotate == self.rotate_old and self.rotate_enable:
|
||
|
|
self.rotate = int(2*3.1415*float(self.rotate)) # rad to deg
|
||
|
|
self.pub.publish(msg.String('ROTATE ' + str(self.rotate)))
|
||
|
|
|
||
|
|
if not self.elevate == self.elevate_old:
|
||
|
|
if self.elevate < -1.4 and self.elevate_enable:
|
||
|
|
self.pub.publish(msg.String('STOW'))
|
||
|
|
elif self.elevate >= -1.4 and self.elevate <= 0.4 and self.elevate_enable:
|
||
|
|
self.pub.publish(msg.String('ELEVATE'))
|
||
|
|
elif self.elevate > 0.4:
|
||
|
|
self.pub.publish(msg.String('TILT'))
|
||
|
|
|
||
|
|
if not self.servo == self.servo_old and self.servo_enable:
|
||
|
|
self.servo = int(2*3.1415*float(self.camera)) # rad to deg
|
||
|
|
self.pub.publish(msg.String('SERVO ' + str(self.servo)))
|
||
|
|
|
||
|
|
def callback(self, data):
|
||
|
|
rospy.loginfo(rospy.get_caller_id() + ' I heard %s',
|
||
|
|
data.data)
|
||
|
|
if 'ELEVATE' in data.data:
|
||
|
|
if 'ENABLE' in data.data:
|
||
|
|
self.elevate_enable = True
|
||
|
|
else:
|
||
|
|
self.elevate_enable = False
|
||
|
|
|
||
|
|
if 'ROTATE' in data.data:
|
||
|
|
if 'ENABLE' in data.data:
|
||
|
|
self.rotate_enable = True
|
||
|
|
else:
|
||
|
|
self.rotate_enable = False
|
||
|
|
|
||
|
|
if 'SERVO' in data.data:
|
||
|
|
if 'ENABLE' in data.data:
|
||
|
|
self.servo_enable = True
|
||
|
|
else:
|
||
|
|
self.servo_enable = False
|
||
|
|
|
||
|
|
if __name__ == '__main__':
|
||
|
|
try:
|
||
|
|
Commander()
|
||
|
|
except rospy.ROSInterruptException:
|
||
|
|
pass
|