From aebade68c7280136d8874286b0b9ebf1a3643f2b Mon Sep 17 00:00:00 2001 From: Markus Bader Date: Mon, 28 Aug 2023 14:56:42 +0200 Subject: [PATCH] rviz plugin added --- README.md | 3 + tuw_graph_rviz_plugins/CMakeLists.txt | 120 +++++++++ tuw_graph_rviz_plugins/LICENSE | 27 ++ .../icons/classes/Graph.png | Bin 0 -> 6574 bytes tuw_graph_rviz_plugins/icons/classes/tuw.png | Bin 0 -> 5995 bytes .../tuw_graph_rviz_plugins/graph_display.hpp | 128 +++++++++ .../graph_display_selection_handler.hpp | 86 ++++++ .../visibility_control.hpp | 64 +++++ tuw_graph_rviz_plugins/package.xml | 35 +++ .../plugins_description.xml | 14 + tuw_graph_rviz_plugins/src/graph_display.cpp | 245 ++++++++++++++++++ .../src/graph_display_selection_handler.cpp | 126 +++++++++ tuw_pose_rviz_plugins/LICENSE | 2 + 13 files changed, 850 insertions(+) create mode 100644 tuw_graph_rviz_plugins/CMakeLists.txt create mode 100644 tuw_graph_rviz_plugins/LICENSE create mode 100644 tuw_graph_rviz_plugins/icons/classes/Graph.png create mode 100644 tuw_graph_rviz_plugins/icons/classes/tuw.png create mode 100644 tuw_graph_rviz_plugins/include/tuw_graph_rviz_plugins/graph_display.hpp create mode 100644 tuw_graph_rviz_plugins/include/tuw_graph_rviz_plugins/graph_display_selection_handler.hpp create mode 100644 tuw_graph_rviz_plugins/include/tuw_graph_rviz_plugins/visibility_control.hpp create mode 100644 tuw_graph_rviz_plugins/package.xml create mode 100644 tuw_graph_rviz_plugins/plugins_description.xml create mode 100644 tuw_graph_rviz_plugins/src/graph_display.cpp create mode 100644 tuw_graph_rviz_plugins/src/graph_display_selection_handler.cpp diff --git a/README.md b/README.md index d43f240..81b7e3c 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,5 @@ # tuw_rviz package with rviz plugins to support tuw_msgs + +## tuw_pose_rviz_plugins +This plugin is a copy of the common rviz pose plugin and can be used as a template for new plugins. diff --git a/tuw_graph_rviz_plugins/CMakeLists.txt b/tuw_graph_rviz_plugins/CMakeLists.txt new file mode 100644 index 0000000..9410dd5 --- /dev/null +++ b/tuw_graph_rviz_plugins/CMakeLists.txt @@ -0,0 +1,120 @@ +cmake_minimum_required(VERSION 3.8) +project(tuw_graph_rviz_plugins) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(Qt5 REQUIRED COMPONENTS Widgets Core) +find_package(yaml_cpp_vendor REQUIRED) +find_package(ament_cmake REQUIRED) +find_package(rcpputils) +find_package(rclcpp REQUIRED) +find_package(rclpy REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(rviz_common REQUIRED) +find_package(rviz_default_plugins REQUIRED) +find_package(rviz_rendering REQUIRED) + +## This setting causes Qt's "MOC" generation to happen automatically, which is required for +## Qt's signal/slot connections to work. +set(CMAKE_AUTOMOC ON) + + +set(tuw_graph_rviz_plugins_headers_to_moc + include/tuw_graph_rviz_plugins/graph_display.hpp +) + +foreach(header "${tuw_graph_rviz_plugins_headers_to_moc}") + qt5_wrap_cpp(tuw_graph_rviz_plugins_moc_files "${header}") +endforeach() + +## Here we specify the list of source files. +## The generated MOC files are included automatically as headers. +set(tuw_graph_rviz_plugins_source_files +src/graph_display.cpp +src/graph_display_selection_handler.cpp +) + +add_library(${PROJECT_NAME} SHARED + ${tuw_graph_rviz_plugins_moc_files} + ${tuw_graph_rviz_plugins_source_files} +) + +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ + ${Qt5Widgets_INCLUDE_DIRS} +) + +target_link_libraries(${PROJECT_NAME} PUBLIC + rviz_ogre_vendor::OgreMain + rviz_ogre_vendor::OgreOverlay + rviz_common::rviz_common +) + +# Causes the visibility macros to use dllexport rather than dllimport (for Windows, when your plugin should be used as library) +target_compile_definitions(${PROJECT_NAME} PRIVATE "RVIZ_DEFAULT_PLUGINS_BUILDING_LIBRARY") + + +ament_target_dependencies(${PROJECT_NAME} + PUBLIC + rcpputils + rclcpp + geometry_msgs + rviz_common + rviz_rendering + rviz_default_plugins + yaml_cpp_vendor +) + +# Export the plugin to be imported by rviz2 +pluginlib_export_plugin_description_file(rviz_common plugins_description.xml) + +# Export old-style CMake variables +ament_export_include_directories("include/${PROJECT_NAME}") + +# Export modern CMake targets +ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET) + +ament_export_dependencies( + rcpputils + rclcpp + geometry_msgs + rviz_common + rviz_rendering +) + +install( + TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) + +install( + DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/icons" + DESTINATION "share/${PROJECT_NAME}" +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/tuw_graph_rviz_plugins/LICENSE b/tuw_graph_rviz_plugins/LICENSE new file mode 100644 index 0000000..0dc4926 --- /dev/null +++ b/tuw_graph_rviz_plugins/LICENSE @@ -0,0 +1,27 @@ +Copyright (c) 2023 Markus Bader + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +POSSIBILITY OF SUCH DAMAGE. diff --git a/tuw_graph_rviz_plugins/icons/classes/Graph.png b/tuw_graph_rviz_plugins/icons/classes/Graph.png new file mode 100644 index 0000000000000000000000000000000000000000..f6e4c561e0619660700d6ca1f4c34b0b65570836 GIT binary patch literal 6574 zcmeHKc{o&U8y~wQ+4nRi+B9Z0X6CiU*pekwQpwCX!!XR48H~KiQpl3ED6ec;qO_=N z6-mYWQi&)cTNK%f6sqrx_Urrp>AJr6zhHITv~$ zc7xR7{SEWtVJ@c!!@}lp=35gzI!FwBt?A4WlI1 z(3{Mhav9qM1J!Zylr=Gnp?ycgE+qn~tdD;l>YBUWlREYB3I$~Jk8T@8PhIadmfI;9;qMw;9aDE9Ag;Le07=l*L~gCc(~6bV zPme+}#CKb+9~2jd=vlUEdzQ~VAt9d!TlsaaGYEX7+PR?&gS$|mGX$zqx#|8b|o!z`)qr=j9}dCY;3TlzP!` zY%^Q*I`jOFt?SFO$SsC5kDcyk)y$wxu56|qlCe|`o=V!L(Jpm%U)k!MVJYuWO}%x) zVtVnzf0|dS*bOvDh6T&1OW&6hl z>lV?DKUY-#pwbtB(!-cOJ;Z5nqVkyk5_`zg` z{elC$7$!1Fy=+pybJtsU)Lrhm#{6daO7k6#IQfPC(z>Ap=fvEURrLq4%n=?SSZ{Hg zacr!)v~K!E{b|;two-$qVsM-5Z-LPj1|`v!If64YMRCoB5wl|XP5Z1nVfCvOHyovH z>`c)e6?Iujln>P&QChv(q{iJztY_tJ`R)}rTQySOJ!p%J$rCns6+P9uw#3p$Y@(}W z#G1&{j7;k0g`VHwZTZ5pDk>L(*SL}YPU||pU!5t2sx@dzkIcAo^N`cANoyYDwwy+Z zsU*6^E#`G+;q8eSNw)N8@nri8IWJQ6v3miS2Q~_6?KMuDQnuQ&_lw_3NG#t|gi4%? z@2f~PiF}#@wK}hRE4l9YIg6d0k&U@Efnpjp<}Xz&PCFu4H{?=;@+uswlxGjO7HHPD zLWgek#1R`>2QCxH`3qMSrl#L~d>HA}WAECu=k2Z$oqxs*Giiz2pnsBtqX(yOwusnu zH{>fB)?}MkcJnO%vB#I!nQlRMX0cAL{U96K?0!(apo<(CWriJUyUuxSm6x}B?zp7) zvx$}JdO*V?eSGg^41HoyQf1k}6TRx)>w+SbaZSMRhXn55viIo8UOm$78Vg|pM?P(R zqvbu{@0HkAKIc}99Gq3rynaF(<~EiMOA9uAG?*8&5##8VZbqpOX;`b!%aE#(W?U+% zY>Q*CSK151S?V8a3tsiEoU3fJ>R*HKwY#Z`4@6WOV1rX#j0 zaTCxIjp{M(kj>nQ*@&0@4UW#~`u54(V&Pqzs~1jfSHIkS)e9TLVJWRn6lRUyeIOCx-}m`^-1E7!=ss z2ajm387Nne_Koed9FLwYhAF&!S54VhX4SpSF&vY93r}vAyMOX#l8pe{=kLIsD)2UFBy=S3my1|ko9x`pJVX0hGc!~^B*tytfCUSVZ zX`@^Ae!;kA*3{*R)Mj<@F=DH&TXD&;9@heqC2M<<>SLY;bkVB&u3x#3Ns???kG zvH;Hz6*7jH8i50rMjl#a^JI;Q9378 zr?vc@A94RyLhF|PRe|>@*yz)nT8m6;OHZ$~<47;t{1oubNw;cZc(Po^V3-JJA0x7bjbDIY78g}C|wB`h2mZqahJHqp1nU+`Ua zxX$(Gh|chr_78pF@cqIwenLu}aN8$BxzO#NXJk{17v|YVP70x>Y=vB*>(fD5Gno^(G$*knyD3`+MfgzeO`McT5=057`Lem3B= z-_Du3--k-@fEgP}81RT7fG@x$LwUXoKMs*+2wTD>g6I5U1Pr=l!u2tPx!5{D&6sQe ziq*yHBH`vddLSBRBmp&Gd(eoEB#SQ);FTfFi_2vZ5s09mAl)F0E|cwvKoJN81QLxv zqv4NkH@ z0GqK?jt3P1FaTfBlmkXZ{S?yD+ScKl2cH5@x-V)Eovfd7pIQ zVR2MEfThvMKS9{D>7Xjfj32Y&LwWF_a8wEzsfU5<YvL}FG`FSo)@ChewcCa>tp>>hpM;sVrE)8@rgl(n!1@OL4 zIMaOrCoY-KCJINupuoD3IDI{Rf*$_6(GGyk0hP$dL?LxCpKlAY=F#VvYnYsh9Qif5|r<`ByXs}=cZWEIiN)_U-4fw?*#aNzWRI%Fz8EDQ0S6u ziDc^MARKZa;IR}4^!q$T^&Hniy`WO_Rf=6QE6g&+B$Kr4}I2ohQ z=NgGcgN^h6aO9uSIZPTih|C5ydxDjM)qnz8ss_4#iOLN>TL*aoe4dbK3>=AvqxGFp zIHDekh$QGB(L^K?hWPhO#1hCz0s)DHQwcOY9E;Yc!6{@E9!|i3m3q*y0736_2fq>c ze<=}*`?*8|1b^ZB+EfF?f3^Ly!4E4PNX_Rla0LYyWW=`>`3q-!rTmMZFLL`ABY@Dq zgZvV|zv=o-*Do>fOUA#|^_#9=V&IpIf2-^NMwi6*cRIii+~@^?Z)_W}p=#jUo+!n} zk_7q0|K?mTN(D!jv8;A+Aduy<{6pYjPv{~zD8{w6H5VHZSChi%I`-~ELLh>{)}+nO z_J!!BxRbhPO>$ThJ+zx%CdaygicFVwy*OLah{&xl@E_W{*8|^!MHDyJ_ zvQj4Yp=AL6Oi=Wlgun}l`LOWXj|p%^`lZQO@{5Nh9w+GQq0kK1ok z%|tG9+?r?N*|ayM-CWrIzRU|mer`c_;c7+uy3e=L(d}7P* znheNLI(Kb!aqDxjL2r=CJ0cFoMMW1TjdY8`aO<;lHoSj*u1X*__RRXr)WC*0EClLaJ9^0fHj6y^;e(Td2**v5Q6LE7BZ{Cy((m zV-m#Qr$(}OC7oMjJt}k@7nX{S@dF&Y!~R&9G_uX@QaRe(JsxE9Nw|%s6VWE(o|@CK kN2o*~JK#q7iuDUC0<@-{{4<~G4(bwOZEi;@Gj)&n50`pq-2eap literal 0 HcmV?d00001 diff --git a/tuw_graph_rviz_plugins/icons/classes/tuw.png b/tuw_graph_rviz_plugins/icons/classes/tuw.png new file mode 100644 index 0000000000000000000000000000000000000000..db823b2cff2471485e3829ed13155003e02db5f1 GIT binary patch literal 5995 zcmeH~c|4R|8^^~kMMxJSrXd!MHCXD zD2h_3gph27l9Y-TdGDcm>hrw+^!dE+f6aXEnfpHHy1wUJzjK{)?i26gWT&pCrv`yQ z)E(@pZs5PB>{3+*e+@<^Xb_09rPRYq>_!WN3WR(Xn*%__;Q{~(NZ2e0MADk$<(*KUX^cir#q zd1M+Ot{QUQz|W7kT3@l@U{SaTi8{5wrzlpt{%g>o!TF2t^vCF~a;skzzR~6H{#uoz zl{Jp>a}S294jA@IkPU`=ihS_rPOO`~`YE>*mppDhvbcnqS@of{x@C#i1*PHOyHAn} zB+J7T1tTwa*7mD*3~J+368lvy1%0)b-$s5s@YQ6Ip1a3H@qqf?QoH(*=d<({I(a#I&siwt$vc*zGd05Tb0a=gV37r z*6n;q{H29AjfP*StqZ)|8htuNTOoJlAw58^ah>nkRn++c^!Fz>g|pzs+{dhf)+Sjx-9fxJO8dxrd)ia$(jn=^~zoy zDr3%X0sh1buz5})aL!b*4UsvYs($K&T}zB@>qQF0MmrggApd}DED?^8j~ zVOW5%_$hU0UHc)Ipb@=u)ARIW9-e73{6w0O@rH23D=c4Sq}w+q&tR4ncY<&u|v z?ClUgQRpuJ^H0$B_p?WW5`DRc&8`)gGGvL2X_d;I2TFq7IvpYu_ zHGgp%Xk#Pazc}$hpse3w7QxCNfHI0ql zOa(uwT0wZ6!5u zuw-AU*S040W7n!Hrw=z82TgP6IexM$Jaz6xzr@zd7gy@&BI(@J!M!ETQrjFDlb`I< zPCD%q0`>099rsipF`_JNL|HxPS^4kV*V3Iiv1=(l7E;Zt16ohJh?tBVJBnXS=Z&oT zGb;{BT*o-HGnHpyMarq}Ej=~%V}%Cwv28EQwtO(@iry;sE@P%)rd|s|@$QVA>q|Nk zZQivWoSWq_Z@Grnu_s-jEg4->DXqpK;7nA*?#eo=8M~&dji{LATkS8?K%On8oFAcE zS{Py9-l%&#uW4&x%EHp3IQr#+h3b*?yz=CawMkOTg{FI+xIX^W<5o8<=1uzQ!p$j+ z%oUU^88^I2#5-zi+{hW2=askrj^4bvHr_za{#tb{mJ_iC__}B)taTzmGc#g9{dM4p zLM~;$^NA(#l`qtZoM(fI)V9Qzr*R5FU;SeF$0WHQB6D#1#`X}1*RH;`j`h0AdUTB)HWF2}EtBgPfvv{!yI(#oxFgM>VdwYLvl z5WDJ0T4B*kOmx}8C`nAsTY_2XK7gH=01JJO@w=IHMGBBJ!+d&r5T`G+ z0fzVc8tq5y9%C}^#;~5e9ema>a+$0FKjPV{3!j+XU-S1&A8!1iOKu(KZ-#DG+uyBI z`1CBf%wP`SOkBFdub$rOV6(gH^_Ih*)@udpy7gr5xibb|{_?ozsfMFb%i5aUw*&Lc zuJ$!QcaoPMvGNlQuv^=DBFaifi#MyNzSn+uez)t2ho-D$TDsWBv(du|Q00bg=j(Ztsep*TS){oMi3x|(t!Xrj{Lyq7X~2F78Vvr6b6aGz(EVRD4ZvzN#Hz@kqj}#paLR>kS!3i`8=qMNu%>a z#1t3|tcQN@A6MY$_yf-qeNzF{11X^ikZ1%7$>kz{_7I6}LP5y44*gdTkq7vOgmeQ$ z{171nun7ftVxylCOvVp?L5Pqu84i>}Y zi=~*&`bVr^`X;NH4Cm*LfbKuIzgT~debN}Ta&#n7`HT=*_Z+Acn5=yglh0r?Nt3tc z7HArs$e_V3m_$4rhb5rlL<_(Ijw4v$&^Q#Gfd?2rp&WQ3F^$InWGE1hV1pcUIst{D zU+z+Fs}sfos0V9_`N5f7#(kzhX6=SkjQfKUWxqKt_~ zA+S?i*|3nnU_fDMvOEO=lUOholC=<^iTOegKA%H@$q-PP=VW?A$y0-3&lZ6e;j)bX zmhuO+U^HL?O-2JXnUr$r zuhwCKfJ`S81`9_K;TVDk8c#AulTa3>C=3aOf+4>JjFipl?-7%c|I&m!Y4FoB0Qyap zfy)cHS|NWdSKl;~$@qW#d>f1ZV+$bqPb0sj?;pAT$n{$a{Fd>b?D`|uZz=Fw#(%Qw ze=eAQAnA-#*PY^kbRF_J)H`cOcU69iy)Bc+OkXT!IOwD zV4;fG!O=#gPj$WqcG_s;3t#ZSJL*7P=|QYLnc{4p8oc0^RY7x32i%xrCQb$ZBF)XV z{rfNdR$A-PHEGeYB||RTw7uY|!Ta_{9j8-L82{;;&!!PTehV;gd3ne(F$q-aB~!aM{) z&hI^`zc6k_38$UdtLR_zM6KdT?X8}N8xSQNNc%f(UoU=^g2qUeQt8bqfWPF3tE-2E zQ12SHJS-r8LGa_H*?J<+dIieo;i`hWi_#@&p9=Np%uNgH5zCdgV)J6Q9IU%1mD}&I zF=rIEd@#K{%S-RGcPr*?r_!_h8=UtK#$q20m47-=)%+;I>(z}Gj|1Z1n1pk3X<|id j>k4?q#X88EieUrSvWpK}@*D!etbjP!I8k#~`A7c;q}O~b literal 0 HcmV?d00001 diff --git a/tuw_graph_rviz_plugins/include/tuw_graph_rviz_plugins/graph_display.hpp b/tuw_graph_rviz_plugins/include/tuw_graph_rviz_plugins/graph_display.hpp new file mode 100644 index 0000000..259426c --- /dev/null +++ b/tuw_graph_rviz_plugins/include/tuw_graph_rviz_plugins/graph_display.hpp @@ -0,0 +1,128 @@ +/* + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + + +#ifndef TUW_GRAPH_RVIZ_PLUGINS__DISPLAYS__POSE__POSE_DISPLAY_HPP_ +#define TUW_GRAPH_RVIZ_PLUGINS__DISPLAYS__POSE__POSE_DISPLAY_HPP_ + +#include + +#include "geometry_msgs/msg/pose_stamped.hpp" + +#include "rviz_common/message_filter_display.hpp" +#include "rviz_common/interaction/forwards.hpp" + +#include "tuw_graph_rviz_plugins/visibility_control.hpp" + +namespace rviz_rendering +{ +class Arrow; +class Axes; +class Shape; +} // namespace rviz_rendering + +namespace rviz_common +{ +namespace properties +{ +class ColorProperty; +class EnumProperty; +class FloatProperty; +} // namespace properties +} // namespace rviz_common + +namespace tuw_graph_rviz_plugins +{ +namespace displays +{ + +class PoseDisplaySelectionHandler; + +typedef std::shared_ptr PoseDisplaySelectionHandlerPtr; + +/** @brief Accumulates and displays the pose from a geometry_msgs::PoseStamped message. */ +class TUW_GRAPH_RVIZ_PLUGINS_PUBLIC PoseDisplay : public + rviz_common::MessageFilterDisplay +{ + Q_OBJECT + +public: + enum Shape + { + Arrow, + Axes, + }; + + PoseDisplay(); + + ~PoseDisplay() override; + void onInitialize() override; + void reset() override; + +protected: + /** @brief Overridden from MessageFilterDisplay to get arrow/axes visibility correct. */ + void onEnable() override; + void onDisable() override; + void processMessage(geometry_msgs::msg::PoseStamped::ConstSharedPtr message) override; + +private Q_SLOTS: + void updateShapeVisibility(); + void updateColorAndAlpha(); + void updateShapeChoice(); + void updateAxisGeometry(); + void updateArrowGeometry(); + +private: + void setupSelectionHandler(); + + std::unique_ptr arrow_; + std::unique_ptr axes_; + bool pose_valid_; + PoseDisplaySelectionHandlerPtr coll_handler_; + + rviz_common::properties::EnumProperty * shape_property_; + + rviz_common::properties::ColorProperty * color_property_; + rviz_common::properties::FloatProperty * alpha_property_; + + rviz_common::properties::FloatProperty * head_radius_property_; + rviz_common::properties::FloatProperty * head_length_property_; + rviz_common::properties::FloatProperty * shaft_radius_property_; + rviz_common::properties::FloatProperty * shaft_length_property_; + + rviz_common::properties::FloatProperty * axes_length_property_; + rviz_common::properties::FloatProperty * axes_radius_property_; + + friend class PoseDisplaySelectionHandler; +}; + +} // namespace displays +} // namespace tuw_graph_rviz_plugins + +#endif // TUW_GRAPH_RVIZ_PLUGINS__DISPLAYS__POSE__POSE_DISPLAY_HPP_ \ No newline at end of file diff --git a/tuw_graph_rviz_plugins/include/tuw_graph_rviz_plugins/graph_display_selection_handler.hpp b/tuw_graph_rviz_plugins/include/tuw_graph_rviz_plugins/graph_display_selection_handler.hpp new file mode 100644 index 0000000..14db5f1 --- /dev/null +++ b/tuw_graph_rviz_plugins/include/tuw_graph_rviz_plugins/graph_display_selection_handler.hpp @@ -0,0 +1,86 @@ +/* + * Copyright (c) 2008, Willow Garage, Inc. + * Copyright (c) 2018, Bosch Software Innovations GmbH. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef TUW_GRAPH_RVIZ_PLUGINS__DISPLAYS__POSE__POSE_DISPLAY_SELECTION_HANDLER_HPP_ +#define TUW_GRAPH_RVIZ_PLUGINS__DISPLAYS__POSE__POSE_DISPLAY_SELECTION_HANDLER_HPP_ + +#include + +#include "geometry_msgs/msg/pose_stamped.hpp" + +#include "rviz_common/interaction/selection_handler.hpp" + +#include "tuw_graph_rviz_plugins/graph_display.hpp" +#include "tuw_graph_rviz_plugins/visibility_control.hpp" + +namespace rviz_common +{ +namespace properties +{ +class StringProperty; +class VectorProperty; +class QuaternionProperty; +} // namespace properties +} // namespace rviz_common + +namespace tuw_graph_rviz_plugins +{ +namespace displays +{ + +class TUW_GRAPH_RVIZ_PLUGINS_PUBLIC PoseDisplaySelectionHandler : public + rviz_common::interaction::SelectionHandler +{ +public: + void createProperties( + const rviz_common::interaction::Picked & obj, + rviz_common::properties::Property * parent_property) override; + + rviz_common::interaction::V_AABB getAABBs(const rviz_common::interaction::Picked & obj) override; + + void setMessage(geometry_msgs::msg::PoseStamped::ConstSharedPtr message); + +private: + PoseDisplaySelectionHandler(PoseDisplay * display, rviz_common::DisplayContext * context); + + PoseDisplay * display_; + rviz_common::properties::StringProperty * frame_property_; + rviz_common::properties::VectorProperty * position_property_; + rviz_common::properties::QuaternionProperty * orientation_property_; + + template + friend typename std::shared_ptr + rviz_common::interaction::createSelectionHandler(Args ... arguments); +}; + +} // namespace displays +} // namespace tuw_graph_rviz_plugins + +#endif // TUW_GRAPH_RVIZ_PLUGINS__DISPLAYS__POSE__POSE_DISPLAY_SELECTION_HANDLER_HPP_ \ No newline at end of file diff --git a/tuw_graph_rviz_plugins/include/tuw_graph_rviz_plugins/visibility_control.hpp b/tuw_graph_rviz_plugins/include/tuw_graph_rviz_plugins/visibility_control.hpp new file mode 100644 index 0000000..15882ba --- /dev/null +++ b/tuw_graph_rviz_plugins/include/tuw_graph_rviz_plugins/visibility_control.hpp @@ -0,0 +1,64 @@ +/* + * Copyright (c) 2018, Open Source Robotics Foundation, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef TUW_GRAPH_RVIZ_PLUGINS__VISIBILITY_CONTROL_HPP_ +#define TUW_GRAPH_RVIZ_PLUGINS__VISIBILITY_CONTROL_HPP_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define TUW_GRAPH_RVIZ_PLUGINS_EXPORT __attribute__ ((dllexport)) + #define TUW_GRAPH_RVIZ_PLUGINS_IMPORT __attribute__ ((dllimport)) + #else + #define TUW_GRAPH_RVIZ_PLUGINS_EXPORT __declspec(dllexport) + #define TUW_GRAPH_RVIZ_PLUGINS_IMPORT __declspec(dllimport) + #endif + #ifdef TUW_GRAPH_RVIZ_PLUGINS_BUILDING_LIBRARY + #define TUW_GRAPH_RVIZ_PLUGINS_PUBLIC TUW_GRAPH_RVIZ_PLUGINS_EXPORT + #else + #define TUW_GRAPH_RVIZ_PLUGINS_PUBLIC TUW_GRAPH_RVIZ_PLUGINS_IMPORT + #endif + #define TUW_GRAPH_RVIZ_PLUGINS_PUBLIC_TYPE TUW_GRAPH_RVIZ_PLUGINS_PUBLIC + #define TUW_GRAPH_RVIZ_PLUGINS_LOCAL +#else + #define TUW_GRAPH_RVIZ_PLUGINS_EXPORT __attribute__ ((visibility("default"))) + #define TUW_GRAPH_RVIZ_PLUGINS_IMPORT + #if __GNUC__ >= 4 + #define TUW_GRAPH_RVIZ_PLUGINS_PUBLIC __attribute__ ((visibility("default"))) + #define TUW_GRAPH_RVIZ_PLUGINS_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define TUW_GRAPH_RVIZ_PLUGINS_PUBLIC + #define TUW_GRAPH_RVIZ_PLUGINS_LOCAL + #endif + #define TUW_GRAPH_RVIZ_PLUGINS_PUBLIC_TYPE +#endif + +#endif // TUW_GRAPH_RVIZ_PLUGINS__VISIBILITY_CONTROL_HPP_ \ No newline at end of file diff --git a/tuw_graph_rviz_plugins/package.xml b/tuw_graph_rviz_plugins/package.xml new file mode 100644 index 0000000..e3e02c4 --- /dev/null +++ b/tuw_graph_rviz_plugins/package.xml @@ -0,0 +1,35 @@ + + + + tuw_graph_rviz_plugins + 0.0.0 + TODO: Package description + markus + BSD-3-Clause + + ament_cmake + ament_cmake_python + + + yaml_cpp_vendor + rviz2 + + + rclcpp + rclpy + pluginlib + + rviz_common + rviz_default_plugins + rviz_rendering + geometry_msgs + + + python3-numpy + + ament_lint_auto + ament_lint_common + + ament_cmake + + diff --git a/tuw_graph_rviz_plugins/plugins_description.xml b/tuw_graph_rviz_plugins/plugins_description.xml new file mode 100644 index 0000000..1447ca1 --- /dev/null +++ b/tuw_graph_rviz_plugins/plugins_description.xml @@ -0,0 +1,14 @@ + + + + + + + Displays a geometry_msgs::PoseStamped message. <a href="http://www.ros.org/wiki/rviz/DisplayTypes/Pose">More Information</a>. + + geometry_msgs/msg/PoseStamped + + \ No newline at end of file diff --git a/tuw_graph_rviz_plugins/src/graph_display.cpp b/tuw_graph_rviz_plugins/src/graph_display.cpp new file mode 100644 index 0000000..0114478 --- /dev/null +++ b/tuw_graph_rviz_plugins/src/graph_display.cpp @@ -0,0 +1,245 @@ +/* + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "tuw_graph_rviz_plugins/graph_display.hpp" + +#include + +#include + +#include "rviz_rendering/objects/arrow.hpp" +#include "rviz_rendering/objects/axes.hpp" +#include "rviz_rendering/objects/shape.hpp" +#include "rviz_common/display_context.hpp" +#include "rviz_common/frame_manager_iface.hpp" +#include "rviz_common/logging.hpp" +#include "rviz_common/properties/color_property.hpp" +#include "rviz_common/properties/enum_property.hpp" +#include "rviz_common/properties/float_property.hpp" +#include "rviz_common/properties/quaternion_property.hpp" +#include "rviz_common/properties/vector_property.hpp" +#include "rviz_common/interaction/selection_manager.hpp" +#include "rviz_common/validate_floats.hpp" +#include "tuw_graph_rviz_plugins/graph_display_selection_handler.hpp" + +namespace tuw_graph_rviz_plugins +{ +namespace displays +{ + +PoseDisplay::PoseDisplay() +: arrow_(nullptr), axes_(nullptr), pose_valid_(false) +{ + shape_property_ = new rviz_common::properties::EnumProperty( + "Shape", "Arrow", "Shape to display the pose as.", + this, SLOT(updateShapeChoice())); + shape_property_->addOption("Arrow", Arrow); + shape_property_->addOption("Axes", Axes); + + color_property_ = new rviz_common::properties::ColorProperty( + "Color", QColor(255, 25, 0), "Color to draw the arrow.", + this, SLOT(updateColorAndAlpha())); + + alpha_property_ = new rviz_common::properties::FloatProperty( + "Alpha", 1, "Amount of transparency to apply to the arrow.", + this, SLOT(updateColorAndAlpha())); + alpha_property_->setMin(0); + alpha_property_->setMax(1); + + shaft_length_property_ = new rviz_common::properties::FloatProperty( + "Shaft Length", 1, "Length of the arrow's shaft, in meters.", + this, SLOT(updateArrowGeometry())); + + shaft_radius_property_ = new rviz_common::properties::FloatProperty( + "Shaft Radius", 0.05f, "Radius of the arrow's shaft, in meters.", + this, SLOT(updateArrowGeometry())); + + head_length_property_ = new rviz_common::properties::FloatProperty( + "Head Length", 0.3f, "Length of the arrow's head, in meters.", + this, SLOT(updateArrowGeometry())); + + head_radius_property_ = new rviz_common::properties::FloatProperty( + "Head Radius", 0.1f, "Radius of the arrow's head, in meters.", + this, SLOT(updateArrowGeometry())); + + axes_length_property_ = new rviz_common::properties::FloatProperty( + "Axes Length", 1, "Length of each axis, in meters.", + this, SLOT(updateAxisGeometry())); + + axes_radius_property_ = new rviz_common::properties::FloatProperty( + "Axes Radius", 0.1f, "Radius of each axis, in meters.", + this, SLOT(updateAxisGeometry())); +} + +void PoseDisplay::onInitialize() +{ + MFDClass::onInitialize(); + + arrow_ = std::make_unique( + scene_manager_, scene_node_, + shaft_length_property_->getFloat(), + shaft_radius_property_->getFloat(), + head_length_property_->getFloat(), + head_radius_property_->getFloat()); + arrow_->setDirection(Ogre::Vector3::UNIT_X); + + axes_ = std::make_unique( + scene_manager_, scene_node_, + axes_length_property_->getFloat(), + axes_radius_property_->getFloat()); + + updateShapeChoice(); + updateColorAndAlpha(); +} + +PoseDisplay::~PoseDisplay() = default; + +void PoseDisplay::onEnable() +{ + MFDClass::onEnable(); + updateShapeVisibility(); + setupSelectionHandler(); +} + +void PoseDisplay::setupSelectionHandler() +{ + coll_handler_ = rviz_common::interaction::createSelectionHandler + (this, context_); + coll_handler_->addTrackedObjects(arrow_->getSceneNode()); + coll_handler_->addTrackedObjects(axes_->getSceneNode()); +} + +void PoseDisplay::onDisable() +{ + MFDClass::onDisable(); + coll_handler_.reset(); +} + +void PoseDisplay::updateColorAndAlpha() +{ + Ogre::ColourValue color = color_property_->getOgreColor(); + color.a = alpha_property_->getFloat(); + + arrow_->setColor(color); + + context_->queueRender(); +} + +void PoseDisplay::updateArrowGeometry() +{ + arrow_->set( + shaft_length_property_->getFloat(), + shaft_radius_property_->getFloat(), + head_length_property_->getFloat(), + head_radius_property_->getFloat()); + context_->queueRender(); +} + +void PoseDisplay::updateAxisGeometry() +{ + axes_->set( + axes_length_property_->getFloat(), + axes_radius_property_->getFloat()); + context_->queueRender(); +} + +void PoseDisplay::updateShapeChoice() +{ + bool use_arrow = (shape_property_->getOptionInt() == Arrow); + + color_property_->setHidden(!use_arrow); + alpha_property_->setHidden(!use_arrow); + shaft_length_property_->setHidden(!use_arrow); + shaft_radius_property_->setHidden(!use_arrow); + head_length_property_->setHidden(!use_arrow); + head_radius_property_->setHidden(!use_arrow); + + axes_length_property_->setHidden(use_arrow); + axes_radius_property_->setHidden(use_arrow); + + updateShapeVisibility(); + + context_->queueRender(); +} + +void PoseDisplay::updateShapeVisibility() +{ + if (!pose_valid_) { + arrow_->getSceneNode()->setVisible(false); + axes_->getSceneNode()->setVisible(false); + } else { + bool use_arrow = (shape_property_->getOptionInt() == Arrow); + arrow_->getSceneNode()->setVisible(use_arrow); + axes_->getSceneNode()->setVisible(!use_arrow); + } +} + +void PoseDisplay::processMessage(geometry_msgs::msg::PoseStamped::ConstSharedPtr message) +{ + if (!rviz_common::validateFloats(*message)) { + setStatus( + rviz_common::properties::StatusProperty::Error, "Topic", + "Message contained invalid floating point values (nans or infs)"); + return; + } + + Ogre::Vector3 position; + Ogre::Quaternion orientation; + if ( + !context_->getFrameManager()->transform( + message->header, message->pose, position, orientation)) + { + setMissingTransformToFixedFrame(message->header.frame_id); + return; + } + setTransformOk(); + + pose_valid_ = true; + updateShapeVisibility(); + + scene_node_->setPosition(position); + scene_node_->setOrientation(orientation); + + coll_handler_->setMessage(message); + + context_->queueRender(); +} + +void PoseDisplay::reset() +{ + MFDClass::reset(); + pose_valid_ = false; + updateShapeVisibility(); +} + +} // namespace displays +} // namespace tuw_graph_rviz_plugins + +#include // NOLINT +PLUGINLIB_EXPORT_CLASS(tuw_graph_rviz_plugins::displays::PoseDisplay, rviz_common::Display) \ No newline at end of file diff --git a/tuw_graph_rviz_plugins/src/graph_display_selection_handler.cpp b/tuw_graph_rviz_plugins/src/graph_display_selection_handler.cpp new file mode 100644 index 0000000..e839a56 --- /dev/null +++ b/tuw_graph_rviz_plugins/src/graph_display_selection_handler.cpp @@ -0,0 +1,126 @@ +/* + * Copyright (c) 2008, Willow Garage, Inc. + * Copyright (c) 2018, Bosch Software Innovations GmbH. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "tuw_graph_rviz_plugins/graph_display_selection_handler.hpp" + +#include + +#include "rviz_rendering/objects/axes.hpp" +#include "rviz_rendering/objects/arrow.hpp" +#include "rviz_rendering/objects/shape.hpp" +#include "rviz_common/interaction/selection_handler.hpp" +#include "rviz_common/msg_conversions.hpp" +#include "rviz_common/properties/vector_property.hpp" +#include "rviz_common/properties/string_property.hpp" +#include "rviz_common/properties/quaternion_property.hpp" +#include "rviz_common/properties/enum_property.hpp" +#include "rviz_common/display_context.hpp" +#include "tuw_graph_rviz_plugins/graph_display.hpp" + +namespace tuw_graph_rviz_plugins +{ +namespace displays +{ + +PoseDisplaySelectionHandler::PoseDisplaySelectionHandler( + PoseDisplay * display, rviz_common::DisplayContext * context) +: SelectionHandler(context), + display_(display), + frame_property_(nullptr), + position_property_(nullptr), + orientation_property_(nullptr) +{} + +void PoseDisplaySelectionHandler::createProperties( + const rviz_common::interaction::Picked & obj, + rviz_common::properties::Property * parent_property) +{ + (void) obj; + rviz_common::properties::Property * cat = new rviz_common::properties::Property( + "Pose " + display_->getName(), QVariant(), "", parent_property); + properties_.push_back(cat); + + frame_property_ = new rviz_common::properties::StringProperty("Frame", "", "", cat); + frame_property_->setReadOnly(true); + + position_property_ = new rviz_common::properties::VectorProperty( + "Position", Ogre::Vector3::ZERO, "", cat); + position_property_->setReadOnly(true); + + orientation_property_ = new rviz_common::properties::QuaternionProperty( + "Orientation", Ogre::Quaternion::IDENTITY, "", cat); + orientation_property_->setReadOnly(true); +} + +rviz_common::interaction::V_AABB PoseDisplaySelectionHandler::getAABBs( + const rviz_common::interaction::Picked & obj) +{ + (void) obj; + rviz_common::interaction::V_AABB aabbs; + if (display_->pose_valid_) { + /** with 'derive_world_bounding_box' set to 'true', the WorldBoundingBox is derived each time. + setting it to 'false' results in the wire box not properly following the pose arrow, but it + would be less computationally expensive. + */ + bool derive_world_bounding_box = true; + if (display_->shape_property_->getOptionInt() == PoseDisplay::Arrow) { + aabbs.push_back( + display_->arrow_->getHead()->getEntity()->getWorldBoundingBox(derive_world_bounding_box)); + aabbs.push_back( + display_->arrow_->getShaft()->getEntity()->getWorldBoundingBox(derive_world_bounding_box)); + } else { + aabbs.push_back( + display_->axes_->getXShape().getEntity()->getWorldBoundingBox(derive_world_bounding_box)); + aabbs.push_back( + display_->axes_->getYShape().getEntity()->getWorldBoundingBox(derive_world_bounding_box)); + aabbs.push_back( + display_->axes_->getZShape().getEntity()->getWorldBoundingBox(derive_world_bounding_box)); + } + } + return aabbs; +} + +void PoseDisplaySelectionHandler::setMessage( + geometry_msgs::msg::PoseStamped::ConstSharedPtr message) +{ + // properties_.size() should only be > 0 after createProperties() + // and before destroyProperties(), during which frame_property_, + // position_property_, and orientation_property_ should be valid + // pointers. + if (properties_.size() > 0) { + frame_property_->setStdString(message->header.frame_id); + position_property_->setVector(rviz_common::pointMsgToOgre(message->pose.position)); + orientation_property_->setQuaternion( + rviz_common::quaternionMsgToOgre(message->pose.orientation)); + } +} + +} // namespace displays +} // namespace tuw_graph_rviz_plugins \ No newline at end of file diff --git a/tuw_pose_rviz_plugins/LICENSE b/tuw_pose_rviz_plugins/LICENSE index 574ef07..0dc4926 100644 --- a/tuw_pose_rviz_plugins/LICENSE +++ b/tuw_pose_rviz_plugins/LICENSE @@ -1,3 +1,5 @@ +Copyright (c) 2023 Markus Bader + Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: