readme extended

This commit is contained in:
winterhalderp 2021-04-08 13:04:58 +02:00
parent 4ebdee8f01
commit f3b3b9f358
54 changed files with 169206 additions and 0 deletions

View File

@ -33,3 +33,16 @@ Do not upload to the `master` branch but instead create your own branch. This pr
* `git commit -am "new branch created"`
* `git push origin <branch_name>`
* Continue working in your local and remote branch but do not push to `master`!
## Folders
* __documentation__ contains instruction material, text and pictures for documentation purposes
* __Steve__ contains the ROS package for running the Steve Camera Mast on a Raspberry Pi (3/4)
## Prerequisites
* Read about setting up the [CAN bus on Linux](https://developer.ridgerun.com/wiki/index.php/How_to_configure_and_use_CAN_bus) and performing a [loopback test](https://aptofun.de/CAN_Bus_Shield_MCP2515).
* Setup the Raspberry Pi OBC and CAN shield by following these [instructions](https://egit.irs.uni-stuttgart.de/DLR-Libs/ROS-CANopen/src/branch/master/documentation/instruction_material/rpi-shield-setup.md).
* Learn how to use ROS2 [here](https://egit.irs.uni-stuttgart.de/RoverLehre/ROS2_pubsub).
## How to use
% Describe how to use the package %
**LINK to separate document describing software**

209
Steve/CMakeLists.txt Normal file
View File

@ -0,0 +1,209 @@
cmake_minimum_required(VERSION 2.8.3)
project(Steve)
## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
#diagnostic_msgs
sensor_msgs
message_generation
dynamic_reconfigure
)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
################################################
## Declare ROS messages, services and actions ##
################################################
# To declare and build messages, services or actions from within this
# package, follow these steps:
# * Let MSG_DEP_SET be the set of packages whose message types you use in
# your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
# * In the file package.xml:
# * add a build_depend tag for "message_generation"
# * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
# * If MSG_DEP_SET isn't empty the following dependency has been pulled in
# but can be declared for certainty nonetheless:
# * add a exec_depend tag for "message_runtime"
# * In this file (CMakeLists.txt):
# * add "message_generation" and every package in MSG_DEP_SET to
# find_package(catkin REQUIRED COMPONENTS ...)
# * add "message_runtime" and every package in MSG_DEP_SET to
# catkin_package(CATKIN_DEPENDS ...)
# * uncomment the add_*_files sections below as needed
# and list every .msg/.srv/.action file to be processed
# * uncomment the generate_messages entry below
# * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
# Generate messages in the 'msg' folder
add_message_files(
FILES
node_example.msg
)
## Generate services in the 'srv' folder
# add_service_files(
# FILES
# Service1.srv
# Service2.srv
# )
## Generate actions in the 'action' folder
# add_action_files(
# FILES
# Action1.action
# Action2.action
# )
# Generate added messages and services with any dependencies listed here
generate_messages(
DEPENDENCIES
std_msgs
sensor_msgs
)
###############################################
# Declare ROS dynamic reconfigure parameters ##
###############################################
# To declare and build dynamic reconfigure parameters within this
# package, follow these steps:
# * In the file package.xml:
# * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
# * In this file (CMakeLists.txt):
# * add "dynamic_reconfigure" to
# find_package(catkin REQUIRED COMPONENTS ...)
# * uncomment the "generate_dynamic_reconfigure_options" section below
# and list every .cfg file to be processed
# Generate dynamic reconfigure parameters in the 'cfg' folder
generate_dynamic_reconfigure_options(
cfg/node_example.cfg
)
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if your package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES Steve
CATKIN_DEPENDS dynamic_reconfigure message_runtime rospy roscpp std_msgs sensor_msgs
# DEPENDS system_lib
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
# include
${catkin_INCLUDE_DIRS}
)
## Declare a C++ library
# add_library(${PROJECT_NAME}
# src/${PROJECT_NAME}/Steve.cpp
# )
## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
# add_executable(${PROJECT_NAME}_node src/Steve_node.cpp)
## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
# Add cmake target dependencies of the executable
# same as for the library above
# add_dependencies(Steve ${PROJECT_NAME}_gencfg) # TODO error here
## Specify libraries to link a library or executable target against
# target_link_libraries(${PROJECT_NAME}_node
# ${catkin_LIBRARIES}
# )
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# install(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
# install(TARGETS ${PROJECT_NAME}_node
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark libraries for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
# install(TARGETS ${PROJECT_NAME}
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
# )
## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
# FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE
# )
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
# # myfile1
# # myfile2
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_Steve.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)

124
Steve/Scripts/Commander.py Normal file
View File

@ -0,0 +1,124 @@
#!/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

268
Steve/Scripts/Elevation.py Normal file
View File

@ -0,0 +1,268 @@
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
Created on Mon Feb 10 11:22:52 2020
@author: Maxi
"""
import roslib
import rospy
import sys
import time
import std_msgs.msg as msg
import pigpio
import RPi.GPIO as GPIO
import Pin
class Elevation:
def __init__(self):
self.test = False
self.direction = None
self.destination = None
self.state = 'stowed'
self.angle = 0.0 # deg
self.speed = 0.05 # deg per second
self.instances = 0
""" setup ROS """
rospy.init_node('Elevation')
self.pub = rospy.Publisher('elevation',
msg.String,
queue_size=2)
self.listener_test = rospy.Subscriber('tests',
msg.String,
self.callback)
self.listener_cmd = rospy.Subscriber('command',
msg.String,
self.callback)
self.listener_stow = rospy.Subscriber('stowed_switch',
msg.String,
self.callback,
callback_args='stow')
self.listener_raise = rospy.Subscriber('raised_switch',
msg.String,
self.callback,
callback_args='raise')
self.listener_tilt = rospy.Subscriber('tilt_switch',
msg.String,
self.callback,
callback_args='tilt')
""" setup GPIO """
self.frequency = 500 # Hz
self.dutycycle = 0 # %
self.pi = pigpio.pi()
self.pi.set_mode(Pin.M_Elev_dir, pigpio.OUTPUT)
self.pi.set_mode(Pin.M_Elev_speed, pigpio.OUTPUT)
self.pi.set_PWM_dutycycle(Pin.M_Elev_speed, 0)
self.pi.set_PWM_frequency(Pin.M_Elev_speed, self.frequency)
# GPIO.setup(Pin.M_Elev, GPIO.OUT)
# self.pwm = GPIO.PWM(Pin.M_Cam, self.frequency)
# self.pwm.start(self.dutycycle)
""" keep node running """
while not rospy.is_shutdown():
rospy.spin()
def callback(self, data, switch=None):
rospy.loginfo(rospy.get_caller_id() + ' I heard %s from %s',
data.data, switch)
tilt_cmd_recognized = 'TILT' in data.data # true,false
if data.data == 'STOP':
self.state = None
self.stop()
elif data.data == 'HIGH' and switch == 'raise':
self.state = 'raised'
self.angle = 0
if self.destination == 'up':
rospy.sleep(0.01)
self.stop()
elif self.destination == 'tilt': # Achtung! Tilt ist End stop!
self.tilt()
elif data.data == 'LOW' and switch == 'stow':
self.stop()
self.state = 'stowed'
self.angle = 90
if not self.direction == 'forwards': # einklappen?
rospy.sleep(0.3)
self.stop()
if self.destination == 'up':
self.elevate()
pass
elif data.data == 'HIGH' and switch == 'stow':
pass
#if self.destination == 'down' and not self.state == 'moving':
# self.retract()
elif data.data == 'HIGH' and switch == 'tilt': # Tilt ist Elevation end stop
if self.state == "stowed" or self.state == None or self.state == "raised":
self.stop()
else:
self.state = 'tilted' # tilted backwards, touching end stop
self.angle = -30
self.stop()
if not self.direction == 'backwards':
self.stop()
if self.destination == 'up':
if not self.state == "raised": #pw
self.elevate()
elif data.data == 'ELEVATE' and not self.state == 'raised':
self.destination = 'up'
self.state = "moving" # pw
self.elevate()
elif data.data == 'STOW' and not self.state == 'stowed':
self.destination = 'down'
self.state = "moving" # pw
self.retract()
"""
elif data.data == 'TILT' and not self.state == 'tilted':
self.destination = 'tilt'
self.state == "moving" #pw
self.tilt() #pw
elif data.data == 'TILT_10' and not self.state == 'tilted':
self.destination = 'tilt'
self.state == "moving" #pw
self.tilt_variabletime(10) #pw
elif data.data == 'TILT_15' and not self.state == 'tilted':
self.destination = 'tilt'
self.state == "moving" #pw
self.tilt_variabletime(15) #pw
elif data.data == 'TILT_18' and not self.state == 'tilted':
self.destination = 'tilt'
self.state == "moving" #pw
self.tilt_variabletime(18) #pw
"""
#elif tilt_cmd_recognized == True and not self.state == 'tilted':
elif 'TILT' in data.data and not self.state == 'tilted':
self.destination = 'tilt'
self.state == "moving" #pw
# Extract info from string
start_marker = data.data.find('TILT_') + 5 #pw
tilt_goal = data.data[start_marker:-1]
for i in range(0,5):
print(">> Tilt Goal set: " + str(tilt_goal))
tilt_goal = int(tilt_goal)
for i in range(0,5):
print(">> Tilt Goal set: " + str(tilt_goal))
if tilt_goal < 21:
self.tilt_variabletime(tilt_goal) #pw
else:
print(">> Tilt Goal out of bounds!")
rospy.loginfo('STATE: %s', self.state)
rospy.loginfo('DIRECTION: %s', self.destination)
self.pub.publish(msg.String('STATE: %s' % self.state))
if not self.state == 'moving':
self.pub.publish(msg.String('ANGLE: %s' % self.angle))
rospy.loginfo('ANGLE: %s', self.angle)
def elevate(self):
if self.state == 'tilted':
self.pi.write(Pin.M_Elev_dir, 1)
self.direction = 'backwards'
rospy.loginfo('moving backwards')
else:
self.pi.write(Pin.M_Elev_dir, 0)
self.direction = 'forwards'
rospy.loginfo('moving forwards')
self.move()
self.pub.publish(msg.String('raising mast'))
def retract(self):
self.pi.write(Pin.M_Elev_dir, 1)
self.direction = 'forwards'
self.pub.publish(msg.String('retracting mast'))
then = time.time()
self.move() # self.state = "moving" in dieser funktion!
maxtime = 25.3 # max Zeit fuer retract von raised position
now = time.time()
while (now-then)<maxtime:
now = time.time()
self.stop()
self.state = 'stowed'
def tilt(self):
self.pi.write(Pin.M_Elev_dir, 1) # 1: forwards, 0: backwards, siehe oben
self.direction = 'forwards'
self.pub.publish(msg.String('tilting mast'))
then = time.time()
self.move()
maxtime = 10.0 # Zeit bis in tilt position
now = time.time()
while (now-then)<maxtime:
now = time.time()
self.stop()
self.state = None # do not set als tilted, tilted means end stop
def tilt_variabletime(self, dt_movement=0):
self.pi.write(Pin.M_Elev_dir, 1) # 1: forwards, 0: backwards, siehe oben
self.direction = 'forwards'
self.pub.publish(msg.String('tilting mast'))
then = time.time()
self.move()
maxtime = int(dt_movement) # Zeit bis in tilt position
now = time.time()
while (now-then)<maxtime:
now = time.time()
self.stop()
self.state = None # do not set als tilted, tilted means end stop
def move(self):
# self.instances = self.instances + 1
self.state = 'moving'
self.pub.publish(msg.String('STATE: %s' % self.state))
self.dutycycle = 160
self.pi.set_PWM_frequency(Pin.M_Elev_speed, self.frequency)
self.pi.set_PWM_dutycycle(Pin.M_Elev_speed, self.dutycycle)
rospy.loginfo("Moving at Frequency: %s, Dutycycle %s, Direction %s", self.frequency, self.dutycycle, self.direction)
# then = time.time()
# while self.state == 'moving':
# now = time.time()
# delta_t = now - then
# if self.direction == 'backwards':
# self.angle = self.angle + self.speed * delta_t
# else:
# self.angle = self.angle - self.speed * delta_t
# rospy.loginfo('ANGLE: %s', self.angle)
# self.pub.publish(msg.String('ANGLE: %s' % self.angle))
# rospy.sleep(0.2)
# if not self.instances == 1:
# self.instances = self.instances - 1
# break
def stop(self):
self.dutycycle = 0
self.pi.set_PWM_dutycycle(Pin.M_Elev_speed, 0)
self.pub.publish(msg.String('was moving in direction: %s' % self.direction))
self.pub.publish(msg.String('Stopped Moving'))
rospy.loginfo('State: %s' % self.state)
if __name__ == '__main__':
try:
e = Elevation()
except rospy.ROSInterruptException:
try:
e.disable()
except Exception as E:
print(E)

View File

@ -0,0 +1,42 @@
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
Created on Mon Feb 10 17:09:57 2020
@author: Maxi
"""
import roslib
import rospy
import sys
import std_msgs.msg as msg
from dynamic_reconfigure.server import Server as DynamicReconfigureServer
# Import custom message data and dynamic reconfigure variables.
from Steve.msg import node_example
from Steve.cfg import node_example_paramsConfig as ConfigType
import RPi.GPIO as GPIO
import Pin
from Switch import Switch
class ForwardSwitch(Switch):
def __init__(self):
self.pin = Pin.limit_forward
self.pin_out = Pin.limit_forward_out
self.event_based = True
self.setup_pwr_high()
self.SwitchPub = rospy.Publisher('forward_switch', msg.String, queue_size=1)
Switch.__init__(self)
if __name__ == '__main__': # name == main when ros opens ***.py
# instantiate class
try:
forward_switch = ForwardSwitch()
except rospy.ROSInterruptException:
pass

View File

@ -0,0 +1,42 @@
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
Created on Mon Feb 10 17:09:57 2020
@author: Maxi
"""
import roslib
import rospy
import sys
import std_msgs.msg as msg
from dynamic_reconfigure.server import Server as DynamicReconfigureServer
# Import custom message data and dynamic reconfigure variables.
from Steve.msg import node_example
from Steve.cfg import node_example_paramsConfig as ConfigType
import RPi.GPIO as GPIO
import Pin
from Switch import Switch
class TiltSwitch(Switch):
def __init__(self):
self.pin = Pin.limit_tilt
self.pin_out = Pin.limit_tilt_out
self.event_based = True
self.setup_gnd_high()
self.SwitchPub = rospy.Publisher('tilt_switch', msg.String, queue_size=1)
Switch.__init__(self)
if __name__ == '__main__': # name == main when ros opens ***.py
# instantiate class
try:
tilt_switch = TiltSwitch()
except rospy.ROSInterruptException:
pass

View File

@ -0,0 +1,174 @@
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
Created on Mon Feb 10 17:09:57 2020
@author: Maxi
"""
import re
import roslib
import rospy
import sys
import std_msgs.msg as msg
from sensor_msgs.msg import JointState
from dynamic_reconfigure.server import Server as DynamicReconfigureServer
# Import custom message data and dynamic reconfigure variables.
from Steve.msg import node_example
from Steve.cfg import node_example_paramsConfig as ConfigType
class Monitor:
def __init__(self):
self.statusOK = True
self.counter = 0
""" status """
self.rotation = 0
self.elevation = 'stowed'
self.elev_angle = 90 # deg
self.servo = 0
""" ROS setup """
rospy.init_node('Monitor')
self.pub = rospy.Publisher('monitor',
msg.String,
queue_size=5)
self.InterruptPub = rospy.Publisher('interrupt',
msg.String,
queue_size=3)
self.pub_gs_monitor = rospy.Publisher('groundstation_monitoring',
JointState,
queue_size=1)
self.listener_gs_cmd = rospy.Subscriber('groundstation_command',
JointState,
callback=self.callback_cmd)
self.listener_gs_monitor = rospy.Subscriber('groundstation_monitoring',
JointState,
callback=self.callback_mon)
self.listener_ch = rospy.Subscriber('chatter',
node_example,
callback=self.callback_custom_msg)
self.listener_st = rospy.Subscriber('stowed_switch',
msg.String,
callback=self.callback_generic,
callback_args='stowed_switch')
self.listener_ra = rospy.Subscriber('raised_switch',
msg.String,
callback=self.callback_generic,
callback_args='raised_switch')
self.listener_lim = rospy.Subscriber('limit_switch',
msg.String,
callback=self.callback_generic,
callback_args='limit_switch')
self.listener_test = rospy.Subscriber('tests',
msg.String,
callback=self.callback_generic,
callback_args='tests')
self.listener_int = rospy.Subscriber('interrupt',
msg.String,
callback=self.callback_generic,
callback_args='interrupt')
self.listener_cmd = rospy.Subscriber('command',
msg.String,
callback=self.callback_generic,
callback_args='command')
self.listener_forw = rospy.Subscriber('forward_switch',
msg.String,
callback=self.callback_generic,
callback_args='forward_switch')
self.listener_rot = rospy.Subscriber('rotation',
msg.String,
callback=self.callback_generic,
callback_args='rotation')
self.listener_elev = rospy.Subscriber('elevation',
msg.String,
callback=self.callback_generic,
callback_args='elevation')
self.listener_servo = rospy.Subscriber('servo',
msg.String,
callback=self.callback_generic,
callback_args='servo')
while not rospy.is_shutdown():
self.enable_activities()
rospy.spin()
# time.sleep(0.1)
def callback_generic(self, data, topic):
rospy.loginfo('%s: sent me %s', topic, data.data)
if topic == 'rotation':
self.rotation = float(data.data)
elif topic == 'elevation':
if 'STATE' in data.data:
self.elevation = re.sub('STATE: ', '', data.data)
rospy.loginfo(self.elevation)
elif 'ANGLE' in data.data:
self.elev_angle = int(re.findall('-*[0-9]+', data.data)[0])
rospy.loginfo(self.elev_angle)
elif topic == 'servo':
self.servo = float(data.data)
if self.counter > 10:
self.enable_activities()
self.send_status()
self.counter = 0
self.counter += 1
def callback_custom_msg(self, data):
rospy.loginfo(rospy.get_caller_id() + ' I heard %s',
data.message)
def callback_cmd(self, 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)
def callback_mon(self, cmd):
rospy.loginfo(rospy.get_caller_id() + 'Status was sent \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)
def enable_activities(self):
if abs(self.rotation) < 1.5:
self.InterruptPub.publish(msg.String('ELEVATE: ENABLE'))
else:
self.InterruptPub.publish(msg.String('ELEVATE: DISABLE'))
if self.elevation == 'raised':
self.InterruptPub.publish(msg.String('ROTATE: ENABLE'))
self.InterruptPub.publish(msg.String('SERVO: ENABLE'))
else:
self.InterruptPub.publish(msg.String('ROTATE: DISABLE'))
self.InterruptPub.publish(msg.String('SERVO: DISABLE'))
def send_status(self):
msg = JointState()
msg.name = ['J1_turntable', 'J2_fold', 'J3_nod']
msg.position = [self.deg_to_rad(self.rotation),
self.deg_to_rad(self.elev_angle),
self.deg_to_rad(self.servo)]
msg.velocity = []
msg.effort = []
self.pub_gs_monitor.publish(msg)
def deg_to_rad(self, degrees):
rad = degrees * 3.1415 / 180.0
return rad
def elev_to_rad(self, state):
if state == 'raised':
return 0.0
elif state == 'stowed':
return deg_to_rad(90)
elif state == 'tilted':
return deg_to_rad(-30)
else:
return
if __name__ == '__main__':
try:
monitor = Monitor()
except rospy.ROSInterruptException:
pass

38
Steve/Scripts/Pin.py Normal file
View File

@ -0,0 +1,38 @@
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
Created on Mon Feb 10 11:26:41 2020
@author: Maxi
"""
import RPi.GPIO as GPIO
# GPIO.setmode(GPIO.BOARD)
GPIO.setmode(GPIO.BCM)
"""
TODO
PWM1 und PWM0 beide freischalten
I2C aktivieren?
"""
# Pin Numbering a la GPIOxx (BCM)
# pwr33V = [1, 17]
# pwr5V = [2, 4]
# GND = [6, 9, 14, 20, 25, 30, 34, 39]
M_Elev_speed = sw_PWM = 4
M_Elev_dir = 14
M_Rot_speed = PWM1 = 18 # motor
M_Rot_dir = 15 # motor
M_Rot_enable = 24
M_Cam = PWM0 = 13 # motor
limit_stowed = 27
limit_stowed_out = 9
limit_raised = 17
limit_raised_out = 10
limit_forward = 22
limit_forward_out = 11
limit_tilt = 23
limit_tilt_out = 8
I2C = {'Data': 2, 'Clock': 3}

BIN
Steve/Scripts/Pin.pyc Normal file

Binary file not shown.

View File

@ -0,0 +1,42 @@
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
Created on Mon Feb 10 17:09:57 2020
@author: Maxi
"""
import roslib
import rospy
import sys
import std_msgs.msg as msg
from dynamic_reconfigure.server import Server as DynamicReconfigureServer
# Import custom message data and dynamic reconfigure variables.
from Steve.msg import node_example
from Steve.cfg import node_example_paramsConfig as ConfigType
import RPi.GPIO as GPIO
import Pin
from Switch import Switch
class RaisedSwitch(Switch):
def __init__(self):
self.pin = Pin.limit_raised
self.pin_out = Pin.limit_raised_out
self.event_based = True
self.setup_pwr_high()
self.SwitchPub = rospy.Publisher('raised_switch', msg.String, queue_size=1)
Switch.__init__(self)
if __name__ == '__main__': # name == main when ros opens ***.py
# instantiate class
try:
raised_switch = RaisedSwitch()
except rospy.ROSInterruptException:
pass

125
Steve/Scripts/Rotation.py Normal file
View File

@ -0,0 +1,125 @@
#!/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
import pigpio
import Pin
class Rotation:
def __init__(self):
""" setup ROS """
rospy.init_node('Rotation')
self.pub = rospy.Publisher('rotation',
msg.String,
queue_size=2)
self.listener_test = rospy.Subscriber('tests',
msg.String,
self.callback)
self.listener_switch = rospy.Subscriber('forward_switch',
msg.String,
self.calibrate_forward)
self.listener_cmd = rospy.Subscriber('command',
msg.String,
self.callback)
""" constants """
self.position = 0 # degrees
self.rot_speed = 0.081 # degrees per step
self.test = False
self.epsilon = 1 # degrees
""" setup GPIO """
self.frequency = 100 # Hz = steps per second
self.dutycycle = 0 # % - 50 % is good
self.pi = pigpio.pi()
self.pi.set_mode(Pin.M_Rot_enable, pigpio.OUTPUT) # high wenn aus, low wenn an
self.pi.set_mode(Pin.M_Rot_dir, pigpio.OUTPUT)
self.pi.set_mode(Pin.M_Rot_speed, pigpio.OUTPUT)
self.pi.hardware_PWM(Pin.M_Rot_speed, 0, 0) # frequency, dutycycle
while not rospy.is_shutdown():
self.pi.write(Pin.M_Rot_enable, 1) # high wenn aus
rospy.spin()
def callback(self, data):
rospy.loginfo(rospy.get_caller_id() + ' I heard %s',
data.data)
if data.data == 'STOP':
self.stop()
elif data.data == 'CENTER':
self.center_position()
elif 'ROTATE' in data.data:
try:
numbers = re.findall('-*[0-9]+', data.data)
self.go_to_degrees(int(numbers[0]))
except ValueError as E:
rospy.loginfo(E)
def calibrate_forward(self, data):
rospy.loginfo(' I heard %s', data.data)
if data.data == 'HIGH':
self.position = 0 # degrees
def center_position(self):
self.go_to_degrees(0)
def go_to_degrees(self, degrees):
start = time.time()
while not abs(self.position - degrees) <= self.epsilon:
then = time.time()
if self.position > degrees:
self.counterclockwise()
signum = -1
else:
self.clockwise()
signum = 1
self.rotate()
rospy.loginfo('rotating at %s degrees now' % self.position)
rospy.sleep(0.2)
now = time.time()
self.position = self.position + signum * self.frequency * self.rot_speed * (now - then)
self.pub.publish(str(self.position))
self.stop()
def counterclockwise(self):
self.pi.write(Pin.M_Rot_dir, 0)
def clockwise(self):
self.pi.write(Pin.M_Rot_dir, 1)
def rotate(self):
self.pi.write(Pin.M_Rot_enable, 0) # low wenn an
self.dutycycle = 50
self.pi.hardware_PWM(Pin.M_Rot_speed, self.frequency, self.dutycycle * 10000)
def stop(self):
self.pi.hardware_PWM(Pin.M_Rot_speed, 0, 0)
self.pi.write(Pin.M_Rot_enable, 1) # high wenn aus
def disable(self):
self.pwm.stop()
self.pi.write(Pin.M_Rot_enable, 1) # high wenn aus
if __name__ == '__main__':
try:
Rotation()
except rospy.ROSInterruptException:
pass
# finally:
# GPIO.cleanup()

162
Steve/Scripts/Servo.py Normal file
View File

@ -0,0 +1,162 @@
#!/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
import pigpio
import Pin
class Servo:
def __init__(self):
""" setup ROS """
rospy.init_node('Servo')
self.pub = rospy.Publisher('servo',
msg.String,
queue_size=10)
self.listener_test = rospy.Subscriber('tests',
msg.String,
self.callback)
self.listener_cmd = rospy.Subscriber('command',
msg.String,
self.callback)
""" constants """
# TODO ggf. konstanten tauschen
self.position = 0 # degrees
self.t_max = 1500E-6 # ms == 0 deg
self.t_min = 500E-6 # ms == 180 deg
self.alpha_max = 0 # deg
self.alpha_min = 90 # deg
""" setup GPIO """
self.frequency = 50.0 # Hz
self.dutycycle = 0 # %
self.pi = pigpio.pi()
#self.pi.set_mode(Pin.M_Cam, pigpio.OUTPUT) # eher ALT1=PWM1 fuer bcm13
#self.pi.set_mode(Pin.M_Cam, pigpio.ALT1)
#print(">> GPIO Mode: " + str(self.pi.get_mode(Pin.M_Cam)) + " (1: OUTPUT; 5: ALT1=PWM1)")
#self.pi.hardware_PWM(Pin.M_Cam, 0, 0)
#self.pi.set_PWM_dutycycle(Pin.M_Cam, 0)
#self.pi.set_PWM_frequency(Pin.M_Cam, self.frequency)
## Ersatz GPIO: BCM19
self.pi.set_mode(19, pigpio.ALT5)
self.pi.hardware_PWM(19, 400, 0)
# Test Servo Motion
# CAUTION: Make sure servo movement is possible, not on storage position
print(">> Servo Test ...")
test_servo = False
if test_servo == True:
self.test_servo_motion()
print(">> Test Done")
while not rospy.is_shutdown():
rospy.spin()
def callback(self, data):
rospy.loginfo(rospy.get_caller_id() + ' I heard %s',
data.data)
if data.data == 'STOP':
self.disable()
elif 'SERVO' in data.data:
try:
numbers = re.findall('[0-9]+', data.data)
angle = int(numbers[0])
rospy.loginfo('Angle %s', angle)
if angle >= 0 and angle <= 75:
self.go_to_degrees(int(angle))
else:
rospy.loginfo('angle %s not possible', angle)
except Exception as E:
rospy.loginfo(E)
rospy.loginfo('unrecognised message format %s', data.data)
def go_to_degrees(self, degrees):
self.step_calc(degrees)
if self.stepwidth <= 1500E-6 and self.stepwidth >=740E-6:
# Catching runtime errors
if self.dutycycle < 0:
self.dutycycle = 0
if self.dutycycle > 255:
self.dutycycle = 255
self.pi.hardware_PWM(19, 400, self.calc_dutycycle(degrees))
#self.pi.hardware_PWM(Pin.M_Cam, self.frequency, self.dutycycle)
#self.pi.set_PWM_dutycycle(Pin.M_Cam, self.dutycycle)
rospy.loginfo('rotating at %s degrees now', degrees)
else:
rospy.loginfo('stepwidth %s not possible', self.stepwidth)
self.pub.publish(str(degrees))
def disable(self):
self.dutycycle = 0
self.pi.hardware_PWM(19, 0, 0)
#self.pi.hardware_PWM(Pin.M_Cam, self.frequency, 0)
#self.pi.set_PWM_dutycycle(Pin.M_Cam, self.dutycycle)
def step_calc(self, angle):
# linearer Zusammenhang
self.stepwidth = ((self.t_max - self.t_min)/(self.alpha_max - self.alpha_min)) * (angle - self.alpha_min) + self.t_min
self.dutycycle = self.calculate_pwm(self.stepwidth)
def calculate_pwm(self, stepwidth):
time_per_tick = 1 / self.frequency
pwm_fraction = stepwidth / time_per_tick
pwm_percent = pwm_fraction * 100
#pwm_set = pwm_percent * 10000
pwm_set = pwm_percent
rospy.loginfo('stepwidth: %s sec\nticktime: %s sec\npwm_set: %s %%', stepwidth, time_per_tick, pwm_set)
return pwm_set
def test_servo_motion(self):
for i in range(0,65,5):
self.go_to_degrees(i)
rospy.sleep(3.0)
self.go_to_degrees(0)
def calc_dutycycle(angle):
frequency = 400
steps_max = 250000000/frequency
t_servo_max = 2500 # us
t_servo_min = 500 # us
angle_max = 100 # enstpricht pwm > 50 percent
angle_min = -100 # enspricht pwm = 0
steps_min = (t_servo_min / t_servo_max) * steps_max
y0 = (steps_max-steps_min)/2
duty = (steps_max-steps_min)/(angle_max-angle_min) * angle + y0
print("\nDutycycle: " + str(duty) + "\n")
return duty
if __name__ == '__main__':
try:
s = Servo()
except rospy.ROSInterruptException:
pass
finally:
try:
s.disable()
except:
pass

View File

@ -0,0 +1,47 @@
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
Created on Mon Feb 10 17:09:57 2020
@author: Maxi
"""
import roslib
import rospy
import sys
import std_msgs.msg as msg
from dynamic_reconfigure.server import Server as DynamicReconfigureServer
# Import custom message data and dynamic reconfigure variables.
from Steve.msg import node_example
from Steve.cfg import node_example_paramsConfig as ConfigType
import RPi.GPIO as GPIO
import Pin
from Switch import Switch
class StowedSwitch(Switch):
def __init__(self):
""" TODO : Flanke abstellen, nur auf HIGH / LOW abfragen """
self.pin = Pin.limit_stowed
self.pin_out = Pin.limit_stowed_out
self.event_based = False # detect only status changes
self.setup_pwr_low()
self.SwitchPub = rospy.Publisher('stowed_switch', msg.String, queue_size=1)
Switch.__init__(self)
if __name__ == '__main__': # name == main when ros opens ***.py
# initialize the node
# rospy.init_node('Switch')
# instantiate class
try:
# my_switch = StowedSwitch()
stow_switch = StowedSwitch()
# raised_switch = RaisedSwitch()
except rospy.ROSInterruptException:
pass

189
Steve/Scripts/Switch.py Normal file
View File

@ -0,0 +1,189 @@
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
Created on Mon Feb 10 17:09:57 2020
@author: Maxi
"""
import roslib