From 0c8a437f1b2135895f19f94aebf08b92e1ebb9a6 Mon Sep 17 00:00:00 2001
From: muelmarc <muelmarc@student.ethz.ch>
Date: Fri, 31 Mar 2017 17:05:12 +0200
Subject: [PATCH] Publishers and callbacks to send data to crazyradio.py added
to PPSClient.pps; Crayzradio node added to ppsLaunch.launch; Subcriber added
to Crazyradio.py which successfully subscribes to motorCommand even if no cf
is connected.
---
.../src/d_fall_pps/crazyradio/CrazyRadio.py | 44 ++++++---
.../d_fall_pps/crazyradio/cflib/__init__.pyc | Bin 1182 -> 1182 bytes
.../crazyradio/cflib/crazyflie/__init__.pyc | Bin 15562 -> 15562 bytes
.../crazyradio/cflib/crazyflie/commander.pyc | Bin 2173 -> 2173 bytes
.../crazyradio/cflib/crazyflie/console.pyc | Bin 1678 -> 1678 bytes
.../crazyradio/cflib/crazyflie/log.pyc | Bin 19930 -> 19930 bytes
.../crazyradio/cflib/crazyflie/mem.pyc | Bin 27976 -> 27976 bytes
.../crazyradio/cflib/crazyflie/param.pyc | Bin 11522 -> 11522 bytes
.../cflib/crazyflie/platformservice.pyc | Bin 1580 -> 1580 bytes
.../crazyradio/cflib/crazyflie/toc.pyc | Bin 7071 -> 7071 bytes
.../crazyradio/cflib/crazyflie/toccache.pyc | Bin 3809 -> 3809 bytes
.../crazyradio/cflib/crtp/__init__.pyc | Bin 2559 -> 2559 bytes
.../crazyradio/cflib/crtp/crtpdriver.pyc | Bin 3669 -> 3669 bytes
.../crazyradio/cflib/crtp/crtpstack.pyc | Bin 5176 -> 5176 bytes
.../crazyradio/cflib/crtp/debugdriver.pyc | Bin 27858 -> 27858 bytes
.../crazyradio/cflib/crtp/exceptions.pyc | Bin 1069 -> 1069 bytes
.../crazyradio/cflib/crtp/radiodriver.pyc | Bin 12349 -> 12349 bytes
.../crazyradio/cflib/crtp/serialdriver.pyc | Bin 1867 -> 1867 bytes
.../crazyradio/cflib/crtp/udpdriver.pyc | Bin 3552 -> 3552 bytes
.../crazyradio/cflib/crtp/usbdriver.pyc | Bin 7434 -> 7434 bytes
.../crazyradio/cflib/drivers/__init__.pyc | Bin 254 -> 254 bytes
.../crazyradio/cflib/drivers/cfusb.pyc | Bin 5866 -> 5866 bytes
.../crazyradio/cflib/drivers/crazyradio.pyc | Bin 9622 -> 9622 bytes
.../crazyradio/cflib/utils/__init__.pyc | Bin 241 -> 241 bytes
.../crazyradio/cflib/utils/callbacks.pyc | Bin 1737 -> 1737 bytes
pps_ws/src/d_fall_pps/launch/ppsLaunch.launch | 5 +-
pps_ws/src/d_fall_pps/src/PPSClient.cpp | 90 ++++++++++++++----
.../d_fall_pps/src/SafeControllerService.cpp | 2 +-
28 files changed, 102 insertions(+), 39 deletions(-)
diff --git a/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py b/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py
index 782ff3d7..e137fc0c 100755
--- a/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py
+++ b/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py
@@ -1,9 +1,11 @@
#!/usr/bin/env python
# -*- coding: utf-8 -*-
-import roslib; roslib.load_manifest('crazypkg')
+import roslib; roslib.load_manifest('d_fall_pps')
import rospy
-from crazypkg.msg import ControllerOutputPackage
+from d_fall_pps.msg import AngleCommand
+from d_fall_pps.msg import RateCommand
+from d_fall_pps.msg import MotorCommand
# General import
@@ -91,25 +93,31 @@ class PPSRadioClient:
self._cf.send_packet(pk)
def motorCommandCallback(data):
- """Callback for motor controller actions"""
- cf_client._send_to_commander(0, 0, 0, data.motorCmd1, data.motorCmd2, data.motorCmd3, data.motorCmd4, CONTROLLER_MOTOR)
- rospy.loginfo("motor controller callback: %s, %s, %s, %s", data.motorCmd1, data.motorCmd2, data.motorCmd3, data.motorCmd4)
+ """Callback for motor controller actions"""
+ rospy.loginfo("test motorCommandCallback")
def angleCommandCallback(data):
- """Callback for angle controller actions"""
- cf_client._send_to_commander(data.roll,data.pitch,data.yaw,data.thrust, 0, 0, 0, 0, CONTROLLER_ANGLE)
- rospy.loginfo("angle controller callback: %s, %s, %s", data.roll,data.pitch,data.yaw,data.thrust)
+ """Callback for angle controller actions"""
+ ###cf_client._send_to_commander(data.roll,data.pitch,data.yaw,data.thrust, 0, 0, 0, 0, CONTROLLER_ANGLE)
+ ####rospy.loginfo("angle controller callback: %s, %s, %s", data.roll,data.pitch,data.yaw,data.thrust)
+ rospy.loginfo("test angleCommandCallback")
def rateCommandCallback(data):
- """Callback for rate controller actions"""
- cf_client._send_to_commander(data.roll,data.pitch,data.yaw,data.thrust, 0, 0, 0, 0, CONTROLLER_RATE)
- rospy.loginfo("rate controller callback: %s, %s, %s", data.roll,data.pitch,data.yaw,data.thrust)
+ """Callback for rate controller actions"""
+ ###cf_client._send_to_commander(data.roll,data.pitch,data.yaw,data.thrust, 0, 0, 0, 0, CONTROLLER_RATE)
+ ###rospy.loginfo("rate controller callback: %s, %s, %s", data.roll,data.pitch,data.yaw,data.thrust)
+ rospy.loginfo("test rateCommandCallback")
+
+def testCallback(data):
+ """Callback used to test data receipt if no crazyfly was found"""
+ rospy.loginfo("Crazyradio.py successfully subscribed and received testvalues: %f", data.cmd1)
if __name__ == '__main__':
rospy.init_node('CrazyRadio', anonymous=True)
# Initialize the low-level drivers (don't list the debug drivers)
cflib.crtp.init_drivers(enable_debug_driver=False)
+
while not rospy.is_shutdown():
# Scan for Crazyflies and use the first one found
@@ -126,13 +134,19 @@ if __name__ == '__main__':
cf_client = PPSRadioClient(available[0][0])
time.sleep(3.0)
#TODO: change publisher name if not correct
- rospy.Subscriber("PPSClient/MotorCommands", MotorCommand, motorCommandCallback)
- rospy.Subscriber("PPSClient/AngleCommands", AngleCommand, angleCommandCallback)
- rospy.Subscriber("PPSClient/RateCommands", RateCommand, rateCommandCallback)
+ rospy.Subscriber("PPSClient/topicMotorCommand", MotorCommand, motorCommandCallback)
+ rospy.loginfo("trying to subscribe")
+ #rospy.Subscriber("PPSClient/AngleCommands", AngleCommand, angleCommandCallback)
+ #rospy.Subscriber("PPSClient/RateCommands", RateCommand, rateCommandCallback)
rospy.spin()
else:
- rospy.logerr("No Crazyflies found, cannot run example")
+ #rospy.logerr("No Crazyflies found, cannot run example")
+ #for testing try to subscribe even if no crazyflie was found
+ rospy.loginfo("No Crazyflies found, still trying to subscribe")
+ rospy.Subscriber("PPSClient/topicMotorCommand", MotorCommand, testCallback)
+ rospy.spin()
+
time.sleep(0.5)
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/__init__.pyc b/pps_ws/src/d_fall_pps/crazyradio/cflib/__init__.pyc
index c691ab2630d8b6d69d40d6d126cd82676449f86c..f5e936ccbb0fd4da975be38c2a121c3f618e5b17 100644
GIT binary patch
delta 42
ycmbQoIggW_`7<w<!`#~&*{xXEQgqW2b8;qov8Zw-7bR9z7A2-+=1(qT(EtD+zYW9y
delta 42
ycmbQoIggW_`7<xqw&+tE*{xVOlZz6oD${Z@QzyH#sInI)7iAWd6i+T=(EtE2Iu1<$
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/__init__.pyc b/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/__init__.pyc
index 4ce7384997efaabbe7f55a3e6b14360cde89900a..b09a12f550842caeaa953ba1c7a810c6c4aaaf89 100644
GIT binary patch
delta 1026
zcmX?Ad8(3~`7<w<!`#~&*^^n=QgqW2b8;pZ%BXTB7bR9z7A2-+=1-oeAhUTlOBilx
z*~!VA{kSzxcIH}uN7jUUE*@DWp2>J**?6bomfie@*O3XQwCv=1feScgWi~GmG-Jdm
zExTDmI1!)pZ4qWX(vuCuzTq@mW^=jtGkluYOFqY`Sr%wC>*lvo)wm6i*}PL`F*9}(
zHcwPoj$5bf<SwOC*foL@$>gQVn{dl+UZJ9j$LP)SYPWGmmds{vO&J_+23x&DXAT~#
zm+Fe)wpwO0kG=>#=|Y1XeA0SGviPLu7)Rifb~UZUC(UoZ8+WM4PL{E1$K&lUR<5|s
KmYv*fV-Em#Q)`?6
delta 1026
zcmX?Ad8(3~`7<xqw&+tE*^^l~lZz6oD${Z@QztLvRb?+uF3Kz@DV{u0L1y!AmN4AX
zvXhfJ`*CZY?98<QkE{vzTs*Q$Jd^RrvhhyEExY**uOkyqY1zs30vB+~%4}XBXvT<B
zT6VLBa3VhG+ak<(q$eAQeZy(C%;s|OXZSR)mwb*>vn<eP*3ECFs&N}2vw5e?VrJ|n
zY@VpF9Jfx{$z4jPuxkV*lF3VzH{q7uyh244kI|du)o$aCESb&Tnld=t47PfQ&Kx{e
zFVz*pZMDp19(@sf(uD>&_@woWWbsMQF^<3|?P^+yPnzF+H||i8oh)P3j>p?ytXy%M
KEjzi}#vTA3J$;D)
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/commander.pyc b/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/commander.pyc
index 60e1d661e5f06bdbfeaf1bbe0d8449209016fa04..1a4551165f13f85143fbb07424587f5960cf074a 100644
GIT binary patch
delta 164
zcmew>@K=DH`7<w<!`#~&*|VA0QgqW2b8;r%Wme@%E=sJbEJ{qt%%AMQp|trV6E`DH
cX}QhnEM~ZM%T3<E)`U~D?Bo{qcAT=d0E6B+YXATM
delta 164
zcmew>@K=DH`7<xqw&+tE*|V8AlZz6oD${Z@Qzr{@sInI)7iAWd6i;^GP}+QwiJK9p
cwA^NO7Bk$s<tA@nYr?5nc5(}QJ5E_!03rf6xBvhE
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/console.pyc b/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/console.pyc
index 7dbd2d9d6d6da40aa68d74f41c7059d3ff08e269..7eda3dbedd01ce70ec7b102c900beb60cab58b61 100644
GIT binary patch
delta 129
zcmeC<?c-%<{>;neF!%OG_Nz>6DY|KiIXRQdSyZ`_ixR6UixN{Z^EV4J$1-A<nk>jH
TJ$WBXE*{xx)`d7^1KDf=`Y|hn
delta 129
zcmeC<?c-%<{>;m@E&9|(_Nz>s$wi4(m1#Mdsgn&^RoRP^i!uvJiZ=@~$1-A<nk>jH
TJ$WBXE*{xx)`d7^1KDf=K&UJ?
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/log.pyc b/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/log.pyc
index 40ace216e4a3a559c79ba9354e98c46ae7d1585c..d210b97174cb49fbd0506298c0ccae2dd1f3d8e8 100644
GIT binary patch
delta 980
zcmcaLoAK6cMt0`Uyj%`*Z*OFO$jz3bo0gc9Gx;O8Dpzt*VpU~PVoGNIWJ7g{$)-HN
z@W|HiZpS0r!1oT1>~8*coU-DZ`vl^d@km>Wlr!O!mYA#{!?KxC+z6kN2FX=;3}}#K
znOrTEN<f97^ddYe6lAjT%Cc_0E0d1flQNs9Dv09o!RGHuw;6F;uvuR9E)MC<hU%es
z44!;h^8ikB#W&yAI)z)a_~x&=!Z@r2rYG^uA^QCIq}LmW;gc3LV#8;5vWY!D-S15A
z;?pf|`JEM~za=I&NOEjWwx5gB0w<ZtfsU)O8?f2X=?pt|DR8KGdI{h(7Z_+uy=UW=
KmYCe@>k9x-&|a|s
delta 980
zcmcaLoAK6cMt0`Uyj<I&Pi<s>$jzBtlvq`nmXnz}d7_LedvS76W<g2uWJ7g{$)-HN
z@W|HiZpS0r!1oT1>~8*coU-DZ`vl^d@km>Wlr!O!mYA#{!?KxC+z6kN2FX=;3}}#K
znOrTEN<f97^ddYe6lAjT%Cc_0E0d1flQNs9Dv09o!RGHuw;6F;uvuR9E)MC<hU%es
z44!;h^8ikB#W&yAI)z)a_~x&=!Z@r2rYG^uA^QCIq}LmW;gc3LV#8;5vWY!D-S15A
z;?pf|`JEM~za=I&NOEjWwx5gB0w<ZtfsU)O8?f2X=?pt|DR8KGdI{h(7Z_+uy=UW=
KmYCe@>k9xi8DaVW
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/mem.pyc b/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/mem.pyc
index d61009696ecb5f8d5e8365430732bb8265f8f88e..81bb975962c1dd992cb3a97871cedbca6f3738ce 100644
GIT binary patch
delta 1775
zcmaKsODM!q6vq2A56vJ6|4V<SX+~Li<Smp(o*~bKQZt!kLNbGpJu4Q6mDe!Enp1W{
z3Rx<f5oJNhVq_=s7~A>o`ghLvefQjR??phl2q?zL_ZU;<`pus4JksFG8g!0~P<vCl
zX~5&0o$)wb?lH<WspK>0*`l*4l9EOJnS}Zb&k}lVT!vKeK~x<!{(_Av&6@im6>N`o
zM?kFcU91_q3NDJ1bgQ(Pq~~C~11qSsmhcGeLjn|xH&P&RMp`ulso*Yk4hPWKntYG%
zTu^K3Dbx;a1P!G1hOgkav_d>v!Cq@M4kpM^pd*8AH@x=mJ_`D<O-tyJ9o##&r|07%
zYkZyYEsJf<Op#z($lk!A*qftZYr!XZ9g>l-qo5h#`@&ajODV-u5_(%ajw!E}d?M3a
zlSWUa>$sV&%J7?3dB1#8GD@DP*u>8N%jc@{Vt2MzS7CjP4r=_knRaR~G3CR$H7qT7
zr*Ruc7qrqeEYWUhw&=oR-fCOIx+=|e+=Ooo{onr17YY6Q-S4`RB=TI@{eZDxPmk=-
g?Ce{_BU8Do|JV%U32t!D%e=MWWsLuKMH#LC0YETm2LJ#7
delta 1775
zcmX?ci}A!QMt0`Uyj<I&Pi<sB&dixylvq`nmXnz}*-=lGy*Rljv!JASvLToF<OOW3
zo8?)Q8SyCTV?BmPb^+T<JhE5Wr{I*8*u0Iy4Y$RAacUHwT*V!WQ&wWL3Qr~zPHCCV
z_xQMR+bXfyOaPB=@yYYWST}zVO2BP`_~bbv&v04~Hem7xA@<3)L@V*C%D^cru{lJ%
z2DbxbHY-X##2x1nll^56<1`zbA|@xxMPgU6`J<dE?y!~ET&(DX+hG!u6HHhpvnemf
zZZ<GIN=*KxT!BY+zY30c-yEoFjN8jHo3Cp8$LDNC9VtB0lRNb0;da#KVtq#3R!eL?
zXcUAeNH!;!_~4R$X!-`XwUedH%kaqFH7~?1yLq<7A6%9ia>-17VKo=G=E;|=ap!6A
z&6{jW@dV}O68rhMZH6Si6vtxRwr^H(a>Q-E%;aq@i*ak7yv+3!ZrROy-R9snT4M7u
z&qcVME-`tkcQzigeSPFvuuE@V7|@T~T=B^@LASAM1ef27gJ0p11(ojiLd5VGzu6(|
wDQ;<r%|#LTJiR$Fst<RNiEp-!J;06Ac@mpFGTZUlEt5S3w=^WJFy^`g09dJZh5!Hn
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/param.pyc b/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/param.pyc
index 2f3cf00bc16fad78ecb3bbf4bb7cdd384f04d9ac..300adf07e0912abb81c395eab49e516c272d5c0f 100644
GIT binary patch
delta 688
zcmZpQYKmfK{>;neF!%OG_M2>MDY|KiIXROXl~lQsixR6UixN{Z^C$aDNlw;dUyNIJ
zvph!&BTi|l&7U|wGU1Un;x)moTXORjK39Cw?gA3bcnn`BT!>G1uILwh((A>;@kmb&
zkvxXo>dpRA%(yj5ZC)rXgWJ)Pley(_hqlz_zw*_1f^+h5rE}OF1&m0k%}<m$@j1*!
jH4C5gSG8Dt(hD_wa7#;0w${qV<Fc<>)%Y~;({TX+&vWl-
delta 688
zcmZpQYKmfK{>;m@E&9|(_M2>+$wi4(m1#Mdsgo6?RN0G@i!uvJiYNO^Nlw;dUyNIJ
zvph!&BTi|l&7U|wGU1Un;x)moTXORjK39Cw?gA3bcnn`BT!>G1uILwh((A>;@kmb&
zkvxXo>dpRA%(yj5ZC)rXgWJ)Pley(_hqlz_zw*_1f^+h5rE}OF1&m0k%}<m$@j1*!
jH4C5gSG8Dt(hD_wa7#;0w${qV<Fc<>)%Y~;({TX+lD+SY
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/platformservice.pyc b/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/platformservice.pyc
index e0f185d94aab0d8b11573478ed5d61d8a63cfdf4..c983623f14c9112fa7340dc67dae8cf239564dad 100644
GIT binary patch
delta 133
zcmZ3(vxbMA`7<w<!`#~&*?E}QQgqW2b8;sCXIAA(E=sJbEJ{qt%%7~yqOv)Msf!V(
V^fH_>%9Ax&yK%~@Oy*^i2LJ*tEujDa
delta 133
zcmZ3(vxbMA`7<xqw&+tE*?E{alZz6oD${Z@Qz!pnR%I_vF3Kz@DW0s&qOv)Msf!V(
V^fH_>%9Ax&yK%~@Oy*^i2LOb$FI)fs
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/toc.pyc b/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/toc.pyc
index aa0e28b806abec352c54ff269edcf545f3adc1a8..4f1b78dbfca9968abd4baa1f92115eb83a5a37ac 100644
GIT binary patch
delta 485
zcmbPlKHr?3`7<w<!`#~&+21j-rRb(5=HyJ0Qsqi6O023ZN=(VjpWG-ZG1-avKQ2k}
z$-OK!I3+iSva&JamR-(fhey_sqX(y~#AGh6FSrbr*zC=1ibv~No($Ytn|N7phs5T~
r{AY0*EWY`TpeP<$UtviW-1cr3pNUg%8BT4JU8HW~k`$kuEo}k-z>}wX
delta 501
zcmbPlKHr?3`7<xqw&+tE+21j7CKn}ERi@=+rcO>|Q)MqsF3Kz@DW2RYDKXiJ`9Drs
z@yWd`HMnIrhqAIU;*nm?W`|GOkfR5;w8UgCt}i&vme}mgZHiCxS)L3$nwxl8@C3=`
s%lv0?n=QWiji4w#X<uPU7Cd%u7N3b*cNuPtlU<~4<CGPjoGonv0IEB)4FCWD
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/toccache.pyc b/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/toccache.pyc
index f7ec39db18ee58402fe076ba5710ded82671d339..b8a5da1d4d2221ea5b2848d45378c56f3f277164 100644
GIT binary patch
delta 226
zcmaDT`%sph`7<w<!`#~&*^`;sQgqW2b8;r<^Qm$r7bR9z7A2-+=1&&jl-<0CIg$yF
rbUIrJ4(ZJToJSdP>Xg~6#+`#ldh%nQ2Hct_Z{gj9Q&x8J4n9o)yEaTE
delta 226
zcmaDT`%sph`7<xqw&+tE*^`+$lZz6oD${Z@Qzt*<P-QPpF3Kz@DV{9ADZ6<Ob0iZU
r>2$Ud9MYQwIFB;o)G4!BjXMXA^yJ4p4Y)N=-om>Hr>yMc9ekPqb|g|8
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/__init__.pyc b/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/__init__.pyc
index 69bc9be5a104d24a54da83c47d8b7f4d927a029b..d51361206bc0697d66635adddef42367f3445640 100644
GIT binary patch
delta 150
zcmew_{9l-z`7<w<!`#~&*-tUCrRb(5=HyJi#iYuWT$ET<S(KQPnZKEhxt$S*)FBpK
VTv94*CvizBaYW;gn!JhA7XSx=HWvT@
delta 150
zcmew_{9l-z`7<xqw&+tE*-tTXCKn}ERi@=+rcS=jq{?2LT$EW*QoNasxt$S*)FBpK
VTv94*CvizBaYW;gn!JhA7XY|YI5+?R
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/crtpdriver.pyc b/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/crtpdriver.pyc
index b3a490ee42b9334900c9ee9cb7b6c67a8c54c7f4..ccd50c1a2b57d9278b134e4e69ca71755eafaea9 100644
GIT binary patch
delta 362
zcmcaAb5(|&`7<w<!`#~&*=?BEQgqW2b8;s0v#N3>7bR9z7A2-+=1*4OliNIvX*DAb
z=?!dBlP9p0;g;PT%BqH2^Df*PC+}omfI}AOrpaqK&f}2Xz<Cvy4A8Mxxc1<X1)2Jo
aI|GMo1J8OKG8@<=C)e`k;gD6}GX(%U-g~+L
delta 362
zcmcaAb5(|&`7<xqw&+tE*=?9OlZz6oD${Z@QztjFsj?R*7iAWd6i-&*liNIvX*DAb
z=?!dBlP9p0;g;PT%BqH2^Df*PC+}omfI}AOrpaqK&f}2Xz<Cvy4A8Mxxc1<X1)2Jo
aI|GMo1J8OKG8@<=C)e`k;gD6}GX(&%!iFsX
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/crtpstack.pyc b/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/crtpstack.pyc
index 3eb90eb5ed7b7874acca4dbdf74ef08c6117a12f..442e4dafa5e3fe75cdaf89fafdabe6efc3fad89e 100644
GIT binary patch
delta 506
zcmdm?u|tEM`7<w<!`#~&*$tW4QgqW2b8;p-GO2PU7bR9z7A2-+=1=~}C%L(eX*F(X
ziOF@WT{tyMOb%t+ghw`ny%CS>2Oh4;Ivf-6s$iQez}bXH#eL4Jcx729vvJ?XsX}tH
zK2HfwS&7Xbc+N25ls<}6Mq)Cvz+s%Sl9NRQkKmM*n5-&<+hobf7Q&P9=#>yr000ZT
Bv=smV
delta 506
zcmdm?u|tEM`7<xqw&+tE*$tUElZz6oD${Z@Qzu^(R%I_vF3Kz@DW3e1PjYh`(`ww(
z5|is#yKrijm>kNs36E?Ddm|p%4?J9xbvP#CRlzn{fU^mYiu;^b@yfDJX5+q%Q-$PY
zeV!7WvJ#s=@SI`9DSZ^DjKpMSfx|duB`1pr9>FOqF<Dgzx5<)|ErciI(JLXM006Mj
B#Ml4;
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/debugdriver.pyc b/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/debugdriver.pyc
index f28249048fede5906ec54557b98d959df95754e6..6b2576f59196bc4b770a3d5df22a1ff1427fd798 100644
GIT binary patch
delta 1103
zcmca~lkw6`Mt0`Uyj%`*Z*OF8Wn)XxO-szlnS9Y&l`FX@v8u8tF(os9@<FyGIAj)T
z$xq(NUV%eaF-v-LBgZEpJkp1ylbCTxA7qoBTq<9JN7hmy9jC1P<}V7xtT?5mH*Z&K
zXT&W%xldyy9@%!yr#NI6YE8i*0}72+?ITRMr6<qSn~qy{bA!G%p0L{d+lU{Re?eA9
zS>)l;4U(3y`HL$!K+>sp<~R)Be9?IpE}g(A+`QZMIzD?Jd4Ax)<7cH{MtlKQ99oV?
zvsKuA7Ch3K(bBja4)pTmD>3_VYo7c#HVe1x=DBgd@j3fNVig|Eo06RIxyv=B43B2+
g)OviueJeu<x7oPz8Ylrg&GNvd@gST0<i?yp01n!q{Qv*}
delta 1103
zcmca~lkw6`Mt0`Uyj<I&Pi<sxW#ddPO023(%gIcgywOONy*Rljv!JAS@<FyGIAj)T
z$xq(NUV%eaF-v-LBgZEpJkp1ylbCTxA7qoBTq<9JN7hmy9jC1P<}V7xtT?5mH*Z&K
zXT&W%xldyy9@%!yr#NI6YE8i*0}72+?ITRMr6<qSn~qy{bA!G%p0L{d+lU{Re?eA9
zS>)l;4U(3y`HL$!K+>sp<~R)Be9?IpE}g(A+`QZMIzD?Jd4Ax)<7cH{MtlKQ99oV?
zvsKuA7Ch3K(bBja4)pTmD>3_VYo7c#HVe1x=DBgd@j3fNVig|Eo06RIxyv=B43B2+
g)OviueJeu<x7oPz8Ylrg&GNvd@gST0<i?yp0AxC<AOHXW
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/exceptions.pyc b/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/exceptions.pyc
index 3eafb262e3d3ac3a49949b7b8635706859b222b8..60f4731de97ab8939d73b92c20b3cf87f32cdbfe 100644
GIT binary patch
delta 102
zcmZ3>v6h3K`7<w<!`#~&*}0k6QgqW2b8;q2GpTYV7bR9z7A2-+=1;a`mYST%v;wDW
MGfo-F$^0yG0Ft~QcK`qY
delta 102
zcmZ3>v6h3K`7<xqw&+tE*}0iGlZz6oD${Z@Qz!E?tFjj-7iAWd6i>EemYST%v;wDW
MGfo-F$^0yG0Oe&Nv;Y7A
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/radiodriver.pyc b/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/radiodriver.pyc
index 5c813e26bef192cc8be323764abc89307a9d2694..ba8b4dd57c47ce72b0371ab0982658814c939458 100644
GIT binary patch
delta 757
zcmdm+us4C7`7<w<!`#~&*(F)nQgqW2b8;pp3aWA?7bR9z7A2-+=1*4CkltLta*z#&
zw4jLe=0<KsCfw4KKPa$l{>Gchh)2nK{>gY`CkY(GBfCQIGY(mx(<i?c`iNb2v!I9;
zGj=JkpS#5m;*xHZUWi-cWCvN6$z?KCcvVcnE6XxjL#~~G3TAmiD(=Z2!eIs|&ORs{
z#1|oq%C$H&H%iM-zN9=2hb$<y>`>9dl^j4ZQozD8*-U*Kb^|smYACW|mzoT8_T)*r
WI5dLeY=)i;E>AT|D@^7zFaZGnN&$5M
delta 757
zcmdm+us4C7`7<xqw&+tE*(F&xlZz6oD${Z@QztJJQe`hrF3Kz@DW0sTA-%bP<scgl
zX+aU`&5hiOOt_^de^6lA{Eau05s#Af{FCv>P7*kVM|OqaXB@IXr%!$@^bx!4W<e1x
zX6#a6KX;2C#3kJ*y%4v?$quqClgnhP@T!=CSC(b6hFm)V70mL4RNRw4gu@I_oPAI@
zh%Z7Im1}WmZj_dvd`Wp44p~rY*`cC^D>;B-q=1EGvYGle>;`OB)KFx_E;SkG?8%dK
WacBg`*$h1yT%Kx_R+!9bU;+R#<q?Pg
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/serialdriver.pyc b/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/serialdriver.pyc
index cf7f19bdc9799ac60bf5936db90efd5de8f4a75e..a750b4443eac55cb030929271f9cd05899348aec 100644
GIT binary patch
delta 193
zcmX@jcbbo#`7<w<!`#~&*_oKwQgqW2b8;qMWLD)$E=sJbEJ{qt%%5z)CcQb5DW4Iy
e^kfN^JUp`3STgX)_Oa&TlwFBaMtZU)yAlBM@j@X0
delta 193
zcmX@jcbbo#`7<xqw&+tE*_oI)lZz6oD${Z@QztXBsj?R*7iAWd6i+r_linQ3l+TD;
eda?vd9v<0iEE#xY`&e^v%C5vIBRyG@T?qhBw?FIv
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/udpdriver.pyc b/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/udpdriver.pyc
index cb845b82eba07b47050b5b3820bc2441ef2e033a..d1bd738f1d44c57b3967c315484bead65cc82ee4 100644
GIT binary patch
delta 315
zcmaDL{Xm+X`7<w<!`#~&+5MQ<QgqW2b8;qs<Wl8IE=sJbEJ{qt%%8lFMPl<Jrc;dA
zr8h5RQNX8@k3AEIPLRQqk8n)DZZ24MA?Hjyvh7@ncw|kvGw{g1;;z9dD>-=!&sLnW
K5|g*_Y5@QrrgSL)
delta 315
zcmaDL{Xm+X`7<xqw&+tE+5MO}lZz6oD${Z@Qztucsj?R*7iAWd6i;5rBC&Z9(<w&m
z(wi5uDB#n{$DWBpC&=K*M>r;6Hy13skaH#;*><i(JhG<T8F*x0ao6CKm7KhVXDd!w
JiOJh|wE(>EaeM#(
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/usbdriver.pyc b/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/usbdriver.pyc
index 0706d7d037264bc184b7d997125cf8538a1288fe..ce85a2d9158d09302e4287d02ca52494a49aea28 100644
GIT binary patch
delta 496
zcmeCO>at>I{>;neF!%OG_Fv3wDY|KiIXROJxK+85ixR6UixN{Z^CuUGNNo0FnaYAw
zT5_`?=Ql<?(xu$9aZ5`~j^*vet$DHq-)`Kpn;rRm@EE=MmcVJ;(vp)u3r)gp_T<aL
y88~HE;*^otoF=*px1|!3bH%sg(Of9O#e_q8gH!_!8Bi3}O7q~hRd(`i8GQg4IH@-P
delta 496
zcmeCO>at>I{>;m@E&9|(_Fv4L$wi4(m1#MdsgoT=RN0G@i!uvJiYFI{NNo0FnaYAw
zT5_`?=Ql<?(xu$9aZ5`~j^*vet$DHq-)`Kpn;rRm@EE=MmcVJ;(vp)u3r)gp_T<aL
y88~HE;*^otoF=*px1|!3bH%sg(Of9O#e_q8gH!_!8Bi3}O7q~hRd(`i8GQh9m$8Zf
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/drivers/__init__.pyc b/pps_ws/src/d_fall_pps/crazyradio/cflib/drivers/__init__.pyc
index 55c08d1fb913181c63048089e35802f5bfab0ad3..3a1be3d44723ddc6e2148aa5d17d34b9b6948f6e 100644
GIT binary patch
delta 39
vcmeyz_>YmD`7<w<!`#~w*(b53=%yv+<V;+o%9UJ{SXEh+n39=4@tgqwDZ&p_
delta 39
vcmeyz_>YmD`7<xqw&+t6*(Y%(7bR9zrsZU&PMojGUYuN%Sx{0u@tgqwHs253
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/drivers/cfusb.pyc b/pps_ws/src/d_fall_pps/crazyradio/cflib/drivers/cfusb.pyc
index a9e4663144f91afb1d934d409fa194847e80b9b9..8c8e137aa5f0c8add477e9fb323ae6583c6f6e8d 100644
GIT binary patch
delta 378
zcmaE*`%0Ic`7<w<!`#~&+3z#6rRb(5=HyI%&#cOoT$ET<S(KQPnLjy^TVk^@%R)TT
zlNC9x;?_Lbne!JO*)Fa{IA!1Al#$pxhligDkMs||<BYhaC-Vp{#VISk`LAFMZq4GG
VmkLYZmX_GeCF+Amdh%j1cK{(3h-?4=
delta 378
zcmaE*`%0Ic`7<xqw&+tE+3z!RCKn}ERi@=+rcU0-r^;TOT$EW*Qam}4TVk^@%R)TT
zlNC9x;?_Lbne!JO*)Fa{IA!1Al#$pxhligDkMs||<BYhaC-Vp{#VISk`LAFMZq4GG
VmkLYZmX_GeCF+Amdh%j1cK}v?ix&U@
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/drivers/crazyradio.pyc b/pps_ws/src/d_fall_pps/crazyradio/cflib/drivers/crazyradio.pyc
index d1609aa6f8b03c5cca508565b9065d939f830156..fc0771ba3cc7a0596f8cb15beeb6f4c3ad6c6b0c 100644
GIT binary patch
delta 619
zcmbQ{J<Xe)`7<w<!`#~&*+p2`QgqW2b8;p-NUL%s7bR9z7A2-+=5G#Q;bX)uH93Gq
zX7hShIX0ZqvYX|33~}q0oxF(m9Zt<Mn;-I-;MOcNIZfakZq1Wh1&`p7oh5V!x9sNQ
z!n(LEmYwV-x*oUY$!=n2@yMo#$KsTg-7GAjfy3g>1(L^cNKGz~RNFjBTAmrF!7`JX
h<p1NA-5jJ)fy-=A97`)n;FE4q&c-b*JK0e+6aeXJ&c*-$
delta 619
zcmbQ{J<Xe)`7<xqw&+tE*+p15lZz6oD${Z@Qzt93sInI)7iAWd6mJe-;bX)uH93Gq
zX7hShIX0ZqvYX|33~}q0oxF(m9Zt<Mn;-I-;MOcNIZfakZq1Wh1&`p7oh5V!x9sNQ
z!n(LEmYwV-x*oUY$!=n2@yMo#$KsTg-7GAjfy3g>1(L^cNKGz~RNFjBTAmrF!7`JX
h<p1NA-5jJ)fy-=A97`)n;FE4q&c-b*JK0e+6aaF*(&YdE
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/utils/__init__.pyc b/pps_ws/src/d_fall_pps/crazyradio/cflib/utils/__init__.pyc
index 9cb912d8126dc08dbd5fbdcd9e63c645575bff6f..9a836124b1252f7c20851b151691a737f68a4bde 100644
GIT binary patch
delta 39
vcmey!_>qyF`7<w<!`#~w+1uGtbkh=Zawbk!<w`C}tg0+ZOv%ijcvuerBnuB0
delta 39
vcmey!_>qyF`7<xqw&+t6+1ojjixR6U({eIXCr(jiFHSDXEGQ|ScvuerF-Z@g
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/utils/callbacks.pyc b/pps_ws/src/d_fall_pps/crazyradio/cflib/utils/callbacks.pyc
index 7a74eeba740be7823fb8adb7248b5c4ead6e1897..74e35185cd8928cfd392adc026a7e4c35c1703a1 100644
GIT binary patch
delta 188
zcmX@fdy<!(`7<w<!`#~&*=I1arRb(5=HyJi$E3=YT$ET<S(KQPnLn9}S!(h-#^*R?
fPvDf1nykoj9*?X&YXJ_~C#=;tWI(Q2$0iK`>TN>`
delta 188
zcmX@fdy<!(`7<xqw&+tE*=I0vCKn}ERi@=+rcS=iq{?2LT$EW*QaqW9S!(h-#^*R?
fPvDf1nykoj9*?X&YXJ_~C#=;tWI(Q2$0iK`@Igh9
diff --git a/pps_ws/src/d_fall_pps/launch/ppsLaunch.launch b/pps_ws/src/d_fall_pps/launch/ppsLaunch.launch
index 8075f512..b2bb001d 100644
--- a/pps_ws/src/d_fall_pps/launch/ppsLaunch.launch
+++ b/pps_ws/src/d_fall_pps/launch/ppsLaunch.launch
@@ -1,7 +1,6 @@
<launch>
- <!-- Might be needed when our CrazyRadio works. There is no CrazyRadio.launch file yet!
- <include file="$(find d_fall_pps)/launch/CrazyRadio.launch" />
- -->
+
+ <node pkg="d_fall_pps" name="CrazyRadio" output="screen" type="CrazyRadio.py"/>
<node pkg="d_fall_pps" name="ViconDataPublisher" output="screen" type="ViconDataPublisher">
</node>
diff --git a/pps_ws/src/d_fall_pps/src/PPSClient.cpp b/pps_ws/src/d_fall_pps/src/PPSClient.cpp
index c95e2149..481eeb7b 100644
--- a/pps_ws/src/d_fall_pps/src/PPSClient.cpp
+++ b/pps_ws/src/d_fall_pps/src/PPSClient.cpp
@@ -26,6 +26,9 @@
#include "ros/ros.h"
#include "d_fall_pps/ViconData.h"
#include "d_fall_pps/RateController.h"
+#include "d_fall_pps/AngleCommand.h"
+#include "d_fall_pps/RateCommand.h"
+#include "d_fall_pps/MotorCommand.h"
using namespace d_fall_pps;
@@ -36,6 +39,14 @@ std::string cflie;
//global sevices
ros::ServiceClient rateClient;
+ros::Publisher AngleCommandPublisher;
+ros::Publisher RateCommandPublisher;
+ros::Publisher MotorCommandPublisher;
+
+//uncommenting the next line causes FATAL Error at runtime: "You must call ros::init() before creating the first NodeHandle"
+//ros::NodeHandle nodeHandle;
+
+
//extract data from "data" and publish/add to service for controller
//not void: sould give back controlldata
void ppsClientToController(ViconData data){
@@ -138,7 +149,7 @@ void CControlMgr::SendToCrazyflie(ControllerOutput package)
{
if(m_isStopped)
{
- m_packageToSend.motorCmd1=0;
+ c.motorCmd1=0;
m_packageToSend.motorCmd2=0;
m_packageToSend.motorCmd3=0;
m_packageToSend.motorCmd4=0;
@@ -168,6 +179,48 @@ void CControlMgr::SendToCrazyflie(ControllerOutput package)
*/
//<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
+
+
+//callback method to publish d_fall_pps::AngleCommand
+void callbackAngleCommand(const ros::TimerEvent&)
+{
+ d_fall_pps::AngleCommand AngleCommandPkg;
+ AngleCommandPkg.rollAngle = 1;
+ AngleCommandPkg.pitchAngle = 1;
+ AngleCommandPkg.yawAngle = 1;
+
+ AngleCommandPublisher.publish(AngleCommandPkg);
+ ROS_INFO_STREAM("AngleCommandTimer pubslishes: " << AngleCommandPkg.rollAngle << ", " << AngleCommandPkg.pitchAngle << ", " << AngleCommandPkg.yawAngle);
+}
+
+//callback method to publish d_fall_pps::RateCommand
+void callbackRateCommand(const ros::TimerEvent&)
+{
+ d_fall_pps::RateCommand RateCommandPkg;
+ RateCommandPkg.rollRate = 2;
+ RateCommandPkg.pitchRate = 2;
+ RateCommandPkg.yawRate = 2;
+
+ RateCommandPublisher.publish(RateCommandPkg);
+ ROS_INFO_STREAM("RateCommandTimer pubslishes: " << RateCommandPkg.rollRate << ", " << RateCommandPkg.pitchRate << ", " << RateCommandPkg.yawRate);
+}
+
+//callback method to publish d_fall_pps::MotorCommand
+void callbackMotorCommand(const ros::TimerEvent&)
+{
+ d_fall_pps::MotorCommand MotorCommandPkg;
+ MotorCommandPkg.cmd1 = 3;
+ MotorCommandPkg.cmd2 = 3;
+ MotorCommandPkg.cmd3 = 3;
+ MotorCommandPkg.cmd4 = 3;
+
+ MotorCommandPublisher.publish(MotorCommandPkg);
+ ROS_INFO_STREAM("MotorCommandTimer pubslishes: " << MotorCommandPkg.cmd1 << ", " << MotorCommandPkg.cmd2 << ", " << MotorCommandPkg.cmd3 << ", " << MotorCommandPkg.cmd4);
+}
+
+
+
+
int main(int argc, char* argv[]){
ROS_INFO_STREAM("PPSClient started");
@@ -183,26 +236,26 @@ int main(int argc, char* argv[]){
ROS_ERROR("Failed to get CrazyFlieName");
}
- ROS_INFO_STREAM("about to subscribe");
ros::Subscriber ViconSubscriber = nodeHandle.subscribe("/ViconDataPublisher/ViconData", 1, viconCallback);
- ROS_INFO_STREAM("subscribed");
+ ROS_INFO_STREAM("successfully subscribed to ViconData");
+
+ //ros::Timers to call method that publishes controller outputs for crayzradio node
/*
- //publish package_for_crazyradio>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
- //m_pNodeHandle=nodeHandle;
- //m_pCallbackQueueControlMgr=new ros::CallbackQueue();
- //m_pNodeHandle->setCallbackQueue(m_pCallbackQueueControlMgr);
-
-
- ROS_INFO_STREAM("creating publishers for package_for_crazyradio");
- ros::Pubslisher AngleCommandsPublisher = nodeHandle.advertise <d_fall_pps::AngleCommandsPackage>("AngleCommands", 10));
- ros::Pubslisher AngleCommandsPublisher = nodeHandle.advertise <d_fall_pps::RateCommandsPackage>("RateCommands", 10));
- ros::Pubslisher AngleCommandsPublisher = nodeHandle.advertise <d_fall_pps::MotorCommandsPackage>("MotorCommands", 10));
-
- //<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
+ Timers let you schedule a callback to happen at a specific rate through the same callback queue mechanism used by subscription, service, etc. callbacks.
+ Timers are not a realtime thread/kernel replacement, rather they are useful for things that do not have hard realtime requirements.
+ Reference: http://wiki.ros.org/roscpp/Overview/Timers
*/
-
-
+ ROS_INFO("creating publishers for package_for_crazyradio");
+ ros::Timer AngleCommandTimer = nodeHandle.createTimer(ros::Duration(0.1), callbackAngleCommand);
+ ros::Timer RateCommandTimer = nodeHandle.createTimer(ros::Duration(0.1), callbackRateCommand);
+ ros::Timer MotorCommandTimer = nodeHandle.createTimer(ros::Duration(0.1), callbackMotorCommand);
+
+
+ //ros::Publishers to advertise on the three command type topics
+ AngleCommandPublisher = nodeHandle.advertise <d_fall_pps::AngleCommand>("topicAngleCommand", 1000);
+ RateCommandPublisher = nodeHandle.advertise<d_fall_pps::RateCommand>("topicRateCommand", 1000);
+ MotorCommandPublisher = nodeHandle.advertise <d_fall_pps::MotorCommand>("topicMotorCommand", 1000);
//service: now only one available: to add several services depending on controller
@@ -210,9 +263,6 @@ int main(int argc, char* argv[]){
-
-
-
ros::spin();
return 0;
}
diff --git a/pps_ws/src/d_fall_pps/src/SafeControllerService.cpp b/pps_ws/src/d_fall_pps/src/SafeControllerService.cpp
index 122fec3b..e56560cf 100644
--- a/pps_ws/src/d_fall_pps/src/SafeControllerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/SafeControllerService.cpp
@@ -55,7 +55,7 @@ int main(int argc, char* argv[]) {
ros::NodeHandle nodeHandle("~");
ros::ServiceServer service = nodeHandle.advertiseService("RateController", calculateControlOutput);
- ROS_INFO("SafeControllerService ready");
+ ROS_INFO("SafeControllerService ready to send");
ros::spin();
return 0;
--
GitLab