From 1af6d91613ce973492f114ca189cd44d49a76e3c Mon Sep 17 00:00:00 2001 From: Jonas Hirshland Date: Mon, 9 Dec 2019 07:23:11 -0800 Subject: [PATCH] sooner update jhirsh --- drivers/planner.cpp | 4 ++-- include/planner/.ball.hpp.swp | Bin 0 -> 20480 bytes include/planner/arm_planner.hpp | 14 ++++++-------- 3 files changed, 8 insertions(+), 10 deletions(-) create mode 100644 include/planner/.ball.hpp.swp diff --git a/drivers/planner.cpp b/drivers/planner.cpp index 722dcbf..84b5085 100644 --- a/drivers/planner.cpp +++ b/drivers/planner.cpp @@ -49,8 +49,8 @@ class Handler void handleEKFMessage( const lcm::ReceiveBuffer*, const std::string &channel, const ball_detections_t *ball ) { - cout << "Handling message" << endl; - cout << " update received at " << ball->utime << endl; + //cout << "Handling message" << endl; + //cout << " update received at " << ball->utime << endl; planner->updateBalls( *ball ); diff --git a/include/planner/.ball.hpp.swp b/include/planner/.ball.hpp.swp new file mode 100644 index 0000000000000000000000000000000000000000..5d63028afd6dac3b530ea5410d74484521f43f24 GIT binary patch literal 20480 zcmeHPYm6jS6)s#vnuiZmzz62;kgU}^JzX=iuwu^xVP+RLKxW(>cLU2%)>Pf;DWjG*BqH#eZF+yUDny4iB5AYEopfQoi4-++FB#N#vsJ!%tVL{_}?xP;vJ!G?g z3`uqJP4~TZ?z!ild+xcn?m3yI(S!T0W0%!O4Se2i7(ZNoVe$D3KVTex(lBmb@uPU9 z>3{vOa0=Z?d+ZBeeC4hmz1RjN)416C^b-O zpwvLAfl>pd21*UQ;WdzSwi&l!6mL=^`IP!Tt>^o6RbEx!pYD0St*88aPgxbquTlf0 z21*T-8YneTYM|6Wsew`hr3Ok3lo}{CP-@`+paI7-jQ61557Pwz;{W>n|6gx2jDG^h zfoFjSfuq1CumRi%3&SR9s}+L zJ_+0eOaa>g23!D~4?KF7VcY@C1J?o*Km)iC_(#Ps{tEmQcno+H_!961fCC=~Y+x5q z2mXB~bO*c)JOSJeOatcvFTBMt{sue;d<*zGa0u853<2AK<7XJg!@y^O+kp=PFTL3? zehEAT+zWgLxDB`!xDj|Sa6a$^E^593+yPtzTm|d}-VMA1IDw0rhk=KHF9Wv%M}ZeG z|Mvhcumq4jTn23UVY$y5Z-uU(Y+mihNf>R`J7LFOj=~@@s}oRj?$e3*+AL4FoA_Zc zAI5BodAZ_5f%qWE!f4zpkf4RGH?s%mGWLn*;nBs~pz5(j23c~FFfDX};T9~}2 zFk;QFFWPxwcd5nEJsO*`P&HFs*1L({<`6)9Q;(WvY*c)Tx(GclX2Vl#j5-Og^O`&y^&ce)kQp}A<3G!-s#3G_LAeSni=20 z&ogyV+c@zWjWw};CZ(9u=pkjDG9%;JJkV{!QUn9c$p(tTM4iN`$Nnvx7O|8_l90V+ z&!~msKrK@U_gF!ejr5)47N64Zk0megpQ zrxt?dhHJrK5m9sC3vd&nQXKU@$rWo*nkGYPxwLP1Wj&hAA9N?v|m3J4z?76JRvnF z&nJDZ@KqHpT)JzeYi;0Y)p%(-D#8lL3FWQe>U{?m=JsE2AJ})po(VQQ>@%Tt=@~0= zhjXvLD+o~!!l-Rlr8-ZQz1Tnc*^BB^;fz7TdF>0ZsX8q<0l7i36}iL1w)-|OvW6oD znSK6~?Q5oDQLUw0uSvh4C3IG4FyAu)n}NU$TVX^E@U$lPFq4TL4Mp0lV1{^thF4-+ zLNl5P&#s9dXeEB^z684zHL9iaDMiSYssEgDlVxLP!q__4U46Gfqg;-B9(c@cIdRMu zA~<6|Xdd+8O!Ytu6jwy;d6~OP8p&1~{BL1t(NmH;Z#Ke8=|OHKXTndXic=HaL#Z8o z-qdV*JoL1r#B_$rW(%@G@4`C#pwmqjU?O64 zd#>5b;>|eWZE12TBbp|%Izc7QO6we(xF4+2-&mZD7`~UCRn7vw9?Pi}U5EpiXh`Gn z!)~mY%h?x}P9u{hj9;C|U&Fz#VPG{$n?n^X+5lN&qCu{DveaKkQ6LaJtu-{e7~ zAxD3SrwI$y5UYj1#PjcD~MSJITs)UL2hZo>>@WgcnG}=&YGADu|5}WZb;j$=`p)kBotaJJ8z)f2_ z4H&$5GD98=dhJ9`v+dw0>T-kL|9_A7_}2h>|JQMUD!)o~<)_p@sew`hr3Ok3lo}{C zP->vmK&gRJ1EmH^4U`&qLu;UNbV9uU(>wpq=?~ujSJjAqgLnU51K$Vk295y%Z~^c% z;`ZMId|(B*7B~&~4`T8sfS&=614n^%UAfo;GaD~9n0;6dPH4~zq6 z0)IxV{Tbi~z`elTz!LC&;CG0*KLT6{yo|W|cYtpLp9Ag!=79GB7XTD@e;Tp%CxQEb z8Q^kYJMau*>%Rpk);r%0oE0Om=_M=bj{xaaXp<)gdCRQ!Hptzl4rqc%eSZ^iUEIc%r`>V<$cz&w zLQoWUlhp~vz(fQ-^j&N|!dIc|^eMW8<o?csloWBI1!USVQ9I3*78Ec;FJ5B6oH+UuGKGPZio1 z@L6t!4k9F4;fOWX5xUGOCfQh>CLpCi5R~Rb=&&MNYpspoQeRHsI4O=MN>~CJfr>Tj zVEDA2Bf4T=2=`L}MCBo{oZd8fe)d%Kyy>rT<}eY+)3=;b5E22`o+7B?AQJ*&;GT1w z#T@wx9^JsB=&o?wq2e+%=E8_5@V-Qsr0YKZh_>J@tOdMaGwVKG>!*u@A{2Tl?TK** zi+PiC)>oTOoeJdZ*+VV_1^$bzi&bb^qb^C}g}K?e2HST)1{T~fig0Isc`=DbtqjW| zV>m#@Q+E;_<~PS-Q3D_Bf^9KF8#;YkR>X!!WJkV!6ql!5YkhI z=m);3$h-|%?o%HH`6b2X78KADQDEK?`;f{i_NA5vR%e3^36g9X=BEIwC|f;jn3=u8 zP`1H>Ttk*s>;t5XBO7L!{#OmUA&L5$qfWF+MeU=ovYefK_)iF};>EpfL?O zUXpr+l>BJ=OD8R<)_Kn=tUz~w)tQ?0{Qf<&`(_r{%-r1mSvI?%EJ0cMjlocEp;gjh z!SHkzmPzrPlS0h+v%JO_R zCLK5FEtjyF*%D>YQ0}H;->X72%iJKjxn7M*A0d<}f5OPIhZpwr;!Y>Ya!I|?oJD#` zCAaI6hD7Eiaw^k2()lo^)E^|mQo@PIreachPrediction ); - if ( bestBall && convertUTimeToSeconds( getUtime() - lastUpdateArm ) > 0.1 ) + if ( bestBall && convertUTimeToSeconds( getUtime() - lastUpdateArm ) > 0.02 ) { - std::cout << "x: " << bestBall->reachPrediction.ball_inrange_position_[0] << std::endl; + std::cout << "x: " << bestBall->reachPrediction.ball_inrange_position_[0] + << " y: " << best->reachPrediction.ball_inrange_position_[1] << std::endl; publishPlan( bestBall->reachPrediction.ball_inrange_position_ ); } - std::cout << "corresponded: " << balls.size() << std::endl; + //std::cout << "corresponded: " << balls.size() << std::endl; cond_var->notify_all(); } @@ -285,8 +286,8 @@ class ArmPlanner std::vector > waypoints( 1, std::vector(3, 0)); // Flip X and Y for Arm Coordinate system - waypoints[0][0] = endpoint[1]; - waypoints[0][1] = endpoint[0]; + waypoints[0][0] = endpoint[0]; + waypoints[0][1] = endpoint[1]; waypoints[0][2] = 1; path.waypoints = waypoints; path.speed = 1.0; @@ -295,9 +296,6 @@ class ArmPlanner path.angles = angles; lcm->publish( "ARM_PATH", &path ); - - // path.waypoints[0][0] = pr.second.x; - // path.waypoints[0][1] = pr.second.y; } const std::vector& getBalls()