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
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
class Switch:
def __init__(self):
""" configure RPi, GPIO """
print(GPIO.getmode())
self.is_high = False
""" configure ROS """
self.node()
init_message = rospy.get_param('~message', 'hello')
rate = float(rospy.get_param('~rate', '1.0'))
topic = rospy.get_param('~topic', 'chatter')
rospy.loginfo('rate = %d', rate)
rospy.loginfo('topic = %s', topic)
# create a dynamic reconfigure server
self.server = DynamicReconfigureServer(ConfigType, self.reconfigure)
# create a publisher
self.pub = rospy.Publisher(topic, node_example, queue_size=10)
# set the message to publish
msg = node_example()
msg.a = 1
msg.b = 2
msg.message = init_message
""" start main processing loop """
while not rospy.is_shutdown():
# self.send_message()
self.detect_IO_change()
# if self.event_based:
# self.check_IO_value()
# sleep before next message
rospy.sleep(1/rate) # rate != 0
def reconfigure(self, config, level):
# fill in local variables from dynamic reconfigure items
self.message = config["message"]
self.a = config["a"]
self.b = config["b"]
rospy.loginfo('reconfiguring with %s' % config)
return config
""" trying to split stuff into functions """
def setup_pwr_low(self):
""" for switches connected to 3.3V --> will send 'LOW' when closing """
print("setting up GPIO")
print('setting PULL DOWN, expect to send LOW')
GPIO.setup(self.pin_out,
GPIO.OUT)
GPIO.output(self.pin_out,
1)
GPIO.setup(self.pin,
GPIO.IN,
pull_up_down=GPIO.PUD_DOWN)
GPIO.add_event_detect(self.pin,
GPIO.FALLING,
callback=self.send_if_low,
bouncetime=1000)
def setup_pwr_high(self):
""" for switches connected to 3.3V --> will send 'HIGH' when closing """
print("setting up GPIO")
print('setting PULL DOWN, expect to send LOW')
GPIO.setup(self.pin_out,
GPIO.OUT)
GPIO.output(self.pin_out,
1)
GPIO.setup(self.pin,
GPIO.IN,
pull_up_down=GPIO.PUD_UP)
GPIO.add_event_detect(self.pin,
GPIO.RISING,
callback=self.send_if_high,
bouncetime=1000)
def setup_gnd_low(self):
""" for switches connected to GND --> will send 'LOW' when triggered """
print("setting up GPIO")
print('setting PULL UP, expect to send HIGH')
GPIO.setup(self.pin_out,
GPIO.OUT)
GPIO.output(self.pin_out,
0)
GPIO.setup(self.pin,
GPIO.IN,
pull_up_down=GPIO.PUD_UP)
GPIO.add_event_detect(self.pin,
GPIO.FALLING,
callback=self.send_if_low,
bouncetime=1000)
def setup_gnd_high(self):
""" for switches connected to GND --> will send 'HIGH' when triggered """
print("setting up GPIO")
print('setting PULL UP, expect to send HIGH')
GPIO.setup(self.pin_out,
GPIO.OUT)
GPIO.output(self.pin_out,
0)
GPIO.setup(self.pin,
GPIO.IN,
pull_up_down=GPIO.PUD_UP)
GPIO.add_event_detect(self.pin,
GPIO.RISING,
callback=self.send_if_high,
bouncetime=1000)
def node(self):
# initialize the node
rospy.loginfo('launching node')
rospy.init_node('switch', anonymous=True)
def send_message(self):
# set the message to publish
msg = node_example()
msg.message = self.message
msg.a = self.a
msg.b = self.b
self.pub.publish(msg)
rospy.loginfo(rospy.get_caller_id() + ' sent message a: %s, b: %s' % (self.a, self.b))
def set_output(self):
GPIO.setup(self.output_pin,
GPIO.OUT)
def send_if_high(self, channel=None):
# if self.check_IO_value():
rospy.loginfo(rospy.get_caller_id() + 'Switch is HIGH == released')
my_msg = msg.String()
my_msg.data = 'HIGH'
self.SwitchPub.publish(my_msg)
def send_if_low(self, channel=None):
# if not self.check_IO_value():
rospy.loginfo(rospy.get_caller_id() + 'Switch is LOW == pressed')
my_msg = msg.String()
my_msg.data = 'LOW'
self.SwitchPub.publish(my_msg)
def check_IO_value(self):
if GPIO.input(self.pin) == GPIO.LOW:
rospy.loginfo('%s is LOW' % self.pin)
return False
elif GPIO.input(self.pin) == GPIO.HIGH:
rospy.loginfo('%s is HIGH' % self.pin)
return True
def detect_IO_change(self):
self.is_high_old = self.is_high
self.is_high = self.check_IO_value()
# if self.is_high_old and not self.is_high:
# self.send_if_low()
# elif not self.is_high_old and self.is_high:
# self.send_if_high()
# else:
# pass
if self.is_high:
self.send_if_high()
else:
self.send_if_low()
rospy.sleep(0.3)

BIN
Steve/Scripts/Switch.pyc Normal file

Binary file not shown.

View File

@ -0,0 +1,7 @@
# -*- coding: utf-8 -*-
"""
Created on Mon Feb 10 11:22:10 2020
@author: Maxi
"""

View File

@ -0,0 +1,8 @@
#!/bin/bash
# just print this out
echo "Starting GPIO Deamon (sudo pigpiod)"
sudo pigpiod
# exit gracefully by returning a status
exit 0

216
Steve/Scripts/tests.py Normal file
View File

@ -0,0 +1,216 @@
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
Created on Mon Feb 10 12:10:34 2020
@author: Maxi
"""
import roslib
import rospy
import sys
import time
import std_msgs.msg as msg
class Test:
def __init__(self):
""" setup ROS """
rospy.init_node('Tests')
self.pub = rospy.Publisher('tests',
msg.String,
queue_size=10)
self.CmdPub = rospy.Publisher('command',
msg.String,
queue_size=10)
self.InterruptPub = rospy.Publisher('interrupt',
msg.String,
queue_size=10)
self.RaisedSwitchPub = rospy.Publisher('raised_switch',
msg.String,
queue_size=10)
self.ForwardSwitchPub = rospy.Publisher('forward_switch',
msg.String,
queue_size=10)
self.StowedSwitchPub = rospy.Publisher('stowed_switch',
msg.String,
queue_size=10)
self.RotatePub = rospy.Publisher('rotation',
msg.String,
queue_size=10)
self.ElevatePub = rospy.Publisher('elevation',
msg.String,
queue_size=10)
self.ServoPub = rospy.Publisher('servo',
msg.String,
queue_size=10)
# while not rospy.is_shutdown():
# rospy.spin()
def test_elevate(self):
self.pub.publish(msg.String('Test 0'))
rospy.sleep(1.0)
self.StowedSwitchPub.publish(msg.String('LOW'))
rospy.sleep(1.0)
self.CmdPub.publish(msg.String('ELEVATE'))
rospy.sleep(10.0) # seconds
# self.RaisedSwitchPub.publish(msg.String('HIGH'))
def test_retract(self):
self.pub.publish(msg.String('Test 0'))
# rospy.sleep(1.0)
# self.RaisedSwitchPub.publish(msg.String('HIGH'))
rospy.sleep(1.0)
self.CmdPub.publish(msg.String('SERVO 68'))
rospy.sleep(5.0)
self.CmdPub.publish(msg.String('STOW'))
rospy.sleep(20.0) # seconds
# self.StowedSwitchPub.publish(msg.String('LOW'))
def test_tilt(self):
self.pub.publish(msg.String('Test 0'))
rospy.sleep(1.0)
self.CmdPub.publish(msg.String('TILT'))
rospy.sleep(10.0) # seconds
def test_sweep(self):
self.pub.publish(msg.String('Test 0'))
rospy.sleep(1.0)
self.CmdPub.publish('ROTATE %s' % str(180))
rospy.sleep(45.0)
self.CmdPub.publish('ROTATE %s' % str(-180))
rospy.sleep(45.0)
self.CmdPub.publish('CENTER')
rospy.sleep(30.0)
def test_center(self):
self.pub.publish(msg.String('Test 0'))
rospy.sleep(1.0)
# self.ForwardSwitchPub.publish('HIGH')
# rospy.sleep(1.0)
self.CmdPub.publish('ROTATE %s' % str(10))
rospy.sleep(15.0)
self.CmdPub.publish('ROTATE %s' % str(-10))
rospy.sleep(15.0)
self.CmdPub.publish('CENTER')
rospy.sleep(30.0)
# self.CmdPub.publish('ROTATE %s' % str(-80))
# rospy.sleep(10.0)
# self.CmdPub.publish('CENTER')
# rospy.sleep(20.0)
# rospy.sleep(1.0)
# self.ForwardSwitchPub.publish('HIGH')
def test_rotate(self, angle):
self.pub.publish(msg.String('Test 0'))
rospy.sleep(1.0)
self.CmdPub.publish('ROTATE %s' % str(angle))
# speed = 8 # grad pro sekunde
# rospy.sleep(angle/speed)
rospy.sleep(5)
def test_elev_switch(self):
self.pub.publish(msg.String('Test 0'))
rospy.sleep(1.0)
self.CmdPub.publish(msg.String('ELEVATE'))
rospy.sleep(5.0)
self.pub.publish(msg.String('Test 0'))
rospy.sleep(1.0)
self.CmdPub.publish(msg.String('STOW'))
def test_servo(self, angle):
self.pub.publish(msg.String('Test 0'))
rospy.sleep(0.2)
# self.CmdPub.publish(msg.String('SERVO 0'))
# rospy.sleep(5.0)
self.CmdPub.publish(msg.String('SERVO ' + str(angle)))
# rospy.sleep(5.0)
# self.CmdPub.publish(msg.String('SERVO 20'))
# rospy.sleep(5.0)
# self.CmdPub.publish(msg.String('SERVO 30'))
# rospy.sleep(5.0)
# self.CmdPub.publish(msg.String('SERVO 40'))
# rospy.sleep(5.0)
# self.CmdPub.publish(msg.String('SERVO 50'))
# rospy.sleep(5.0)
# self.CmdPub.publish(msg.String('SERVO 55'))
# rospy.sleep(5.0)
# self.CmdPub.publish(msg.String('SERVO 60'))
# rospy.sleep(5.0)
# self.CmdPub.publish(msg.String('SERVO 62'))
# rospy.sleep(5.0)
# self.CmdPub.publish(msg.String('SERVO 64'))
# rospy.sleep(5.0)
# self.CmdPub.publish(msg.String('SERVO 66'))
# rospy.sleep(5.0)
# self.CmdPub.publish(msg.String('SERVO 68'))
# rospy.sleep(5.0)
def test1(self):
for t in range(40):
my_msg = msg.String()
my_msg.data = str(t)
rospy.loginfo('sent %s' % my_msg.data)
pub.publish(my_msg)
if rospy.is_shutdown():
return
rospy.Rate(0.8).sleep()
if __name__ == '__main__':
try:
t = Test()
rospy.sleep(2.0)
# First Run of tests.py should start with:
#t.test_elevate()
#t.test_sweep() # Search Forward Position
#t.test_center() # Move to Rearward Position
#t.test_retract()
#t.test_rotate(180)
# t.test_center()
# rospy.sleep(10.0)
#t.test_tilt()
# rospy.sleep(10.0)
# t.test_center()
# rospy.sleep(10.0)
# rospy.sleep(30.0)
# t.test_retract()
# rospy.sleep(30.0)
#t.test_elevate() # Aufrichten in die Vertikale
# rospy.sleep(10.0)
# t.test_sweep()
# rospy.sleep(30.0)
# t.test_center()
# rospy.sleep(30.0)
# t.test_retract()
# rospy.sleep(10.0)
# t.test_elevate()
# rospy.sleep(10.0)
# # rospy.sleep(30.0)
# t.test_tilt() # Tilt until Limit Switch pressed
# rospy.sleep(20.0)
# t.test_elevate()
# rospy.sleep(30.0)
#for angle in range(20, 30, 2):
## angle = 40 - angle
#t.test_servo(angle)
#rospy.sleep(0.6)
t.test_servo(20)
# rospy.sleep(1.0)
# t.test_rotate(-90)
# rospy.sleep(10.0).
# t.test_rotate(-270)
# rospy.sleep(10.0)
# t.test_rotate(-180)
# rospy.sleep(5.0)
# t.test_rotate(0)
except rospy.ROSInterruptException:
pass

304
Steve/Scripts/tests_cli.py Normal file
View File

@ -0,0 +1,304 @@
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
Created on Mon Feb 10 12:10:34 2020
@author: Maxi
@edit: PatrickW
"""
import roslib
import rospy
import sys
import time
import std_msgs.msg as msg
class Test:
def __init__(self):
""" setup ROS """
rospy.init_node('Tests')
self.pub = rospy.Publisher('tests',
msg.String,
queue_size=10)
self.CmdPub = rospy.Publisher('command',
msg.String,
queue_size=10)
self.InterruptPub = rospy.Publisher('interrupt',
msg.String,
queue_size=10)
self.RaisedSwitchPub = rospy.Publisher('raised_switch',
msg.String,
queue_size=10)
self.ForwardSwitchPub = rospy.Publisher('forward_switch',
msg.String,
queue_size=10)
self.StowedSwitchPub = rospy.Publisher('stowed_switch',
msg.String,
queue_size=10)
self.RotatePub = rospy.Publisher('rotation',
msg.String,
queue_size=10)
self.ElevatePub = rospy.Publisher('elevation',
msg.String,
queue_size=10)
self.ServoPub = rospy.Publisher('servo',
msg.String,
queue_size=10)
# while not rospy.is_shutdown():
# rospy.spin()
def test_elevate(self):
self.pub.publish(msg.String('Test 0'))
rospy.sleep(1.0)
self.StowedSwitchPub.publish(msg.String('LOW'))
rospy.sleep(1.0)
self.CmdPub.publish(msg.String('ELEVATE'))
rospy.sleep(2.0) # seconds
# self.RaisedSwitchPub.publish(msg.String('HIGH'))
def test_retract(self):
self.pub.publish(msg.String('Test 0'))
# rospy.sleep(1.0)
# self.RaisedSwitchPub.publish(msg.String('HIGH'))
rospy.sleep(1.0)
#self.CmdPub.publish(msg.String('SERVO 68'))
#rospy.sleep(5.0)
self.CmdPub.publish(msg.String('STOW'))
#rospy.sleep(20.0) # seconds
# self.StowedSwitchPub.publish(msg.String('LOW'))
def test_tilt(self, tilt_state=""):
self.pub.publish(msg.String('Test 0'))
rospy.sleep(1.0)
string = 'TILT' + str(tilt_state)
self.CmdPub.publish(msg.String(string))
#self.CmdPub.publish(msg.String('TILT'))
rospy.sleep(10.0) # seconds
def test_sweep(self):
self.pub.publish(msg.String('Test 0'))
rospy.sleep(1.0)
self.CmdPub.publish('ROTATE %s' % str(180))
rospy.sleep(45.0)
self.CmdPub.publish('ROTATE %s' % str(-180))
rospy.sleep(45.0)
self.CmdPub.publish('CENTER')
rospy.sleep(30.0)
def test_center(self):
self.pub.publish(msg.String('Test 0'))
rospy.sleep(1.0)
# self.ForwardSwitchPub.publish('HIGH')
# rospy.sleep(1.0)
self.CmdPub.publish('ROTATE %s' % str(10))
rospy.sleep(15.0)
self.CmdPub.publish('ROTATE %s' % str(-10))
rospy.sleep(15.0)
self.CmdPub.publish('CENTER')
rospy.sleep(30.0)
# self.CmdPub.publish('ROTATE %s' % str(-80))
# rospy.sleep(10.0)
# self.CmdPub.publish('CENTER')
# rospy.sleep(20.0)
# rospy.sleep(1.0)
# self.ForwardSwitchPub.publish('HIGH')
def test_rotate(self, angle):
self.pub.publish(msg.String('Test 0'))
rospy.sleep(1.0)
self.CmdPub.publish('ROTATE %s' % str(angle))
# speed = 8 # grad pro sekunde
# rospy.sleep(angle/speed)
rospy.sleep(10)
def test_elev_switch(self):
self.pub.publish(msg.String('Test 0'))
rospy.sleep(1.0)
self.CmdPub.publish(msg.String('ELEVATE'))
rospy.sleep(5.0)
self.pub.publish(msg.String('Test 0'))
rospy.sleep(1.0)
self.CmdPub.publish(msg.String('STOW'))
def test_servo(self, angle):
self.pub.publish(msg.String('Test 0'))
rospy.sleep(0.2)
# self.CmdPub.publish(msg.String('SERVO 0'))
# rospy.sleep(5.0)
self.CmdPub.publish(msg.String('SERVO ' + str(angle)))
# rospy.sleep(5.0)
# self.CmdPub.publish(msg.String('SERVO 20'))
# rospy.sleep(5.0)
# self.CmdPub.publish(msg.String('SERVO 30'))
# rospy.sleep(5.0)
# self.CmdPub.publish(msg.String('SERVO 40'))
# rospy.sleep(5.0)
# self.CmdPub.publish(msg.String('SERVO 50'))
# rospy.sleep(5.0)
# self.CmdPub.publish(msg.String('SERVO 55'))
# rospy.sleep(5.0)
# self.CmdPub.publish(msg.String('SERVO 60'))
# rospy.sleep(5.0)
# self.CmdPub.publish(msg.String('SERVO 62'))
# rospy.sleep(5.0)
# self.CmdPub.publish(msg.String('SERVO 64'))
# rospy.sleep(5.0)
# self.CmdPub.publish(msg.String('SERVO 66'))
# rospy.sleep(5.0)
# self.CmdPub.publish(msg.String('SERVO 68'))
# rospy.sleep(5.0)
def test1(self):
for t in range(40):
my_msg = msg.String()
my_msg.data = str(t)
rospy.loginfo('sent %s' % my_msg.data)
pub.publish(my_msg)
if rospy.is_shutdown():
return
rospy.Rate(0.8).sleep()
def print_menu():
print("--- STEVE Console Interface ---")
print 5*"-", "Choose Command", 5*"-"
print "1. Raise from Storage" # Raise Mast (test_elevate)
print "2. Initiate Azimuth" # Initiate Az (find forward switch) (test_sweep)
print "3. Rotate Azimuth"
print "4. Rotate Azimuth to Forward Position (180 deg)"
print "5. Rotate Pitch Servo -SERVO NOT CONNECTED-"
print "6. Move to Tilted Position" # Rotate Elevation to tilted
print "7. Move to storage Azimuth (0 deg)" # Move to storage Az (test_center)
print "8. Retract (Caution: Center Az first)" # test_retract
return
if __name__ == '__main__':
t = Test()
rospy.sleep(1.0)
rate=rospy.Rate(10)
print_limit = 8
number_of_commands = 0
print_menu()
while not rospy.is_shutdown():
if number_of_commands>=print_limit:
print_menu()
number_of_commands = 0
try:
choice = input("Command [1-8]: ")
choice = int(choice)
if choice == 1:
print("Raising Mast...")
t.test_elevate()
print("Finished :)")
elif choice == 2:
choice = raw_input("Caution: Is Mast raised? [y/n] ")
choice = str(choice)
if choice == "y":
print("Initiating: Finding Switch...")
t.test_sweep()
print("Finished :)")
else:
print("Cancelling...")
elif choice == 3:
set_angle_az = int(input("Enter set value [-359,359] -> [cw>0; ccw<0]: "))
if set_angle_az >-359 and set_angle_az < 359:
print("Moving Base...")
t.test_rotate(set_angle_az)
print("Finished :)")
else:
print("Azimuth Set Value out of bounds!")
elif choice == 4:
choice = raw_input("Rotation cw/ccw? [cw: 1; ccw: 2]: ")
choice = int(choice)
if choice == 1:
print("Moving to Forward Direction (cw)...")
set_angle_az = 180
t.test_rotate(set_angle_az)
print("Finished :)")
elif choice == 2:
print("Moving to Forward Direction (cw)...")
set_angle_az = -180
t.test_rotate(set_angle_az)
print("Finished :)")
else:
print("Cancelling...")
elif choice== 5:
set_angle_servo = input("Enter set value [0,68]: ")
if set_angle_servo>68 or set_angle_servo<0:
print("\n Error: Value out of bounds \n")
else:
print("Moving Servo...")
t.test_servo(set_angle_servo)
print("Finished :)")
elif choice == 6:
time = [0,18]
tilt_choice = input("Choose tilt stage " + str(time) + ": ")
tilt_choice = int(tilt_choice)
if tilt_choice>=time[0] and tilt_choice<=time[1]:
choice = raw_input("Caution: Is Mast raised? [y/n] ")
choice = str(choice)
if choice == "y":
print("Moving to Tilted Elevation...")
goal = "_" + str(tilt_choice) + "_"
t.test_tilt(tilt_state = goal)
t.CmdPub.publish(msg.String('STOP'))
print("Finished :)")
else:
print("Cancelling...")
else:
print("Entry out of bounds")
print("Cancelling...")
elif choice == 7:
print("Moving to storage Azimuth...")
t.test_center()
print("Finished :)")
elif choice == 8:
choice = raw_input("Caution: Is Mast centered && raised? [y/n] ")
choice = str(choice)
if choice == "y":
print("Retracting Mast...")
t.test_retract()
print("Finished :)")
else:
print("Cancelling...")
else:
print("Unkown Command! Enter any key to try again..")
except ValueError:
print("ValueError:\tinvalid... try again..")
except NameError as e:
print(e)
print("NameError:\tinvalid... try again..")
except KeyboardInterrupt:
print("KeyboardInterrupt")
t.pub.publish(msg.String('STOP'))
t.CmdPub.publish(msg.String('STOP'))
sys.exit()
except rospy.ROSInterruptException:
pass
except:
print("invalid... try again..")
finally:
number_of_commands += 1
print("")
#rospy.spin()

View File

@ -0,0 +1,515 @@
//#line 2 "/home/pi/ros_catkin_ws/src/dynamic_reconfigure/templates/ConfigType.h.template"
// *********************************************************
//
// File autogenerated for the Steve package
// by the dynamic_reconfigure package.
// Please do not edit.
//
// ********************************************************/
#ifndef __Steve__NODE_EXAMPLE_PARAMSCONFIG_H__
#define __Steve__NODE_EXAMPLE_PARAMSCONFIG_H__
#if __cplusplus >= 201103L
#define DYNAMIC_RECONFIGURE_FINAL final
#else
#define DYNAMIC_RECONFIGURE_FINAL
#endif
#include <dynamic_reconfigure/config_tools.h>
#include <limits>
#include <ros/node_handle.h>
#include <dynamic_reconfigure/ConfigDescription.h>
#include <dynamic_reconfigure/ParamDescription.h>
#include <dynamic_reconfigure/Group.h>
#include <dynamic_reconfigure/config_init_mutex.h>
#include <boost/any.hpp>
namespace Steve
{
class node_example_paramsConfigStatics;
class node_example_paramsConfig
{
public:
class AbstractParamDescription : public dynamic_reconfigure::ParamDescription
{
public:
AbstractParamDescription(std::string n, std::string t, uint32_t l,
std::string d, std::string e)
{
name = n;
type = t;
level = l;
description = d;
edit_method = e;
}
virtual void clamp(node_example_paramsConfig &config, const node_example_paramsConfig &max, const node_example_paramsConfig &min) const = 0;
virtual void calcLevel(uint32_t &level, const node_example_paramsConfig &config1, const node_example_paramsConfig &config2) const = 0;
virtual void fromServer(const ros::NodeHandle &nh, node_example_paramsConfig &config) const = 0;
virtual void toServer(const ros::NodeHandle &nh, const node_example_paramsConfig &config) const = 0;
virtual bool fromMessage(const dynamic_reconfigure::Config &msg, node_example_paramsConfig &config) const = 0;
virtual void toMessage(dynamic_reconfigure::Config &msg, const node_example_paramsConfig &config) const = 0;
virtual void getValue(const node_example_paramsConfig &config, boost::any &val) const = 0;
};
typedef boost::shared_ptr<AbstractParamDescription> AbstractParamDescriptionPtr;
typedef boost::shared_ptr<const AbstractParamDescription> AbstractParamDescriptionConstPtr;
// Final keyword added to class because it has virtual methods and inherits
// from a class with a non-virtual destructor.
template <class T>
class ParamDescription DYNAMIC_RECONFIGURE_FINAL : public AbstractParamDescription
{
public:
ParamDescription(std::string a_name, std::string a_type, uint32_t a_level,
std::string a_description, std::string a_edit_method, T node_example_paramsConfig::* a_f) :
AbstractParamDescription(a_name, a_type, a_level, a_description, a_edit_method),
field(a_f)
{}
T node_example_paramsConfig::* field;
virtual void clamp(node_example_paramsConfig &config, const node_example_paramsConfig &max, const node_example_paramsConfig &min) const
{
if (config.*field > max.*field)
config.*field = max.*field;
if (config.*field < min.*field)
config.*field = min.*field;
}
virtual void calcLevel(uint32_t &comb_level, const node_example_paramsConfig &config1, const node_example_paramsConfig &config2) const
{
if (config1.*field != config2.*field)
comb_level |= level;
}
virtual void fromServer(const ros::NodeHandle &nh, node_example_paramsConfig &config) const
{
nh.getParam(name, config.*field);
}
virtual void toServer(const ros::NodeHandle &nh, const node_example_paramsConfig &config) const
{
nh.setParam(name, config.*field);
}
virtual bool fromMessage(const dynamic_reconfigure::Config &msg, node_example_paramsConfig &config) const
{
return dynamic_reconfigure::ConfigTools::getParameter(msg, name, config.*field);
}
virtual void toMessage(dynamic_reconfigure::Config &msg, const node_example_paramsConfig &config) const
{
dynamic_reconfigure::ConfigTools::appendParameter(msg, name, config.*field);
}
virtual void getValue(const node_example_paramsConfig &config, boost::any &val) const
{
val = config.*field;
}
};
class AbstractGroupDescription : public dynamic_reconfigure::Group
{
public:
AbstractGroupDescription(std::string n, std::string t, int p, int i, bool s)
{
name = n;
type = t;
parent = p;
state = s;
id = i;
}
std::vector<AbstractParamDescriptionConstPtr> abstract_parameters;
bool state;
virtual void toMessage(dynamic_reconfigure::Config &msg, const boost::any &config) const = 0;
virtual bool fromMessage(const dynamic_reconfigure::Config &msg, boost::any &config) const =0;
virtual void updateParams(boost::any &cfg, node_example_paramsConfig &top) const= 0;
virtual void setInitialState(boost::any &cfg) const = 0;
void convertParams()
{
for(std::vector<AbstractParamDescriptionConstPtr>::const_iterator i = abstract_parameters.begin(); i != abstract_parameters.end(); ++i)
{
parameters.push_back(dynamic_reconfigure::ParamDescription(**i));
}
}
};
typedef boost::shared_ptr<AbstractGroupDescription> AbstractGroupDescriptionPtr;
typedef boost::shared_ptr<const AbstractGroupDescription> AbstractGroupDescriptionConstPtr;
// Final keyword added to class because it has virtual methods and inherits
// from a class with a non-virtual destructor.
template<class T, class PT>
class GroupDescription DYNAMIC_RECONFIGURE_FINAL : public AbstractGroupDescription
{
public:
GroupDescription(std::string a_name, std::string a_type, int a_parent, int a_id, bool a_s, T PT::* a_f) : AbstractGroupDescription(a_name, a_type, a_parent, a_id, a_s), field(a_f)
{
}
GroupDescription(const GroupDescription<T, PT>& g): AbstractGroupDescription(g.name, g.type, g.parent, g.id, g.state), field(g.field), groups(g.groups)
{
parameters = g.parameters;
abstract_parameters = g.abstract_parameters;
}
virtual bool fromMessage(const dynamic_reconfigure::Config &msg, boost::any &cfg) const
{
PT* config = boost::any_cast<PT*>(cfg);
if(!dynamic_reconfigure::ConfigTools::getGroupState(msg, name, (*config).*field))
return false;
for(std::vector<AbstractGroupDescriptionConstPtr>::const_iterator i = groups.begin(); i != groups.end(); ++i)
{
boost::any n = &((*config).*field);
if(!(*i)->fromMessage(msg, n))
return false;
}
return true;
}
virtual void setInitialState(boost::any &cfg) const
{
PT* config = boost::any_cast<PT*>(cfg);
T* group = &((*config).*field);
group->state = state;
for(std::vector<AbstractGroupDescriptionConstPtr>::const_iterator i = groups.begin(); i != groups.end(); ++i)
{
boost::any n = boost::any(&((*config).*field));
(*i)->setInitialState(n);
}
}
virtual void updateParams(boost::any &cfg, node_example_paramsConfig &top) const
{
PT* config = boost::any_cast<PT*>(cfg);
T* f = &((*config).*field);
f->setParams(top, abstract_parameters);
for(std::vector<AbstractGroupDescriptionConstPtr>::const_iterator i = groups.begin(); i != groups.end(); ++i)
{
boost::any n = &((*config).*field);
(*i)->updateParams(n, top);
}
}
virtual void toMessage(dynamic_reconfigure::Config &msg, const boost::any &cfg) const
{
const PT config = boost::any_cast<PT>(cfg);
dynamic_reconfigure::ConfigTools::appendGroup<T>(msg, name, id, parent, config.*field);
for(std::vector<AbstractGroupDescriptionConstPtr>::const_iterator i = groups.begin(); i != groups.end(); ++i)
{
(*i)->toMessage(msg, config.*field);
}
}
T PT::* field;
std::vector<node_example_paramsConfig::AbstractGroupDescriptionConstPtr> groups;
};
class DEFAULT
{
public:
DEFAULT()
{
state = true;
name = "Default";
}
void setParams(node_example_paramsConfig &config, const std::vector<AbstractParamDescriptionConstPtr> params)
{
for (std::vector<AbstractParamDescriptionConstPtr>::const_iterator _i = params.begin(); _i != params.end(); ++_i)
{
boost::any val;
(*_i)->getValue(config, val);
if("message"==(*_i)->name){message = boost::any_cast<std::string>(val);}
if("a"==(*_i)->name){a = boost::any_cast<int>(val);}
if("b"==(*_i)->name){b = boost::any_cast<int>(val);}
}
}
std::string message;
int a;
int b;
bool state;
std::string name;
}groups;
//#line 274 "/home/pi/ros_catkin_ws/src/dynamic_reconfigure/src/dynamic_reconfigure/parameter_generator.py"
std::string message;
//#line 274 "/home/pi/ros_catkin_ws/src/dynamic_reconfigure/src/dynamic_reconfigure/parameter_generator.py"
int a;
//#line 274 "/home/pi/ros_catkin_ws/src/dynamic_reconfigure/src/dynamic_reconfigure/parameter_generator.py"
int b;
//#line 228 "/home/pi/ros_catkin_ws/src/dynamic_reconfigure/templates/ConfigType.h.template"
bool __fromMessage__(dynamic_reconfigure::Config &msg)
{
const std::vector<AbstractParamDescriptionConstPtr> &__param_descriptions__ = __getParamDescriptions__();
const std::vector<AbstractGroupDescriptionConstPtr> &__group_descriptions__ = __getGroupDescriptions__();
int count = 0;
for (std::vector<AbstractParamDescriptionConstPtr>::const_iterator i = __param_descriptions__.begin(); i != __param_descriptions__.end(); ++i)
if ((*i)->fromMessage(msg, *this))
count++;
for (std::vector<AbstractGroupDescriptionConstPtr>::const_iterator i = __group_descriptions__.begin(); i != __group_descriptions__.end(); i ++)
{
if ((*i)->id == 0)
{
boost::any n = boost::any(this);
(*i)->updateParams(n, *this);
(*i)->fromMessage(msg, n);
}
}
if (count != dynamic_reconfigure::ConfigTools::size(msg))
{
ROS_ERROR("node_example_paramsConfig::__fromMessage__ called with an unexpected parameter.");
ROS_ERROR("Booleans:");
for (unsigned int i = 0; i < msg.bools.size(); i++)
ROS_ERROR(" %s", msg.bools[i].name.c_str());
ROS_ERROR("Integers:");
for (unsigned int i = 0; i < msg.ints.size(); i++)
ROS_ERROR(" %s", msg.ints[i].name.c_str());
ROS_ERROR("Doubles:");
for (unsigned int i = 0; i < msg.doubles.size(); i++)
ROS_ERROR(" %s", msg.doubles[i].name.c_str());
ROS_ERROR("Strings:");
for (unsigned int i = 0; i < msg.strs.size(); i++)
ROS_ERROR(" %s", msg.strs[i].name.c_str());
// @todo Check that there are no duplicates. Make this error more
// explicit.
return false;
}
return true;
}
// This version of __toMessage__ is used during initialization of
// statics when __getParamDescriptions__ can't be called yet.
void __toMessage__(dynamic_reconfigure::Config &msg, const std::vector<AbstractParamDescriptionConstPtr> &__param_descriptions__, const std::vector<AbstractGroupDescriptionConstPtr> &__group_descriptions__) const
{
dynamic_reconfigure::ConfigTools::clear(msg);
for (std::vector<AbstractParamDescriptionConstPtr>::const_iterator i = __param_descriptions__.begin(); i != __param_descriptions__.end(); ++i)
(*i)->toMessage(msg, *this);
for (std::vector<AbstractGroupDescriptionConstPtr>::const_iterator i = __group_descriptions__.begin(); i != __group_descriptions__.end(); ++i)
{
if((*i)->id == 0)
{
(*i)->toMessage(msg, *this);
}
}
}
void __toMessage__(dynamic_reconfigure::Config &msg) const
{
const std::vector<AbstractParamDescriptionConstPtr> &__param_descriptions__ = __getParamDescriptions__();
const std::vector<AbstractGroupDescriptionConstPtr> &__group_descriptions__ = __getGroupDescriptions__();
__toMessage__(msg, __param_descriptions__, __group_descriptions__);
}
void __toServer__(const ros::NodeHandle &nh) const
{
const std::vector<AbstractParamDescriptionConstPtr> &__param_descriptions__ = __getParamDescriptions__();
for (std::vector<AbstractParamDescriptionConstPtr>::const_iterator i = __param_descriptions__.begin(); i != __param_descriptions__.end(); ++i)
(*i)->toServer(nh, *this);
}
void __fromServer__(const ros::NodeHandle &nh)
{
static bool setup=false;
const std::vector<AbstractParamDescriptionConstPtr> &__param_descriptions__ = __getParamDescriptions__();
for (std::vector<AbstractParamDescriptionConstPtr>::const_iterator i = __param_descriptions__.begin(); i != __param_descriptions__.end(); ++i)
(*i)->fromServer(nh, *this);
const std::vector<AbstractGroupDescriptionConstPtr> &__group_descriptions__ = __getGroupDescriptions__();
for (std::vector<AbstractGroupDescriptionConstPtr>::const_iterator i = __group_descriptions__.begin(); i != __group_descriptions__.end(); i++){
if (!setup && (*i)->id == 0) {
setup = true;
boost::any n = boost::any(this);
(*i)->setInitialState(n);
}
}
}
void __clamp__()
{
const std::vector<AbstractParamDescriptionConstPtr> &__param_descriptions__ = __getParamDescriptions__();
const node_example_paramsConfig &__max__ = __getMax__();
const node_example_paramsConfig &__min__ = __getMin__();
for (std::vector<AbstractParamDescriptionConstPtr>::const_iterator i = __param_descriptions__.begin(); i != __param_descriptions__.end(); ++i)
(*i)->clamp(*this, __max__, __min__);
}
uint32_t __level__(const node_example_paramsConfig &config) const
{
const std::vector<AbstractParamDescriptionConstPtr> &__param_descriptions__ = __getParamDescriptions__();
uint32_t level = 0;
for (std::vector<AbstractParamDescriptionConstPtr>::const_iterator i = __param_descriptions__.begin(); i != __param_descriptions__.end(); ++i)
(*i)->calcLevel(level, config, *this);
return level;
}
static const dynamic_reconfigure::ConfigDescription &__getDescriptionMessage__();
static const node_example_paramsConfig &__getDefault__();
static const node_example_paramsConfig &__getMax__();
static const node_example_paramsConfig &__getMin__();
static const std::vector<AbstractParamDescriptionConstPtr> &__getParamDescriptions__();
static const std::vector<AbstractGroupDescriptionConstPtr> &__getGroupDescriptions__();
private:
static const node_example_paramsConfigStatics *__get_statics__();
};
template <> // Max and min are ignored for strings.
inline void node_example_paramsConfig::ParamDescription<std::string>::clamp(node_example_paramsConfig &config, const node_example_paramsConfig &max, const node_example_paramsConfig &min) const
{
(void) config;
(void) min;
(void) max;
return;
}
class node_example_paramsConfigStatics
{
friend class node_example_paramsConfig;
node_example_paramsConfigStatics()
{
node_example_paramsConfig::GroupDescription<node_example_paramsConfig::DEFAULT, node_example_paramsConfig> Default("Default", "", 0, 0, true, &node_example_paramsConfig::groups);
//#line 274 "/home/pi/ros_catkin_ws/src/dynamic_reconfigure/src/dynamic_reconfigure/parameter_generator.py"
__min__.message = "";
//#line 274 "/home/pi/ros_catkin_ws/src/dynamic_reconfigure/src/dynamic_reconfigure/parameter_generator.py"
__max__.message = "";
//#line 274 "/home/pi/ros_catkin_ws/src/dynamic_reconfigure/src/dynamic_reconfigure/parameter_generator.py"
__default__.message = "hello";
//#line 274 "/home/pi/ros_catkin_ws/src/dynamic_reconfigure/src/dynamic_reconfigure/parameter_generator.py"
Default.abstract_parameters.push_back(node_example_paramsConfig::AbstractParamDescriptionConstPtr(new node_example_paramsConfig::ParamDescription<std::string>("message", "str", 0, "The message.", "", &node_example_paramsConfig::message)));
//#line 274 "/home/pi/ros_catkin_ws/src/dynamic_reconfigure/src/dynamic_reconfigure/parameter_generator.py"
__param_descriptions__.push_back(node_example_paramsConfig::AbstractParamDescriptionConstPtr(new node_example_paramsConfig::ParamDescription<std::string>("message", "str", 0, "The message.", "", &node_example_paramsConfig::message)));
//#line 274 "/home/pi/ros_catkin_ws/src/dynamic_reconfigure/src/dynamic_reconfigure/parameter_generator.py"
__min__.a = -100;
//#line 274 "/home/pi/ros_catkin_ws/src/dynamic_reconfigure/src/dynamic_reconfigure/parameter_generator.py"
__max__.a = 100;
//#line 274 "/home/pi/ros_catkin_ws/src/dynamic_reconfigure/src/dynamic_reconfigure/parameter_generator.py"
__default__.a = 1;
//#line 274 "/home/pi/ros_catkin_ws/src/dynamic_reconfigure/src/dynamic_reconfigure/parameter_generator.py"
Default.abstract_parameters.push_back(node_example_paramsConfig::AbstractParamDescriptionConstPtr(new node_example_paramsConfig::ParamDescription<int>("a", "int", 0, "First number.", "", &node_example_paramsConfig::a)));
//#line 274 "/home/pi/ros_catkin_ws/src/dynamic_reconfigure/src/dynamic_reconfigure/parameter_generator.py"
__param_descriptions__.push_back(node_example_paramsConfig::AbstractParamDescriptionConstPtr(new node_example_paramsConfig::ParamDescription<int>("a", "int", 0, "First number.", "", &node_example_paramsConfig::a)));
//#line 274 "/home/pi/ros_catkin_ws/src/dynamic_reconfigure/src/dynamic_reconfigure/parameter_generator.py"
__min__.b = -100;
//#line 274 "/home/pi/ros_catkin_ws/src/dynamic_reconfigure/src/dynamic_reconfigure/parameter_generator.py"
__max__.b = 100;
//#line 274 "/home/pi/ros_catkin_ws/src/dynamic_reconfigure/src/dynamic_reconfigure/parameter_generator.py"
__default__.b = 2;
//#line 274 "/home/pi/ros_catkin_ws/src/dynamic_reconfigure/src/dynamic_reconfigure/parameter_generator.py"
Default.abstract_parameters.push_back(node_example_paramsConfig::AbstractParamDescriptionConstPtr(new node_example_paramsConfig::ParamDescription<int>("b", "int", 0, "First number.", "", &node_example_paramsConfig::b)));
//#line 274 "/home/pi/ros_catkin_ws/src/dynamic_reconfigure/src/dynamic_reconfigure/parameter_generator.py"
__param_descriptions__.push_back(node_example_paramsConfig::AbstractParamDescriptionConstPtr(new node_example_paramsConfig::ParamDescription<int>("b", "int", 0, "First number.", "", &node_example_paramsConfig::b)));
//#line 246 "/home/pi/ros_catkin_ws/src/dynamic_reconfigure/src/dynamic_reconfigure/parameter_generator.py"
Default.convertParams();
//#line 246 "/home/pi/ros_catkin_ws/src/dynamic_reconfigure/src/dynamic_reconfigure/parameter_generator.py"
__group_descriptions__.push_back(node_example_paramsConfig::AbstractGroupDescriptionConstPtr(new node_example_paramsConfig::GroupDescription<node_example_paramsConfig::DEFAULT, node_example_paramsConfig>(Default)));
//#line 366 "/home/pi/ros_catkin_ws/src/dynamic_reconfigure/templates/ConfigType.h.template"
for (std::vector<node_example_paramsConfig::AbstractGroupDescriptionConstPtr>::const_iterator i = __group_descriptions__.begin(); i != __group_descriptions__.end(); ++i)
{
__description_message__.groups.push_back(**i);
}
__max__.__toMessage__(__description_message__.max, __param_descriptions__, __group_descriptions__);
__min__.__toMessage__(__description_message__.min, __param_descriptions__, __group_descriptions__);
__default__.__toMessage__(__description_message__.dflt, __param_descriptions__, __group_descriptions__);
}
std::vector<node_example_paramsConfig::AbstractParamDescriptionConstPtr> __param_descriptions__;
std::vector<node_example_paramsConfig::AbstractGroupDescriptionConstPtr> __group_descriptions__;
node_example_paramsConfig __max__;
node_example_paramsConfig __min__;
node_example_paramsConfig __default__;
dynamic_reconfigure::ConfigDescription __description_message__;
static const node_example_paramsConfigStatics *get_instance()
{
// Split this off in a separate function because I know that
// instance will get initialized the first time get_instance is
// called, and I am guaranteeing that get_instance gets called at
// most once.
static node_example_paramsConfigStatics instance;
return &instance;
}
};
inline const dynamic_reconfigure::ConfigDescription &node_example_paramsConfig::__getDescriptionMessage__()
{
return __get_statics__()->__description_message__;
}
inline const node_example_paramsConfig &node_example_paramsConfig::__getDefault__()
{
return __get_statics__()->__default__;
}
inline const node_example_paramsConfig &node_example_paramsConfig::__getMax__()
{
return __get_statics__()->__max__;
}
inline const node_example_paramsConfig &node_example_paramsConfig::__getMin__()
{
return __get_statics__()->__min__;
}
inline const std::vector<node_example_paramsConfig::AbstractParamDescriptionConstPtr> &node_example_paramsConfig::__getParamDescriptions__()
{
return __get_statics__()->__param_descriptions__;
}
inline const std::vector<node_example_paramsConfig::AbstractGroupDescriptionConstPtr> &node_example_paramsConfig::__getGroupDescriptions__()
{
return __get_statics__()->__group_descriptions__;
}
inline const node_example_paramsConfigStatics *node_example_paramsConfig::__get_statics__()
{
const static node_example_paramsConfigStatics *statics;
if (statics) // Common case
return statics;
boost::mutex::scoped_lock lock(dynamic_reconfigure::__init_mutex__);
if (statics) // In case we lost a race.
return statics;
statics = node_example_paramsConfigStatics::get_instance();
return statics;
}
}
#undef DYNAMIC_RECONFIGURE_FINAL
#endif // __NODE_EXAMPLE_PARAMSRECONFIGURATOR_H__

View File

@ -0,0 +1,16 @@
#! /usr/bin/env python
PACKAGE='Steve'
import roslib
roslib.load_manifest(PACKAGE)
from dynamic_reconfigure.parameter_generator_catkin import *
gen = ParameterGenerator()
# Name Type Level Description Default Min Max
gen.add("message", str_t, 0, "The message.", "hello")
gen.add("a", int_t, 0, "First number.", 1, -100, 100)
gen.add("b", int_t, 0, "First number.", 2, -100, 100)
exit(gen.generate(PACKAGE, "node_example", "node_example_params"))

View File

@ -0,0 +1,9 @@
\subsubsection usage Usage
\verbatim
<node name="node_example" pkg="Steve" type="node_example">
<param name="message" type="str" value="hello" />
<param name="a" type="int" value="1" />
<param name="b" type="int" value="2" />
</node>
\endverbatim

View File

@ -0,0 +1,8 @@
\subsubsection parameters ROS parameters
Reads and maintains the following parameters on the ROS server
- \b "~message" : \b [str] The message. min: , default: hello, max:
- \b "~a" : \b [int] First number. min: -100, default: 1, max: 100
- \b "~b" : \b [int] First number. min: -100, default: 2, max: 100

View File

@ -0,0 +1,20 @@
# Autogenerated param section. Do not hand edit.
param {
group.0 {
name=Dynamically Reconfigurable Parameters
desc=See the [[dynamic_reconfigure]] package for details on dynamically reconfigurable parameters.
0.name= ~message
0.default= hello
0.type= str
0.desc=The message.
1.name= ~a
1.default= 1
1.type= int
1.desc=First number. Range: -100 to 100
2.name= ~b
2.default= 2
2.type= int
2.desc=First number. Range: -100 to 100
}
}
# End of autogenerated section. You may edit below.

View File

@ -0,0 +1,11 @@
<launch>
<node pkg="Steve" type="runpigpiod.sh" name="runpigpiod" respawn="false" output="log"/>
<node pkg="Steve" type="MastMonitor.py" name="MastMonitor" respawn="true" output="log"/>
<node pkg="Steve" type="StowedSwitch.py" name="StowedSwitch" respawn="true" output="log"/>
<node pkg="Steve" type="LimitSwitch.py" name="LimitSwitch" respawn="true" output="log"/>
<node pkg="Steve" type="RaisedSwitch.py" name="RaisedSwitch" respawn="true" output="log"/>
<node pkg="Steve" type="ForwardSwitch.py" name="ForwardSwitch" respawn="true" output="log"/>
<node pkg="Steve" type="Rotation.py" name="Rotation" respawn="true" output="log"/>
<node pkg="Steve" type="Elevation.py" name="Elevation" respawn="true" output="screen"/>
<node pkg="Steve" type="Servo.py" name="Servo" respawn="true" output="log"/>
</launch>

View File

@ -0,0 +1,5 @@
<launch>
<node pkg="Steve" type="Rotation.py" name="Rotation" respawn="true" output="screen"/>
<node pkg="Steve" type="Elevation.py" name="Elevation" respawn="true" output="screen"/>
<node pkg="Steve" type="Servo.py" name="Servo" respawn="true" output="screen"/>
</launch>

View File

@ -0,0 +1,6 @@
<launch>
<node pkg="Steve" type="runpigpiod.sh" name="runpigpiod" respawn="false" output="screen"/>
<!-- <node pkg="Steve" type="Rotation.py" name="Rotation" respawn="true" output="screen"/> -->
<!-- <node pkg="Steve" type="Elevation.py" name="Elevation" respawn="true" output="screen"/> -->
<node pkg="Steve" type="Servo.py" name="Servo" respawn="true" output="screen"/>
</launch>

View File

@ -0,0 +1,3 @@
<launch>
<node pkg="Steve" type="MastMonitor.py" name="MastMonitor" respawn="true" output="screen"/>
</launch>

View File

@ -0,0 +1,7 @@
<launch>
<node pkg="Steve" type="MastMonitor.py" name="MastMonitor" respawn="true" output="screen"/>
<node pkg="Steve" type="StowedSwitch.py" name="StowedSwitch" respawn="true" output="screen"/>
<node pkg="Steve" type="LimitSwitch.py" name="LimitSwitch" respawn="true" output="screen"/>
<node pkg="Steve" type="RaisedSwitch.py" name="RaisedSwitch" respawn="true" output="screen"/>
<node pkg="Steve" type="ForwardSwitch.py" name="ForwardSwitch" respawn="true" output="screen"/>
</launch>

View File

@ -0,0 +1,4 @@
<launch>
<node pkg="Steve" type="runpigpiod.sh" name="runpigpiod" respawn="false" output="screen"/>
<node pkg="Steve" type="MastMonitor.py" name="MastMonitor" respawn="true" output="screen"/>
</launch>

View File

@ -0,0 +1,6 @@
<launch>
<node pkg="Steve" type="StowedSwitch.py" name="StowedSwitch" respawn="true" output="screen"/>
<node pkg="Steve" type="LimitSwitch.py" name="LimitSwitch" respawn="true" output="screen"/>
<node pkg="Steve" type="RaisedSwitch.py" name="RaisedSwitch" respawn="true" output="screen"/>
<node pkg="Steve" type="ForwardSwitch.py" name="ForwardSwitch" respawn="true" output="screen"/>
</launch>

0
Steve/msg/__init__.py Normal file
View File

View File

@ -0,0 +1,3 @@
string message
int32 a
int32 b

75
Steve/package.xml Normal file
View File

@ -0,0 +1,75 @@
<?xml version="1.0"?>
<package format="2">
<name>Steve</name>
<version>0.0.0</version>
<description>The Steve package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="pi@todo.todo">pi</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/Steve</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>message_generation</build_depend>
<build_depend>dynamic_reconfigure</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>rospy</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<build_export_depend>sensor_msgs</build_export_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>message_runtime</exec_depend>
<exec_depend>dynamic_reconfigure</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>

View File

View File

View File

@ -0,0 +1,36 @@
## *********************************************************
##
## File autogenerated for the Steve package
## by the dynamic_reconfigure package.
## Please do not edit.
##
## ********************************************************/
from dynamic_reconfigure.encoding import extract_params
inf = float('inf')
config_description = {'upper': 'DEFAULT', 'lower': 'groups', 'srcline': 246, 'name': 'Default', 'parent': 0, 'srcfile': '/home/pi/ros_catkin_ws/src/dynamic_reconfigure/src/dynamic_reconfigure/parameter_generator.py', 'cstate': 'true', 'parentname': 'Default', 'class': 'DEFAULT', 'field': 'default', 'state': True, 'parentclass': '', 'groups': [], 'parameters': [{'srcline': 274, 'description': 'The message.', 'max': '', 'cconsttype': 'const char * const', 'ctype': 'std::string', 'srcfile': '/home/pi/ros_catkin_ws/src/dynamic_reconfigure/src/dynamic_reconfigure/parameter_generator.py', 'name': 'message', 'edit_method': '', 'default': 'hello', 'level': 0, 'min': '', 'type': 'str'}, {'srcline': 274, 'description': 'First number.', 'max': 100, 'cconsttype': 'const int', 'ctype': 'int', 'srcfile': '/home/pi/ros_catkin_ws/src/dynamic_reconfigure/src/dynamic_reconfigure/parameter_generator.py', 'name': 'a', 'edit_method': '', 'default': 1, 'level': 0, 'min': -100, 'type': 'int'}, {'srcline': 274, 'description': 'First number.', 'max': 100, 'cconsttype': 'const int', 'ctype': 'int', 'srcfile': '/home/pi/ros_catkin_ws/src/dynamic_reconfigure/src/dynamic_reconfigure/parameter_generator.py', 'name': 'b', 'edit_method': '', 'default': 2, 'level': 0, 'min': -100, 'type': 'int'}], 'type': '', 'id': 0}
min = {}
max = {}
defaults = {}
level = {}
type = {}
all_level = 0
#def extract_params(config):
# params = []
# params.extend(config['parameters'])
# for group in config['groups']:
# params.extend(extract_params(group))
# return params
for param in extract_params(config_description):
min[param['name']] = param['min']
max[param['name']] = param['max']
defaults[param['name']] = param['default']
level[param['name']] = param['level']
type[param['name']] = param['type']
all_level = all_level | param['level']

3
commands to run.txt Normal file
View File

@ -0,0 +1,3 @@
roslaunch Steve LaunchSteveAll.launch
rosrun Steve tests_cli.py

File diff suppressed because it is too large Load Diff

View File

@ -0,0 +1 @@

File diff suppressed because one or more lines are too long

File diff suppressed because it is too large Load Diff

File diff suppressed because it is too large Load Diff

View File

@ -0,0 +1 @@

View File

@ -0,0 +1,14 @@
https://raspberrypi.stackexchange.com/questions/94178/wifi-wont-start
sudo killall wpa_supplicant
sudo wpa_supplicant -c/etc/wpa_supplicant/wpa_supplicant.conf -iwlan0
--> Autostart-Script:
etc/rc.local abändern und code aus obigem Link einfügen!
Danach Konfiguration der Wifi-Setting:
* "Wireless and Wired Network Settings"
* Interface
* wlan0
* Aktivieren: "automatic configuration" && "disable IPv6"
* Alle Felder leeren

File diff suppressed because one or more lines are too long