├── .gitignore ├── README.md ├── flir_ptu_description ├── CHANGELOG.rst ├── CMakeLists.txt ├── meshes │ ├── flir-ptu-camera-mount.stl │ ├── flir-ptu-pan-motor-collision.stl │ ├── flir-ptu-pan-motor.stl │ ├── flir-ptu-tilt-motor-collision.stl │ └── flir-ptu-tilt-motor.stl ├── package.xml └── urdf │ ├── d46.urdf.xacro │ └── example.urdf.xacro ├── flir_ptu_driver ├── CHANGELOG.rst ├── CMakeLists.txt ├── debian │ └── udev ├── include │ └── flir_ptu_driver │ │ └── driver.h ├── launch │ └── ptu.launch ├── package.xml ├── scripts │ └── cmd_angles └── src │ ├── driver.cpp │ └── node.cpp └── flir_ptu_viz ├── CHANGELOG.rst ├── CMakeLists.txt ├── launch ├── view_model.launch └── view_ptu.launch ├── package.xml └── rviz └── urdf.rviz /.gitignore: -------------------------------------------------------------------------------- 1 | # Build files 2 | ptu_control/msg/ 3 | msg_gen/ 4 | srv_gen/ 5 | **/src/**/msg/ 6 | **/src/**/srv/ 7 | **/src/**/__init__.py 8 | **/src/*msgs 9 | build/ 10 | doc/ 11 | bin/ 12 | lib/ 13 | cmake_install.cmake 14 | _gtest_from_src/ 15 | 16 | #QT 17 | CMakeLists.txt.user 18 | qtcreator-build/ 19 | 20 | # Dynamic reconfigure 21 | docs/ 22 | *.cfgc 23 | **/cfg/cpp/ 24 | **/src/*/cfg/ 25 | 26 | # Eclipse project 27 | .project 28 | .cproject 29 | .pydevproject 30 | .settings 31 | 32 | # Python 33 | *.pyc 34 | *.pkl 35 | 36 | 37 | # Temporary files 38 | *.swp 39 | *~ 40 | 41 | # Mac os 42 | .DS_Store 43 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | flir_ptu 2 | ======== 3 | 4 | Basic serial ROS driver for FLIR PTUs. Currently tested with: 5 | 6 | - [FLIR D46](http://www.flir.com/mcs/view/?id=53707) 7 | 8 | 9 | License 10 | ======= 11 | 12 | This repo originated at [Washington University](https://wu-robotics.googlecode.com/svn/branches/stable/wu_ptu), 13 | where the code was licensed as GPLv2. The initial copy was made at svn revision r2226. 14 | 15 | Thanks to Nick Hawes (@hawesie) for the first pass at catkinizing this repo. 16 | -------------------------------------------------------------------------------- /flir_ptu_description/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package flir_ptu_description 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.2.1 (2021-03-26) 6 | ------------------ 7 | * Fix missing xacro namespace prefixes 8 | * Updated PTU mesh geometry (`#40 `_) 9 | * Contributors: Dave Niewinski, Robert Haschke 10 | 11 | 0.2.0 (2018-06-21) 12 | ------------------ 13 | * Add pan offset for centering joint based on calibration 14 | * Update xacro URL to include www to not give warnings from redefinition 15 | * Fixed spaces, fixed gazebo errors about inertia 16 | * Revert "Fix gazebo errors" 17 | * Updated package.xml for my maintainership 18 | * Fix gazebo errors 19 | * updated CMakeLists to install new meshes folder 20 | * added mesh to visual so openrave can view it, added in transmissions 21 | * Added transmissions and inertial elements for sim 22 | * Reverse pan joint direction. 23 | Per `#15 `_. 24 | * Contributors: Allison Thackston, Dash, Devon Ash, DevonAsh, Mike Purvis, Will Baker 25 | 26 | 0.1.4 (2014-07-17) 27 | ------------------ 28 | * Fix weird urdf issue with floating tilt joint. 29 | * Add comments to the example URDF. 30 | * Contributors: Mike Purvis 31 | 32 | 0.1.3 (2014-04-13) 33 | ------------------ 34 | 35 | 0.1.2 (2014-04-12) 36 | ------------------ 37 | * Remove $(find) macro referencing self, since this doesn't work on the first build. 38 | * Contributors: Mike Purvis 39 | 40 | 0.1.1 (2014-04-11) 41 | ------------------ 42 | * Remove urdf as build-time dep. 43 | * Reverse the pan joint; now tested with real hardware. 44 | * Contributors: Mike Purvis 45 | 46 | 0.1.0 (2014-04-10) 47 | ------------------ 48 | * Completed basic tf tree and model of D46. 49 | * Contributors: Mike Purvis 50 | -------------------------------------------------------------------------------- /flir_ptu_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(flir_ptu_description) 3 | 4 | find_package(catkin REQUIRED COMPONENTS xacro) 5 | 6 | catkin_package() 7 | 8 | set(FLIR_PTU_DEVEL_SHARE ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}) 9 | file(MAKE_DIRECTORY ${FLIR_PTU_DEVEL_SHARE}/urdf) 10 | 11 | xacro_add_xacro_file( 12 | ${CMAKE_CURRENT_SOURCE_DIR}/urdf/example.urdf.xacro 13 | ${FLIR_PTU_DEVEL_SHARE}/urdf/example.urdf) 14 | add_custom_target(flir_ptu_example_urdf ALL 15 | DEPENDS ${FLIR_PTU_DEVEL_SHARE}/urdf/example.urdf) 16 | 17 | install(FILES ${FLIR_PTU_DEVEL_SHARE}/urdf/example.urdf 18 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/urdf) 19 | 20 | install(DIRECTORY meshes urdf 21 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 22 | -------------------------------------------------------------------------------- /flir_ptu_description/meshes/flir-ptu-camera-mount.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-drivers/flir_ptu/33d1dbfeef81cac64761b700ee726bacdd43ac7b/flir_ptu_description/meshes/flir-ptu-camera-mount.stl -------------------------------------------------------------------------------- /flir_ptu_description/meshes/flir-ptu-pan-motor-collision.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-drivers/flir_ptu/33d1dbfeef81cac64761b700ee726bacdd43ac7b/flir_ptu_description/meshes/flir-ptu-pan-motor-collision.stl -------------------------------------------------------------------------------- /flir_ptu_description/meshes/flir-ptu-pan-motor.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-drivers/flir_ptu/33d1dbfeef81cac64761b700ee726bacdd43ac7b/flir_ptu_description/meshes/flir-ptu-pan-motor.stl -------------------------------------------------------------------------------- /flir_ptu_description/meshes/flir-ptu-tilt-motor-collision.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-drivers/flir_ptu/33d1dbfeef81cac64761b700ee726bacdd43ac7b/flir_ptu_description/meshes/flir-ptu-tilt-motor-collision.stl -------------------------------------------------------------------------------- /flir_ptu_description/meshes/flir-ptu-tilt-motor.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-drivers/flir_ptu/33d1dbfeef81cac64761b700ee726bacdd43ac7b/flir_ptu_description/meshes/flir-ptu-tilt-motor.stl -------------------------------------------------------------------------------- /flir_ptu_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | flir_ptu_description 4 | 0.2.1 5 | URDF description the FLIR PTUs, currently the D46. 6 | 7 | Devon Ash 8 | http://wiki.ros.org/flir_ptu_driver 9 | 10 | BSD 11 | 12 | catkin 13 | xacro 14 | urdf 15 | xacro 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /flir_ptu_description/urdf/d46.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | transmission_interface/SimpleTransmission 13 | 14 | PositionJointInterface 15 | 16 | 17 | 1 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | Gazebo/DarkGrey 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | Gazebo/DarkGrey 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | -------------------------------------------------------------------------------- /flir_ptu_description/urdf/example.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 19 | 20 | -------------------------------------------------------------------------------- /flir_ptu_driver/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package flir_ptu_driver 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.2.1 (2021-03-26) 6 | ------------------ 7 | * Don't load the description & start the robot_state_publisher by default 8 | * Contributors: Chris Iverach-Brereton 9 | 10 | 0.2.0 (2018-06-21) 11 | ------------------ 12 | * Linter fixes. 13 | * Added udev rule (`#39 `_) 14 | * Added disable limits and home commands (`#38 `_) 15 | * Adding parameters for software range limits disable 16 | * Adding parameters for software range limits disable 17 | * Added ability to disable software limits 18 | * Added reset subscribed topic to home PTU. This causes driver crash, so 19 | added respawn directive to launch file. 20 | * Minor edits to comply with style guide 21 | * Add default velocity support 22 | * Driver was crashing. Change nulls and emptys to 0s to allow typecasting to double 23 | * added queue_size to cmd_angles (`#23 `_) 24 | * Fix linter, add sleep to blocking loop. 25 | * Fix publish topic in cmd_angles script. 26 | * Contributors: Allison Thackston, Ilia Baranov, Mike Purvis, TheDash, Tony Baltovski, Will Baker, dniewinski 27 | 28 | 0.1.4 (2014-07-17) 29 | ------------------ 30 | * Fix repository and bug-tracker URLs. 31 | * Add more URL elements 32 | * Update package description 33 | * Fix tabs->spaces in launch file. 34 | * Contributors: Mike Purvis 35 | 36 | 0.1.3 (2014-04-13) 37 | ------------------ 38 | * catkin_lint fixes 39 | * Contributors: Mike Purvis 40 | 41 | 0.1.2 (2014-04-12) 42 | ------------------ 43 | 44 | 0.1.1 (2014-04-11) 45 | ------------------ 46 | 47 | 0.1.0 (2014-04-10) 48 | ------------------ 49 | * Parameterize the joint name. 50 | * Add a new simple script for commanding the PTU. 51 | * Remove package for actions; should use control_msgs instead. 52 | * Remove Rate in favour of a Timer callback. 53 | * Add roslint. 54 | * Automatic astyle whitespace/padding fixes. 55 | * Major refactoring. 56 | * Contributors: Mike Purvis 57 | -------------------------------------------------------------------------------- /flir_ptu_driver/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(flir_ptu_driver) 3 | 4 | find_package(catkin REQUIRED COMPONENTS actionlib diagnostic_updater roscpp roslaunch roslint rospy serial sensor_msgs tf) 5 | find_package(Boost REQUIRED) 6 | 7 | catkin_package( 8 | LIBRARIES flir_ptu_driver 9 | CATKIN_DEPENDS sensor_msgs serial 10 | ) 11 | 12 | ## Specify additional locations of header files 13 | ## Your package locations should be listed before other locations 14 | include_directories(include 15 | ${catkin_INCLUDE_DIRS} 16 | ${Boost_INCLUDE_DIRS} 17 | ) 18 | 19 | ## Declare a cpp library 20 | add_library(flir_ptu_driver src/driver.cpp) 21 | target_link_libraries(flir_ptu_driver ${catkin_LIBRARIES}) 22 | 23 | ## Declare a cpp executable 24 | add_executable(flir_ptu_node src/node.cpp) 25 | target_link_libraries(flir_ptu_node ${catkin_LIBRARIES} flir_ptu_driver) 26 | add_dependencies(flir_ptu_node flir_ptu_msgs_generate_messages_cpp) 27 | set_target_properties(flir_ptu_node 28 | PROPERTIES OUTPUT_NAME ptu_node PREFIX "") 29 | 30 | install(TARGETS flir_ptu_driver flir_ptu_node 31 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 32 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 33 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 34 | ) 35 | 36 | install(PROGRAMS scripts/cmd_angles 37 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 38 | 39 | install(DIRECTORY launch 40 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 41 | 42 | ## Tests 43 | roslint_cpp() 44 | roslint_add_test() 45 | roslaunch_add_file_check(launch/ptu.launch 46 | DEPENDENCIES flir_ptu_example_urdf flir_ptu_node) 47 | -------------------------------------------------------------------------------- /flir_ptu_driver/debian/udev: -------------------------------------------------------------------------------- 1 | SUBSYSTEM=="tty", ATTRS{idVendor}=="067b", ATTRS{idProduct}=="2303", MODE="0666", SYMLINK+="ptu" 2 | -------------------------------------------------------------------------------- /flir_ptu_driver/include/flir_ptu_driver/driver.h: -------------------------------------------------------------------------------- 1 | /* 2 | * flir_ptu_driver ROS package 3 | * Copyright (C) 2014 Mike Purvis (mpurvis@clearpathrobotics.com) 4 | * 5 | * PTU ROS Package 6 | * Copyright (C) 2009 Erik Karulf (erik@cse.wustl.edu) 7 | * 8 | * Author: Toby Collett (University of Auckland) 9 | * Date: 2003-02-10 10 | * 11 | * Player - One Hell of a Robot Server 12 | * Copyright (C) 2000 Brian Gerkey & Kasper Stoy 13 | * gerkey@usc.edu kaspers@robotics.usc.edu 14 | * 15 | * This program is free software; you can redistribute it and/or modify 16 | * it under the terms of the GNU General Public License as published by 17 | * the Free Software Foundation; either version 2 of the License, or 18 | * (at your option) any later version. 19 | * 20 | * This program is distributed in the hope that it will be useful, 21 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 22 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 23 | * GNU General Public License for more details. 24 | * 25 | * You should have received a copy of the GNU General Public License 26 | * along with this program; if not, write to the Free Software 27 | * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 28 | * 29 | */ 30 | 31 | #ifndef FLIR_PTU_DRIVER_DRIVER_H 32 | #define FLIR_PTU_DRIVER_DRIVER_H 33 | 34 | // serial defines 35 | #define PTU_DEFAULT_BAUD 9600 36 | #define PTU_BUFFER_LEN 255 37 | #define PTU_DEFAULT_PORT "/dev/ttyUSB0" 38 | #define PTU_DEFAULT_HZ 10 39 | #define PTU_DEFAULT_VEL 0.0 40 | 41 | // command defines 42 | #define PTU_PAN 'p' 43 | #define PTU_TILT 't' 44 | #define PTU_MIN 'n' 45 | #define PTU_MAX 'x' 46 | #define PTU_MIN_SPEED 'l' 47 | #define PTU_MAX_SPEED 'u' 48 | #define PTU_VELOCITY 'v' 49 | #define PTU_POSITION 'i' 50 | 51 | #include 52 | 53 | namespace serial 54 | { 55 | class Serial; 56 | } 57 | 58 | namespace flir_ptu_driver 59 | { 60 | 61 | class PTU 62 | { 63 | public: 64 | /** Constructor - opens port 65 | * \param ser serial::Serial instance ready to communciate with device. 66 | */ 67 | explicit PTU(serial::Serial* ser) : 68 | ser_(ser), initialized_(false) 69 | { 70 | } 71 | 72 | /** \return true if initialization succeeds. */ 73 | bool initialize(); 74 | 75 | /** \return true if PTU software motion limits are disabled. */ 76 | bool disableLimits(); 77 | 78 | /** \return true if the serial port is open and PTU initialized. */ 79 | bool initialized(); 80 | 81 | /** 82 | * \param type 'p' or 't' 83 | * \return position in radians 84 | */ 85 | float getPosition(char type); 86 | 87 | /** 88 | * \param type 'p' or 't' 89 | * \return speed in radians/second 90 | */ 91 | float getSpeed(char type); 92 | 93 | /** 94 | * \param type 'p' or 't' 95 | * \return resolution in radians/count 96 | */ 97 | float getResolution(char type) 98 | { 99 | return (type == PTU_TILT ? tr : pr); 100 | } 101 | 102 | /** 103 | * \param type 'p' or 't' 104 | * \return Minimum position in radians 105 | */ 106 | float getMin(char type) 107 | { 108 | return getResolution(type) * (type == PTU_TILT ? TMin : PMin); 109 | } 110 | /** 111 | * \param type 'p' or 't' 112 | * \return Maximum position in radians 113 | */ 114 | float getMax(char type) 115 | { 116 | return getResolution(type) * (type == PTU_TILT ? TMax : PMax); 117 | } 118 | 119 | /** 120 | * \param type 'p' or 't' 121 | * \return Minimum speed in radians/second 122 | */ 123 | float getMinSpeed(char type) 124 | { 125 | return getResolution(type) * (type == PTU_TILT ? TSMin : PSMin); 126 | } 127 | /** 128 | * \param type 'p' or 't' 129 | * \return Maximum speed in radians/second 130 | */ 131 | float getMaxSpeed(char type) 132 | { 133 | return getResolution(type) * (type == PTU_TILT ? TSMax : PSMax); 134 | } 135 | 136 | /** 137 | * Moves the PTU to the desired position. If Block is true, 138 | * the call blocks until the desired position is reached 139 | * \param type 'p' or 't' 140 | * \param pos desired position in radians 141 | * \param Block block until ready 142 | * \return True if successfully sent command 143 | */ 144 | bool setPosition(char type, float pos, bool Block = false); 145 | 146 | /** 147 | * sets the desired speed in radians/second 148 | * \param type 'p' or 't' 149 | * \param speed desired speed in radians/second 150 | * \return True if successfully sent command 151 | */ 152 | bool setSpeed(char type, float speed); 153 | 154 | /** 155 | * set the control mode, position or velocity 156 | * \param type 'v' for velocity, 'i' for position 157 | * \return True if successfully sent command 158 | */ 159 | bool setMode(char type); 160 | 161 | /** 162 | * get the control mode, position or velocity 163 | * \return 'v' for velocity, 'i' for position 164 | */ 165 | char getMode(); 166 | 167 | bool home(); 168 | 169 | private: 170 | /** get radian/count resolution 171 | * \param type 'p' or 't' 172 | * \return pan resolution if type=='p', tilt resolution if type=='t' 173 | */ 174 | float getRes(char type); 175 | 176 | /** get limiting position/speed in counts or counts/second 177 | * 178 | * \param type 'p' or 't' (pan or tilt) 179 | * \param limType {'n', 'x', 'l', 'u'} (min position, max position, min speed, max speed) 180 | * \return limiting position/speed 181 | */ 182 | int getLimit(char type, char limType); 183 | 184 | // Position Limits 185 | int TMin; ///< Min Tilt in Counts 186 | int TMax; ///< Max Tilt in Counts 187 | int PMin; ///< Min Pan in Counts 188 | int PMax; ///< Max Pan in Counts 189 | bool Lim; ///< Position Limits enabled 190 | 191 | // Speed Limits 192 | int TSMin; ///< Min Tilt Speed in Counts/second 193 | int TSMax; ///< Max Tilt Speed in Counts/second 194 | int PSMin; ///< Min Pan Speed in Counts/second 195 | int PSMax; ///< Max Pan Speed in Counts/second 196 | 197 | protected: 198 | /** Sends a string to the PTU 199 | * 200 | * \param command string to be sent 201 | * \return response string from unit. 202 | */ 203 | std::string sendCommand(std::string command); 204 | 205 | serial::Serial* ser_; 206 | bool initialized_; 207 | 208 | float tr; ///< tilt resolution (rads/count) 209 | float pr; ///< pan resolution (rads/count) 210 | }; 211 | 212 | } // namespace flir_ptu_driver 213 | 214 | #endif // FLIR_PTU_DRIVER_DRIVER_H 215 | -------------------------------------------------------------------------------- /flir_ptu_driver/launch/ptu.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /flir_ptu_driver/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | flir_ptu_driver 4 | 0.2.1 5 | Driver for the FLIR pan/tilt units. 6 | 7 | Mike Purvis 8 | 9 | http://wiki.ros.org/flir_ptu_driver 10 | https://github.com/ros-drivers/flir_ptu.git 11 | https://github.com/ros-drivers/flir_ptu/issues 12 | 13 | 14 | Erik Karulf 15 | David V. Lu 16 | Nick Hawes 17 | 18 | GPL 19 | 20 | catkin 21 | actionlib 22 | boost 23 | diagnostic_updater 24 | roscpp 25 | roslaunch 26 | roslint 27 | rospy 28 | sensor_msgs 29 | serial 30 | tf 31 | actionlib 32 | diagnostic_updater 33 | flir_ptu_description 34 | robot_state_publisher 35 | roscpp 36 | rospy 37 | sensor_msgs 38 | serial 39 | tf 40 | 41 | 42 | -------------------------------------------------------------------------------- /flir_ptu_driver/scripts/cmd_angles: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | import sys 5 | import time 6 | 7 | from sensor_msgs.msg import JointState 8 | 9 | 10 | def set_angles(pan, tilt): 11 | pub = rospy.Publisher("ptu/cmd", JointState, queue_size=1) 12 | time.sleep(0.5) 13 | 14 | js = JointState() 15 | js.name = [ "ptu_pan", "ptu_tilt" ] 16 | js.velocity = [ 0.6, 0.6 ] 17 | js.position = [ pan, tilt ] 18 | pub.publish(js) 19 | 20 | if __name__ == '__main__': 21 | rospy.init_node('ptu_cmd_angles') 22 | set_angles( 23 | float(sys.argv[1]), 24 | float(sys.argv[2])) 25 | -------------------------------------------------------------------------------- /flir_ptu_driver/src/driver.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * flir_ptu_driver ROS package 3 | * Copyright (C) 2014 Mike Purvis (mpurvis@clearpathrobotics.com) 4 | * 5 | * PTU ROS Package 6 | * Copyright (C) 2009 Erik Karulf (erik@cse.wustl.edu) 7 | * 8 | * Author: Toby Collett (University of Auckland) 9 | * Date: 2003-02-10 10 | * 11 | * Player - One Hell of a Robot Server 12 | * Copyright (C) 2000 Brian Gerkey & Kasper Stoy 13 | * gerkey@usc.edu kaspers@robotics.usc.edu 14 | * 15 | * This program is free software; you can redistribute it and/or modify 16 | * it under the terms of the GNU General Public License as published by 17 | * the Free Software Foundation; either version 2 of the License, or 18 | * (at your option) any later version. 19 | * 20 | * This program is distributed in the hope that it will be useful, 21 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 22 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 23 | * GNU General Public License for more details. 24 | * 25 | * You should have received a copy of the GNU General Public License 26 | * along with this program; if not, write to the Free Software 27 | * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 28 | * 29 | */ 30 | 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | 37 | #include 38 | #include 39 | 40 | using boost::lexical_cast; 41 | 42 | 43 | namespace flir_ptu_driver 44 | { 45 | 46 | /** Templated wrapper function on lexical_cast to assist with extracting 47 | * values from serial response strings. 48 | */ 49 | template 50 | T parseResponse(std::string responseBuffer) 51 | { 52 | std::string trimmed = responseBuffer.substr(1); 53 | boost::trim(trimmed); 54 | 55 | if (trimmed.empty()) 56 | { 57 | trimmed = "0"; 58 | } 59 | 60 | T parsed = lexical_cast(trimmed); 61 | ROS_DEBUG_STREAM("Parsed response value: " << parsed); 62 | return parsed; 63 | } 64 | 65 | bool PTU::initialized() 66 | { 67 | return !!ser_ && ser_->isOpen() && initialized_; 68 | } 69 | 70 | bool PTU::disableLimits() 71 | { 72 | ser_->write("ld "); // Disable Limits 73 | ser_->read(20); 74 | Lim = false; 75 | return true; 76 | } 77 | 78 | bool PTU::initialize() 79 | { 80 | ser_->write("ft "); // terse feedback 81 | ser_->write("ed "); // disable echo 82 | ser_->write("ci "); // position mode 83 | ser_->read(20); 84 | 85 | // get pan tilt encoder res 86 | tr = getRes(PTU_TILT); 87 | pr = getRes(PTU_PAN); 88 | 89 | PMin = getLimit(PTU_PAN, PTU_MIN); 90 | PMax = getLimit(PTU_PAN, PTU_MAX); 91 | TMin = getLimit(PTU_TILT, PTU_MIN); 92 | TMax = getLimit(PTU_TILT, PTU_MAX); 93 | PSMin = getLimit(PTU_PAN, PTU_MIN_SPEED); 94 | PSMax = getLimit(PTU_PAN, PTU_MAX_SPEED); 95 | TSMin = getLimit(PTU_TILT, PTU_MIN_SPEED); 96 | TSMax = getLimit(PTU_TILT, PTU_MAX_SPEED); 97 | Lim = true; 98 | 99 | if (tr <= 0 || pr <= 0 || PMin == -1 || PMax == -1 || TMin == -1 || TMax == -1) 100 | { 101 | initialized_ = false; 102 | } 103 | else 104 | { 105 | initialized_ = true; 106 | } 107 | 108 | return initialized(); 109 | } 110 | 111 | std::string PTU::sendCommand(std::string command) 112 | { 113 | ser_->write(command); 114 | ROS_DEBUG_STREAM("TX: " << command); 115 | std::string buffer = ser_->readline(PTU_BUFFER_LEN); 116 | ROS_DEBUG_STREAM("RX: " << buffer); 117 | return buffer; 118 | } 119 | 120 | bool PTU::home() 121 | { 122 | ROS_INFO("Sending command to reset PTU."); 123 | 124 | // Issue reset command 125 | ser_->flush(); 126 | ser_->write(" r "); 127 | 128 | std::string actual_response, expected_response("!T!T!P!P*"); 129 | 130 | // 30 seconds to receive full confirmation of reset action completed. 131 | for (int i = 0; i < 300; i++) 132 | { 133 | usleep(100000); 134 | 135 | if (ser_->available() >= expected_response.length()) 136 | { 137 | ROS_INFO("PTU reset command response received."); 138 | ser_->read(actual_response, expected_response.length()); 139 | return (actual_response == expected_response); 140 | } 141 | } 142 | 143 | ROS_WARN("PTU reset command response not received before timeout."); 144 | return false; 145 | } 146 | 147 | // get radians/count resolution 148 | float PTU::getRes(char type) 149 | { 150 | if (!ser_ || !ser_->isOpen()) return -1; 151 | 152 | std::string buffer = sendCommand(std::string() + type + "r "); 153 | 154 | if (buffer.length() < 3 || buffer[0] != '*') 155 | { 156 | ROS_ERROR_THROTTLE(30, "Error getting pan-tilt res"); 157 | return -1; 158 | } 159 | 160 | double z = parseResponse(buffer); 161 | z = z / 3600; // degrees/count 162 | return z * M_PI / 180; // radians/count 163 | } 164 | 165 | // get position limit 166 | int PTU::getLimit(char type, char limType) 167 | { 168 | if (!ser_ || !ser_->isOpen()) return -1; 169 | 170 | std::string buffer = sendCommand(std::string() + type + limType + " "); 171 | 172 | if (buffer.length() < 3 || buffer[0] != '*') 173 | { 174 | ROS_ERROR_THROTTLE(30, "Error getting pan-tilt limit"); 175 | return -1; 176 | } 177 | 178 | return parseResponse(buffer); 179 | } 180 | 181 | 182 | // get position in radians 183 | float PTU::getPosition(char type) 184 | { 185 | if (!initialized()) return -1; 186 | 187 | std::string buffer = sendCommand(std::string() + type + "p "); 188 | 189 | if (buffer.length() < 3 || buffer[0] != '*') 190 | { 191 | ROS_ERROR_THROTTLE(30, "Error getting pan-tilt pos"); 192 | return -1; 193 | } 194 | 195 | return parseResponse(buffer) * getResolution(type); 196 | } 197 | 198 | 199 | // set position in radians 200 | bool PTU::setPosition(char type, float pos, bool block) 201 | { 202 | if (!initialized()) return false; 203 | 204 | // get raw encoder count to move 205 | int count = static_cast(pos / getResolution(type)); 206 | 207 | // Check limits 208 | if (Lim) 209 | { 210 | if (count < (type == PTU_TILT ? TMin : PMin) || count > (type == PTU_TILT ? TMax : PMax)) 211 | { 212 | ROS_ERROR_THROTTLE(30, "Pan Tilt Value out of Range: %c %f(%d) (%d-%d)\n", 213 | type, pos, count, (type == PTU_TILT ? TMin : PMin), (type == PTU_TILT ? TMax : PMax)); 214 | return false; 215 | } 216 | } 217 | 218 | std::string buffer = sendCommand(std::string() + type + "p" + 219 | lexical_cast(count) + " "); 220 | 221 | if (buffer.empty() || buffer[0] != '*') 222 | { 223 | ROS_ERROR("Error setting pan-tilt pos"); 224 | return false; 225 | } 226 | 227 | if (block) 228 | { 229 | while (getPosition(type) != pos) 230 | { 231 | usleep(1000); 232 | } 233 | } 234 | 235 | return true; 236 | } 237 | 238 | // get speed in radians/sec 239 | float PTU::getSpeed(char type) 240 | { 241 | if (!initialized()) return -1; 242 | 243 | std::string buffer = sendCommand(std::string() + type + "s "); 244 | 245 | if (buffer.length() < 3 || buffer[0] != '*') 246 | { 247 | ROS_ERROR("Error getting pan-tilt speed"); 248 | return -1; 249 | } 250 | 251 | return parseResponse(buffer) * getResolution(type); 252 | } 253 | 254 | 255 | 256 | // set speed in radians/sec 257 | bool PTU::setSpeed(char type, float pos) 258 | { 259 | if (!initialized()) return false; 260 | 261 | // get raw encoder speed to move 262 | int count = static_cast(pos / getResolution(type)); 263 | 264 | // Check limits 265 | if (abs(count) < (type == PTU_TILT ? TSMin : PSMin) || abs(count) > (type == PTU_TILT ? TSMax : PSMax)) 266 | { 267 | ROS_ERROR("Pan Tilt Speed Value out of Range: %c %f(%d) (%d-%d)\n", 268 | type, pos, count, (type == PTU_TILT ? TSMin : PSMin), (type == PTU_TILT ? TSMax : PSMax)); 269 | return false; 270 | } 271 | 272 | std::string buffer = sendCommand(std::string() + type + "s" + 273 | lexical_cast(count) + " "); 274 | 275 | if (buffer.empty() || buffer[0] != '*') 276 | { 277 | ROS_ERROR("Error setting pan-tilt speed\n"); 278 | return false; 279 | } 280 | 281 | return true; 282 | } 283 | 284 | 285 | // set movement mode (position/velocity) 286 | bool PTU::setMode(char type) 287 | { 288 | if (!initialized()) return false; 289 | 290 | std::string buffer = sendCommand(std::string("c") + type + " "); 291 | 292 | if (buffer.empty() || buffer[0] != '*') 293 | { 294 | ROS_ERROR("Error setting pan-tilt move mode"); 295 | return false; 296 | } 297 | 298 | return true; 299 | } 300 | 301 | // get ptu mode 302 | char PTU::getMode() 303 | { 304 | if (!initialized()) return -1; 305 | 306 | // get pan tilt mode 307 | std::string buffer = sendCommand("c "); 308 | 309 | if (buffer.length() < 3 || buffer[0] != '*') 310 | { 311 | ROS_ERROR("Error getting pan-tilt pos"); 312 | return -1; 313 | } 314 | 315 | if (buffer[2] == 'p') 316 | return PTU_VELOCITY; 317 | else if (buffer[2] == 'i') 318 | return PTU_POSITION; 319 | else 320 | return -1; 321 | } 322 | 323 | } // namespace flir_ptu_driver 324 | -------------------------------------------------------------------------------- /flir_ptu_driver/src/node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * flir_ptu_driver ROS package 3 | * Copyright (C) 2014 Mike Purvis (mpurvis@clearpathrobotics.com) 4 | * 5 | * PTU ROS Package 6 | * Copyright (C) 2009 Erik Karulf (erik@cse.wustl.edu) 7 | * 8 | * Author: Toby Collett (University of Auckland) 9 | * Date: 2003-02-10 10 | * 11 | * Player - One Hell of a Robot Server 12 | * Copyright (C) 2000 Brian Gerkey & Kasper Stoy 13 | * gerkey@usc.edu kaspers@robotics.usc.edu 14 | * 15 | * This program is free software; you can redistribute it and/or modify 16 | * it under the terms of the GNU General Public License as published by 17 | * the Free Software Foundation; either version 2 of the License, or 18 | * (at your option) any later version. 19 | * 20 | * This program is distributed in the hope that it will be useful, 21 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 22 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 23 | * GNU General Public License for more details. 24 | * 25 | * You should have received a copy of the GNU General Public License 26 | * along with this program; if not, write to the Free Software 27 | * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 28 | * 29 | */ 30 | 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | #include 40 | 41 | namespace flir_ptu_driver 42 | { 43 | 44 | class Node 45 | { 46 | public: 47 | explicit Node(ros::NodeHandle& node_handle); 48 | ~Node(); 49 | 50 | // Service Control 51 | void connect(); 52 | bool ok() 53 | { 54 | return m_pantilt != NULL; 55 | } 56 | void disconnect(); 57 | 58 | // Service Execution 59 | void spinCallback(const ros::TimerEvent&); 60 | 61 | // Callback Methods 62 | void cmdCallback(const sensor_msgs::JointState::ConstPtr& msg); 63 | void resetCallback(const std_msgs::Bool::ConstPtr& msg); 64 | 65 | void produce_diagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat); 66 | 67 | protected: 68 | diagnostic_updater::Updater* m_updater; 69 | PTU* m_pantilt; 70 | ros::NodeHandle m_node; 71 | ros::Publisher m_joint_pub; 72 | ros::Subscriber m_joint_sub; 73 | ros::Subscriber m_reset_sub; 74 | 75 | serial::Serial m_ser; 76 | std::string m_joint_name_prefix; 77 | double default_velocity_; 78 | }; 79 | 80 | Node::Node(ros::NodeHandle& node_handle) 81 | : m_pantilt(NULL), m_node(node_handle) 82 | { 83 | m_updater = new diagnostic_updater::Updater(); 84 | m_updater->setHardwareID("none"); 85 | m_updater->add("PTU Status", this, &Node::produce_diagnostics); 86 | 87 | ros::param::param("~joint_name_prefix", m_joint_name_prefix, "ptu_"); 88 | } 89 | 90 | Node::~Node() 91 | { 92 | disconnect(); 93 | delete m_updater; 94 | } 95 | 96 | /** Opens the connection to the PTU and sets appropriate parameters. 97 | Also manages subscriptions/publishers */ 98 | void Node::connect() 99 | { 100 | // If we are reconnecting, first make sure to disconnect 101 | if (ok()) 102 | { 103 | disconnect(); 104 | } 105 | 106 | // Query for serial configuration 107 | std::string port; 108 | int32_t baud; 109 | bool limit; 110 | ros::param::param("~port", port, PTU_DEFAULT_PORT); 111 | ros::param::param("~limits_enabled", limit, true); 112 | ros::param::param("~baud", baud, PTU_DEFAULT_BAUD); 113 | ros::param::param("~default_velocity", default_velocity_, PTU_DEFAULT_VEL); 114 | 115 | // Connect to the PTU 116 | ROS_INFO_STREAM("Attempting to connect to FLIR PTU on " << port); 117 | 118 | try 119 | { 120 | m_ser.setPort(port); 121 | m_ser.setBaudrate(baud); 122 | serial::Timeout to = serial::Timeout(200, 200, 0, 200, 0); 123 | m_ser.setTimeout(to); 124 | m_ser.open(); 125 | } 126 | catch (serial::IOException& e) 127 | { 128 | ROS_ERROR_STREAM("Unable to open port " << port); 129 | return; 130 | } 131 | 132 | ROS_INFO_STREAM("FLIR PTU serial port opened, now initializing."); 133 | 134 | m_pantilt = new PTU(&m_ser); 135 | 136 | if (!m_pantilt->initialize()) 137 | { 138 | ROS_ERROR_STREAM("Could not initialize FLIR PTU on " << port); 139 | disconnect(); 140 | return; 141 | } 142 | 143 | if (!limit) 144 | { 145 | m_pantilt->disableLimits(); 146 | ROS_INFO("FLIR PTU limits disabled."); 147 | } 148 | 149 | ROS_INFO("FLIR PTU initialized."); 150 | 151 | m_node.setParam("min_tilt", m_pantilt->getMin(PTU_TILT)); 152 | m_node.setParam("max_tilt", m_pantilt->getMax(PTU_TILT)); 153 | m_node.setParam("min_tilt_speed", m_pantilt->getMinSpeed(PTU_TILT)); 154 | m_node.setParam("max_tilt_speed", m_pantilt->getMaxSpeed(PTU_TILT)); 155 | m_node.setParam("tilt_step", m_pantilt->getResolution(PTU_TILT)); 156 | 157 | m_node.setParam("min_pan", m_pantilt->getMin(PTU_PAN)); 158 | m_node.setParam("max_pan", m_pantilt->getMax(PTU_PAN)); 159 | m_node.setParam("min_pan_speed", m_pantilt->getMinSpeed(PTU_PAN)); 160 | m_node.setParam("max_pan_speed", m_pantilt->getMaxSpeed(PTU_PAN)); 161 | m_node.setParam("pan_step", m_pantilt->getResolution(PTU_PAN)); 162 | 163 | // Publishers : Only publish the most recent reading 164 | m_joint_pub = m_node.advertise 165 | ("state", 1); 166 | 167 | // Subscribers : Only subscribe to the most recent instructions 168 | m_joint_sub = m_node.subscribe 169 | ("cmd", 1, &Node::cmdCallback, this); 170 | 171 | m_reset_sub = m_node.subscribe 172 | ("reset", 1, &Node::resetCallback, this); 173 | } 174 | 175 | /** Disconnect */ 176 | void Node::disconnect() 177 | { 178 | if (m_pantilt != NULL) 179 | { 180 | delete m_pantilt; // Closes the connection 181 | m_pantilt = NULL; // Marks the service as disconnected 182 | } 183 | } 184 | 185 | /** Callback for resetting PTU */ 186 | void Node::resetCallback(const std_msgs::Bool::ConstPtr& msg) 187 | { 188 | ROS_INFO("Resetting the PTU"); 189 | m_pantilt->home(); 190 | } 191 | 192 | /** Callback for getting new Goal JointState */ 193 | void Node::cmdCallback(const sensor_msgs::JointState::ConstPtr& msg) 194 | { 195 | ROS_DEBUG("PTU command callback."); 196 | if (!ok()) return; 197 | 198 | if (msg->position.size() != 2) 199 | { 200 | ROS_ERROR("JointState command to PTU has wrong number of position elements."); 201 | return; 202 | } 203 | 204 | double pan = msg->position[0]; 205 | double tilt = msg->position[1]; 206 | double panspeed, tiltspeed; 207 | 208 | if (msg->velocity.size() == 2) 209 | { 210 | panspeed = msg->velocity[0]; 211 | tiltspeed = msg->velocity[1]; 212 | } 213 | else 214 | { 215 | ROS_WARN_ONCE("JointState command to PTU has wrong number of velocity elements; using default velocity."); 216 | panspeed = default_velocity_; 217 | tiltspeed = default_velocity_; 218 | } 219 | 220 | m_pantilt->setPosition(PTU_PAN, pan); 221 | m_pantilt->setPosition(PTU_TILT, tilt); 222 | m_pantilt->setSpeed(PTU_PAN, panspeed); 223 | m_pantilt->setSpeed(PTU_TILT, tiltspeed); 224 | } 225 | 226 | void Node::produce_diagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat) 227 | { 228 | stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "All normal."); 229 | stat.add("PTU Mode", m_pantilt->getMode() == PTU_POSITION ? "Position" : "Velocity"); 230 | } 231 | 232 | 233 | /** 234 | * Publishes a joint_state message with position and speed. 235 | * Also sends out updated TF info. 236 | */ 237 | void Node::spinCallback(const ros::TimerEvent&) 238 | { 239 | if (!ok()) return; 240 | 241 | // Read Position & Speed 242 | double pan = m_pantilt->getPosition(PTU_PAN); 243 | double tilt = m_pantilt->getPosition(PTU_TILT); 244 | 245 | double panspeed = m_pantilt->getSpeed(PTU_PAN); 246 | double tiltspeed = m_pantilt->getSpeed(PTU_TILT); 247 | 248 | // Publish Position & Speed 249 | sensor_msgs::JointState joint_state; 250 | joint_state.header.stamp = ros::Time::now(); 251 | joint_state.name.resize(2); 252 | joint_state.position.resize(2); 253 | joint_state.velocity.resize(2); 254 | joint_state.name[0] = m_joint_name_prefix + "pan"; 255 | joint_state.position[0] = pan; 256 | joint_state.velocity[0] = panspeed; 257 | joint_state.name[1] = m_joint_name_prefix + "tilt"; 258 | joint_state.position[1] = tilt; 259 | joint_state.velocity[1] = tiltspeed; 260 | m_joint_pub.publish(joint_state); 261 | 262 | m_updater->update(); 263 | } 264 | 265 | } // namespace flir_ptu_driver 266 | 267 | int main(int argc, char** argv) 268 | { 269 | ros::init(argc, argv, "ptu"); 270 | ros::NodeHandle n; 271 | 272 | while (ros::ok()) 273 | { 274 | // Connect to PTU 275 | flir_ptu_driver::Node ptu_node(n); 276 | ptu_node.connect(); 277 | 278 | // Set up polling callback 279 | int hz; 280 | ros::param::param("~hz", hz, PTU_DEFAULT_HZ); 281 | ros::Timer spin_timer = n.createTimer(ros::Duration(1 / hz), 282 | &flir_ptu_driver::Node::spinCallback, &ptu_node); 283 | 284 | // Spin until there's a problem or we're in shutdown 285 | ros::spin(); 286 | 287 | if (!ptu_node.ok()) 288 | { 289 | ROS_ERROR("FLIR PTU disconnected, attempting reconnection."); 290 | ros::Duration(1.0).sleep(); 291 | } 292 | } 293 | 294 | return 0; 295 | } 296 | -------------------------------------------------------------------------------- /flir_ptu_viz/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package flir_ptu_viz 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.2.1 (2021-03-26) 6 | ------------------ 7 | 8 | 0.2.0 (2018-06-21) 9 | ------------------ 10 | 11 | 0.1.4 (2014-07-17) 12 | ------------------ 13 | * Update and install the rviz config as well. 14 | * Install launch files for viz. 15 | * Contributors: Mike Purvis 16 | 17 | 0.1.3 (2014-04-13) 18 | ------------------ 19 | * catkin_lint fixes 20 | * Switch roslaunch to a build dependency 21 | * Add launchfile tests 22 | * Contributors: Mike Purvis 23 | 24 | 0.1.2 (2014-04-12) 25 | ------------------ 26 | 27 | 0.1.1 (2014-04-11) 28 | ------------------ 29 | 30 | 0.1.0 (2014-04-10) 31 | ------------------ 32 | * Add stub package for viz launchers. 33 | * Contributors: Mike Purvis 34 | -------------------------------------------------------------------------------- /flir_ptu_viz/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(flir_ptu_viz) 3 | 4 | find_package(catkin REQUIRED COMPONENTS roslaunch) 5 | catkin_package() 6 | 7 | roslaunch_add_file_check(launch/view_model.launch 8 | DEPENDENCIES flir_ptu_example_urdf robot_state_publisher) 9 | roslaunch_add_file_check(launch/view_ptu.launch 10 | DEPENDENCIES rviz) 11 | 12 | install(DIRECTORY launch rviz 13 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 14 | -------------------------------------------------------------------------------- /flir_ptu_viz/launch/view_model.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /flir_ptu_viz/launch/view_ptu.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /flir_ptu_viz/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | flir_ptu_viz 4 | 0.2.1 5 | Launchfiles and rviz configs to assist with visualizing the FLIR PTUs. 6 | 7 | Mike Purvis 8 | http://wiki.ros.org/flir_ptu_driver 9 | 10 | BSD 11 | 12 | catkin 13 | roslaunch 14 | flir_ptu_description 15 | joint_state_publisher 16 | robot_state_publisher 17 | rviz 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /flir_ptu_viz/rviz/urdf.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /TF1 10 | - /TF1/Frames1 11 | Splitter Ratio: 0.5 12 | Tree Height: 601 13 | - Class: rviz/Selection 14 | Name: Selection 15 | - Class: rviz/Tool Properties 16 | Expanded: 17 | - /2D Pose Estimate1 18 | - /2D Nav Goal1 19 | - /Publish Point1 20 | Name: Tool Properties 21 | Splitter Ratio: 0.588679 22 | - Class: rviz/Views 23 | Expanded: 24 | - /Current View1 25 | Name: Views 26 | Splitter Ratio: 0.5 27 | - Class: rviz/Time 28 | Experimental: false 29 | Name: Time 30 | SyncMode: 0 31 | SyncSource: "" 32 | Visualization Manager: 33 | Class: "" 34 | Displays: 35 | - Alpha: 0.5 36 | Cell Size: 1 37 | Class: rviz/Grid 38 | Color: 160; 160; 164 39 | Enabled: true 40 | Line Style: 41 | Line Width: 0.03 42 | Value: Lines 43 | Name: Grid 44 | Normal Cell Count: 0 45 | Offset: 46 | X: 0 47 | Y: 0 48 | Z: 0 49 | Plane: XY 50 | Plane Cell Count: 10 51 | Reference Frame: 52 | Value: true 53 | - Alpha: 1 54 | Class: rviz/RobotModel 55 | Collision Enabled: false 56 | Enabled: true 57 | Links: 58 | All Links Enabled: true 59 | Expand Joint Details: false 60 | Expand Link Details: false 61 | Expand Tree: false 62 | Link Tree Style: Links in Alphabetic Order 63 | base_link: 64 | Alpha: 1 65 | Show Axes: false 66 | Show Trail: false 67 | ptu_base_link: 68 | Alpha: 1 69 | Show Axes: false 70 | Show Trail: false 71 | ptu_mount_link: 72 | Alpha: 1 73 | Show Axes: false 74 | Show Trail: false 75 | ptu_pan_link: 76 | Alpha: 1 77 | Show Axes: false 78 | Show Trail: false 79 | Value: true 80 | ptu_tilt_link: 81 | Alpha: 1 82 | Show Axes: false 83 | Show Trail: false 84 | Value: true 85 | ptu_tilted_link: 86 | Alpha: 1 87 | Show Axes: false 88 | Show Trail: false 89 | Name: RobotModel 90 | Robot Description: robot_description 91 | TF Prefix: "" 92 | Update Interval: 0 93 | Value: true 94 | Visual Enabled: true 95 | - Class: rviz/TF 96 | Enabled: true 97 | Frame Timeout: 15 98 | Frames: 99 | All Enabled: false 100 | base_link: 101 | Value: false 102 | ptu_base_link: 103 | Value: true 104 | ptu_mount_link: 105 | Value: true 106 | ptu_pan_link: 107 | Value: false 108 | ptu_tilt_link: 109 | Value: false 110 | ptu_tilted_link: 111 | Value: false 112 | Marker Scale: 0.3 113 | Name: TF 114 | Show Arrows: true 115 | Show Axes: true 116 | Show Names: true 117 | Tree: 118 | base_link: 119 | ptu_base_link: 120 | ptu_pan_link: 121 | ptu_tilt_link: 122 | ptu_tilted_link: 123 | ptu_mount_link: 124 | {} 125 | Update Interval: 0 126 | Value: true 127 | Enabled: true 128 | Global Options: 129 | Background Color: 48; 48; 48 130 | Fixed Frame: base_link 131 | Frame Rate: 30 132 | Name: root 133 | Tools: 134 | - Class: rviz/Interact 135 | Hide Inactive Objects: true 136 | - Class: rviz/MoveCamera 137 | - Class: rviz/Select 138 | - Class: rviz/FocusCamera 139 | - Class: rviz/Measure 140 | - Class: rviz/SetInitialPose 141 | Topic: /initialpose 142 | - Class: rviz/SetGoal 143 | Topic: /move_base_simple/goal 144 | - Class: rviz/PublishPoint 145 | Single click: true 146 | Topic: /clicked_point 147 | Value: true 148 | Views: 149 | Current: 150 | Class: rviz/Orbit 151 | Distance: 0.6 152 | Focal Point: 153 | X: 0 154 | Y: 0 155 | Z: 0 156 | Name: Current View 157 | Near Clip Distance: 0.01 158 | Pitch: 0.570398 159 | Target Frame: 160 | Value: Orbit (rviz) 161 | Yaw: 2.6104 162 | Saved: ~ 163 | Window Geometry: 164 | Displays: 165 | collapsed: false 166 | Height: 882 167 | Hide Left Dock: false 168 | Hide Right Dock: false 169 | QMainWindow State: 000000ff00000000fd00000004000000000000013c000002e8fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002e8000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002e8fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000028000002e8000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004c00000003efc0100000002fb0000000800540069006d00650100000000000004c0000002f600fffffffb0000000800540069006d0065010000000000000450000000000000000000000269000002e800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 170 | Selection: 171 | collapsed: false 172 | Time: 173 | collapsed: false 174 | Tool Properties: 175 | collapsed: false 176 | Views: 177 | collapsed: false 178 | Width: 1216 179 | X: 63 180 | Y: 24 181 | --------------------------------------------------------------------------------