Commit ed99bd04 authored by bucyril's avatar bucyril
Browse files

added crazy radio from Dusan

parent 4f836caa
......@@ -57,6 +57,7 @@ find_package(catkin REQUIRED COMPONENTS
add_message_files(
FILES
ViconData.msg
ControlParameters.msg
)
## Generate services in the 'srv' folder
......@@ -145,7 +146,7 @@ include_directories(
## 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/d_fall_pps_node.cpp)
add_executable(ViconDataNode src/ViconDataNode.cpp)
add_executable(ViconDataPublisher src/ViconDataPublisher.cpp)
add_executable(PPSClient src/PPSClient.cpp)
## Rename C++ executable without prefix
......@@ -165,8 +166,8 @@ add_executable(PPSClient src/PPSClient.cpp)
find_library(VICON_LIBRARY ViconDataStreamSDK_CPP HINTS lib/vicon)
target_link_libraries(ViconDataNode ${catkin_LIBRARIES})
target_link_libraries(ViconDataNode ${VICON_LIBRARY})
target_link_libraries(ViconDataPublisher ${catkin_LIBRARIES})
target_link_libraries(ViconDataPublisher ${VICON_LIBRARY})
target_link_libraries(PPSClient ${catkin_LIBRARIES})
......
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import roslib
roslib.load_manifest('d_fall_pps')
import rospy
from d_fall_pps.msg import ControlParameters
# General import
import time, sys
import struct
import logging
# Add library
#sys.path.append("lib")
# CrazyFlie client imports
import cflib
from cflib.crazyflie import Crazyflie
from cflib.crtp.crtpstack import CRTPPacket, CRTPPort
import cflib.drivers.crazyradio
# Logging import
from cflib.crazyflie.log import LogConfig
# Logging settings
logging.basicConfig(level=logging.ERROR)
class SimpleClient:
"""
Example script that runs several threads that read Vicon measurements
from a file and send it together with the setpoints to the Crazyflie.
It also employs a keyboard event detector that allows the user to
manipulate the setpoints with keys.
"""
def __init__(self, link_uri):
# Setpoints to be sent to the CrazyFlie
self.roll = 0.0
self.pitch = 0.0
self.yaw = 0.0
self.motor1cmd = 0.0
self.motor2cmd = 0.0
self.motor3cmd = 0.0
self.motor4cmd = 0.0
# Initialize the CrazyFlie and add callbacks
self._cf = Crazyflie()
# Add callbacks that get executed depending on the connection status.
self._cf.connected.add_callback(self._connected)
self._cf.disconnected.add_callback(self._disconnected)
self._cf.connection_failed.add_callback(self._connection_failed)
self._cf.connection_lost.add_callback(self._connection_lost)
# Connect to the Crazyflie
print "Connecting to %s" % link_uri
self._cf.open_link(link_uri)
def _connected(self, link_uri):
"""
This callback is executed as soon as the connection to the
quadrotor is established.
"""
#print "Connection to %s successful: " % link_uri
rospy.loginfo("Connection to %s successful: " % link_uri)
def _connection_failed(self, link_uri, msg):
"""Callback when connection initial connection fails (i.e no Crazyflie
at the specified address)"""
#print "Connection to %s failed: %s" % (link_uri, msg)
rospy.logerr("Connection to %s failed: %s" % (link_uri, msg))
def _connection_lost(self, link_uri, msg):
"""Callback when disconnected after a connection has been made (i.e
Crazyflie moves out of range)"""
#print "Connection to %s lost: %s" % (link_uri, msg)
rospy.logerr("Connection to %s lost: %s" % (link_uri, msg))
def _disconnected(self, link_uri):
"""Callback when the Crazyflie is disconnected (called in all cases)"""
#print "Disconnected from %s" % link_uri
rospy.logwarn("Disconnected from %s" % link_uri)
def _send_commands(self,cmd1,cmd2,cmd3,cmd4):
# Send setpoints at the given frequency.
# Fill the CRTP packet with the setpoints and send it to the stabilizer
pk = CRTPPacket()
pk.port = CRTPPort.STABILIZER
pk.data = struct.pack('<ffff', cmd1,cmd2,cmd3,cmd4)
self._cf.send_packet(pk)
#print "Motor commands: %f, %f, %f, %f" % (cmd1,cmd2,cmd3,cmd4)
def _send_to_commander(self,roll, pitch, yaw, thrust,cmd1,cmd2,cmd3,cmd4,mode):
pk = CRTPPacket()
pk.port = CRTPPort.COMMANDER
pk.data = struct.pack('<fffHHHHHH', roll, pitch, yaw, thrust,cmd1,cmd2,cmd3,cmd4,mode)
self._cf.send_packet(pk)
# self._cf.commander.send_setpoint (roll, pitch, yaw, thrust,cmd1,cmd2,cmd3,cmd4,mode)
# print "Motor commands: %f, %f, %f, %f" % (cmd1,cmd2,cmd3,cmd4)
def subscriberControllerOutputCallback(data):
#cf_client._send_commands(data.cmd1,data.cmd2,data.cmd3,data.cmd4)
cf_client._send_to_commander(data.roll,data.pitch,data.yaw,data.thrust,data.motorCmd1, data.motorCmd2, data.motorCmd3, data.motorCmd4, data.onboardControllerType)
#rospy.loginfo(data.onboardControllerType);
#rospy.loginfo(data.motorCmd1);
#rospy.logdebug_throttle(2,"sending motor commands to crazyflie: ")
if __name__ == '__main__':
rospy.init_node('CrazyRadio', anonymous=True)
cflib.crtp.init_drivers(enable_debug_driver=False)
while not rospy.is_shutdown():
# Initialize the low-level drivers (don't list the debug drivers)
# Scan for Crazyflies and use the first one found
rospy.loginfo("Scanning interfaces for Crazyflies...")
available=[]
available = cflib.crtp.scan_interfaces()
rospy.loginfo("Crazyflies found:")
for i in available:
print i[0]
if len(available) > 0:
# uri would can be specified directly, as for example: radio://0/70/250K
# instead of available[0][0]
global cf_client
cf_client = SimpleClient('radio://0/80/250K')
#cf_client = SimpleClient(available[0][0])
time.sleep(5.0)
rospy.Subscriber("FlightControl/topicControllerOutput", ControllerOutputPackage, subscriberControllerOutputCallback)
rospy.spin()
else:
rospy.logerr("No Crazyflies found, cannot run example")
#inp=raw_input('press any key');
time.sleep(0.5)
cf_client._cf.close_link()
# -*- coding: utf-8 -*-
#
# || ____ _ __
# +------+ / __ )(_) /_______________ _____ ___
# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
#
# Copyright (C) 2011-2013 Bitcraze AB
#
# Crazyflie Nano Quadcopter Client
#
# This program is free software; you can redistribute it and/or
# modify it under the terms of the GNU General Public License
# as published by the Free Software Foundation; either version 2
# of the License, or (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
# GNU General Public License for more details.
# You should have received a copy of the GNU General Public License along with
# this program; if not, write to the Free Software Foundation, Inc., 51
# Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
from .cfclient import main
try:
from .version import VERSION
except:
try:
import subprocess
VERSION = subprocess.check_output(["git", "describe"])
except:
VERSION = "dev"
try:
import subprocess
ret = subprocess.call(["git", "diff", "--quiet", "HEAD"])
if ret > 0:
VERSION += "+"
except:
VERSION += "+"
# -*- coding: utf-8 -*-
#
# || ____ _ __
# +------+ / __ )(_) /_______________ _____ ___
# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
#
# Copyright (C) 2011-2013 Bitcraze AB
#
# Crazyflie Nano Quadcopter Client
#
# This program is free software; you can redistribute it and/or
# modify it under the terms of the GNU General Public License
# as published by the Free Software Foundation; either version 2
# of the License, or (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
# GNU General Public License for more details.
# You should have received a copy of the GNU General Public License along with
# this program; if not, write to the Free Software Foundation, Inc., 51
# Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
"""Initialization of the PC Client GUI."""
__author__ = 'Bitcraze AB'
__all__ = ['']
import sys
import os
import argparse
import datetime
import logging
def main():
"""
Check starting conditions and start GUI.
First, check command line arguments and start loggers. Set log levels. Try
all imports and exit verbosely if a library is not found. Disable outputs
to stdout and start the GUI.
"""
# Set ERROR level for PyQt4 logger
qtlogger = logging.getLogger('PyQt4')
qtlogger.setLevel(logging.ERROR)
parser = argparse.ArgumentParser(description="cfclient - "
"Crazyflie graphical control client")
parser.add_argument('--debug', '-d', nargs=1, default='info', type=str,
help="set debug level "
"[minimal, info, debug, debugfile]")
args = parser.parse_args()
globals().update(vars(args))
cflogger = logging.getLogger('')
# Set correct logging fuctionality according to commandline
if ("debugfile" in debug):
logging.basicConfig(level=logging.DEBUG)
# Add extra format options for file logger (thread and time)
formatter = logging.Formatter('%(asctime)s:%(threadName)s:%(name)'
's:%(levelname)s:%(message)s')
filename = "debug-%s.log" % datetime.datetime.now()
filehandler = logging.FileHandler(filename)
filehandler.setLevel(logging.DEBUG)
filehandler.setFormatter(formatter)
cflogger.addHandler(filehandler)
elif ("debug" in debug):
logging.basicConfig(level=logging.DEBUG)
elif ("minimal" in debug):
logging.basicConfig(level=logging.WARNING)
elif ("info" in debug):
logging.basicConfig(level=logging.INFO)
logger = logging.getLogger(__name__)
logger.debug("Using config path {}".format(sys.path[1]))
logger.debug("sys.path={}".format(sys.path))
# Try all the imports used in the project here to control what happens....
try:
import usb
except ImportError:
logger.critical("No pyusb installation found, exiting!")
sys.exit(1)
if not sys.platform.startswith('linux'):
try:
import sdl2
except ImportError:
logger.critical("No pysdl2 installation found, exiting!")
sys.exit(1)
try:
import PyQt4
except ImportError:
logger.critical("No PyQT4 installation found, exiting!")
sys.exit(1)
# Disable printouts from STL
if os.name == 'posix':
stdout = os.dup(1)
os.dup2(os.open('/dev/null', os.O_WRONLY), 1)
sys.stdout = os.fdopen(stdout, 'w')
logger.info("Disabling STL printouts")
if os.name == 'nt':
stdout = os.dup(1)
os.dup2(os.open('NUL', os.O_WRONLY), 1)
sys.stdout = os.fdopen(stdout, 'w')
logger.info("Disabling STL printouts")
# Start up the main user-interface
from ui.main import MainUI
from PyQt4.QtGui import QApplication, QIcon
app = QApplication(sys.argv)
app.setWindowIcon(QIcon(sys.path[0] + "/cfclient/icon-256.png"))
# Make sure the right icon is set in Windows 7+ taskbar
if os.name == 'nt':
import ctypes
try:
myappid = 'mycompany.myproduct.subproduct.version'
ctypes.windll.shell32.SetCurrentProcessExplicitAppUserModelID(myappid)
except Exception:
pass
main_window = MainUI()
main_window.show()
sys.exit(app.exec_())
{
"writable" : {
"input_device": "",
"link_uri": "",
"flightmode": "Normal",
"open_tabs": "Flight Control",
"trim_pitch": 0.0,
"slew_limit": 45,
"slew_rate": 30,
"trim_roll": 0.0,
"max_thrust": 80,
"min_thrust": 25,
"max_yaw": 200,
"max_rp": 30,
"client_side_xmode": false,
"auto_reconnect": false,
"device_config_mapping": {},
"enable_debug_driver": false,
"input_device_blacklist": "(VirtualBox|VMware)",
"ui_update_period": 100,
"enable_zmq_input": false
},
"read-only" : {
"normal_slew_limit": 45,
"normal_slew_rate": 30,
"normal_max_thrust": 80,
"normal_min_thrust": 25,
"normal_max_yaw": 200,
"normal_max_rp": 30,
"default_cf_channel": 10,
"default_cf_speed": 0,
"default_cf_trim": 0
}
}
{
"inputconfig": {
"inputdevice":
{"name": "Generic 2-axis gamepad on OS X", "updateperiod":10,
"axis": [
{"name":"Roll", "type":"Input.AXIS", "id":3, "scale":1.0, "key":"roll"},
{"name":"Pitch", "type":"Input.AXIS", "id":4, "scale":-1.0, "key":"pitch"},
{"name":"Yaw", "type":"Input.AXIS", "id":0, "scale":1.0, "key":"yaw"},
{"name":"Thrust", "type":"Input.AXIS", "id":1, "scale":-1.0, "key":"thrust"},
{"name":"Pitch Cal+", "type":"Input.BUTTON", "id":6, "scale":-1.0, "key":"pitchNeg"},
{"name":"Pitch Cal-", "type":"Input.BUTTON", "id":4, "scale":1.0, "key":"pitchPos"},
{"name":"Roll Cal +", "type":"Input.BUTTON", "id":5, "scale":1.0, "key":"rollPos"},
{"name":"Roll Cal -", "type":"Input.BUTTON", "id":7, "scale":-1.0, "key":"rollNeg"},
{"name":"Killswtich", "type":"Input.BUTTON", "id":14, "scale":1.0, "key":"estop"},
{"name":"Exit", "type":"Input.BUTTON", "id":12, "scale":1.0, "key":"exit"}
]}
}
}
{
"inputconfig": {
"inputdevice": {
"updateperiod": 10,
"springythrottle": false,
"name": "Joystick",
"axis": [
{
"scale": -1.0,
"type": "Input.AXIS",
"id": 3,
"key": "thrust",
"name": "thrust"
},
{
"scale": 1.0,
"type": "Input.AXIS",
"id": 2,
"key": "yaw",
"name": "yaw"
},
{
"scale": 1.0,
"type": "Input.AXIS",
"id": 0,
"key": "roll",
"name": "roll"
},
{
"scale": -1.0,
"type": "Input.AXIS",
"id": 1,
"key": "pitch",
"name": "pitch"
},
{
"scale": 1.0,
"type": "Input.HAT",
"id": 0,
"key": "trim",
"name": "trim"
},
{
"scale": -1.0,
"type": "Input.BUTTON",
"id": 4,
"key": "pitchNeg",
"name": "pitchNeg"
},
{
"scale": 1.0,
"type": "Input.BUTTON",
"id": 5,
"key": "pitchPos",
"name": "pitchPos"
},
{
"scale": 1.0,
"type": "Input.BUTTON",
"id": 1,
"key": "estop",
"name": "killswitch"
},
{
"scale": -1.0,
"type": "Input.BUTTON",
"id": 2,
"key": "rollNeg",
"name": "rollNeg"
},
{
"scale": 1.0,
"type": "Input.BUTTON",
"id": 3,
"key": "rollPos",
"name": "rollPos"
},
{
"scale": 1.0,
"type": "Input.BUTTON",
"id": 0,
"key": "althold",
"name": "althold"
},
{
"scale": 1.0,
"type": "Input.BUTTON",
"id": 6,
"key": "exit",
"name": "exitapp"
}
]
}
}
}
{
"inputconfig": {
"inputdevice":
{"name": "Playstation 3 Mode 1", "updateperiod":10,
"axis": [
{"name":"Roll", "type":"Input.AXIS", "id":0, "scale":1.0, "key":"roll"},
{"name":"Pitch", "type":"Input.AXIS", "id":1, "scale":-1.0, "key":"pitch"},
{"name":"Yaw", "type":"Input.AXIS", "id":2, "scale":1.0, "key":"yaw"},
{"name":"Thrust", "type":"Input.AXIS", "id":3, "scale":-1.0, "key":"thrust"},
{"name":"Pitch Cal+", "type":"Input.BUTTON", "id":6, "scale":-1.0, "key":"pitchNeg"},
{"name":"Pitch Cal-", "type":"Input.BUTTON", "id":4, "scale":1.0, "key":"pitchPos"},
{"name":"Roll Cal +", "type":"Input.BUTTON", "id":5, "scale":1.0, "key":"rollPos"},
{"name":"Roll Cal -", "type":"Input.BUTTON", "id":7, "scale":-1.0, "key":"rollNeg"},
{"name":"Killswtich", "type":"Input.BUTTON", "id":14, "scale":1.0, "key":"estop"},
{"name":"Altitude hold", "type":"Input.BUTTON", "id":10, "scale":1.0, "key":"althold"},
{"name":"Exit", "type":"Input.BUTTON", "id":12, "scale":1.0, "key":"exit"}
]}
}
}
{
"inputconfig": {
"inputdevice":
{"name": "Playstation 3 Mode 2", "updateperiod":10,
"axis": [
{"name":"Roll", "type":"Input.AXIS", "id":2, "scale":1.0, "key":"roll"},
{"name":"Pitch", "type":"Input.AXIS", "id":3, "scale":-1.0, "key":"pitch"},
{"name":"Yaw", "type":"Input.AXIS", "id":0, "scale":1.0, "key":"yaw"},
{"name":"Thrust", "type":"Input.AXIS", "id":1, "scale":-1.0, "key":"thrust"},
{"name":"Pitch Cal+", "type":"Input.BUTTON", "id":6, "scale":-1.0, "key":"pitchNeg"},
{"name":"Pitch Cal-", "type":"Input.BUTTON", "id":4, "scale":1.0, "key":"pitchPos"},
{"name":"Roll Cal +", "type":"Input.BUTTON", "id":5, "scale":1.0, "key":"rollPos"},
{"name":"Roll Cal -", "type":"Input.BUTTON", "id":7, "scale":-1.0, "key":"rollNeg"},
{"name":"Killswtich", "type":"Input.BUTTON", "id":14, "scale":1.0, "key":"estop"},
{"name":"Exit", "type":"Input.BUTTON", "id":12, "scale":1.0, "key":"exit"}
]}
}
}
{
"inputconfig": {
"inputdevice": {
"updateperiod": 10,
"name": "PS3_Mode_3",
"axis": [
{
"scale": -1.0,
"type": "Input.AXIS",
"id": 1,
"key": "thrust",
"name": "thrust"
},
{
"scale": 1.0,
"type": "Input.AXIS",