STEVE_Cammast/Steve/Scripts/Commander.py

125 lines
4.1 KiB
Python
Raw Normal View History

2021-04-08 13:04:58 +02:00
#!/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