├── .gitignore ├── CMakeLists.txt ├── License.txt ├── README.md ├── cmake └── FindHalcon.cmake ├── include └── asr_halcon_bridge │ ├── halcon_image.h │ └── halcon_pointcloud.h ├── licenses └── third_party_licenses.txt ├── package.xml └── src └── asr_halcon_bridge ├── halcon_image.cpp └── halcon_pointcloud.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | # General 2 | .directory 3 | .deps 4 | .libs 5 | *.pyc 6 | *.pcd 7 | *.cfgc 8 | *.autosave 9 | *~ 10 | *.svn 11 | *.swp 12 | *__init__.py 13 | *.o 14 | *.la 15 | *.lo 16 | *.kdev[0-9] 17 | msg_gen/ 18 | srv_gen/ 19 | cfg/cpp/ 20 | 21 | # Build directories 22 | build/ 23 | _build* 24 | 25 | # Visual Studio 26 | *.opensdf 27 | *.sdf 28 | *.suo 29 | 30 | # KDevelop 31 | *.kate-swp 32 | *.kdev4 33 | 34 | # Qt Creator 35 | *.creator* 36 | *.files 37 | *.includes 38 | *.user 39 | qtcreator-* 40 | 41 | # Eclipse 42 | .project 43 | .cproject 44 | 45 | # Emacs 46 | .#* 47 | 48 | # Ignore generated docs 49 | docs/ 50 | *.wikidoc -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(asr_halcon_bridge) 3 | 4 | set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${CMAKE_CURRENT_SOURCE_DIR}/cmake) 5 | 6 | find_package(Halcon) 7 | if(NOT ${Halcon_FOUND}) 8 | message(WARNING "Skip processing ${PROJECT_NAME}, because HALCON library is missing!!! (see http://wiki.ros.org/asr_halcon_bridge)") 9 | return() 10 | endif() 11 | 12 | find_package(catkin REQUIRED COMPONENTS 13 | roscpp 14 | sensor_msgs 15 | ) 16 | 17 | catkin_package( 18 | CATKIN_DEPENDS roscpp sensor_msgs 19 | LIBRARIES ${PROJECT_NAME} 20 | INCLUDE_DIRS include 21 | DEPENDS Halcon 22 | ) 23 | 24 | include_directories( 25 | include 26 | ${Halcon_INCLUDE_DIRS} 27 | ${catkin_INCLUDE_DIRS} 28 | ) 29 | 30 | add_library(${PROJECT_NAME} 31 | src/${PROJECT_NAME}/halcon_image.cpp 32 | src/${PROJECT_NAME}/halcon_pointcloud.cpp 33 | ) 34 | 35 | target_link_libraries(${PROJECT_NAME} 36 | ${CATKIN_LIBRARIES} 37 | ${Halcon_LIBRARIES} 38 | ) 39 | 40 | install(TARGETS ${PROJECT_NAME} 41 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 42 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 43 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 44 | ) 45 | 46 | install(DIRECTORY include/${PROJECT_NAME}/ 47 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 48 | ) 49 | -------------------------------------------------------------------------------- /License.txt: -------------------------------------------------------------------------------- 1 | Copyright (c) 2016, Allgeyer Tobias 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 5 | 6 | 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 7 | 8 | 2. 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. 9 | 10 | 3. 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. 11 | 12 | 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. 13 | 14 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # asr_halcon_bridge 2 | Documentation: http://wiki.ros.org/asr_halcon_bridge 3 | -------------------------------------------------------------------------------- /cmake/FindHalcon.cmake: -------------------------------------------------------------------------------- 1 | include(FindPackageHandleStandardArgs) 2 | 3 | find_path (Halcon_INCLUDE_DIR halconcpp/HalconCpp.h 4 | PATHS 5 | $ENV{HALCON_ROOT}/include 6 | /usr/local/include 7 | /usr/include 8 | /sw/include 9 | /opt/halcon/include 10 | ) 11 | 12 | find_library (Halcon_ENGINE_LIBRARY hdevenginecpp 13 | PATHS 14 | $ENV{HALCON_ROOT}/lib/x64-linux 15 | /usr/local/lib/x64-linux 16 | /usr/lib/x64-linux 17 | /lib/x64-linux 18 | /sw/lib/x64-linux 19 | /opt/halcon/lib/x64-linux 20 | ) 21 | 22 | find_library (Halcon_CPP_LIBRARY halconcpp 23 | PATHS 24 | $ENV{HALCON_ROOT}/lib/x64-linux 25 | /usr/local/lib/x64-linux 26 | /usr/lib/x64-linux 27 | /lib/x64-linux 28 | /sw/lib/x64-linux 29 | /opt/halcon/lib/x64-linux 30 | ) 31 | 32 | find_library (Halcon_LIBRARY halcon 33 | PATHS 34 | $ENV{HALCON_ROOT}/lib/x64-linux 35 | /usr/local/lib/x64-linux 36 | /usr/lib/x64-linux 37 | /lib/x64-linux 38 | /sw/lib/x64-linux 39 | /opt/halcon/lib/x64-linux 40 | ) 41 | 42 | 43 | find_package_handle_standard_args(Halcon 44 | DEFAULT_MSG 45 | Halcon_INCLUDE_DIR 46 | Halcon_LIBRARY 47 | ) 48 | 49 | if(Halcon_FOUND) 50 | set(Halcon_LIBRARIES ${Halcon_LIBRARY} ${Halcon_ENGINE_LIBRARY} ${Halcon_CPP_LIBRARY}) 51 | set(Halcon_INCLUDE_DIRS ${Halcon_INCLUDE_DIR}) 52 | endif() 53 | 54 | mark_as_advanced(Halcon_INCLUDE_DIRS 55 | Halcon_LIBRARIES 56 | ) 57 | -------------------------------------------------------------------------------- /include/asr_halcon_bridge/halcon_image.h: -------------------------------------------------------------------------------- 1 | /** 2 | 3 | Copyright (c) 2016, Allgeyer Tobias 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 9 | 10 | 2. 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. 11 | 12 | 3. 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. 13 | 14 | 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. 15 | 16 | */ 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | namespace halcon_bridge { 23 | 24 | class Exception: public std::runtime_error { 25 | public: 26 | Exception(const std::string& description) : 27 | std::runtime_error(description) { 28 | } 29 | }; 30 | 31 | 32 | class HalconImage; 33 | 34 | typedef boost::shared_ptr HalconImagePtr; 35 | 36 | /** 37 | * \brief Image message class that is interoperable with sensor_msgs/Image but uses a HImage representation for the image data. 38 | * 39 | * @author Allgeyer Tobias 40 | */ 41 | class HalconImage { 42 | public: 43 | std_msgs::Header header; 44 | std::string encoding; 45 | HalconCpp::HImage *image; 46 | 47 | ~HalconImage(); 48 | 49 | /** 50 | * \brief Convert this message to a ROS sensor_msgs::Image message. 51 | * 52 | * The returned sensor_msgs::Image message contains a copy of the image data. 53 | */ 54 | sensor_msgs::ImagePtr toImageMsg() const; 55 | 56 | /** 57 | * \brief Copy the message data to a ROS sensor_msgs::Image message. 58 | * 59 | * This overload is intended mainly for aggregate messages such as stereo_msgs::DisparityImage, 60 | * which contains a sensor_msgs::Image as a data member. 61 | */ 62 | void toImageMsg(sensor_msgs::Image& ros_image) const; 63 | }; 64 | 65 | 66 | /** 67 | * \brief Convert a sensor_msgs::Image message to a Halcon-compatible HImage, copying the 68 | * image data. 69 | * 70 | * \param source A shared_ptr to a sensor_msgs::Image message 71 | */ 72 | HalconImagePtr toHalconCopy(const sensor_msgs::ImageConstPtr& source); 73 | 74 | /** 75 | * \brief Convert a sensor_msgs::Image message to a Halcon-compatible HImage, copying the 76 | * image data. 77 | * 78 | * \param source A sensor_msgs::Image message 79 | */ 80 | HalconImagePtr toHalconCopy(const sensor_msgs::Image& source); 81 | 82 | 83 | 84 | } 85 | -------------------------------------------------------------------------------- /include/asr_halcon_bridge/halcon_pointcloud.h: -------------------------------------------------------------------------------- 1 | /** 2 | 3 | Copyright (c) 2016, Allgeyer Tobias 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 9 | 10 | 2. 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. 11 | 12 | 3. 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. 13 | 14 | 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. 15 | 16 | */ 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | namespace halcon_bridge { 23 | 24 | 25 | 26 | 27 | class HalconPointcloud; 28 | 29 | typedef boost::shared_ptr HalconPointcloudPtr; 30 | 31 | /** 32 | * \brief PointCloud message class that is interoperable with sensor_msgs/PointCloud2 but uses a HObjectModel3D representation for the point cloud data. 33 | * 34 | * @author Allgeyer Tobias 35 | */ 36 | class HalconPointcloud { 37 | public: 38 | std_msgs::Header header; 39 | HalconCpp::HTuple curvature; 40 | HalconCpp::HObjectModel3D *model; 41 | 42 | ~HalconPointcloud(); 43 | 44 | /** 45 | * \brief Convert this message to a ROS sensor_msgs::PointCloud2 message. 46 | * 47 | * The returned sensor_msgs::PointCloud2 message contains a copy of the Halcon-ObjectModel data. 48 | */ 49 | sensor_msgs::PointCloud2Ptr toPointcloudMsg() const; 50 | 51 | 52 | /** 53 | * \brief Copy the message data to a ROS sensor_msgs::PointCloud2 message. 54 | * 55 | */ 56 | void toPointcloudMsg(sensor_msgs::PointCloud2& ros_pointcloud) const; 57 | }; 58 | 59 | 60 | /** 61 | * \brief Convert a sensor_msgs::PointCloud2 message to a Halcon-compatible HObjectModel3D, copying the 62 | * point cloud data. 63 | * 64 | * \param source A shared_ptr to a sensor_msgs::PointCloud2 message 65 | * 66 | */ 67 | HalconPointcloudPtr toHalconCopy(const sensor_msgs::PointCloud2ConstPtr& source); 68 | 69 | 70 | /** 71 | * \brief Convert a sensor_msgs::PointCloud2 message to a Halcon-compatible HObjectModel3D, copying the 72 | * point cloud data. 73 | * 74 | * \param source A sensor_msgs::PointCloud2 message 75 | * 76 | */ 77 | HalconPointcloudPtr toHalconCopy(const sensor_msgs::PointCloud2& source); 78 | 79 | 80 | 81 | } 82 | 83 | -------------------------------------------------------------------------------- /licenses/third_party_licenses.txt: -------------------------------------------------------------------------------- 1 | Halcon 2 | ============== 3 | 4 | The use of halcon_bridge requires a licence to the HALCON library. Please refer to the HALCON End User Legal Agreement (EULA) for details: 5 | 6 | 7 | MVTec Software GmbH - Software License Agreement for HALCON 8 | 9 | IMPORTANT - READ CAREFULLY: This Software License Agreement 10 | (Agreement) is a legal Agreement between you (either an individual or 11 | a single entity) and MVTec Software GmbH (MVTec) for the MVTec 12 | software product identified above, which includes computer software 13 | and may include associated media, printed materials and online or 14 | electronic documentation. YOU AGREE TO BE BOUND BY THE TERMS OF THIS 15 | AGREEMENT BY INSTALLING, COPYING, OR OTHERWISE USING THE PRODUCT. IF 16 | YOU DO NOT AGREE, DO NOT INSTALL OR USE THE PRODUCT. YOU MAY RECEIVE 17 | A REFUND OF THE AMOUNT YOU ORIGINALLY PAID IF YOU (1) DO NOT USE THE 18 | SOFTWARE AND (2) RETURN IT, WITH PROOF OF PAYMENT, TO THE LOCATION 19 | FROM WHICH IT WAS OBTAINED WITHIN THIRTY (30) DAYS OF THE PURCHASE 20 | DATE. 21 | 22 | 23 | MVTec grants you the rights described in this Agreement provided that 24 | you comply with all terms and conditions of this Agreement. 25 | 26 | 27 | 1. DEFINITIONS 28 | 29 | Software means all of the contents of the files, disk(s), CD-ROM(s), 30 | DVD(s) or other media with which this Agreement is provided, including 31 | but not limited to, MVTec or third party computer software, digital 32 | images, example programs and Documentation; this also includes 33 | upgrades, modified versions, updates, additions and copies of the 34 | Software, if any, licensed to you by MVTec. 35 | 36 | Documentation means the User's Guides, User's Manuals and Reference 37 | Manuals, if any, accompanying delivery of the Software. Documentation 38 | may be delivered in printed and/or electronic and/or online forms. 39 | 40 | Derivative means a computer software (Application) created by you that 41 | includes or is based in whole or in part on the Software, including, 42 | but not limited to, incorporating the Software into the Application by 43 | linking or otherwise using the Software in any form whatsoever in your 44 | Application. 45 | 46 | Software Configuration means the Demo Version, the Evaluation Version, 47 | the Development Version, or the Runtime Version of the Software. 48 | 49 | License Type means a Floating License or a Nodelocked License. 50 | 51 | License File means a computer file that contains license keys that 52 | permit you to use the Software in a particular Software Configuration. 53 | 54 | Floating License means a license, supplied to you in the form of a 55 | License File, that allows you to install the Software on any number of 56 | computers in your local network. A Floating License limits the number 57 | of simultaneous uses of the Software to the number of licenses 58 | specified in the Floating License File. 59 | 60 | Nodelocked License means a license, supplied to you in the form of a 61 | License File, that allows you to install and use the Software only on 62 | the computer specified in the Nodelocked License File. 63 | 64 | Evaluation License means a license, supplied to you in the form of a 65 | license file, that allows you to install and evaluate the Software on 66 | any computer for a certain period of time. 67 | 68 | 69 | 2. LICENSE GRANT 70 | 71 | MVTec grants to you a nonexclusive license to install and use the 72 | Software as provided herein. The licensed Software and Documentation 73 | shall at all time remain the property of MVTec and/or its licensors, 74 | and you, as licensee, shall have no right, title, or interest in the 75 | Software, except as expressly set forth in this Agreement. 76 | 77 | 2.1. SOFTWARE CONFIGURATIONS AND LICENSE OPTIONS 78 | 79 | Different rights, obligations and restrictions apply with each 80 | Software Configuration and License Type. Your right to install and 81 | use the Software is determined by the Software Configuration selected 82 | and the associated License Type as set forth below: 83 | 84 | 2.1.1. DEMO VERSION 85 | 86 | The Demo Version is a version of the Software that contains certain 87 | limitations in functionality, as described in the Documentation. It 88 | does not require a License File. You may use the Demo Version for 89 | evaluation purposes only. The Software may be installed on any number 90 | of computers. The right to use the Demo Version contains no time 91 | limitation. Except as aforesaid, you may not use the Software for any 92 | other purposes, including, but not limited to, commercial purposes. 93 | You acknowledge that the Demo Version contains limited functionality. 94 | 95 | 2.1.2. EVALUATION VERSION 96 | 97 | The Evaluation Version is a version of the Software that allows you to 98 | evaluate the full functionality of the Software for a certain period 99 | of time, typically one month. It requires an Evaluation License File. 100 | The Software may be installed on any number of computers. Your right 101 | to use the Software and to create Derivatives is restricted to the 102 | purpose of evaluating the Software. Except as aforesaid, you may not 103 | use the Software for any other purposes, including, but not limited 104 | to, commercial purposes. In particular, you may not use the 105 | Evaluation Version to develop commercial applications. The Evaluation 106 | Version may be used only in the time period that is specified in the 107 | Evaluation License File. After expiration of this time period, the 108 | Software may no longer be used. The transfer of the Software, 109 | including, but not limited to, sublicensing and distribution to a 110 | third party is not permitted. 111 | 112 | 2.1.3. DEVELOPMENT VERSION 113 | 114 | The Development Version is a version of the Software that allows you 115 | to use the full functionality of the Software for an unlimited period 116 | of time. The Development Version requires a Development License File. 117 | Depending on the License Type you have selected, the Software may only 118 | be installed on the computer that is specified in the License File 119 | (Nodelocked License) or on any computer in the local network (Floating 120 | License); see Section 2.2. You may use the Software to create 121 | Derivatives, including commercial applications. In particular, you 122 | may use the Development Version to compile and link applications. You 123 | may not sublicense and/or distribute the Development Version to a 124 | third party. 125 | 126 | 2.1.4. RUNTIME VERSION 127 | 128 | The Runtime Version is a version of the Software that only allows you 129 | to use the runtime functionality of the Software. In particular, 130 | certain development tools of the Software cannot be used, as described 131 | in the Documentation. The Runtime Version requires a Runtime License 132 | File. The Software may only be installed on the computer that is 133 | specified in the License File; the Runtime Version is only available 134 | with Nodelocked Licenses. You may distribute and sublicense 135 | Derivatives. You must possess a valid license for the Development 136 | Version before you are authorized to distribute and/or sublicense your 137 | Derivatives. The Runtime Version may not be used for the creation of 138 | Derivatives, including, but not limited to, compiling and linking of 139 | applications. If you are authorized and choose to distribute such 140 | Derivatives, you agree (1) not to use MVTec's name, logo or trademarks 141 | to market the Derivatives; (2) to display your own valid copyright 142 | notice which shall be sufficient to protect MVTec's copyright in the 143 | Software; (3) not to remove or obscure any copyright, trademark or 144 | patent notice that appear on the Software as delivered to you; (4) to 145 | accompany the Derivative with a license whose terms and conditions are 146 | at least as restrictive as the terms in this Agreement; (5) to 147 | explicitly exclude MVTec and its suppliers from all liability for 148 | damages or any obligation to provide remedial actions that may result 149 | from the use and distribution of Derivatives; (6) to indemnify, hold 150 | harmless and defend MVTec from and against any claims or lawsuits, 151 | including attorney's fees, that arise or result from the use or 152 | distribution of the Derivatives; (7) otherwise comply with the terms 153 | of this Agreement; and (8) agree that MVTec reserves all rights not 154 | expressly granted. MVTec shall have no support or warranty 155 | obligations, and disclaims all liability, for Derivatives developed 156 | and/or distributed by you. In particular, you will be solely 157 | responsible to your end users (or anyone else who uses your 158 | Derivatives) for support, service, upgrades, or technical or other 159 | assistance, and such persons will have no right to contact MVTec for 160 | any services or assistance. 161 | 162 | 2.2. LICENSE TYPES 163 | 164 | 2.2.1. NODELOCKED LICENSES 165 | 166 | A Nodelocked License allows you to install and use the Software only 167 | on the computer specified in the Nodelocked License File. The 168 | computer is specified by a Host ID that is derived from a certain 169 | hardware component, which may be bound to an irremovable computer 170 | component (Irremovable Host ID), for example, a network card, or may 171 | be bound to a removable hardware component (Removable Host ID), for 172 | example, a dongle. If you use an Irremovable Host ID for licensing 173 | the Software, you may only install and use the Software on the 174 | computer that includes the Irremovable Host ID. If you use a 175 | Removable Host ID you may install the Software on any number of 176 | computers, but you may only use the Software on the computer to which 177 | the Removable Host ID is attached. 178 | 179 | 2.2.2. FLOATING LICENSES 180 | 181 | A Floating License allows you to install the Software on any number of 182 | computers in your local network. A Floating License limits the number 183 | of simultaneous uses of the Software to the number of licenses 184 | specified in the Floating License File. The Floating License requires 185 | that a special program, the License Server, is installed on one 186 | computer in the network. Like for Nodelocked Licenses, the License 187 | Server computer is identified either by an Irremovable Host ID or by a 188 | Removable Host ID. If you use an Irremovable Host ID the License 189 | Server may only be executed on the computer identified by the 190 | Irremovable Host ID. If you use a Removable Host ID the License 191 | Server may only be executed on the computer to which the Removable 192 | Host ID is attached. You are obliged to stop the execution of the 193 | License Server before moving the Removable Host ID to a different 194 | computer. 195 | 196 | 2.3. COMPLIANCE WITH LICENSES 197 | 198 | You agree that upon request from MVTec or MVTec's authorized 199 | representative, you will within thirty (30) days fully document and 200 | certify that use of any and all MVTec Software at the time of the 201 | request is in conformity with your valid licenses from MVTec. 202 | 203 | 2.4. DOCUMENTATION 204 | 205 | You may make and use a reasonable number of copies of any 206 | Documentation, provided that such copies shall be used only for 207 | internal purposes and are not to be republished or distributed (either 208 | in hard copy or electronic form) beyond your premises. 209 | 210 | 2.5. EXAMPLE PROGRAMS AND IMAGES 211 | 212 | MVTec grants you the right to use and modify the example programs and 213 | example images included in the Software for the sole purposes of 214 | designing, developing, and testing your software product(s). Example 215 | programs are stored in the directory "examples" in the directory or 216 | directories in which you have installed the Software, while example 217 | images are stored in the directory "examples/images". 218 | 219 | 220 | 3. DESCRIPTION OF OTHER RIGHTS AND LIMITATIONS 221 | 222 | 3.1. RENTAL 223 | 224 | You may not rent, lease or lend the Software. 225 | 226 | 3.2. SOFTWARE TRANSFER 227 | 228 | Except as may be expressly prohibited herein, you may transfer all 229 | your rights to use the Software to a third party provided that (1) you 230 | also transfer this Agreement, the Software and all other software 231 | bundled with the Software, including all copies, updates and prior 232 | versions, to such third party; (2) you retain no copies, including 233 | backups and copies stored on a computer; and (3) the receiving party 234 | accepts the terms and conditions upon which you legally purchased a 235 | license to the Software. 236 | 237 | 3.3. BACKUP COPY 238 | 239 | You may make a reasonable number of copies of the Software to backup 240 | devices such as hard disks, optical media, or tape and a reasonable 241 | number of physical DVD media backup copies of the Software solely to 242 | replace the original copy provided to you if the original copy is 243 | damaged or destroyed. All rights not specifically granted to you 244 | herein are retained by MVTec. 245 | 246 | 3.4. LIMITATION ON REVERSE ENGINEERING, DECOMPILATION, DISASSEMBLY AND 247 | MODIFICATION 248 | 249 | You may not reverse engineer, decompile, disassemble or otherwise 250 | attempt to discover the source code of the Software, except to the 251 | extent you may be expressly permitted under applicable law, it is 252 | essential to do so in order to achieve interoperability of the 253 | Software with another software program, and you have first requested 254 | MVTec to provide the information necessary to achieve such 255 | interoperability and MVTec has not made such information available. 256 | MVTec has the right to impose reasonable conditions and to request a 257 | reasonable fee before providing such information. Any information 258 | supplied by MVTec or obtained by you, as permitted hereunder, may only 259 | be used by you for the purpose described herein and may not be 260 | disclosed to any third party. 261 | 262 | You may not modify, adapt, or otherwise alter the Software except as 263 | expressly permitted herein. In particular, you may not attempt to 264 | remove the license protection mechanism from the Software. 265 | 266 | 3.5. TRADEMARKS 267 | 268 | This Agreement does not grant you any rights in connection with any 269 | trademarks or service marks of MVTec. You may not use the name, 270 | trademarks, or service marks of MVTec in any advertising, promotional 271 | literature or any other material, whether in written, electronic, or 272 | other form, distributed to any third party, except as expressly 273 | permitted by MVTec. 274 | 275 | 3.6. UPGRADES 276 | 277 | If the Software is an upgrade to a previous version of the Software, 278 | you must possess a valid license to such previous version in order to 279 | use the upgrade. You may continue to use the previous version of the 280 | Software on your computer after you receive the upgrade to assist you 281 | in the transition to the upgrade, provided that (1) the previous 282 | version or copies thereof are not transferred to another party or 283 | computer unless all copies of the upgrade are also transferred to such 284 | party or computer; and (2) you acknowledge that any obligation MVTec 285 | may have to support the previous version of the Software may be ended 286 | within one (1) year upon availability of the upgrade. 287 | 288 | 3.7. LICENSE FOR THIRD PARTY SOFTWARE 289 | 290 | MVTec has been granted licenses to distribute certain third party 291 | software. As a condition of those licenses, MVTec is required to 292 | distribute the software subject to specific terms and conditions, 293 | which may be different from or additional to those contained herein 294 | for MVTec's Software. You understand and agree that acceptance of 295 | this Agreement also confirms your acceptance of the applicable 296 | provisions for use, including the restrictions on use, of such third 297 | party software. You may contact MVTec to obtain the current 298 | applicable provisions. Any breach of the applicable provisions of any 299 | third party's license terms shall also be considered a material breach 300 | of this Agreement. 301 | 302 | 3.8. TERMINATION 303 | 304 | Without prejudice to any other rights, MVTec may terminate this 305 | Agreement if you fail to comply with the terms and conditions of this 306 | Agreement. In such event, you must destroy all copies of the Software 307 | and all of its components. 308 | 309 | 310 | 4. INTELLECTUAL PROPERTY RIGHTS 311 | 312 | The Software is protected by copyright and other intellectual property 313 | laws and treaties. All title and intellectual property rights in and 314 | to the Software, including, but not limited to, any digital images or 315 | example programs, incorporated into the Software, the Documentation 316 | and any copies of the Software are owned by MVTec or its suppliers. 317 | The Software is licensed, not sold. 318 | 319 | 320 | 5. LIMITED WARRANTY 321 | 322 | MVTec warrants to the person or entity that first purchases a license 323 | for the Software that the Software will perform substantially in 324 | accordance with the Documentation for a period of six (6) months 325 | following receipt of the Software when used on a recommended hardware 326 | configuration. Non-substantial variations of performance from the 327 | Documentation does not establish a warranty right. This warranty does 328 | not apply to updates, pre-releases, or to Software that has been 329 | altered by you, to the extent such alterations caused a defect. To 330 | make a warranty claim, you must return the Software to the location 331 | where you obtained it along with proof of purchase within such six (6) 332 | month period. If the Software does not perform substantially in 333 | accordance with the Documentation, the entire liability of MVTec and 334 | your exclusive remedy shall be limited to either, at MVTec's option, 335 | the replacement of the Software or a refund of the license fee you 336 | paid for the Software. The limited warranty is void if failure of the 337 | Software or hardware has resulted from accident, abuse or 338 | misapplication. EXCEPT FOR THIS EXPRESS LIMITED WARRANTY, THE PRODUCT 339 | IS PROVIDED WITHOUT WARRANTY OF ANY KIND. MVTEC HEREBY EXCLUDES AND 340 | DISCLAIMS ALL IMPLIED OR STATUTORY WARRANTIES, INCLUDING, BUT NOT 341 | LIMITED TO, ANY WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 342 | PARTICULAR PURPOSE. THERE IS NO WARRANTY THAT THE SOFTWARE WILL BE 343 | ERROR-FREE OR WILL FUNCTION WITHOUT INTERRUPTION. TO THE EXTENT THAT 344 | MVTEC MAY NOT DISCLAIM ANY WARRANTY AS A MATTER OF APPLICABLE LAW, THE 345 | SCOPE AND DURATION OF SUCH WARRANTY WILL BE THE MINIMUM PERMITTED 346 | UNDER SUCH LAW. 347 | 348 | SOME STATES/JURISDICTIONS MAY NOT ALLOW THE EXCLUSION OF IMPLIED 349 | WARRANTIES, SO THE ABOVE EXCLUSION MAY NOT APPLY TO YOU. THIS LIMITED 350 | WARRANTY GIVES YOU SPECIFIC LEGAL RIGHTS. YOU MAY HAVE OTHER OR 351 | ADDITIONAL RIGHTS WHICH VARY FROM STATE/JURISDICTION TO 352 | STATE/JURISDICTION. 353 | 354 | 355 | 6. LIMITATION OF LIABILITY 356 | 357 | TO THE MAXIMUM EXTENT PERMITTED BY APPLICABLE LAW, IN NO EVENT SHALL 358 | MVTEC OR ITS SUPPLIERS BE LIABLE FOR ANY SPECIAL, INCIDENTAL, 359 | PUNITIVE, INDIRECT, OR CONSEQUENTIAL DAMAGES WHATSOEVER (INCLUDING, 360 | BUT NOT LIMITED TO, DAMAGES FOR LOSS OF PROFITS OR CONFIDENTIAL OR 361 | OTHER INFORMATION OF ANY KIND, FOR BUSINESS INTERRUPTION, FOR PERSONAL 362 | INJURY AND FOR ANY OTHER PECUNIARY OR OTHER LOSS WHATSOEVER) ARISING 363 | OUT OF OR IN ANY WAY RELATED TO THE USE OF OR INABILITY TO USE THE 364 | SOFTWARE, EVEN IF MVTEC HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH 365 | DAMAGES. MVTEC'S TOTAL LIABILITY (WHETHER IN RELATION TO BREACH 366 | CONTRACT, NEGLIGENCE OR OTHERWISE) SHALL NOT EXCEED THE AMOUNT PAID BY 367 | YOU FOR THE SOFTWARE, IF ANY. MVTec is acting on behalf of its 368 | suppliers for the purpose of disclaiming, excluding and/or limiting 369 | obligations, warranties and liability as provided in this Agreement, 370 | but in no other respects and for no other purpose. 371 | 372 | SOME STATES/JURISDICTIONS DO NOT ALLOW THE EXCLUSION OR LIMITATION OF 373 | INCIDENTAL OR CONSEQUENTIAL DAMAGES, SO THE ABOVE EXCLUSION OR 374 | LIMITATION MAY NOT APPLY TO YOU. 375 | 376 | 377 | 7. SPECIFIC EXCEPTIONS FOR DEMO AND EVALUTION VERSIONS 378 | 379 | If the Software you have received with this Agreement is a Demo 380 | Version or an Evaluation Version, you acknowledge that the Software of 381 | the Demo Version contains limited functionality and/or functions and 382 | that the Evaluation Version is restricted to specific purposes and may 383 | not be used in any other way as permitted. MVTEC IS LICENSING THE 384 | SOFTWARE ON AN "AS IS" BASIS. MVTEC DISCLAIMS ANY WARRANTY OR 385 | LIABILITY OBLIGATIONS TO YOU OF ANY KIND. WHERE LEGALLY LIABILITY 386 | CANNOT BE EXCLUDED FOR SUCH SOFTWARE, BUT IT MAY BE LIMITED, MVTEC'S 387 | LIABILITY SHALL BE LIMITED TO THE SUM OF FIFTY EUROS (EUR 50) IN 388 | TOTAL. 389 | 390 | 391 | 8. JURY TRIAL WAIVER 392 | 393 | EVEN IF TRIAL BY JURY MAY BE ADOPTED UNDER THE APPLICABLE LAW, THE 394 | PARTIES HERETO WAIVE TRIAL BY JURY WITH RESPECT TO ANY MATTERS ARISING 395 | UNDER OR RELATING TO THIS EULA TO THE MAXIMUM EXTENT AS PERMITTED 396 | UNDER THE APPLICABLE LAW. 397 | 398 | 399 | 9. HAZARDOUS USES 400 | 401 | The Software is not designed and/or intended for use in connection 402 | with any application requiring fail-safe performance such as the 403 | operation of nuclear power facilities, air traffic control or 404 | navigation systems, weapon control systems, life support systems, or 405 | any other system whose failure could lead to death, personal injury, 406 | or severe physical or environmental damage. You agree that MVTec will 407 | have no responsibility of any nature, and you are solely responsible 408 | for any expense, loss, injury or damage incurred as a result of such 409 | use of the Software. 410 | 411 | 412 | 10. ENTIRE AGREEMENT/SEVERABILITY 413 | 414 | This Agreement contains the entire, final and exclusive understanding 415 | between MVTec and you, and may not be modified or amended except by 416 | written instrument, executed by authorized representatives of MVTec 417 | and you. If any provisions of this license is held to be illegal, 418 | invalid or enforceable for any reason, then such provision will be 419 | enforced to the maximum extent permissible and the remainder of the 420 | provisions of this license will remain in full force and effect. 421 | 422 | 423 | 11. APPLICABLE LAW 424 | 425 | This Software License Agreement is governed by the laws of Germany. 426 | 427 | 428 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | asr_halcon_bridge 4 | 1.0.0 5 | This package is used to convert between image-messages of the ROS environment and HALCON-images. 6 | Allgeyer Tobias 7 | Meißner Pascal 8 | BSD 9 | http://ros.org/wiki/asr_halcon_bridge 10 | 11 | catkin 12 | roscpp 13 | sensor_msgs 14 | 15 | roscpp 16 | sensor_msgs 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /src/asr_halcon_bridge/halcon_image.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | 3 | Copyright (c) 2016, Allgeyer Tobias 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 9 | 10 | 2. 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. 11 | 12 | 3. 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. 13 | 14 | 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. 15 | 16 | */ 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | namespace halcon_bridge { 23 | 24 | const char* INVALID = "invalid"; 25 | const char* RGB = "rgb"; 26 | const char* BGR = "bgr"; 27 | 28 | int getHalconTypeSize(const std::string& type) { 29 | 30 | if ((type == "byte") || (type == "int1")) return 1; 31 | if ((type == "uint2") || (type == "int2")) return 2; 32 | if ((type == "int4") || (type == "real")) return 4; 33 | if (type == "int8") return 8; 34 | 35 | return -1; 36 | } 37 | 38 | const char* getHalconEncoding(const std::string& encoding) { 39 | // 3/4-channel encodings 40 | if (encoding == sensor_msgs::image_encodings::BGR8) return "bgr"; 41 | if (encoding == sensor_msgs::image_encodings::RGB8) return "rgb"; 42 | if (encoding == sensor_msgs::image_encodings::BGRA8) return "bgrx"; 43 | if (encoding == sensor_msgs::image_encodings::RGBA8) return "rgbx"; 44 | 45 | // 1-channel encoding 46 | if (encoding == sensor_msgs::image_encodings::MONO8) return "mono"; 47 | 48 | // Other formats are not supported 49 | return INVALID; 50 | } 51 | 52 | 53 | 54 | const char* getHalconChannelLength(const std::string& encoding) { 55 | 56 | if ((encoding == sensor_msgs::image_encodings::BGR8) || (encoding == sensor_msgs::image_encodings::RGB8) || 57 | (encoding == sensor_msgs::image_encodings::BGRA8) || (encoding == sensor_msgs::image_encodings::RGBA8) || 58 | (encoding == sensor_msgs::image_encodings::MONO8)) { 59 | return "byte"; 60 | } 61 | if ((encoding == sensor_msgs::image_encodings::BGR16) || (encoding == sensor_msgs::image_encodings::RGB16) || 62 | (encoding == sensor_msgs::image_encodings::BGRA16) || (encoding == sensor_msgs::image_encodings::RGBA16) || 63 | (encoding == sensor_msgs::image_encodings::MONO16)) { 64 | return "uint2"; 65 | } 66 | 67 | return INVALID; 68 | } 69 | 70 | 71 | 72 | const char* getColorChannelOrder(const std::string& encoding) { 73 | if ((encoding == sensor_msgs::image_encodings::BGR8) || (encoding == sensor_msgs::image_encodings::BGRA8) || 74 | (encoding == sensor_msgs::image_encodings::BGR16) || (encoding == sensor_msgs::image_encodings::BGRA16)) { 75 | return BGR; 76 | } 77 | if ((encoding == sensor_msgs::image_encodings::RGB8) || (encoding == sensor_msgs::image_encodings::RGBA8) || 78 | (encoding == sensor_msgs::image_encodings::RGB16) || (encoding == sensor_msgs::image_encodings::RGBA16)) { 79 | return RGB; 80 | } 81 | return INVALID; 82 | } 83 | 84 | 85 | 86 | 87 | HalconImage::~HalconImage() { 88 | delete image; 89 | } 90 | 91 | sensor_msgs::ImagePtr HalconImage::toImageMsg() const { 92 | sensor_msgs::ImagePtr ptr = boost::make_shared(); 93 | toImageMsg(*ptr); 94 | return ptr; 95 | } 96 | 97 | 98 | 99 | void HalconImage::toImageMsg(sensor_msgs::Image& ros_image) const { 100 | long width, height; 101 | width = image->Width(); 102 | height = image->Height(); 103 | 104 | int channel_count = image->CountChannels(); 105 | 106 | ros_image.height = height; 107 | ros_image.width = width; 108 | ros_image.encoding = encoding; 109 | ros_image.is_bigendian = false; 110 | ros_image.step = channel_count * width; 111 | 112 | 113 | HalconCpp::HString typeReturn; 114 | Hlong widthReturn; 115 | Hlong heightReturn; 116 | HalconCpp::HString type = image->GetImageType(); 117 | 118 | if (channel_count > 1) { 119 | 120 | //build interleaved image from 3-channel image 121 | 122 | HalconCpp::HImage *interleavedImage = new HalconCpp::HImage(type, width * 3, height); 123 | 124 | HalconCpp::HHomMat2D homMat; 125 | homMat = homMat.HomMat2dScale(1, 3, 0, 0); 126 | 127 | HalconCpp::HImage transImage = image->AffineTransImageSize(homMat, "constant", width * 3, height); 128 | 129 | HalconCpp::HImage imageRed; 130 | HalconCpp::HImage imageGreen; 131 | HalconCpp::HImage imageBlue; 132 | imageRed = transImage.Decompose3(&imageGreen, &imageBlue); 133 | 134 | 135 | HalconCpp::HRegion regionGrid; 136 | regionGrid.GenGridRegion(2 * height, 3, "lines", width * 3, height + 1); 137 | HalconCpp::HRegion movedRegion = regionGrid.MoveRegion(-1, 0); 138 | HalconCpp::HRegion clippedRegion = movedRegion.ClipRegion(0, 0, height - 1, (3 * width) - 1); 139 | 140 | if (getColorChannelOrder(encoding) == RGB) { 141 | imageRed = imageRed.ReduceDomain(clippedRegion); 142 | } else { 143 | imageBlue = imageBlue.ReduceDomain(clippedRegion); 144 | } 145 | movedRegion = regionGrid.MoveRegion(-1, 1); 146 | clippedRegion = movedRegion.ClipRegion(0, 0, height - 1, (3 * width) - 1); 147 | imageGreen = imageGreen.ReduceDomain(clippedRegion); 148 | movedRegion = regionGrid.MoveRegion(-1, 2); 149 | clippedRegion = movedRegion.ClipRegion(0, 0, height - 1, (3 * width) - 1); 150 | if (getColorChannelOrder(encoding) == RGB) { 151 | imageBlue = imageBlue.ReduceDomain(clippedRegion); 152 | } else { 153 | imageRed = imageRed.ReduceDomain(clippedRegion); 154 | } 155 | 156 | interleavedImage->OverpaintGray(imageRed); 157 | interleavedImage->OverpaintGray(imageGreen); 158 | interleavedImage->OverpaintGray(imageBlue); 159 | 160 | 161 | 162 | // copy data of interleaved image into sensor_msg 163 | unsigned char* colorData = (unsigned char*)interleavedImage->GetImagePointer1(&typeReturn, &widthReturn, &heightReturn); 164 | int interleavedHeight = interleavedImage->Height(); 165 | int interleavedWidth = interleavedImage->Width(); 166 | size_t size = interleavedHeight * interleavedWidth * getHalconTypeSize((std::string)typeReturn); 167 | ros_image.data.resize(size); 168 | memcpy((unsigned char*)(&ros_image.data[0]), colorData, size); 169 | 170 | interleavedImage->Clear(); 171 | imageRed.Clear(); 172 | imageGreen.Clear(); 173 | imageBlue.Clear(); 174 | homMat.Clear(); 175 | regionGrid.Clear(); 176 | clippedRegion.Clear(); 177 | movedRegion.Clear(); 178 | 179 | 180 | } else { 181 | 182 | // 1-channel image: copy data of original image 183 | unsigned char* colorData = (unsigned char*)image->GetImagePointer1(&typeReturn, &widthReturn, &heightReturn); 184 | size_t size = height * width * getHalconTypeSize((std::string)typeReturn); 185 | ros_image.data.resize(size); 186 | memcpy((unsigned char*)(&ros_image.data[0]), colorData, size); 187 | } 188 | 189 | } 190 | 191 | 192 | 193 | HalconImagePtr toHalconCopy(const sensor_msgs::ImageConstPtr& source) { 194 | return toHalconCopy(*source); 195 | } 196 | 197 | HalconImagePtr toHalconCopy(const sensor_msgs::Image& source) { 198 | HalconImagePtr ptr = boost::make_shared(); 199 | ptr->header = source.header; 200 | ptr->encoding = source.encoding; 201 | 202 | if (getHalconEncoding(ptr->encoding) == INVALID) { 203 | throw Exception("Encoding " + ptr->encoding + " not supported"); 204 | } 205 | 206 | long* pixeldata = (long*)const_cast(&source.data[0]); 207 | HalconCpp::HImage *img = new HalconCpp::HImage(); 208 | if ((std::string)getHalconEncoding(source.encoding) == "mono") { 209 | img->GenImage1(getHalconChannelLength(source.encoding), source.width, source.height, pixeldata); 210 | } else { 211 | img->GenImageInterleaved(pixeldata, getHalconEncoding(source.encoding), source.width, source.height, 0, 212 | getHalconChannelLength(source.encoding), source.width, source.height, 0, 0, -1, 0); 213 | } 214 | ptr->image = img; 215 | 216 | return ptr; 217 | } 218 | 219 | } 220 | 221 | 222 | -------------------------------------------------------------------------------- /src/asr_halcon_bridge/halcon_pointcloud.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | 3 | Copyright (c) 2016, Allgeyer Tobias 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 9 | 10 | 2. 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. 11 | 12 | 3. 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. 13 | 14 | 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. 15 | 16 | */ 17 | 18 | #include 19 | #include 20 | 21 | #include 22 | #include 23 | 24 | namespace halcon_bridge { 25 | 26 | int getSizeFromDatatype(int datatype) { 27 | if (datatype == sensor_msgs::PointField::INT8) return 1; 28 | if (datatype == sensor_msgs::PointField::UINT8) return 1; 29 | if (datatype == sensor_msgs::PointField::INT16) return 2; 30 | if (datatype == sensor_msgs::PointField::UINT16) return 2; 31 | if (datatype == sensor_msgs::PointField::INT32) return 4; 32 | if (datatype == sensor_msgs::PointField::UINT32) return 4; 33 | if (datatype == sensor_msgs::PointField::FLOAT32) return 4; 34 | if (datatype == sensor_msgs::PointField::FLOAT64) return 8; 35 | 36 | return -1; 37 | } 38 | 39 | HalconPointcloud::~HalconPointcloud() { 40 | delete model; 41 | curvature.Clear(); 42 | } 43 | 44 | sensor_msgs::PointCloud2Ptr HalconPointcloud::toPointcloudMsg() const { 45 | sensor_msgs::PointCloud2Ptr ptr = boost::make_shared(); 46 | toPointcloudMsg(*ptr); 47 | return ptr; 48 | } 49 | 50 | void HalconPointcloud::toPointcloudMsg(sensor_msgs::PointCloud2& ros_pointcloud) const { 51 | ros_pointcloud.header = header; 52 | ros_pointcloud.height = 1; 53 | ros_pointcloud.width = (int)model->GetObjectModel3dParams("num_points")[0]; 54 | ros_pointcloud.is_dense = false; 55 | ros_pointcloud.is_bigendian = false; 56 | bool has_normals = ((HalconCpp::HString)model->GetObjectModel3dParams("has_point_normals")) == HalconCpp::HString("true"); 57 | std::vector fields; 58 | if (has_normals) { 59 | ros_pointcloud.point_step = 28; 60 | fields = std::vector(7); 61 | } else { 62 | ros_pointcloud.point_step = 12; 63 | fields = std::vector(3); 64 | } 65 | 66 | ros_pointcloud.row_step = ros_pointcloud.width * ros_pointcloud.point_step; 67 | 68 | 69 | sensor_msgs::PointField x_field; 70 | x_field.name = "x"; 71 | x_field.count = 1; 72 | x_field.datatype = sensor_msgs::PointField::FLOAT32; 73 | x_field.offset = 0; 74 | fields[0] = x_field; 75 | 76 | sensor_msgs::PointField y_field; 77 | y_field.name = "y"; 78 | y_field.count = 1; 79 | y_field.datatype = sensor_msgs::PointField::FLOAT32; 80 | y_field.offset = 4; 81 | fields[1] = y_field; 82 | 83 | sensor_msgs::PointField z_field; 84 | z_field.name = "z"; 85 | z_field.count = 1; 86 | z_field.datatype = sensor_msgs::PointField::FLOAT32; 87 | z_field.offset = 8; 88 | fields[2] = z_field; 89 | 90 | if (has_normals) { 91 | sensor_msgs::PointField x_normals_field; 92 | x_normals_field.name = "normal_x"; 93 | x_normals_field.count = 1; 94 | x_normals_field.datatype = sensor_msgs::PointField::FLOAT32; 95 | x_normals_field.offset = 12; 96 | fields[3] = x_normals_field; 97 | 98 | sensor_msgs::PointField y_normals_field; 99 | y_normals_field.name = "normal_y"; 100 | y_normals_field.count = 1; 101 | y_normals_field.datatype = sensor_msgs::PointField::FLOAT32; 102 | y_normals_field.offset = 16; 103 | fields[4] = y_normals_field; 104 | 105 | sensor_msgs::PointField z_normals_field; 106 | z_normals_field.name = "normal_z"; 107 | z_normals_field.count = 1; 108 | z_normals_field.datatype = sensor_msgs::PointField::FLOAT32; 109 | z_normals_field.offset = 20; 110 | fields[5] = z_normals_field; 111 | 112 | sensor_msgs::PointField curvature; 113 | curvature.name = "curvature"; 114 | curvature.count = 1; 115 | curvature.datatype = sensor_msgs::PointField::FLOAT32; 116 | curvature.offset = 24; 117 | fields[6] = curvature; 118 | } 119 | 120 | 121 | ros_pointcloud.fields = fields; 122 | 123 | 124 | HalconCpp::HTuple x_values = model->GetObjectModel3dParams("point_coord_x"); 125 | HalconCpp::HTuple y_values = model->GetObjectModel3dParams("point_coord_y"); 126 | HalconCpp::HTuple z_values = model->GetObjectModel3dParams("point_coord_z"); 127 | 128 | HalconCpp::HTuple x_normal_values; 129 | HalconCpp::HTuple y_normal_values; 130 | HalconCpp::HTuple z_normal_values; 131 | 132 | 133 | float *interleavedPoints; 134 | 135 | if (has_normals) { 136 | x_normal_values = model->GetObjectModel3dParams("point_normal_x"); 137 | y_normal_values = model->GetObjectModel3dParams("point_normal_y"); 138 | z_normal_values = model->GetObjectModel3dParams("point_normal_z"); 139 | interleavedPoints = new float[ros_pointcloud.width * 7]; 140 | } else { 141 | interleavedPoints = new float[ros_pointcloud.width * 3]; 142 | } 143 | 144 | for (size_t i = 0; i < ros_pointcloud.width; i++) { 145 | interleavedPoints[i * 3] = (float)x_values[i]; 146 | interleavedPoints[i * 3 + 1] = (float)y_values[i]; 147 | interleavedPoints[i * 3 + 2] = (float)z_values[i]; 148 | if (has_normals) { 149 | interleavedPoints[i * 3 + 3] = (float)x_normal_values[i]; 150 | interleavedPoints[i * 3 + 4] = (float)y_normal_values[i]; 151 | interleavedPoints[i * 3 + 5] = (float)z_normal_values[i]; 152 | interleavedPoints[i * 3 + 6] = (float)(curvature)[i]; 153 | } 154 | } 155 | 156 | size_t size; 157 | if (has_normals) { 158 | size = ros_pointcloud.width * 7 * 4; 159 | } else { 160 | size = ros_pointcloud.width * 3 * 4; 161 | } 162 | ros_pointcloud.data.resize(size); 163 | 164 | memcpy((float*)(&ros_pointcloud.data[0]), &interleavedPoints[0], size); 165 | 166 | 167 | } 168 | 169 | 170 | 171 | HalconPointcloudPtr toHalconCopy(const sensor_msgs::PointCloud2ConstPtr& source) { 172 | return toHalconCopy(*source); 173 | } 174 | 175 | HalconPointcloudPtr toHalconCopy(const sensor_msgs::PointCloud2& source) { 176 | HalconPointcloudPtr ptr = boost::make_shared(); 177 | ptr->header = source.header; 178 | 179 | 180 | int offset_x, offset_y, offset_z, offset_x_normal, offset_y_normal, offset_z_normal, offset_curvature; 181 | offset_x = offset_y = offset_z = offset_x_normal = offset_y_normal = offset_z_normal = offset_curvature = 0; 182 | int count_x_normal, count_y_normal, count_z_normal, count_curvature; 183 | count_x_normal = count_y_normal = count_z_normal = count_curvature = 0; 184 | 185 | for (unsigned int i = 0; i < source.fields.size(); i++) { 186 | sensor_msgs::PointField field = source.fields[i]; 187 | if (field.name == "x") { 188 | offset_x = field.offset; 189 | } 190 | if (field.name == "y") { 191 | offset_y = field.offset; 192 | } 193 | if (field.name == "z") { 194 | offset_z = field.offset; 195 | } 196 | if (field.name == "normal_x") { 197 | offset_x_normal = field.offset; 198 | count_x_normal = field.count; 199 | } 200 | if (field.name == "normal_y") { 201 | offset_y_normal = field.offset; 202 | count_y_normal = field.count; 203 | } 204 | if (field.name == "normal_z") { 205 | offset_z_normal = field.offset; 206 | count_z_normal = field.count; 207 | } 208 | if (field.name == "curvature") { 209 | offset_curvature = field.offset; 210 | count_curvature = field.count; 211 | } 212 | 213 | } 214 | 215 | HalconCpp::HTuple x_coords = HalconCpp::HTuple::TupleGenConst((int)(source.width * source.height), 0); 216 | HalconCpp::HTuple y_coords = HalconCpp::HTuple::TupleGenConst((int)(source.width * source.height), 0); 217 | HalconCpp::HTuple z_coords = HalconCpp::HTuple::TupleGenConst((int)(source.width * source.height), 0); 218 | 219 | HalconCpp::HTuple x_normals = HalconCpp::HTuple::TupleGenConst((int)(source.width * source.height), 0); 220 | HalconCpp::HTuple y_normals = HalconCpp::HTuple::TupleGenConst((int)(source.width * source.height), 0); 221 | HalconCpp::HTuple z_normals = HalconCpp::HTuple::TupleGenConst((int)(source.width * source.height), 0); 222 | 223 | HalconCpp::HTuple curvature = HalconCpp::HTuple::TupleGenConst((int)(source.width * source.height), 0); 224 | 225 | for (int i = 0; i < x_coords.Length(); i++) { 226 | x_coords[i] = *(float*)&source.data[(i * source.point_step) + offset_x]; 227 | y_coords[i] = *(float*)&source.data[(i * source.point_step) + offset_y]; 228 | z_coords[i] = *(float*)&source.data[(i * source.point_step) + offset_z]; 229 | 230 | if ((count_x_normal > 0) && (count_y_normal > 0) && (count_z_normal > 0)) { 231 | x_normals[i] = *(float*)&source.data[(i * source.point_step) + offset_x_normal]; 232 | y_normals[i] = *(float*)&source.data[(i * source.point_step) + offset_y_normal]; 233 | z_normals[i] = *(float*)&source.data[(i * source.point_step) + offset_z_normal]; 234 | } 235 | 236 | } 237 | ptr->model = new HalconCpp::HObjectModel3D(x_coords, y_coords, z_coords); 238 | 239 | if (source.fields.size() > 4) { 240 | HalconCpp::HTuple attrib_names("point_normal_x"); 241 | attrib_names.Append("point_normal_y"); 242 | attrib_names.Append("point_normal_z"); 243 | HalconCpp::HTuple attrib_values = (x_normals.TupleConcat(y_normals)).TupleConcat(z_normals); 244 | HalconCpp::HObjectModel3D p_n_model = ptr->model->SetObjectModel3dAttrib(attrib_names, "", attrib_values); 245 | *ptr->model = p_n_model; 246 | 247 | for (int i = 0; i < curvature.Length(); i++) { 248 | curvature[i] = *(float*)&source.data[(i * source.point_step) + offset_curvature]; 249 | } 250 | ptr->curvature = curvature; 251 | } 252 | 253 | return ptr; 254 | } 255 | 256 | } 257 | 258 | 259 | 260 | --------------------------------------------------------------------------------