From 4fbed7a57cc519ee08d9ea43a90fa029e3cc8e02 Mon Sep 17 00:00:00 2001 From: cesar Date: Fri, 18 Mar 2022 11:50:09 -0300 Subject: [PATCH] Added service to set waypoints --- config/px4_config.yaml | 270 +++++++++++++++++++++++++++++ config/px4_pluginlists.yaml | 15 ++ frames.gv | 9 - frames.pdf | Bin 17277 -> 0 bytes launch/cortex_bridge.launch | 3 + launch/klausen_dampen.launch | 5 + src/LinkState.py | 46 +++-- src/MoCap_Localization_noTether.py | 56 +++--- src/klausen_control.py | 3 - src/offb_node.cpp | 1 - src/path_follow.cpp | 39 +++-- src/ref_signalGen.py | 165 ++++++++++-------- src/square.py | 30 ++-- src/step.py | 10 +- src/wpoint_tracker.py | 12 +- srv/WaypointTrack.srv | 5 +- 16 files changed, 495 insertions(+), 174 deletions(-) create mode 100644 config/px4_config.yaml create mode 100644 config/px4_pluginlists.yaml delete mode 100644 frames.gv delete mode 100644 frames.pdf diff --git a/config/px4_config.yaml b/config/px4_config.yaml new file mode 100644 index 0000000..8a0d62f --- /dev/null +++ b/config/px4_config.yaml @@ -0,0 +1,270 @@ +# Common configuration for PX4 autopilot +# +# node: +startup_px4_usb_quirk: true + +# --- system plugins --- + +# sys_status & sys_time connection options +conn: + heartbeat_rate: 1.0 # send hertbeat rate in Hertz + timeout: 10.0 # hertbeat timeout in seconds + timesync_rate: 10.0 # TIMESYNC rate in Hertz (feature disabled if 0.0) + system_time_rate: 1.0 # send system time to FCU rate in Hertz (disabled if 0.0) + +# sys_status +sys: + min_voltage: 10.0 # diagnostics min voltage + disable_diag: false # disable all sys_status diagnostics, except heartbeat + +# sys_time +time: + time_ref_source: "fcu" # time_reference source + timesync_mode: MAVLINK + timesync_avg_alpha: 0.6 # timesync averaging factor + +# --- mavros plugins (alphabetical order) --- + +# 3dr_radio +tdr_radio: + low_rssi: 40 # raw rssi lower level for diagnostics + +# actuator_control +# None + +# command +cmd: + use_comp_id_system_control: false # quirk for some old FCUs + +# dummy +# None + +# ftp +# None + +# global_position +global_position: + frame_id: "map" # origin frame + child_frame_id: "base_link" # body-fixed frame + rot_covariance: 99999.0 # covariance for attitude? + gps_uere: 1.0 # User Equivalent Range Error (UERE) of GPS sensor (m) + use_relative_alt: true # use relative altitude for local coordinates + tf: + send: false # send TF? + frame_id: "map" # TF frame_id + global_frame_id: "earth" # TF earth frame_id + child_frame_id: "base_link" # TF child_frame_id + +# imu_pub +imu: + frame_id: "base_link" + # need find actual values + linear_acceleration_stdev: 0.0003 + angular_velocity_stdev: 0.0003490659 // 0.02 degrees + orientation_stdev: 1.0 + magnetic_stdev: 0.0 + +# local_position +local_position: + frame_id: "map" + tf: + send: false + frame_id: "map" + child_frame_id: "base_link" + send_fcu: false + +# param +# None, used for FCU params + +# rc_io +# None + +# safety_area +safety_area: + p1: {x: 1.0, y: 1.0, z: 1.0} + p2: {x: -1.0, y: -1.0, z: -1.0} + +# setpoint_accel +setpoint_accel: + send_force: false + +# setpoint_attitude +setpoint_attitude: + reverse_thrust: false # allow reversed thrust + use_quaternion: true # enable PoseStamped topic subscriber + tf: + listen: false # enable tf listener (disable topic subscribers) + frame_id: "map" + child_frame_id: "target_attitude" + rate_limit: 50.0 + +setpoint_raw: + thrust_scaling: 1.0 # used in setpoint_raw attitude callback. + # Note: PX4 expects normalized thrust values between 0 and 1, which means that + # the scaling needs to be unitary and the inputs should be 0..1 as well. + +# setpoint_position +setpoint_position: + tf: + listen: false # enable tf listener (disable topic subscribers) + frame_id: "map" + child_frame_id: "target_position" + rate_limit: 50.0 + mav_frame: LOCAL_NED + +# setpoint_velocity +setpoint_velocity: + mav_frame: LOCAL_NED + +# vfr_hud +# None + +# waypoint +mission: + pull_after_gcs: true # update mission if gcs updates + +# --- mavros extras plugins (same order) --- + +# adsb +# None + +# debug_value +# None + +# distance_sensor +## Currently available orientations: +# Check http://wiki.ros.org/mavros/Enumerations +## +distance_sensor: + hrlv_ez4_pub: + id: 0 + frame_id: "hrlv_ez4_sonar" + orientation: PITCH_270 # RPY:{0.0, 270.0, 0.0} - downward-facing + field_of_view: 0.0 # XXX TODO + send_tf: true + sensor_position: {x: 0.0, y: 0.0, z: -0.1} + lidarlite_pub: + id: 1 + frame_id: "lidarlite_laser" + orientation: PITCH_270 + field_of_view: 0.0 # XXX TODO + send_tf: true + sensor_position: {x: 0.0, y: 0.0, z: -0.1} + sonar_1_sub: + subscriber: true + id: 2 + orientation: PITCH_270 + laser_1_sub: + subscriber: true + id: 3 + orientation: PITCH_270 + +# image_pub +image: + frame_id: "px4flow" + +# fake_gps +fake_gps: + # select data source + use_mocap: true # ~mocap/pose + mocap_transform: false # ~mocap/tf instead of pose + use_vision: false # ~vision (pose) + # origin (default: Zürich) + geo_origin: + lat: 47.3667 # latitude [degrees] + lon: 8.5500 # longitude [degrees] + alt: 408.0 # altitude (height over the WGS-84 ellipsoid) [meters] + eph: 2.0 + epv: 2.0 + satellites_visible: 5 # virtual number of visible satellites + fix_type: 3 # type of GPS fix (default: 3D) + tf: + listen: false + send: false # send TF? + frame_id: "map" # TF frame_id + child_frame_id: "fix" # TF child_frame_id + rate_limit: 10.0 # TF rate + gps_rate: 5.0 # GPS data publishing rate + +# landing_target +landing_target: + listen_lt: false + mav_frame: "LOCAL_NED" + land_target_type: "VISION_FIDUCIAL" + image: + width: 640 # [pixels] + height: 480 + camera: + fov_x: 2.0071286398 # default: 115 [degrees] + fov_y: 2.0071286398 + tf: + send: true + listen: false + frame_id: "landing_target" + child_frame_id: "camera_center" + rate_limit: 10.0 + target_size: {x: 0.3, y: 0.3} + +# mocap_pose_estimate +mocap: + # select mocap source + use_tf: false # ~mocap/tf + use_pose: true # ~mocap/pose + +# odom +odometry: + in: + frame_id: "odom" + child_frame_id: "base_link" + frame_tf: + local_frame: "local_origin_ned" + body_frame_orientation: "flu" + out: + frame_tf: + # available: check MAV_FRAME odometry local frames in + # https://mavlink.io/en/messages/common.html + local_frame: "vision_ned" + # available: ned, frd or flu (though only the tf to frd is supported) + body_frame_orientation: "frd" + +# px4flow +px4flow: + frame_id: "px4flow" + ranger_fov: 0.118682 # 6.8 degrees at 5 meters, 31 degrees at 1 meter + ranger_min_range: 0.3 # meters + ranger_max_range: 5.0 # meters + +# vision_pose_estimate +vision_pose: + tf: + listen: false # enable tf listener (disable topic subscribers) + frame_id: "map" + child_frame_id: "vision_estimate" + rate_limit: 10.0 + +# vision_speed_estimate +vision_speed: + listen_twist: true # enable listen to twist topic, else listen to vec3d topic + twist_cov: true # enable listen to twist with covariance topic + +# vibration +vibration: + frame_id: "base_link" + +# wheel_odometry +wheel_odometry: + count: 2 # number of wheels to compute odometry + use_rpm: false # use wheel's RPM instead of cumulative distance to compute odometry + wheel0: {x: 0.0, y: -0.15, radius: 0.05} # x-, y-offset (m,NED) and radius (m) + wheel1: {x: 0.0, y: 0.15, radius: 0.05} # x-, y-offset (m,NED) and radius (m) + send_raw: true # send wheel's RPM and cumulative distance (~/wheel_odometry/rpm, ~/wheel_odometry/distance) + send_twist: false # send geometry_msgs/TwistWithCovarianceStamped instead of nav_msgs/Odometry + frame_id: "map" # origin frame + child_frame_id: "base_link" # body-fixed frame + vel_error: 0.1 # wheel velocity measurement error 1-std (m/s) + tf: + send: true + frame_id: "map" + child_frame_id: "base_link" + +# vim:set ts=2 sw=2 et: diff --git a/config/px4_pluginlists.yaml b/config/px4_pluginlists.yaml new file mode 100644 index 0000000..f9087de --- /dev/null +++ b/config/px4_pluginlists.yaml @@ -0,0 +1,15 @@ +plugin_blacklist: +# common +- safety_area +# extras +- image_pub +- vibration +- distance_sensor +- rangefinder +- wheel_odometry +- mission +- tdr_radio +- fake_gps + +plugin_whitelist: [] +#- 'sys_*' diff --git a/frames.gv b/frames.gv deleted file mode 100644 index 3ab6af0..0000000 --- a/frames.gv +++ /dev/null @@ -1,9 +0,0 @@ -digraph G { -"local_origin" -> "local_origin_ned"[label="Broadcaster: /mavros\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 124.428 sec old)\nBuffer length: 0.000 sec\n"]; -"fcu" -> "fcu_frd"[label="Broadcaster: /mavros\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 124.428 sec old)\nBuffer length: 0.000 sec\n"]; -"fcu_frd" -> "fcu"[label="Broadcaster: /mavros\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 124.428 sec old)\nBuffer length: 0.000 sec\n"]; -edge [style=invis]; - subgraph cluster_legend { style=bold; color=black; label ="view_frames Result"; -"Recorded at time: 124.428"[ shape=plaintext ] ; - }->"local_origin"; -} \ No newline at end of file diff --git a/frames.pdf b/frames.pdf deleted file mode 100644 index 455ad028ba64a6d682a8402d8681eabe597188bc..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 17277 zcma*P1ymf%_J<3EV8PuP9D=*MySux)y9Wsp+#xswg1fuBySux~gPeQr|J-}uTJLo; zT~oi=RlBTrF|*d!#Il0IRCLr#P{cJSrB6_d09t^JzBv>pCxAxM$lAow6u|T@$wL7E z02*O4OGhL7_ta9)(MZt9z{b!BiklnC!O`AG&kD*l?L<||28#v0mRcAHjyn|e65>EQ#wh1#^F~T_&sf+wLgC?v z^h=KD)!0f+>%HmCP0OX&7K_3(x1raam<~CGxsA`1v`^PI)9b798MmoL$?Dh2#|YRU zA9A#GLW-A#c-=eu-(=_(zrSRi@yL88<#4jAsigQ4HLG&Ca?U7^(Hd?T zEJN6{v9v#Vj#j1>A=tGdYJJ}i=e#x8L*l?AkA7XB&Do5j)t;TAjHh<>?Kf9A-fY=S z*%2`#zo{aE+{~y_%hXgin!+c5_<(Bx4lRD>jz`Xbacr1m7)v~f8IzQr8AC)7Ce~4j zz%=xR2ZP35Ppb`*iu+j6NJDuHjBQV{BZzXxdcwp!t)VY%4O53Q?lN7w|FaX%C(Ksd ztX=qU8Vn2pU&{=mCEB40IAy{I7(A9srAqd5xnq}> zf$LEj`wOX6NaioXJd7K&Bt5hgusl|=$F*_7eHAI$wP#mrP1N+2&+`wDJG^Ccl~00m z2>O0bkzo&+xB((CGo&${$=wR`QgZ<&cA_tk#9;V@hm=Vc-v zcZra3WQrUNCtw9?uwvrx+SB}K!gVTT|5o^L*-LUjpX(oq2`rz zDQ4HrchZ$_f*`HnO*C~plL+R`>KZT_{$i`F<``s_Z)t`ig#^s`5 zAEhrrLm`j5bxJU}nt}0XF+iZ+QK98h(yIvB&d7$yUy`N!x+E#m)z~{t<2PxK0te!9 zmTo79f9D$jnPcklg~}oMN`!zYo?12-Nr6kFNM_fE#!U;n5#KRKq@r8d4>*%f@HVO( zzRU&|PJm4acTh{BAnnV8Q%G*%A6i!?%=5{0F+S)^(iVTqyY|VR)Nyk)Qn+>%5?YBZ zm|Q-5lWR{voV#qTI|+?jEKE*EJXm1!0&c9$3?w_o>0<~ajS|oq=B>Y)#5?4v)k~U$ zGO{-OS5SI?_YuxMV%SIe9>wVBnCU+ZKC%q|j&^_In~JL?{(4rU6w}5$k0rW-^LZ7@h;H*IMVsGPQ`;WH#<>Qa~cV`sdJ=^P9JJ^2sGI0A_76UliI~o0*6L_~0G;%gG zFp?MH{}BJxRe2)^8z*}MBL~38u*v_UXDsje|JpQI|J|(rw@u??VE@`(=m2zV3=AJ$ z{w2@@=;)c~+5Vw00GJq={&SycQc_G?T4V}oYEr0gvhe2s_HRNt4QfJKUJ39JwRLfE zfxunHnLPGtt-9lWNq@_KDiu+QkW+HtwippvZTQveA*X04o|!Btc@p6lbL%`RX*pc- z3?w9>VSx4{tmSt$S0-mH)cEq}^HB#vS-B#0_9dXVB@ocBPCt?QCs!li)+Bn;e1%8P z0_&r%o?w-}p!tS;X9H@7)!kY*+1lRQ{#0I7dHL*-)q=#+K0L6B?k7IH+R-&Ix!jqI zcRnWq^gAUC4Cm9Qtu<&YJUqPi`K>hsUsiZSGi&gluW)`YvnZwpXS!di9Z-QnTBewp z6G(s@u;9SIz_<5LBbz=3e`}0}8OH>enHe0c>z+aVM9F!QiNE7tNLtmYhc5(@=jHf0 zJ4{3b6#SJdujo!h6!-IvR>ohVF2E^J>Pu(#ZZV@a>(*U9q= z#!szn_^y88ntnhgq-Z~*=6F86g~HFz1LY@>i~wtGl-ehR()@;RYyg#fnM){$;%f%#}x!_l{=_ z#5ZVRboIgOOM6Gp0{FSNGr}t^5IBeL=WX3O~b9t>$^pzi_ENaLemrn}E9L$^G~ zZKYQ_jCUL`j+NCcFR7buD%Er#YR@C*AgY{x(ziZHoGLTEPb2Dw=QW%Eo2NH zXa#MdcZ}_CqQf_(X8JZ)+;u(-%bGx_(QOk?uoG?YV@*G=*Y;cZgWBNFJS2B$y6;Li zFZ=HKNN%NlRQm5)d-iRvwc7@C_M?t^^&jExxxcRStmBP5f3gQs9ex951ExfM^F2GD zdIMf-s(t|X9J6?0G4vR-xP!aTn0v)L*IGDuzhGRym&DuiX}^2bd<9;+YJC0Fy65zS zRoUjU{XqH*N0Cy`+Va%(Fm{&UlKG02s@9qb4j9KV^*={cu!AX_lzd%e`~cp>qM2UX-I<8;IdQVR6}6|f(1s!w%t z%<4##G;)T4v<{Ms+6dAz#*ONXzvf-9^O^-!$)6uh9Dt*4auL7V)#2K> zxWKp0#Gp%i%1T1B!b~j(X321u*;@Iv@4B5jjmpl#&_r|D z2o!=L%Ohcr$psq7kWaMR3Qu!Z%}knKoW2afA~#*!^w=CM>n5f4Cy`|IubY<%!s<`8 zWiV7q31H1yP{5%Yi(h;py;`t?B2!09wRxp1acG&M%dZQqhP-6D;EVG;D!1c+$$35J zR!7&RxeBol)XN~=wD4Os;7pl(6;GOVyo=-ySsbfx0e2fIgjI|OZdfUoRtByymU4{QqFp?mzlSBrw2xOU%ZS!j^GIW zSJmmL_bl^hva?bE$z*o-HdrI-lDDDV5>s^+3-#k;U(8R6?GSGhHUa6Ljn}Fw6-92R z>Y|qI#?}*G1~gecA}8>HinHGCiH_Vi`=5$T=c5td4AGFMQjHni(3kRGMNlCL_=Bb^XZJ0R6w@2=~ z>)Y`VJ$gsF%KmJdd-KyDm$uyEcGFOF>k+@O4r~pa zC%4Y-6*UbF)=SXXa1A?96{WxVWAB@ivPge}IKZ!I<(a$&-N%_iB2$Sj0?_4c3qL@n zr0Q7QHSpMF1;^-`xfkTZE11H+W%#*f>3@!T2&4+ZVpaZ`o~QR#+R zkP75eIh;7_R5=bwHepYgUZY+>uDe#7uCmk{)M?G89;@9oc9xi8E34POMP)8a+LgxU z4EtBokWoy;T1v~ja2mm|#B)7%R#;k1EP!_z4AL;|I+bKB!ixs68Z=d!5@BPV1;vmn=s+bVqZRXqX2Db_`C4 zH%AEg*wISRp%=6K3cQ!(c#|XD%{@!1CunLIQRX+seWF@SHY6p(c7_zc?>NwJo3BO{W0boKaXop zsxYZ==aph^&mm=bfyU)F+q(NhViga0KfoT$gbArlN)59t^{ncr?9~ioq81v2`N?_$ z1_8i}P@kDWTi^KSC1H)9*M$+UB#Nzc7%K}(+Y^E4^A_saNs91=2q=^I%(==p2e}n} zYvg-LLNpkwpv48qBMjjifzIYicasC+q8u&Gv}|lao8oTR_EFKB^ki_~_%_Zf*>klm zb33PGW|kr~1yuGO4?R>#Io&YC*7~3>?*aakFS-qSIBZ?sl0O$OqTN^Q>Vv$vXyE!| zBbZ%~Lkv>}%wh1xWM>y@!+&V%n@o}=m?%cAF;;nC%~q1z zRcnd%E2<>29Vb?E-EZb8+ zRi}|-!!Fn}IQj)S*3u?Tuig3@>!w76^ByBRz|hPsK!S)T_q0B7!9hWjB=J|TL#kL& zGN*0=>zlCQK1x-?P=beYPTpG&Sm1Yjy(Dg~l2M_>ISi%a@6F{CT~^^Zt+FsWKk{p| z0kJwN6(EeyXycszw1dK24z zU*@8mNJb+Ml+3dW`~Zp6Wy{o6{S#+Ce7jkCw9q2UaKApPc8<)NC{_#m+jU{R5SVWr zV2Lc*%Z*x(6rb{|@dP!e>m!__$agdj-RZMi({qFhw$EtMd)4AG^W;J9>cKD7LdsBa z6wf1{Onxc{JzSX}85-tAgP2cGs<@M=4mlmbF=#l%rh<@G`eO3eOo!tx&!edsW9L_M zRJT7S1Z3-Plab|5&z6V9I$S4teSs5!Xbd_WBsW6Sz-atuqxBpv<8%TI^;lMPQh&Vi!?JUHSN1E`f2OIydCl9y2RGg zPs`!{!oKLLJ;X_615Cd2k8I=p?8r^a3cePLRtx}*alm>hWj+x3# zus{*;%pGola2koii{PE6KuP8>P#l$BuHA`h+0k@jd0PgjC|QXYp0u8378&^95fZ_K z-5ZCSPpuEu$67LPhGNtiMxJ*eUFk5=y=|{GDF^n!u)ak!ozv0MYcfu|av*W;xlIE> zZKSFagy+nrK&q!w7w@ZziJ@{m%}QK3&&r)}W7vus_TN%3M#1qVTeKy+W^?~0CMahH zIU0$^4bj3Na@yrxP1YU2OsRs_rhFaA)^h#Csx~swuqL!a1|^y8j-7%o4t*uSir4zI zNxrvum3Ypxqbv0welLH^A5<+#a(TClrF&=l$eaGcJB`o>qJwG%ewIM3{RIsATG+Kf zTNdbaCFFdar*=G{gtUuqC#Fn<6)Lxun# zZ5LeWa0W0Uu%hPpd43MyV)^WB$zw#!-2;0j@9ta(y{NT3fT0~k$@RG;?c1m?31j$o z;~Q1E9{o*hX17Ij9?@aYzNBtT{kB;i6JNc8D*bDgiF$i2Q>k8}!d z5%Nz{=b$Bf2a$EdivW3538l^Z5_ArpVi0k~hg8Jux`>YB!KcI(tvb(#*YKxIpK<{T*X^Hqj}w^AqR04^hIH z?=KDPHm-NOgtreXi2wke5?M9xR60(MPcsa;vx-~#1dTT7M6lIRm=X%ngGn*Jm35e7 zy`91~R%{;_8xJewX$Z5Zo22gIWS&sI3p9lbMkH5pnVQ86&}jXf_9Z1+@bJO4>J$p} zEF$>*cHNXD7cj7UZ-Ny!zZpFA7^rPV{A(w=+fb3=JKM>O8J~a;pv9T=F6@^E_4BUz znLkAwE^Won*PHmss|5hj?`1RJZi8JSXgjPNLxZh;(Tr?3?W1tEgu`k` zJoY26a0P@N?4M;LX%l8byPSo2J~`zJ6(()84!BCbC=+lv(}QWZT(TsSvg^2H{c+4n137vT=cX zu%vGU@;UXwKTHKpDSyzHGgv+YpKBy~hN zqoJNA`aMey;pr;LHk7GS#iupi+ggKZt!Zr*H!-iWvfy2ygKhfOUpJi{!3i>HyI;C7iwT~bSH-+;4?!`YlvMAIpVl;v5+B>zzu z=Gf(z!|`vjl}s942lR@zZ*G=s(e845a|M)s@K9mjA|@KfN=xzCo#=ko-^Ek(HDi5p zEi-Pkabp#E_NkJ~|E6%3;tnpRp(zqAydo3lh(KRvHobREHL}A{3f^X+h?n%b zisES`0mULf5^%{!GBY&jOnlE~T32N*6m6a>n9)#MEn!(>-zjwva5bB%E6>hpStI3P z97;lZ13sjH)wGHL#s>=>@D%A4q2j2T zT54qUsN6BN8Wm!4cOpB2A#(}A{{9@BYG8?p(Z2hAulKrT9V7sRc@z=aex7aH`7zm& z&|9JSiVCPC@s0ARx+~p*UOY)AM{lK8T;n7)Q%34#Wg?ouQ|+4_8OFWHup94vA(Hyx zq5NTZ6x%7M5C5G^HfvUP_F|YpOti37zsgmxyrrc{PO%_9w+gC!C?olc#Bj4a2Fzz? zLxd!;(94d4Thb+(;z^X^ItGPQ)4Ff#IWJ6s`~|BH`^eATVc^7!Ugx!QfH|vnIMgI- zKwo~jkH|KBjiqUrMHsoZw`_V_Ye&V6z+z*i(Sr~z+Ys;2W$U1#KdE;1`DKPiMvlH% z#;x^3ETzUd+H4Q1Gi9TW_z{)XWUya2Q^pVF&rxmWPS+b%jn!L4J|rvf#t1aTjh?^8 z&g@R$4*7AOMPq@(AN0Bno3)=7XcMumr{h_L_AOTXwodZBplWP`CD$n(2%{uIu`rBE zlx*Bdq84lgfp-n+=ZhNm2EgNrD>`%ZgusZ~YP#y#^CP&rJ_{ir^2X4@rudQD$#8kI~wr^DWgGG<{z{dXm76vza^$;MfBh zc6!okzDC8LXGuwJxAF#n@8rCWsVrt z(>a%Pr+C=OD>$(yc7G(i7UNdyOhpiUgyr0?J^QiIbiyN>glD=i9J8hEks}N3MqR?4 zuS!sojOw&)h`4o*(4UBfwgY3gO&jth#48UoRq3;&IbObT`>&od|FkYQ@QJY+d~>JF zOXpl39KY%7^v5I=$(59yn|b1WaPQc$4byBCt#hqcug~}DI`uCC_7hp*_oa@cMt4k zAbzY2W*WPa>$j7Q+Eak${MLfOI<})aKf%9Ga)p_}C0q}dMPbKp=jLlPnYcKCzq(V6 z!Sh$M>{GbuuvvGNsbw?RE36_U#Ip$y-IfZ8jIPz7XSR`z&f2{?ykNC86pE@!n%7(I zFUG}M1g4b#*bp=%479I{#h0#=jZhUCO=iM*4ZS}Vp?qgWdN*vYj>aF z)ao~bu<^04K4ae{Jtzcjcf?G;_z1i6IlvVCB8K1&QShCHaw=X9v~x9k3lNyLZvOe2 zaR_wRXVoJ03D|m#m3bsX9^p)B7!7?!(?H;oiQWrV1}iK|9t=Fk5=_tXOM`}aJPL$d89TqErRGtE-=wbJx%~1TfpRh{EH%pP5hk zWO8&!CkK>4Gc88okk$z{!bs}LZ7Iwj7-l?(g~`-@R80@baN89~Qw~iOqRoq2Qz)xw zvCq1&4Z-V+?QiO2sqPhN?f;@@WW3EcU$5B%S&6t!lLP9?;8!fLau5Y4G{5NO{%uM0 zyfUz!<<4QJeg&JjX3lwULt(z*nn7L>7y`Ne)y2=f&9=yOc=sBYHh||yZb%pNT$AKR zL0*<@Y^r#4ggYa>cL!!vJw@>kBkmM-72pyB*!Zq_?P4M!gt)o{J~z-oh-!eO zMZ|PYh!dg~K-e z&ho(m&@9eUy-wCw_M}Ze>P}h7y^&Fg$x^)>+`TiBO74@bYG{*GLW$#LKDSM73UXvq zDa=y>dX8mM#x^`&M56-n$nweN0r=q)1Gy>_Ki4ajw&3&KuT_5<8l&;thnb?NI58gK z2*<*U!vR=U%K77sQ)?THu40{`amsF*9#LvnN$S*)a}sh~eB6Ul*}m$rNeu&OBrSEp zZUhuNpd@rT+)VwF@MC5tqI6N<{hK#Wj1Bj{7tFf7|(C|-8hffOQ;e$lXHYC*5ocC`mXlq^T&pEaAL0HHRbMr zKx@kVn7kAfNf^v?Yv_fKxFro zkQJnTx4FbR*=r7XXJNC?gtTnG#iOXB9#?#yvo046P66-nc?q99LGZ#Mv2F`q7LNAs z^p*Q*3v&lK&&kQ*vJNSw9D;RdPzu*6%iuWI(yJV%vtGJ0%_<2c>FXA0Q60_1Cm4h+4-m{nD1Gc zCG4whU$A9h$lFh*Ur3j&rLuhE2oFF5vB0BJU!MQ5f8cK9A*sZkojPi znLJX~1~%Zqkl3*dsmdH?a2Z75Myw}*Z;_^}vX`J+l|;vmHRQ75v_}K^E|^>1glF*Z zI6kdBhkk56f*6@Csrg0=H`v5JJ|=GiMZ+Jx{?J32j+l@`!{_MfL2I)L{hH2hsRdfQ z@fYbE|5|;T0p^%8Ks7EAHcRUJ7cIRZPsJxknDpTeUfS>Bjt25E8NL>9MNz|xmI@A~ zK*xcYmyb_e=JMCOSs4RZkO-zb4fzh)7(!-PJIbYpvQf|Op_I^AnMM0Ey=q3q{_2|g zHL%%JnsTldCmvsVROzI_c9b$Ej-8DdTn?cydu>#~yrM-4Yt0JF8!u6e!)FUJ*WzTH za!+VUxrCgR%Ck8yo8d;PiL#|_@Nw9gCNYCc5$zuyNgr!b^uqcP(A!$T3iG)AH&DtU z$56YRy8}hK%U`h*s8lyjzbM^6r|Ji%OX;l?khTn{#0~K(lKTA6WTHu*R!6z6fwpi< z&Z=(i{ZZm|jGAm0q@^@k^%Z@PY-Ck7bv^q8V!c3`WWV zSwAzlP=VKRtX{pGrA95NzUxq*qtjkoUHzpsCj6wD#>W#rRJ@)n5QM~9UqNn{n@}X% z2%Nt~w2!>qOCpYuLf#m&I0^?1alUp-x3K#dk6`K?w{*dkvFUXG;boS72FLuZ2cm}n zuXLGai_pmb%u+n6ts)rQ0d43D!>^X!{4sKKfty04m#*8&wt-7wiD(bKs!ty0Tr=Dk z6Tj|SYBg<|2f zP^!%?5=s@wq$Qf+TPXAB=)q%r-zUr~bDQ9F84?R9r&kGr&MYO_E%|QDHlgI9VPxL; zgaiV8iFFu&ot1;YsA^lnU(W~xVT8;`eKKpyr1s^sG}Nbx%VK!HSnLOhX> z+pML-kU9&k09tn!871+x?l-u3H2i!~$wks><8~6lXakTZ7Iw}F3DdN)mkWiSz{Mii z=YVa}(XQ|WL>TWkJNbj?R#qGvEos~%J?#0lsMFvUgj(?v;=MKf`1D+%S+|>QRUi8j zJQG^*&zoEwvjAFp%twQZ&ZimTEv-64)V|-7?7_Jg1jChXagB&FZneX!3h}IhpW=uW zL54;!5Wg8wy%dl8K6{;p=x`{QLc~=QWN3hZTi!LasydgOfZLwKa)im+L2%*vQ5WaW z9w_APqdh7ytjmt_?}^Fh8iwnZbvm_aDt=4X@&s5E=ggoGaKw?ll*+_&=57)Rnw}&{ zTFlUv2+$95d0n2T8LHP(VFGEdhK^U2?>B*`ycu=5EQ633JqO%Cdh%m+JFTq?DRQW( z4vHxA-IDt$g`2tzd4$>ye1S`68vrHy{8^frzL-`qdmWsNGrAgqp{aU0|%v6B-=V^3%@IVxv&L4doNeF=RVpi*9XvfK%qdAV;ZlNbT<^U)KXStQljV`B7#lS-$t zB1PWlo^eCX39bROG5ssHx_CAwc$F=3IoZKpT{1ezfQ7U%P)Z>a$s&60g3m`BDmgWX zsJv63@yBa*KnP2Vz!2s=38MPbgh((;`bgwdp%=00Sz%pBZL9}S!!knhmU*+ zS#~c-3Y#SHl%%4E%oh(E#`mI@6pEBCUDni>YRzRi&0y)xbNQJm6FF4XYlV$Wn)rIJ zpfIH!U~ZdP&qfA`#M7*m+#UFgW-_$6X}6+GDS0~UDEUZ^nIm-y&rJokwb9N_3 z>pWKG(_kY?&~%)7a=_(PN?B)-#6L4g@r%Bux3bnnaN$87d8bw_RG5k>zZgUmi7VCn zMwm6HhMkMmJeBT;73Go$xO|8`M42cZXl}QeszT4iF)n zO@{bUB*b6s%bMlwbogKsE6@iTMWLY_^jirF59>IqODDOAhg&e)$LG~e{f>%S_)MOf z6r3Q2tFOwH*{ESwNKQyVK1+?VFcmEDVjq1`oWBA_4^le)1BTe#=fYQCHNZ8tuhv$R z0quo$q%t6wcKcRhr@Nb;trU}pvtgnLJZNw-WL(a2AZjw{1odZW2_2!x?D*_eRaNq) zNj4-%J-vjZ|82$!DL)_cSi7etc2YOFD8r+02fL91I)q&3q2*Y1jZsCS3T#~F%`p?D2=(NFUAlxYhR3?1Hin&EgIPu3@nnNZ;m-|Q zM9>QIFk``Fg?%wu(qA=@{oSU+a~`mPd~<%=xrAp&(oC)CZ12}KhVgtu-&-eTTZrQy(}eGa}ayz5Dk%y@(uyRmomNab)oIC$h-@vy>+ahXA{`&8g`TaKszL`Bulb zAjbLqlMsi5wPB#BToa2^#TvL0WA$YDjT&Usv!HE7wLN#P)gC z3p*ydbleZU;2`;A+oO>-a`nWNucR=Un_~}@9(_GKiRt3DX#&0A#*89C)Q;vR7=urO zex&Yy7pCaN=22Bzk_kR2YMEZ26$T{5kQLS%oBLPOw@a9k@0!U6c4gI>qjS;R5 z?k=-VjEh@5RG3q{d(o{avPUJs2uzDUQW3w>_W)+cl6?d0_StA4<|A=>ttM_3jYH6)RbvQV+NP~d2H&XeN}51gQ5u&<^k&cH2(9SZxCPT4kS(nY2l z!(=uuScHAC$D)%38?khH9!^xJ#*R3^*wCnnB}W;xIzNg6vQj4E!42h~9zng1v%Kf{ z&qQ0C14&j>4wt)s7Y2yr`lB2L`k$(W&AQJ6kSrXnmL#LTrt##CFYgGAk=t*mik!Qd z_~FNN9+;K}d^QGgHakG}<}+!op1Zh%oUO1i=_k81_Js>1C&Mf1am@!$icF5Kz|C!-sTzy2MrzS=L?OBfa5oX(r2 zf8I0D-rKf5;`%LMi?78y@fVu1Zp@iebL2Hom=0N>*~Z#)E=aG!yqAcMIx-C^49zGf zfog4;AG`&!Tj!;en%rjW+wPnWOR>so*6Kkw%lhe{8w#cyA`Yc^f&S3_uN zlEN|Ry4TZ=a<$jmG+A@`pbAFr7pJV0 z(i(k5HJAj?#(tr_-=nOkRiSemj3C!DR|gkoKthZ8q7|hwen|{`Vg#|@t^ zfRp|Cx!`wNbrR$P(DOd{;G&G1Y`zn!B>n0n(tQ{ACNaHj;RV@>>T^nZ4po~7f{u|O z#C3M-=Z>LjcLk=~1x+EH3TJmQ7`^KnqG0l5xrxb<-eD*UkmH1V4@NT zZb?#QZqt5^$1*=H3zX`w~o4hy`~E! zBBlm)`z5Yt3A#lx4v>#lAp{m$o5@PE<6`2od@|#?d1pHenr?ds7o3Lad>qR&QFW4b zuGd4paF7DWf=(_K7p=*UU-?pJzqT4OY_;*{yG-H_ruTK|1k?@0KyAfg<08;}UIoiX zelp4qQlNMtixwcJ^P`FmI&7D&03@i64fE5azpfQPlCks76;igl0GOt2r`~_@ZfQna zayt&0Yvyn%K}bND)F*lHQblsqst?UVPqsX%1BRTfvWTyUd}{=>02R(vbCBJ>ZN{ zCvyvl&fleZ`(i5vr=<-}b_zyhpKQNJm)~hj_zuE9MDX!m>`4673J(dEk%-G%9u*># zrF2{otDxxei$kjCcdbOqDp#%(u_m2|{jhi&waV)l^ri*q?09}{wQofG2~9T@RdS-0 z{N7uO*1e=)J;f8&-i6tyjMZvUYpmEd9bd z-RqfiK9ujC8~_uUr&O!85}1oQ2rA`8ftZj1>N`5{38d*+H-k_yQNS-3+U?1uNs2-X zswHr>ygIBqg%UR2q%<;!4on%fPWgse+%MR|5Tgy}R>aP!&3zMNJ@4N-W8PPlzZKGJ zjb1;y?O1_;QWHvIbhg@R{gjSSaN7Y!6CUXzf#+PS)${^8!vGhOax39dw)p9whFPR2 z3)>FR86Qt*`#?HXoMC5-U9)}EuCdsovZRjN6#`zIUFbn3q)B|YRf~Ndy`QVA;ZGEO zEi#8F5{joVM>j>`uUXm_v%7hSnS~ThYP0V>fG6i*no{$N*hcdizePbm-=l58PH zwnM>ryLB(C_aJdYeo7=D{gMcl3G8*cSg{d(9ln7piEN%A>li;PG?J3QwqF6&g)-jo zWpQW1JWg%OF0PaW^7Q>bD|;B2y-8_T_RP(?qeo28k(tWLFnS}T_<6=3mn?3IO-+Z2 z{o%U5K?&UEMK=~;W@2eGTQ3jzhPrI$YiYXee&Cc%iuH>aZ;h04z&3j7I^}2u#$(m> z?R+kNsOcHrYyRB!y*F>7x-;<*yJra@Jc-Cw6e-DTd2o|mHxxk_Y*{XS5)7E?zMk;N z@>DNluB2I&d}yolLA=BoRI=@dSn+;n$2=N#dWQl&=I}bCcL|lCI!s#1QniooC}W1J zWbJGTxz7NiTk&=%OEIR@nm^8S$0P0Chzq7TXoVh4$%*c`<)oZBRooOiyD~;aY$)2# z$2^}9R@SNnEkA+4cwV3m4IQ%b7khc#l66N#K$3wjSN=>y2{#4r&Roe4MRN`0XuDT2 z(|FaA4eKGp73dHy+pt2Hr1}ii&iNk&u!}67E`_u?u6FVy6g$uyEBD8U#wc>7=asE( zMr;k6Tb6y=VH{1VcBo{GCkLU@3y8lzv7OYC%bqVpUjSXP*%6v?KMO5S+$;(uD{CEL zBP)r{pY6kssx&Sfh!+STqu6P^^e%V%2GmqDD7Adq!#!zk30h@XvRNE6Lzm63SI1?U1%{&lJl6%gXrBchau26Y zj5mhM^63#l6K>0#oLb-D-Q#!M13fCT_*VqKulhFidl+#0giZ(My&xF0 zY-2qWSEcSpU;SujR4pIcA5m>s_=IUBJgEBUhb*8ax>{DMR%Hb)_G|=py?{5IaUUP|9LXPluJLju{K$iAwF=W4G$$?2;+D}C|Q>W0k+4xJu<-05(}&+IZqFR=~h46-nP znpJV@+YUG$GGNM?HW1;c*!4b`Mnf(JuuhL!=EarvS32|4s0QI5Wv)nZmZOj!SBJwV zN(;Ndsa8_l;~6uT7EyaIpNty)6yV`U3tOr-#nX8UbF-45N)rMhj01Ft8)oXg_&o65 zzkPn?!PnLQ#At6{TgPNyDSU1Z;-7Ys{{DLcK1k==XG$Z|+Yh+yKT*36^6PKZj)8@i z?Qi^z?SCOLx(}}H{~ubcB|l`|MGN12MByk2*AtoBPoBhUM#FZ^H|6b1_*+4M5UYo6 z`>d%LlKfit%$11&vr2=@DznZk$>}DeuRKTXP9Q8E2o=|-~u+C>rJ{yk46RcqeCi@K>_ytp1CR5-z#n@-ut3vvo(vB zj=32p+KmS_95JO~_ug0!o=G%!lurhp+9Z?to-_#L;nC6xXk;!2yuW@QMOZ(1=aBXI zn9K@Po^`n-DlNI@&oZISJO=wW&}svhp|ilS!fYw&(4y_~9tka>+lF5!?$dl0StrB( z5@}?4=-*IIT1_#*TwfRM;pNLLx|Vg88wK5J=v#m$6P2cI?+!xJG44LWzPz(dy@WCx zbk#l;QXt?>`ao4qg`WJ|kolZt^K5p#jF^-Y>%#Hgh4jt3ovyQ@UsnRmuqtN6-yq0< z_KSkCA2%AtzLPz@`a;LjXrRMrGqsW2P)7eZ=uqn>&WPB;!K*LpSY3<%JKKMz@L$u- z!1iyjlI{az{2K}U3q<@NB?XNf4D8Kp9c}FY@O>YWw4T*F2`Qs0kyLcdtNmDL^6NPm{h>$yzdCPb@8Bq4s%H;i zp#M{sq~5>sbo74-|2eYnfa|~SDk1B4M&HcZ1VE!=X3b~qVD`80ry>ElNH=gGjp0Qh+Qt$kR~v#|a79aXw7JrxcNLlFV0nE%F!}Rz0q5S=x zmGvVe{B{1P)!*u0>0fm-GJcS*AJ>okNBYsezuWWCwh!x%l;t1o{LudN<=xjm+y8ev zKI-`c)V_nUHcHlJACEKud^GEy^ZLJQ`2*Z__%ntQW`=)8@T17T(OjqZrA7A-K9=sE z=&!83jiHml`(qTy4D`(GZ2)xCbWGImw-K4CqoXZ5%{ycKuA12E*_xUeI8fWzn~;B~x z$)D=}bm3pL@&7T(uJ%U8P_zI#Mkv~U9{^@1CI%*eG2kza?tQM_aq5o)VEr$RmX3k- zef9sBM$hs-_5Z2S($f8JS$d}Tsr?VTcSM`^z4`y4(b6%~{*N6KBjfw*{D&PK6C?9~ z`=DcDd+))2+p#gdFPi@>%gn;~zGVJOV`chBKOF7#-tT?;KM%-IFmpHh7^@F@S>DFx rebPV1{I3m7%-Z;UZa*ge&r))5)U$W|GrDxlY;?3x#Kb}}!chMY?kN*2 diff --git a/launch/cortex_bridge.launch b/launch/cortex_bridge.launch index ba8d79f..256d668 100644 --- a/launch/cortex_bridge.launch +++ b/launch/cortex_bridge.launch @@ -14,6 +14,7 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo pkg="oscillation_ctrl" type="MoCap_Localization_noTether.py" name="localize_node" + launch-prefix="xterm -e" /> @@ -21,4 +22,6 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo + + diff --git a/launch/klausen_dampen.launch b/launch/klausen_dampen.launch index ac9b4e6..24bcb44 100644 --- a/launch/klausen_dampen.launch +++ b/launch/klausen_dampen.launch @@ -22,6 +22,11 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo name="linkStates_node" launch-prefix="xterm -e" /> + diff --git a/src/path_follow.cpp b/src/path_follow.cpp index fe2afea..855a614 100644 --- a/src/path_follow.cpp +++ b/src/path_follow.cpp @@ -5,8 +5,9 @@ #include #include -#include -#include +#include +#include +#include #include #include #include @@ -31,7 +32,7 @@ geometry_msgs::PoseStamped desPose; tf2::Quaternion q_cmd /*What I send*/, q_Jae; // What should be sent; double rol_K, pch_K, yaw_K, rol_J, pch_J, yaw_J; // Same logic, K = cmd geometry_msgs::Vector3 K_ang, J_ang; // Needed to populate EulerAngle msg -offboard_ex::EulerAngles eulAng; +oscillation_ctrl::EulerAngles eulAng; mavros_msgs::AttitudeTarget thrust; @@ -42,12 +43,9 @@ void state_cb(const mavros_msgs::State::ConstPtr& msg){ } geometry_msgs::Quaternion q_des; -double alt_des; // Check desired height - // Cb to determine desired pose *Only needed for orientation void q_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){ desPose = *msg; - alt_des = desPose.pose.position.z; q_des = desPose.pose.orientation; tf2::convert(q_des,q_cmd); // Convert msg type to tf2::quaternion type } @@ -83,7 +81,6 @@ void thrust_cb(const mavros_msgs::AttitudeTarget msg){ tf2::convert(thrust.orientation,q_Jae); // Convert from msg to quat } - int main(int argc, char **argv) { ros::init(argc, argv, "path_follow"); @@ -116,7 +113,7 @@ int main(int argc, char **argv) ("mavros/setpoint_raw/attitude",10); // Publish attitude cmds in euler angles - ros::Publisher euler_pub = nh.advertise + ros::Publisher euler_pub = nh.advertise ("command/euler_angles",10); // Service Clients @@ -126,6 +123,8 @@ int main(int argc, char **argv) ("mavros/set_mode"); ros::ServiceClient takeoff_cl = nh.serviceClient ("mavros/cmd/takeoff"); + ros::ServiceClient waypoint_cl = nh.serviceClient + ("status/waypoint_tracker"); //the setpoint publishing rate MUST be faster than 2Hz... PX4 timeout = 500 ms ros::Rate rate(20.0); @@ -140,18 +139,24 @@ int main(int argc, char **argv) /*********** Initiate variables ************************/ // Needed for attitude command mavros_msgs::AttitudeTarget attitude; - + attitude.header.frame_id = "map"; // NED Ref Frame(?) #TODO attitude.type_mask = 1|2|4; // Ignore body rates - // Retrieve parameters from Ctrl_param.yaml file - /*std::vector waypoints; - nh.getParam("sim/waypoints",waypoints); - - // Need to send pose commands until throttle has been established - buff_pose.pose.position.x = waypoints(1); - buff_pose.pose.position.y = waypoints(2); - buff_pose.pose.position.z = waypoints(3);*/ + // Retrieve desired waypoints + oscillation_ctrl::WaypointTrack wpoint_srv; + wpoint_srv.request.query = false; + if (waypoint_cl.call(wpoint_srv)) + { + ROS_INFO("Waypoints received"); + } + + // populate desired waypoints - this is only for original hover as + // a change of waypoints is handled by ref_signal.py + buff_pose.pose.position.x = wpoint_srv.response.xd.x; + buff_pose.pose.position.y = wpoint_srv.response.xd.y; + buff_pose.pose.position.z = wpoint_srv.response.xd.z; + double alt_des = buff_pose.pose.position.z; // Desired height // Desired mode is offboard mavros_msgs::SetMode offb_set_mode; diff --git a/src/ref_signalGen.py b/src/ref_signalGen.py index 889a931..371a399 100755 --- a/src/ref_signalGen.py +++ b/src/ref_signalGen.py @@ -8,22 +8,17 @@ import numpy as np import time import math +from pymavlink import mavutil from scipy import signal from scipy.integrate import odeint from oscillation_ctrl.msg import tethered_status, RefSignal - +from oscillation_ctrl.srv import WaypointTrack, WaypointTrackRequest from controller_msgs.msg import FlatTarget - -from geometry_msgs.msg import Pose, Vector3, PoseStamped, Point - -from geometry_msgs.msg import TwistStamped +from geometry_msgs.msg import Pose, Vector3, PoseStamped, Point, TwistStamped from sensor_msgs.msg import Imu - from mavros_msgs.msg import State -from pymavlink import mavutil - class Main: def __init__(self): @@ -35,6 +30,7 @@ class Main: self.tstart = rospy.get_time() # Keep track of the start time while self.tstart == 0.0: # Need to make sure get_rostime works self.tstart = rospy.get_time() + rospy.loginfo(self.tstart) self.dt = 1.0/rate # self.dt = 0.5 @@ -71,6 +67,33 @@ class Main: if rospy.has_param('sim/tether_length'): self.tetherL = rospy.get_param('sim/tether_length') # Tether length self.param_exists = True + elif rospy.get_time() - self.tstart >= 3.0: + self.tetherL = 0.0 + break + +# --------------------------------------------------------------------------------# +# SUBSCRIBERS # +# --------------------------------------------------------------------------------# + # Topic, msg type, and class callback method + rospy.Subscriber('/status/twoBody_status', tethered_status, self.linkState_cb) + rospy.Subscriber('/mavros/local_position/velocity_body', TwistStamped, self.droneVel_cb) + rospy.Subscriber('/mavros/imu/data', Imu, self.droneAcc_cb) + rospy.Subscriber('/mavros/state', State, self.state_cb) + +# --------------------------------------------------------------------------------# +# PUBLISHERS +# --------------------------------------------------------------------------------# + # Publish desired path to compute attitudes + self.pub_path = rospy.Publisher('/reference/path',FlatTarget,queue_size = 10) + # Needed for geometric controller to compute thrust + self.pub_ref = rospy.Publisher('/reference/flatsetpoint',FlatTarget,queue_size = 10) + + # timer(s), used to control method loop freq(s) as defined by the rate(s) + self.pub_tim = rospy.Timer(rospy.Duration(1.0/rate), self.publisher) + +# --------------------------------------------------------------------------------# +# FEEDBACK AND INPUT SHAPING +# --------------------------------------------------------------------------------# # Smooth path variables self.EPS_F = np.zeros(9) # Final Epsilon/ signal @@ -79,7 +102,12 @@ class Main: # Constants for smooth path generation self.w_tune = 3.13 # 3.13 works well? ######################################################################### self.epsilon = 0.7 # Damping ratio - self.wn = math.sqrt(9.81/self.tetherL) + + # need exception if we do not have tether: + if self.tetherL == 0.0: + self.wn = self.w_tune + else: + self.wn = math.sqrt(9.81/self.tetherL) self.wd = self.wn*math.sqrt(1 - self.epsilon**2) self.k4 = 4*self.epsilon*self.w_tune self.k3 = ((2 + 4*self.epsilon**2)*self.w_tune**2)/self.k4 @@ -100,9 +128,9 @@ class Main: #elif rospy.get_time() - self.tstart >= 3.0: # self.xd = np.array([[0],[0],[5.0]]) # make our own if there are no waypoints - self.xd = Point() - self.get_xd = rospy.ServiceProxy('status/waypoint_tracker',WaypointTrack) - + #self.xd = Point() + self.get_xd = rospy.ServiceProxy('/status/waypoint_tracker',WaypointTrack) + self.empty_point = Point() # Needed to query waypoint_server self.des_waypoints = True # True = changing waypoints @@ -111,10 +139,6 @@ class Main: self.y0 = [0, 0, 0, 0] self.z0 = [0, 0, 0, 0] -# --------------------------------------------------------------------------------# -# FEEDBACK AND INPUT SHAPING -# --------------------------------------------------------------------------------# - # Constants for feedback self.Gd = 0.325 self.td = 2*self.Gd*math.pi/self.wd @@ -148,30 +172,6 @@ class Main: self.sk = len(self.SF_delay_x[0]) # from Klausen 2017 self.ak = np.zeros(len(self.SF_delay_x[0])) self.s_gain = 0.0 # Gain found from sigmoid - -# --------------------------------------------------------------------------------# -# SUBSCRIBERS # -# --------------------------------------------------------------------------------# - # Topic, msg type, and class callback method - rospy.Subscriber('/status/twoBody_status', tethered_status, self.linkState_cb) - rospy.Subscriber('/mavros/local_position/velocity_body', TwistStamped, self.droneVel_cb) - rospy.Subscriber('/mavros/imu/data', Imu, self.droneAcc_cb) - rospy.Subscriber('/mavros/state', State, self.state_cb) - #rospy.Subscriber('/reference/waypoints',Position, self.waypoints_cb) ### Change to position - -# --------------------------------------------------------------------------------# -# PUBLISHERS -# --------------------------------------------------------------------------------# - # Publish desired path to compute attitudes - self.pub_path = rospy.Publisher('/reference/path',FlatTarget,queue_size = 10) - # Needed for geometric controller to compute thrust - self.pub_ref = rospy.Publisher('/reference/flatsetpoint',FlatTarget,queue_size = 10) - - # timer(s), used to control method loop freq(s) as defined by the rate(s) - self.pub_tim = rospy.Timer(rospy.Duration(1.0/rate), self.publisher) - -# ------------------------------------ _init_ ends ------------------------------ # - # --------------------------------------------------------------------------------# # CALLBACK FUNCTIONS # --------------------------------------------------------------------------------# @@ -230,13 +230,18 @@ class Main: pass def waypoints_srv_cb(self): - rospy.wait_for_service('status/waypoint_tracker') + rospy.wait_for_service('/status/waypoint_tracker') try: - self.xd = self.get_xd(True) -# if self.des_waypoints == True: - + resp = self.get_xd(False,self.empty_point) + self.xd = resp.xd except ValueError: pass + +################################################################# +#TODO Will need to add a function that gets a message from # +# a node which lets refsignal_gen.py know there has been a # +# change in xd and therefore runs waypoints_srv_cb again # +################################################################# # --------------------------------------------------------------------------------# # ALGORITHM @@ -322,9 +327,9 @@ class Main: else: print('No delay') - # Convolution and input shape filter, and feedback + # Convolution of shape filter and trajectory, and feedback def convo(self): - + self.waypoints_srv_cb() # needed for delay function # 1 = determine shapefilter array # 2 = determine theta/phi_fb @@ -332,9 +337,9 @@ class Main: feedBack = 2 # SOLVE ODE (get ref pos, vel, accel) - self.x = odeint(self.statespace,self.x0,self.t,args=(self.xd[0],)) - self.y = odeint(self.statespace,self.y0,self.t,args=(self.xd[1],)) - self.z = odeint(self.statespace,self.z0,self.t,args=(self.xd[2],)) + self.x = odeint(self.statespace,self.x0,self.t,args=(self.xd.x,)) + self.y = odeint(self.statespace,self.y0,self.t,args=(self.xd.y,)) + self.z = odeint(self.statespace,self.z0,self.t,args=(self.xd.z,)) for i in range(1,len(self.y0)): self.x[:,i] = np.clip(self.x[:,i], a_min = -self.max[i], a_max = self.max[i]) @@ -343,7 +348,7 @@ class Main: for j in range(3): # 3 is due to pos, vel, acc. NOT due x, y, z - self.delay(j,shapeFil) # Determine the delay array + self.delay(j,shapeFil) # Determine the delay (shapefilter) array if self.SF_idx < len(self.SF_delay_x[0]): self.EPS_I[3*j] = self.x[1,j] @@ -363,9 +368,21 @@ class Main: # Populate EPS_F buffer with desired change based on feedback self.EPS_F[i] = self.EPS_I[i] + EPS_D[i] #+ EPS_D[i] -# self.x0 = [self.dr_pos.position.x, self.dr_vel.x, self.dr_acc.x, self.x[1,3]] -# self.y0 = [self.dr_pos.position.y, self.dr_vel.y, self.dr_acc.y, self.y[1,3]] -# self.z0 = [self.dr_pos.position.z, self.dr_vel.z, self.dr_acc.z, self.z[1,3]] + # Populate msg with epsilon_final + self.ref_sig.header.stamp = rospy.Time.now() + self.ref_sig.type_mask = 2 # Need typemask = 2 to use correct attitude controller - Jaeyoung Lin + + self.ref_sig.position.x = self.EPS_F[0] + self.ref_sig.position.y = self.EPS_F[1] + self.ref_sig.position.z = self.EPS_F[2] + 0.5 + + self.ref_sig.velocity.x = self.EPS_F[3] + self.ref_sig.velocity.y = self.EPS_F[4] + self.ref_sig.velocity.z = self.EPS_F[5] + + self.ref_sig.acceleration.x = self.EPS_F[6] + self.ref_sig.acceleration.y = self.EPS_F[7] + self.ref_sig.acceleration.z = self.EPS_F[8] self.x0 = [self.dr_pos.position.x, self.x[1,1], self.x[1,2], self.x[1,3]] self.y0 = [self.dr_pos.position.y, self.y[1,1], self.y[1,2], self.y[1,3]] @@ -389,37 +406,31 @@ class Main: return EPS_D - def publisher(self,pub_tim): - - # Determine final ref signal - self.convo() - - # Populate msg with epsilon_final - self.ref_sig.position.x = self.EPS_F[0] - self.ref_sig.position.y = self.EPS_F[1] + def screen_output(self): - self.ref_sig.velocity.x = self.EPS_F[3] - self.ref_sig.velocity.y = self.EPS_F[4] - self.ref_sig.velocity.z = self.EPS_F[5] - - self.ref_sig.acceleration.x = self.EPS_F[6] - self.ref_sig.acceleration.y = self.EPS_F[7] - self.ref_sig.acceleration.z = self.EPS_F[8] - - # Need typemask = 2 to use correct attitude controller - Jaeyoung Lin - self.ref_sig.type_mask = 2 - - # Publish command - self.ref_sig.header.stamp = rospy.Time.now() - self.pub_path.publish(self.ref_sig) - self.pub_ref.publish(self.ref_sig) - # Feedback to user rospy.loginfo(' Var | x | y | z ') rospy.loginfo('Pos: %.2f %.2f %.2f',self.EPS_F[0],self.EPS_F[1],self.EPS_F[2]) rospy.loginfo('Vel: %.2f %.2f %.2f',self.EPS_F[3],self.EPS_F[4],self.EPS_F[5]) rospy.loginfo('Acc: %.2f %.2f %.2f',self.EPS_F[6],self.EPS_F[7],self.EPS_F[8]) rospy.loginfo('_______________________') + + def publisher(self,pub_tim): + + # Determine final ref signal + self.convo() + + # Publish reference signal + self.pub_path.publish(self.ref_sig) + self.pub_ref.publish(self.ref_sig) + + # Give user feedback on published message: + rospy.loginfo(' Var | x | y | z ') + rospy.loginfo('Pos: %.2f %.2f %.2f',self.EPS_F[0],self.EPS_F[1],self.EPS_F[2]) + rospy.loginfo('Vel: %.2f %.2f %.2f',self.EPS_F[3],self.EPS_F[4],self.EPS_F[5]) + rospy.loginfo('Acc: %.2f %.2f %.2f',self.EPS_F[6],self.EPS_F[7],self.EPS_F[8]) + rospy.loginfo('_______________________') + if __name__=="__main__": diff --git a/src/square.py b/src/square.py index ef9e43f..73652b0 100755 --- a/src/square.py +++ b/src/square.py @@ -5,7 +5,7 @@ ### Script to generate set points which form a square with 2m side lengths import rospy, tf import time -from geometry_msgs.msg import PoseStamped +from geometry_msgs.msg import Point from std_msgs.msg import Bool class Main: @@ -13,11 +13,11 @@ class Main: def __init__(self): # variable(s) - self.Point = PoseStamped() + self.Point = Point() # Init x, y, & z coordinates - self.Point.pose.position.x = 0 - self.Point.pose.position.y = 0 - self.Point.pose.position.z = 3.5 + self.Point.x = 0 + self.Point.y = 0 + self.Point.z = 3.5 self.xarray = [1,2,2,2,1,0,0] self.yarray = [0,0,1,2,2,2,1] @@ -30,7 +30,7 @@ class Main: rospy.Subscriber('/status/path_follow',Bool, self.wait_cb) # publisher(s), with specified topic, message type, and queue_size - self.pub_square = rospy.Publisher('/reference/waypoints', PoseStamped, queue_size = 5) + self.pub_square = rospy.Publisher('/reference/waypoints', Point, queue_size = 5) # rate(s) pub_rate = 1 # rate for the publisher method, specified in Hz @@ -49,28 +49,28 @@ class Main: else: # Check if i is too large. loop back to first point if self.i >= len(self.xarray): - self.Point.pose.position.x = 0 - self.Point.pose.position.y = 0 + self.Point.x = 0 + self.Point.y = 0 else: - self.Point.pose.position.x = self.xarray[self.i] - self.Point.pose.position.y = self.yarray[self.i] + self.Point.x = self.xarray[self.i] + self.Point.y = self.yarray[self.i] rospy.loginfo("Sending [Point x] %d [Point y] %d", - self.Point.pose.position.x, self.Point.pose.position.y) + self.Point.x, self.Point.y) # Published desired msgs - self.Point.header.stamp = rospy.Time.now() + # self.Point.header.stamp = rospy.Time.now() self.pub_square.publish(self.Point) self.j += 1 self.i = self.j // self.buffer # self.Point.header.stamp = rospy.Time.now() -# self.Point.pose.position.x = self.xarray[self.i] -# self.Point.pose.position.y = self.yarray[self.i] +# self.Point.x = self.xarray[self.i] +# self.Point.y = self.yarray[self.i] # rospy.loginfo("Sending [Point x] %d [Point y] %d", -# self.Point.pose.position.x, self.Point.pose.position.y) +# self.Point.x, self.Point.y) # Published desired msgs # self.pub_square.publish(self.Point) diff --git a/src/step.py b/src/step.py index e229107..1301d0c 100755 --- a/src/step.py +++ b/src/step.py @@ -14,9 +14,9 @@ class Main: # variable(s) self.Point = Point() # Init x, y, & z coordinates - self.Point.pose.position.x = 1 - self.Point.pose.position.y = 0 - self.Point.pose.position.z = 4.0 + self.Point.x = 1 + self.Point.y = 0 + self.Point.z = 4.0 self.bool = False @@ -41,13 +41,13 @@ class Main: if self.bool == False: rospy.loginfo('Waiting...') else: - self.Point.header.stamp = rospy.Time.now() + #self.Point.header.stamp = rospy.Time.now() # Published desired msgs self.pub_step.publish(self.Point) rospy.loginfo("Sending [Point x] %d [Point y] %d", - self.Point.pose.position.x, self.Point.pose.position.y) + self.Point.x, self.Point.y) if __name__=="__main__": diff --git a/src/wpoint_tracker.py b/src/wpoint_tracker.py index 0a7dcd4..831edf1 100755 --- a/src/wpoint_tracker.py +++ b/src/wpoint_tracker.py @@ -36,6 +36,7 @@ class Main: self.xd.x = 0.0 self.xd.y = 0.0 self.xd.z = 5.0 + break # service(s) @@ -47,7 +48,12 @@ class Main: def waypoint_relay(self,req): - self.resp.xd = self.xd + #TODO Need to add check to make sure query is boolean + if req.query: + self.xd = req.xd + self.resp.xd = self.xd + else: + self.resp.xd = self.xd return self.resp # Change desired position if there is a change @@ -63,7 +69,9 @@ if __name__=="__main__": rospy.init_node('waypoints_server',anonymous=False) try: obj = Main() # create class object - s = rospy.Service('status/waypoint_tracker',WaypointTrack,obj.waypoint_relay) + s = rospy.Service('status/waypoint_tracker',WaypointTrack,obj.waypoint_relay) + rospy.loginfo_once('waypoints_server has started with:\nxd.x = %.1f\nxd.y = %.1f\nxd.z = %.1f', obj.xd.x, obj.xd.y, obj.xd.z) + rospy.spin() # loop until shutdown signal except rospy.ROSInterruptException: diff --git a/srv/WaypointTrack.srv b/srv/WaypointTrack.srv index 7352ec2..b9502db 100644 --- a/srv/WaypointTrack.srv +++ b/srv/WaypointTrack.srv @@ -1,4 +1,5 @@ -bool query # bool to request waypoints +bool query # bool to request or set waypoints +geometry_msgs/Point xd # waypoints --- geometry_msgs/Point xd # waypoints - +