Initial import of FaRui Autoware Dual-Orin Stack
This commit is contained in:
li-shihao-code
@@ -0,0 +1,9 @@
|
||||
build
|
||||
install
|
||||
log
|
||||
|
||||
.cache
|
||||
.vscode
|
||||
.idea
|
||||
|
||||
COLCON_IGNORE
|
||||
@@ -0,0 +1,48 @@
|
||||
# 海康威视工业相机ROS2驱动
|
||||
|
||||
支持列表：
|
||||
|
||||
* GIGE相机（测试相机：MV-CS016-10GC）
|
||||
|
||||
* USB相机（未测试）
|
||||
|
||||
---
|
||||
|
||||
依赖项安装：
|
||||
|
||||
1. 在官网下载并安装MVS驱动包。
|
||||
|
||||
2. 运行MVS可视化工具，配置相机参数。设置相机名称(user_name, device_id)，本项目将根据相机名称寻找对应设备。
|
||||
|
||||
---
|
||||
|
||||
独立节点（standalone）运行方式：
|
||||
|
||||
```bash
|
||||
ros2 launch hikvision_ros2_driver standalone.launch.yaml camera_name:=<camera_name>
|
||||
```
|
||||
|
||||
进程内通信（component）运行方式：
|
||||
|
||||
```bash
|
||||
ros2 launch hikvision_ros2_driver component.launch.yaml camera_name:=<camera_name> container_name:=<container_name>
|
||||
```
|
||||
|
||||
* 需要将camera_name替换成对应相机的名称。
|
||||
|
||||
* 对于进程内通信，需要提前启动`component_container`，并将container_name设置为对应container的名称。
|
||||
|
||||
---
|
||||
|
||||
发布话题：
|
||||
|
||||
* raw/image [sensor_msgs/msg/Image]: 相机原始数据，格式可以在MVS工具中修改，默认情况下应该是bayer格式。
|
||||
* info [hikvision_interface/msg/HikImageInfo]: 图像元数据，包括时间戳、曝光长度、增益大小、白平衡参数等。
|
||||
|
||||
---
|
||||
|
||||
补充:
|
||||
|
||||
* 不建议对bayer格式的原始数据使用jpeg压缩。即当相机原始数据格式为bayer时，不建议订阅`raw/image/compressed`话题。
|
||||
|
||||
* rosbag录制时建议开启zstd压缩以节省空间。(ros2 bag record -s mcap --storage-preset-profile zstd_fast)
|
||||
BIN
Binary file not shown.
@@ -0,0 +1,27 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(hikvision_interface)
|
||||
|
||||
# find dependencies
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(std_msgs REQUIRED)
|
||||
find_package(rosidl_default_generators REQUIRED)
|
||||
|
||||
rosidl_generate_interfaces(${PROJECT_NAME}
|
||||
"msg/HikImageInfo.msg"
|
||||
DEPENDENCIES std_msgs
|
||||
)
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
# the following line skips the linter which checks for copyrights
|
||||
# comment the line when a copyright and license is added to all source files
|
||||
set(ament_cmake_copyright_FOUND TRUE)
|
||||
# the following line skips cpplint (only works in a git repo)
|
||||
# comment the line when this package is in a git repo and when
|
||||
# a copyright and license is added to all source files
|
||||
set(ament_cmake_cpplint_FOUND TRUE)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
endif()
|
||||
|
||||
ament_export_dependencies(std_msgs rosidl_default_runtime)
|
||||
ament_package()
|
||||
@@ -0,0 +1,202 @@
|
||||
|
||||
Apache License
|
||||
Version 2.0, January 2004
|
||||
http://www.apache.org/licenses/
|
||||
|
||||
TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
|
||||
|
||||
1. Definitions.
|
||||
|
||||
"License" shall mean the terms and conditions for use, reproduction,
|
||||
and distribution as defined by Sections 1 through 9 of this document.
|
||||
|
||||
"Licensor" shall mean the copyright owner or entity authorized by
|
||||
the copyright owner that is granting the License.
|
||||
|
||||
"Legal Entity" shall mean the union of the acting entity and all
|
||||
other entities that control, are controlled by, or are under common
|
||||
control with that entity. For the purposes of this definition,
|
||||
"control" means (i) the power, direct or indirect, to cause the
|
||||
direction or management of such entity, whether by contract or
|
||||
otherwise, or (ii) ownership of fifty percent (50%) or more of the
|
||||
outstanding shares, or (iii) beneficial ownership of such entity.
|
||||
|
||||
"You" (or "Your") shall mean an individual or Legal Entity
|
||||
exercising permissions granted by this License.
|
||||
|
||||
"Source" form shall mean the preferred form for making modifications,
|
||||
including but not limited to software source code, documentation
|
||||
source, and configuration files.
|
||||
|
||||
"Object" form shall mean any form resulting from mechanical
|
||||
transformation or translation of a Source form, including but
|
||||
not limited to compiled object code, generated documentation,
|
||||
and conversions to other media types.
|
||||
|
||||
"Work" shall mean the work of authorship, whether in Source or
|
||||
Object form, made available under the License, as indicated by a
|
||||
copyright notice that is included in or attached to the work
|
||||
(an example is provided in the Appendix below).
|
||||
|
||||
"Derivative Works" shall mean any work, whether in Source or Object
|
||||
form, that is based on (or derived from) the Work and for which the
|
||||
editorial revisions, annotations, elaborations, or other modifications
|
||||
represent, as a whole, an original work of authorship. For the purposes
|
||||
of this License, Derivative Works shall not include works that remain
|
||||
separable from, or merely link (or bind by name) to the interfaces of,
|
||||
the Work and Derivative Works thereof.
|
||||
|
||||
"Contribution" shall mean any work of authorship, including
|
||||
the original version of the Work and any modifications or additions
|
||||
to that Work or Derivative Works thereof, that is intentionally
|
||||
submitted to Licensor for inclusion in the Work by the copyright owner
|
||||
or by an individual or Legal Entity authorized to submit on behalf of
|
||||
the copyright owner. For the purposes of this definition, "submitted"
|
||||
means any form of electronic, verbal, or written communication sent
|
||||
to the Licensor or its representatives, including but not limited to
|
||||
communication on electronic mailing lists, source code control systems,
|
||||
and issue tracking systems that are managed by, or on behalf of, the
|
||||
Licensor for the purpose of discussing and improving the Work, but
|
||||
excluding communication that is conspicuously marked or otherwise
|
||||
designated in writing by the copyright owner as "Not a Contribution."
|
||||
|
||||
"Contributor" shall mean Licensor and any individual or Legal Entity
|
||||
on behalf of whom a Contribution has been received by Licensor and
|
||||
subsequently incorporated within the Work.
|
||||
|
||||
2. Grant of Copyright License. Subject to the terms and conditions of
|
||||
this License, each Contributor hereby grants to You a perpetual,
|
||||
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
|
||||
copyright license to reproduce, prepare Derivative Works of,
|
||||
publicly display, publicly perform, sublicense, and distribute the
|
||||
Work and such Derivative Works in Source or Object form.
|
||||
|
||||
3. Grant of Patent License. Subject to the terms and conditions of
|
||||
this License, each Contributor hereby grants to You a perpetual,
|
||||
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
|
||||
(except as stated in this section) patent license to make, have made,
|
||||
use, offer to sell, sell, import, and otherwise transfer the Work,
|
||||
where such license applies only to those patent claims licensable
|
||||
by such Contributor that are necessarily infringed by their
|
||||
Contribution(s) alone or by combination of their Contribution(s)
|
||||
with the Work to which such Contribution(s) was submitted. If You
|
||||
institute patent litigation against any entity (including a
|
||||
cross-claim or counterclaim in a lawsuit) alleging that the Work
|
||||
or a Contribution incorporated within the Work constitutes direct
|
||||
or contributory patent infringement, then any patent licenses
|
||||
granted to You under this License for that Work shall terminate
|
||||
as of the date such litigation is filed.
|
||||
|
||||
4. Redistribution. You may reproduce and distribute copies of the
|
||||
Work or Derivative Works thereof in any medium, with or without
|
||||
modifications, and in Source or Object form, provided that You
|
||||
meet the following conditions:
|
||||
|
||||
(a) You must give any other recipients of the Work or
|
||||
Derivative Works a copy of this License; and
|
||||
|
||||
(b) You must cause any modified files to carry prominent notices
|
||||
stating that You changed the files; and
|
||||
|
||||
(c) You must retain, in the Source form of any Derivative Works
|
||||
that You distribute, all copyright, patent, trademark, and
|
||||
attribution notices from the Source form of the Work,
|
||||
excluding those notices that do not pertain to any part of
|
||||
the Derivative Works; and
|
||||
|
||||
(d) If the Work includes a "NOTICE" text file as part of its
|
||||
distribution, then any Derivative Works that You distribute must
|
||||
include a readable copy of the attribution notices contained
|
||||
within such NOTICE file, excluding those notices that do not
|
||||
pertain to any part of the Derivative Works, in at least one
|
||||
of the following places: within a NOTICE text file distributed
|
||||
as part of the Derivative Works; within the Source form or
|
||||
documentation, if provided along with the Derivative Works; or,
|
||||
within a display generated by the Derivative Works, if and
|
||||
wherever such third-party notices normally appear. The contents
|
||||
of the NOTICE file are for informational purposes only and
|
||||
do not modify the License. You may add Your own attribution
|
||||
notices within Derivative Works that You distribute, alongside
|
||||
or as an addendum to the NOTICE text from the Work, provided
|
||||
that such additional attribution notices cannot be construed
|
||||
as modifying the License.
|
||||
|
||||
You may add Your own copyright statement to Your modifications and
|
||||
may provide additional or different license terms and conditions
|
||||
for use, reproduction, or distribution of Your modifications, or
|
||||
for any such Derivative Works as a whole, provided Your use,
|
||||
reproduction, and distribution of the Work otherwise complies with
|
||||
the conditions stated in this License.
|
||||
|
||||
5. Submission of Contributions. Unless You explicitly state otherwise,
|
||||
any Contribution intentionally submitted for inclusion in the Work
|
||||
by You to the Licensor shall be under the terms and conditions of
|
||||
this License, without any additional terms or conditions.
|
||||
Notwithstanding the above, nothing herein shall supersede or modify
|
||||
the terms of any separate license agreement you may have executed
|
||||
with Licensor regarding such Contributions.
|
||||
|
||||
6. Trademarks. This License does not grant permission to use the trade
|
||||
names, trademarks, service marks, or product names of the Licensor,
|
||||
except as required for reasonable and customary use in describing the
|
||||
origin of the Work and reproducing the content of the NOTICE file.
|
||||
|
||||
7. Disclaimer of Warranty. Unless required by applicable law or
|
||||
agreed to in writing, Licensor provides the Work (and each
|
||||
Contributor provides its Contributions) on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
|
||||
implied, including, without limitation, any warranties or conditions
|
||||
of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
|
||||
PARTICULAR PURPOSE. You are solely responsible for determining the
|
||||
appropriateness of using or redistributing the Work and assume any
|
||||
risks associated with Your exercise of permissions under this License.
|
||||
|
||||
8. Limitation of Liability. In no event and under no legal theory,
|
||||
whether in tort (including negligence), contract, or otherwise,
|
||||
unless required by applicable law (such as deliberate and grossly
|
||||
negligent acts) or agreed to in writing, shall any Contributor be
|
||||
liable to You for damages, including any direct, indirect, special,
|
||||
incidental, or consequential damages of any character arising as a
|
||||
result of this License or out of the use or inability to use the
|
||||
Work (including but not limited to damages for loss of goodwill,
|
||||
work stoppage, computer failure or malfunction, or any and all
|
||||
other commercial damages or losses), even if such Contributor
|
||||
has been advised of the possibility of such damages.
|
||||
|
||||
9. Accepting Warranty or Additional Liability. While redistributing
|
||||
the Work or Derivative Works thereof, You may choose to offer,
|
||||
and charge a fee for, acceptance of support, warranty, indemnity,
|
||||
or other liability obligations and/or rights consistent with this
|
||||
License. However, in accepting such obligations, You may act only
|
||||
on Your own behalf and on Your sole responsibility, not on behalf
|
||||
of any other Contributor, and only if You agree to indemnify,
|
||||
defend, and hold each Contributor harmless for any liability
|
||||
incurred by, or claims asserted against, such Contributor by reason
|
||||
of your accepting any such warranty or additional liability.
|
||||
|
||||
END OF TERMS AND CONDITIONS
|
||||
|
||||
APPENDIX: How to apply the Apache License to your work.
|
||||
|
||||
To apply the Apache License to your work, attach the following
|
||||
boilerplate notice, with the fields enclosed by brackets "[]"
|
||||
replaced with your own identifying information. (Don't include
|
||||
the brackets!) The text should be enclosed in the appropriate
|
||||
comment syntax for the file format. We also recommend that a
|
||||
file or class name and description of purpose be included on the
|
||||
same "printed page" as the copyright notice for easier
|
||||
identification within third-party archives.
|
||||
|
||||
Copyright [yyyy] [name of copyright owner]
|
||||
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
+12
@@ -0,0 +1,12 @@
|
||||
# header
|
||||
std_msgs/Header header
|
||||
# sensor stamp
|
||||
builtin_interfaces/Time dev_stamp
|
||||
uint32 frame_num
|
||||
# acqusistion
|
||||
float32 gain
|
||||
float32 exposure
|
||||
# white balance
|
||||
uint32 red
|
||||
uint32 green
|
||||
uint32 blue
|
||||
@@ -0,0 +1,24 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>hikvision_interface</name>
|
||||
<version>0.0.0</version>
|
||||
<description>ros2 interface for hikvision camera</description>
|
||||
<maintainer email="41963211+xinyang-go@users.noreply.github.com">loc</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<build_depend>std_msgs</build_depend>
|
||||
<exec_depend>std_msgs</exec_depend>
|
||||
<buildtool_depend>rosidl_default_generators</buildtool_depend>
|
||||
<exec_depend>rosidl_default_runtime</exec_depend>
|
||||
<member_of_group>rosidl_interface_packages</member_of_group>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,3 @@
|
||||
BasedOnStyle: Google
|
||||
IndentWidth: 4
|
||||
ColumnLimit: 120
|
||||
@@ -0,0 +1,50 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(hikvision_ros2_driver)
|
||||
|
||||
set(CMAKE_CXX_STANDARD 17)
|
||||
set(CMAKE_CXX_EXTENSIONS ON)
|
||||
|
||||
if(NOT CMAKE_BUILD_TYPE)
|
||||
set(CMAKE_BUILD_TYPE Release)
|
||||
endif()
|
||||
|
||||
add_compile_options(-Wall)
|
||||
|
||||
# find ros dependencies
|
||||
find_package(ament_cmake_auto REQUIRED)
|
||||
ament_auto_find_build_dependencies()
|
||||
|
||||
# find hikvision sdk
|
||||
list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)
|
||||
find_package(MVS REQUIRED)
|
||||
|
||||
# build components
|
||||
ament_auto_add_library(${PROJECT_NAME} SHARED
|
||||
src/hikvision_ros2_driver.cpp
|
||||
)
|
||||
target_include_directories(${PROJECT_NAME} PUBLIC include)
|
||||
target_include_directories(${PROJECT_NAME} PRIVATE ${MVS_INCLUDE_DIR})
|
||||
ament_target_dependencies(${PROJECT_NAME} driver_timestamp_sync)
|
||||
target_link_libraries(${PROJECT_NAME} ${MVS_LIBRARIES})
|
||||
|
||||
# register components
|
||||
rclcpp_components_register_node(${PROJECT_NAME}
|
||||
PLUGIN "hikvision_ros2_driver::HikvisionDriver"
|
||||
EXECUTABLE ${PROJECT_NAME}_node
|
||||
)
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
# the following line skips the linter which checks for copyrights
|
||||
# comment the line when a copyright and license is added to all source files
|
||||
set(ament_cmake_copyright_FOUND TRUE)
|
||||
# the following line skips cpplint (only works in a git repo)
|
||||
# comment the line when this package is in a git repo and when
|
||||
# a copyright and license is added to all source files
|
||||
set(ament_cmake_cpplint_FOUND TRUE)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
endif()
|
||||
|
||||
ament_auto_package(INSTALL_TO_SHARE
|
||||
launch
|
||||
)
|
||||
@@ -0,0 +1,202 @@
|
||||
|
||||
Apache License
|
||||
Version 2.0, January 2004
|
||||
http://www.apache.org/licenses/
|
||||
|
||||
TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
|
||||
|
||||
1. Definitions.
|
||||
|
||||
"License" shall mean the terms and conditions for use, reproduction,
|
||||
and distribution as defined by Sections 1 through 9 of this document.
|
||||
|
||||
"Licensor" shall mean the copyright owner or entity authorized by
|
||||
the copyright owner that is granting the License.
|
||||
|
||||
"Legal Entity" shall mean the union of the acting entity and all
|
||||
other entities that control, are controlled by, or are under common
|
||||
control with that entity. For the purposes of this definition,
|
||||
"control" means (i) the power, direct or indirect, to cause the
|
||||
direction or management of such entity, whether by contract or
|
||||
otherwise, or (ii) ownership of fifty percent (50%) or more of the
|
||||
outstanding shares, or (iii) beneficial ownership of such entity.
|
||||
|
||||
"You" (or "Your") shall mean an individual or Legal Entity
|
||||
exercising permissions granted by this License.
|
||||
|
||||
"Source" form shall mean the preferred form for making modifications,
|
||||
including but not limited to software source code, documentation
|
||||
source, and configuration files.
|
||||
|
||||
"Object" form shall mean any form resulting from mechanical
|
||||
transformation or translation of a Source form, including but
|
||||
not limited to compiled object code, generated documentation,
|
||||
and conversions to other media types.
|
||||
|
||||
"Work" shall mean the work of authorship, whether in Source or
|
||||
Object form, made available under the License, as indicated by a
|
||||
copyright notice that is included in or attached to the work
|
||||
(an example is provided in the Appendix below).
|
||||
|
||||
"Derivative Works" shall mean any work, whether in Source or Object
|
||||
form, that is based on (or derived from) the Work and for which the
|
||||
editorial revisions, annotations, elaborations, or other modifications
|
||||
represent, as a whole, an original work of authorship. For the purposes
|
||||
of this License, Derivative Works shall not include works that remain
|
||||
separable from, or merely link (or bind by name) to the interfaces of,
|
||||
the Work and Derivative Works thereof.
|
||||
|
||||
"Contribution" shall mean any work of authorship, including
|
||||
the original version of the Work and any modifications or additions
|
||||
to that Work or Derivative Works thereof, that is intentionally
|
||||
submitted to Licensor for inclusion in the Work by the copyright owner
|
||||
or by an individual or Legal Entity authorized to submit on behalf of
|
||||
the copyright owner. For the purposes of this definition, "submitted"
|
||||
means any form of electronic, verbal, or written communication sent
|
||||
to the Licensor or its representatives, including but not limited to
|
||||
communication on electronic mailing lists, source code control systems,
|
||||
and issue tracking systems that are managed by, or on behalf of, the
|
||||
Licensor for the purpose of discussing and improving the Work, but
|
||||
excluding communication that is conspicuously marked or otherwise
|
||||
designated in writing by the copyright owner as "Not a Contribution."
|
||||
|
||||
"Contributor" shall mean Licensor and any individual or Legal Entity
|
||||
on behalf of whom a Contribution has been received by Licensor and
|
||||
subsequently incorporated within the Work.
|
||||
|
||||
2. Grant of Copyright License. Subject to the terms and conditions of
|
||||
this License, each Contributor hereby grants to You a perpetual,
|
||||
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
|
||||
copyright license to reproduce, prepare Derivative Works of,
|
||||
publicly display, publicly perform, sublicense, and distribute the
|
||||
Work and such Derivative Works in Source or Object form.
|
||||
|
||||
3. Grant of Patent License. Subject to the terms and conditions of
|
||||
this License, each Contributor hereby grants to You a perpetual,
|
||||
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
|
||||
(except as stated in this section) patent license to make, have made,
|
||||
use, offer to sell, sell, import, and otherwise transfer the Work,
|
||||
where such license applies only to those patent claims licensable
|
||||
by such Contributor that are necessarily infringed by their
|
||||
Contribution(s) alone or by combination of their Contribution(s)
|
||||
with the Work to which such Contribution(s) was submitted. If You
|
||||
institute patent litigation against any entity (including a
|
||||
cross-claim or counterclaim in a lawsuit) alleging that the Work
|
||||
or a Contribution incorporated within the Work constitutes direct
|
||||
or contributory patent infringement, then any patent licenses
|
||||
granted to You under this License for that Work shall terminate
|
||||
as of the date such litigation is filed.
|
||||
|
||||
4. Redistribution. You may reproduce and distribute copies of the
|
||||
Work or Derivative Works thereof in any medium, with or without
|
||||
modifications, and in Source or Object form, provided that You
|
||||
meet the following conditions:
|
||||
|
||||
(a) You must give any other recipients of the Work or
|
||||
Derivative Works a copy of this License; and
|
||||
|
||||
(b) You must cause any modified files to carry prominent notices
|
||||
stating that You changed the files; and
|
||||
|
||||
(c) You must retain, in the Source form of any Derivative Works
|
||||
that You distribute, all copyright, patent, trademark, and
|
||||
attribution notices from the Source form of the Work,
|
||||
excluding those notices that do not pertain to any part of
|
||||
the Derivative Works; and
|
||||
|
||||
(d) If the Work includes a "NOTICE" text file as part of its
|
||||
distribution, then any Derivative Works that You distribute must
|
||||
include a readable copy of the attribution notices contained
|
||||
within such NOTICE file, excluding those notices that do not
|
||||
pertain to any part of the Derivative Works, in at least one
|
||||
of the following places: within a NOTICE text file distributed
|
||||
as part of the Derivative Works; within the Source form or
|
||||
documentation, if provided along with the Derivative Works; or,
|
||||
within a display generated by the Derivative Works, if and
|
||||
wherever such third-party notices normally appear. The contents
|
||||
of the NOTICE file are for informational purposes only and
|
||||
do not modify the License. You may add Your own attribution
|
||||
notices within Derivative Works that You distribute, alongside
|
||||
or as an addendum to the NOTICE text from the Work, provided
|
||||
that such additional attribution notices cannot be construed
|
||||
as modifying the License.
|
||||
|
||||
You may add Your own copyright statement to Your modifications and
|
||||
may provide additional or different license terms and conditions
|
||||
for use, reproduction, or distribution of Your modifications, or
|
||||
for any such Derivative Works as a whole, provided Your use,
|
||||
reproduction, and distribution of the Work otherwise complies with
|
||||
the conditions stated in this License.
|
||||
|
||||
5. Submission of Contributions. Unless You explicitly state otherwise,
|
||||
any Contribution intentionally submitted for inclusion in the Work
|
||||
by You to the Licensor shall be under the terms and conditions of
|
||||
this License, without any additional terms or conditions.
|
||||
Notwithstanding the above, nothing herein shall supersede or modify
|
||||
the terms of any separate license agreement you may have executed
|
||||
with Licensor regarding such Contributions.
|
||||
|
||||
6. Trademarks. This License does not grant permission to use the trade
|
||||
names, trademarks, service marks, or product names of the Licensor,
|
||||
except as required for reasonable and customary use in describing the
|
||||
origin of the Work and reproducing the content of the NOTICE file.
|
||||
|
||||
7. Disclaimer of Warranty. Unless required by applicable law or
|
||||
agreed to in writing, Licensor provides the Work (and each
|
||||
Contributor provides its Contributions) on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
|
||||
implied, including, without limitation, any warranties or conditions
|
||||
of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
|
||||
PARTICULAR PURPOSE. You are solely responsible for determining the
|
||||
appropriateness of using or redistributing the Work and assume any
|
||||
risks associated with Your exercise of permissions under this License.
|
||||
|
||||
8. Limitation of Liability. In no event and under no legal theory,
|
||||
whether in tort (including negligence), contract, or otherwise,
|
||||
unless required by applicable law (such as deliberate and grossly
|
||||
negligent acts) or agreed to in writing, shall any Contributor be
|
||||
liable to You for damages, including any direct, indirect, special,
|
||||
incidental, or consequential damages of any character arising as a
|
||||
result of this License or out of the use or inability to use the
|
||||
Work (including but not limited to damages for loss of goodwill,
|
||||
work stoppage, computer failure or malfunction, or any and all
|
||||
other commercial damages or losses), even if such Contributor
|
||||
has been advised of the possibility of such damages.
|
||||
|
||||
9. Accepting Warranty or Additional Liability. While redistributing
|
||||
the Work or Derivative Works thereof, You may choose to offer,
|
||||
and charge a fee for, acceptance of support, warranty, indemnity,
|
||||
or other liability obligations and/or rights consistent with this
|
||||
License. However, in accepting such obligations, You may act only
|
||||
on Your own behalf and on Your sole responsibility, not on behalf
|
||||
of any other Contributor, and only if You agree to indemnify,
|
||||
defend, and hold each Contributor harmless for any liability
|
||||
incurred by, or claims asserted against, such Contributor by reason
|
||||
of your accepting any such warranty or additional liability.
|
||||
|
||||
END OF TERMS AND CONDITIONS
|
||||
|
||||
APPENDIX: How to apply the Apache License to your work.
|
||||
|
||||
To apply the Apache License to your work, attach the following
|
||||
boilerplate notice, with the fields enclosed by brackets "[]"
|
||||
replaced with your own identifying information. (Don't include
|
||||
the brackets!) The text should be enclosed in the appropriate
|
||||
comment syntax for the file format. We also recommend that a
|
||||
file or class name and description of purpose be included on the
|
||||
same "printed page" as the copyright notice for easier
|
||||
identification within third-party archives.
|
||||
|
||||
Copyright [yyyy] [name of copyright owner]
|
||||
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
+37
@@ -0,0 +1,37 @@
|
||||
if(DEFINED ENV{MVCAM_SDK_PATH})
|
||||
set(MVCAM_SDK_PATH $ENV{MVCAM_SDK_PATH} CACHE PATH "hikvision sdk path")
|
||||
else()
|
||||
set(MVCAM_SDK_PATH /opt/MVS CACHE PATH "hikvision sdk path")
|
||||
endif()
|
||||
|
||||
# find header
|
||||
find_path(MVS_INCLUDE_DIR
|
||||
NAMES MvCameraControl.h
|
||||
PATHS ${MVCAM_SDK_PATH}/include
|
||||
)
|
||||
|
||||
# find libraries
|
||||
if(CMAKE_SYSTEM_PROCESSOR MATCHES "x86_64")
|
||||
find_library(MVS_LIBRARIES
|
||||
NAMES MvCameraControl
|
||||
PATHS ${MVCAM_SDK_PATH}/lib/64
|
||||
)
|
||||
elseif(CMAKE_SYSTEM_PROCESSOR MATCHES "aarch64")
|
||||
find_library(MVS_LIBRARIES
|
||||
NAMES MvCameraControl
|
||||
PATHS ${MVCAM_SDK_PATH}/lib/aarch64
|
||||
)
|
||||
endif()
|
||||
|
||||
|
||||
message(VERBOSE "MVCAM_SDK_PATH: ${MVCAM_SDK_PATH}")
|
||||
message(VERBOSE "MVS_INCLUDE_DIR: ${MVS_INCLUDE_DIR}")
|
||||
message(VERBOSE "MVS_LIBRARIES: ${MVS_LIBRARIES}")
|
||||
|
||||
include(FindPackageHandleStandardArgs)
|
||||
find_package_handle_standard_args(MVS
|
||||
FOUND_VAR MVS_FOUND
|
||||
REQUIRED_VARS
|
||||
MVS_INCLUDE_DIR
|
||||
MVS_LIBRARIES
|
||||
)
|
||||
+21
@@ -0,0 +1,21 @@
|
||||
#pragma once
|
||||
#ifndef HIKVISION_ROS2_DRIVER_HPP
|
||||
#define HIKVISION_ROS2_DRIVER_HPP
|
||||
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
|
||||
namespace hikvision_ros2_driver {
|
||||
|
||||
class HikvisionDriver : public rclcpp::Node {
|
||||
public:
|
||||
HikvisionDriver(const rclcpp::NodeOptions &options);
|
||||
~HikvisionDriver();
|
||||
|
||||
private:
|
||||
struct Impl;
|
||||
std::unique_ptr<Impl> pImpl;
|
||||
};
|
||||
|
||||
} // namespace hikvision_ros2_driver
|
||||
|
||||
#endif // HIKVISION_ROS2_DRIVER_HPP
|
||||
+74
@@ -0,0 +1,74 @@
|
||||
launch:
|
||||
- arg:
|
||||
name: camera_name
|
||||
- arg:
|
||||
name: container_name
|
||||
- arg:
|
||||
name: sync_mode
|
||||
default: freerun
|
||||
- arg:
|
||||
name: trigger_activation
|
||||
default: rising_edge
|
||||
- arg:
|
||||
name: trigger_delay_us
|
||||
default: '0.0'
|
||||
- arg:
|
||||
name: device_timestamp_tick_hz
|
||||
default: '0.0'
|
||||
- arg:
|
||||
name: use_timestamp_sync
|
||||
default: 'true'
|
||||
- arg:
|
||||
name: time_offset_sec
|
||||
default: '0.0'
|
||||
- arg:
|
||||
name: bias_alpha
|
||||
default: '0.01'
|
||||
- arg:
|
||||
name: jump_warn_threshold_ms
|
||||
default: '1000'
|
||||
- arg:
|
||||
name: clamp_backward_time
|
||||
default: 'true'
|
||||
- arg:
|
||||
name: timestamp_diagnostics
|
||||
default: 'false'
|
||||
- arg:
|
||||
name: timestamp_diagnostics_throttle_ms
|
||||
default: '2000'
|
||||
|
||||
- load_composable_node:
|
||||
target: $(var container_name)
|
||||
composable_node:
|
||||
- pkg: hikvision_ros2_driver
|
||||
plugin: hikvision_ros2_driver::HikvisionDriver
|
||||
name: hikvision_ros2_driver_node
|
||||
namespace: /driver/hikvision/$(var camera_name)
|
||||
param:
|
||||
- name: camera_name
|
||||
value: $(var camera_name)
|
||||
- name: sync_mode
|
||||
value: $(var sync_mode)
|
||||
- name: trigger_activation
|
||||
value: $(var trigger_activation)
|
||||
- name: trigger_delay_us
|
||||
value: $(var trigger_delay_us)
|
||||
- name: device_timestamp_tick_hz
|
||||
value: $(var device_timestamp_tick_hz)
|
||||
- name: use_timestamp_sync
|
||||
value: $(var use_timestamp_sync)
|
||||
- name: time_offset_sec
|
||||
value: $(var time_offset_sec)
|
||||
- name: bias_alpha
|
||||
value: $(var bias_alpha)
|
||||
- name: jump_warn_threshold_ms
|
||||
value: $(var jump_warn_threshold_ms)
|
||||
- name: clamp_backward_time
|
||||
value: $(var clamp_backward_time)
|
||||
- name: timestamp_diagnostics
|
||||
value: $(var timestamp_diagnostics)
|
||||
- name: timestamp_diagnostics_throttle_ms
|
||||
value: $(var timestamp_diagnostics_throttle_ms)
|
||||
extra_arg:
|
||||
- name: use_intra_process_comms
|
||||
value: 'true'
|
||||
+66
@@ -0,0 +1,66 @@
|
||||
launch:
|
||||
- arg:
|
||||
name: camera_name
|
||||
- arg:
|
||||
name: sync_mode
|
||||
default: freerun
|
||||
- arg:
|
||||
name: trigger_activation
|
||||
default: rising_edge
|
||||
- arg:
|
||||
name: trigger_delay_us
|
||||
default: '0.0'
|
||||
- arg:
|
||||
name: device_timestamp_tick_hz
|
||||
default: '0.0'
|
||||
- arg:
|
||||
name: use_timestamp_sync
|
||||
default: 'true'
|
||||
- arg:
|
||||
name: time_offset_sec
|
||||
default: '0.0'
|
||||
- arg:
|
||||
name: bias_alpha
|
||||
default: '0.01'
|
||||
- arg:
|
||||
name: jump_warn_threshold_ms
|
||||
default: '1000'
|
||||
- arg:
|
||||
name: clamp_backward_time
|
||||
default: 'true'
|
||||
- arg:
|
||||
name: timestamp_diagnostics
|
||||
default: 'false'
|
||||
- arg:
|
||||
name: timestamp_diagnostics_throttle_ms
|
||||
default: '2000'
|
||||
|
||||
- node:
|
||||
pkg: hikvision_ros2_driver
|
||||
exec: hikvision_ros2_driver_node
|
||||
namespace: /driver/hikvision/$(var camera_name)
|
||||
param:
|
||||
- name: camera_name
|
||||
value: $(var camera_name)
|
||||
- name: sync_mode
|
||||
value: $(var sync_mode)
|
||||
- name: trigger_activation
|
||||
value: $(var trigger_activation)
|
||||
- name: trigger_delay_us
|
||||
value: $(var trigger_delay_us)
|
||||
- name: device_timestamp_tick_hz
|
||||
value: $(var device_timestamp_tick_hz)
|
||||
- name: use_timestamp_sync
|
||||
value: $(var use_timestamp_sync)
|
||||
- name: time_offset_sec
|
||||
value: $(var time_offset_sec)
|
||||
- name: bias_alpha
|
||||
value: $(var bias_alpha)
|
||||
- name: jump_warn_threshold_ms
|
||||
value: $(var jump_warn_threshold_ms)
|
||||
- name: clamp_backward_time
|
||||
value: $(var clamp_backward_time)
|
||||
- name: timestamp_diagnostics
|
||||
value: $(var timestamp_diagnostics)
|
||||
- name: timestamp_diagnostics_throttle_ms
|
||||
value: $(var timestamp_diagnostics_throttle_ms)
|
||||
@@ -0,0 +1,26 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>hikvision_ros2_driver</name>
|
||||
<version>0.0.0</version>
|
||||
<description>ros2 driver for hikvision camera</description>
|
||||
<maintainer email="41963211+xinyang-go@users.noreply.github.com">xinyang</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<buildtool_depend>ament_cmake_auto</buildtool_depend>
|
||||
|
||||
<depend>rclcpp</depend>
|
||||
<depend>rclcpp_components</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
<depend>image_transport</depend>
|
||||
<depend>image_transport_plugins</depend>
|
||||
<depend>hikvision_interface</depend>
|
||||
<depend>driver_timestamp_sync</depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
+505
@@ -0,0 +1,505 @@
|
||||
#include "hikvision_ros2_driver/hikvision_ros2_driver.hpp"
|
||||
|
||||
// ros
|
||||
#include <hikvision_interface/msg/hik_image_info.hpp>
|
||||
#include <image_transport/image_transport.hpp>
|
||||
#include <sensor_msgs/image_encodings.hpp>
|
||||
#include <builtin_interfaces/msg/time.hpp>
|
||||
#include <driver_timestamp_sync/timestamp_sync.hpp>
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <cstring>
|
||||
#include <memory>
|
||||
#include <optional>
|
||||
#include <stdexcept>
|
||||
#include <string>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
// hikvision sdk
|
||||
#include <MvCameraControl.h>
|
||||
|
||||
#define MV_CHECK_THROW(logger, func, ...) \
|
||||
do { \
|
||||
int nRet = func(__VA_ARGS__); \
|
||||
if (MV_OK != nRet) { \
|
||||
RCLCPP_ERROR(logger, "hikvision sdk error: " #func " = %d", nRet); \
|
||||
throw std::runtime_error("hikvision sdk call failed: " #func); \
|
||||
} \
|
||||
} while (0)
|
||||
|
||||
using hikvision_interface::msg::HikImageInfo;
|
||||
|
||||
namespace hikvision_ros2_driver {
|
||||
namespace {
|
||||
|
||||
builtin_interfaces::msg::Time ToBuiltinTime(const rclcpp::Time & stamp)
|
||||
{
|
||||
builtin_interfaces::msg::Time builtin;
|
||||
const auto total_ns = stamp.nanoseconds();
|
||||
builtin.sec = static_cast<int32_t>(total_ns / 1000000000ll);
|
||||
builtin.nanosec = static_cast<uint32_t>(total_ns % 1000000000ll);
|
||||
return builtin;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
struct HikvisionDriver::Impl {
|
||||
std::unique_ptr<rclcpp::Logger> logger;
|
||||
|
||||
void *handle{nullptr};
|
||||
std::string camera_name;
|
||||
std::string sync_mode{"freerun"};
|
||||
std::string trigger_activation{"rising_edge"};
|
||||
double trigger_delay_us{0.0};
|
||||
double device_timestamp_tick_hz{0.0};
|
||||
bool use_timestamp_sync{true};
|
||||
bool timestamp_diagnostics{false};
|
||||
int timestamp_diagnostics_throttle_ms{2000};
|
||||
driver_timestamp_sync::TimestampSync timestamp_sync;
|
||||
image_transport::Publisher img_pub;
|
||||
std::shared_ptr<rclcpp::Publisher<HikImageInfo>> p_info_pub;
|
||||
|
||||
void configure_trigger(HikvisionDriver * node);
|
||||
double resolve_device_timestamp_tick_hz(HikvisionDriver * node, MV_CC_DEVICE_INFO * device_info);
|
||||
std::optional<rclcpp::Time> convert_device_stamp_to_ros_time(uint64_t raw_dev_tick) const;
|
||||
uint32_t device_tick_ms(uint64_t raw_dev_tick) const;
|
||||
rclcpp::Time compute_header_stamp(HikvisionDriver * node, const MV_FRAME_OUT_INFO_EX & frame_info,
|
||||
const rclcpp::Time & host_rx_time, uint32_t converted_device_tick_ms, uint64_t raw_dev_tick);
|
||||
static void image_callback_ex(unsigned char *pData, MV_FRAME_OUT_INFO_EX *pFrameInfo, void *pUser);
|
||||
};
|
||||
|
||||
void HikvisionDriver::Impl::configure_trigger(HikvisionDriver * node)
|
||||
{
|
||||
auto logger = node->get_logger();
|
||||
|
||||
if (sync_mode == "freerun") {
|
||||
MV_CHECK_THROW(logger, MV_CC_SetEnumValue, handle, "TriggerMode", MV_TRIGGER_MODE_OFF);
|
||||
RCLCPP_INFO(logger, "hikvision sync_mode=freerun");
|
||||
return;
|
||||
}
|
||||
|
||||
unsigned int trigger_source = 0;
|
||||
if (sync_mode == "external_line0") {
|
||||
trigger_source = MV_TRIGGER_SOURCE_LINE0;
|
||||
} else if (sync_mode == "external_line2") {
|
||||
trigger_source = MV_TRIGGER_SOURCE_LINE2;
|
||||
} else {
|
||||
throw std::runtime_error("unsupported sync_mode: " + sync_mode);
|
||||
}
|
||||
|
||||
MV_CHECK_THROW(logger, MV_CC_SetEnumValueByString, handle, "TriggerSelector", "FrameStart");
|
||||
MV_CHECK_THROW(logger, MV_CC_SetEnumValue, handle, "TriggerMode", MV_TRIGGER_MODE_ON);
|
||||
MV_CHECK_THROW(logger, MV_CC_SetEnumValue, handle, "TriggerSource", trigger_source);
|
||||
|
||||
if (trigger_activation == "rising_edge") {
|
||||
MV_CHECK_THROW(logger, MV_CC_SetEnumValueByString, handle, "TriggerActivation", "RisingEdge");
|
||||
} else if (trigger_activation == "falling_edge") {
|
||||
MV_CHECK_THROW(logger, MV_CC_SetEnumValueByString, handle, "TriggerActivation", "FallingEdge");
|
||||
} else {
|
||||
throw std::runtime_error("unsupported trigger_activation: " + trigger_activation);
|
||||
}
|
||||
|
||||
if (trigger_delay_us > 0.0) {
|
||||
MV_CHECK_THROW(logger, MV_CC_SetFloatValue, handle, "TriggerDelay", static_cast<float>(trigger_delay_us));
|
||||
}
|
||||
|
||||
RCLCPP_INFO(
|
||||
logger,
|
||||
"hikvision sync_mode=%s trigger_activation=%s trigger_delay_us=%.3f",
|
||||
sync_mode.c_str(),
|
||||
trigger_activation.c_str(),
|
||||
trigger_delay_us);
|
||||
}
|
||||
|
||||
double HikvisionDriver::Impl::resolve_device_timestamp_tick_hz(
|
||||
HikvisionDriver * node,
|
||||
MV_CC_DEVICE_INFO * device_info)
|
||||
{
|
||||
auto logger = node->get_logger();
|
||||
MVCC_INTVALUE int_value;
|
||||
memset(&int_value, 0, sizeof(int_value));
|
||||
|
||||
if (device_info->nTLayerType == MV_GIGE_DEVICE &&
|
||||
MV_CC_GetIntValue(handle, "GevTimestampTickFrequency", &int_value) == MV_OK &&
|
||||
int_value.nCurValue > 0)
|
||||
{
|
||||
const auto resolved = static_cast<double>(int_value.nCurValue);
|
||||
RCLCPP_INFO(logger, "resolved GevTimestampTickFrequency=%.3f Hz", resolved);
|
||||
return resolved;
|
||||
}
|
||||
|
||||
if (device_timestamp_tick_hz > 0.0) {
|
||||
RCLCPP_WARN(
|
||||
logger,
|
||||
"falling back to configured device_timestamp_tick_hz=%.3f Hz",
|
||||
device_timestamp_tick_hz);
|
||||
return device_timestamp_tick_hz;
|
||||
}
|
||||
|
||||
throw std::runtime_error("failed to resolve device timestamp tick frequency");
|
||||
}
|
||||
|
||||
std::optional<rclcpp::Time> HikvisionDriver::Impl::convert_device_stamp_to_ros_time(uint64_t raw_dev_tick) const
|
||||
{
|
||||
if (device_timestamp_tick_hz <= 0.0) {
|
||||
return std::nullopt;
|
||||
}
|
||||
|
||||
const long double stamp_ns =
|
||||
static_cast<long double>(raw_dev_tick) * 1000000000.0L / static_cast<long double>(device_timestamp_tick_hz);
|
||||
return rclcpp::Time(static_cast<int64_t>(std::llround(stamp_ns)), RCL_ROS_TIME);
|
||||
}
|
||||
|
||||
uint32_t HikvisionDriver::Impl::device_tick_ms(uint64_t raw_dev_tick) const
|
||||
{
|
||||
if (device_timestamp_tick_hz <= 0.0) {
|
||||
throw std::runtime_error("device timestamp tick frequency must be positive");
|
||||
}
|
||||
|
||||
const long double tick_ms =
|
||||
static_cast<long double>(raw_dev_tick) * 1000.0L / static_cast<long double>(device_timestamp_tick_hz);
|
||||
return static_cast<uint32_t>(std::llround(tick_ms));
|
||||
}
|
||||
|
||||
rclcpp::Time HikvisionDriver::Impl::compute_header_stamp(
|
||||
HikvisionDriver * node,
|
||||
const MV_FRAME_OUT_INFO_EX & frame_info,
|
||||
const rclcpp::Time & host_rx_time,
|
||||
uint32_t converted_device_tick_ms,
|
||||
uint64_t raw_dev_tick)
|
||||
{
|
||||
if (!use_timestamp_sync) {
|
||||
return host_rx_time;
|
||||
}
|
||||
|
||||
auto sync_result = timestamp_sync.reconstruct(converted_device_tick_ms, host_rx_time);
|
||||
|
||||
if (sync_result.large_jump_detected) {
|
||||
RCLCPP_WARN_THROTTLE(
|
||||
node->get_logger(), *node->get_clock(), 2000,
|
||||
"hikvision timestamp jump detected: frame=%u raw_tick=%llu delta_ms=%u unwrapped_ms=%llu",
|
||||
frame_info.nFrameNum,
|
||||
static_cast<unsigned long long>(raw_dev_tick),
|
||||
sync_result.delta_tick_ms,
|
||||
static_cast<unsigned long long>(sync_result.unwrapped_device_tick_ms));
|
||||
}
|
||||
|
||||
if (sync_result.backward_clamped) {
|
||||
RCLCPP_WARN_THROTTLE(
|
||||
node->get_logger(), *node->get_clock(), 2000,
|
||||
"hikvision backward timestamp clamped: frame=%u stamp_ns=%lld",
|
||||
frame_info.nFrameNum,
|
||||
static_cast<long long>(sync_result.stamp.nanoseconds()));
|
||||
}
|
||||
|
||||
if (timestamp_diagnostics) {
|
||||
RCLCPP_INFO_THROTTLE(
|
||||
node->get_logger(), *node->get_clock(), timestamp_diagnostics_throttle_ms,
|
||||
"hikvision timestamp: frame=%u raw_tick=%llu tick_ms=%u host_rx_ns=%lld stamp_ns=%lld bias_ns=%lld init=%s",
|
||||
frame_info.nFrameNum,
|
||||
static_cast<unsigned long long>(raw_dev_tick),
|
||||
converted_device_tick_ms,
|
||||
static_cast<long long>(host_rx_time.nanoseconds()),
|
||||
static_cast<long long>(sync_result.stamp.nanoseconds()),
|
||||
static_cast<long long>(sync_result.filtered_bias_ns),
|
||||
sync_result.initialized_this_call ? "true" : "false");
|
||||
}
|
||||
|
||||
return sync_result.stamp;
|
||||
}
|
||||
|
||||
void HikvisionDriver::Impl::image_callback_ex(
|
||||
unsigned char *pData,
|
||||
MV_FRAME_OUT_INFO_EX *pFrameInfo,
|
||||
void *pUser) {
|
||||
auto node = reinterpret_cast<HikvisionDriver *>(pUser);
|
||||
|
||||
const auto host_rx_time = node->get_clock()->now();
|
||||
const uint64_t raw_dev_tick =
|
||||
(static_cast<uint64_t>(pFrameInfo->nDevTimeStampHigh) << 32ull) |
|
||||
static_cast<uint64_t>(pFrameInfo->nDevTimeStampLow);
|
||||
const uint32_t converted_device_tick_ms = node->pImpl->device_tick_ms(raw_dev_tick);
|
||||
const auto header_stamp =
|
||||
node->pImpl->compute_header_stamp(node, *pFrameInfo, host_rx_time, converted_device_tick_ms, raw_dev_tick);
|
||||
|
||||
auto p_img_msg = std::make_unique<sensor_msgs::msg::Image>();
|
||||
p_img_msg->header.frame_id = node->pImpl->camera_name;
|
||||
p_img_msg->header.stamp = ToBuiltinTime(header_stamp);
|
||||
p_img_msg->is_bigendian = false;
|
||||
p_img_msg->width = pFrameInfo->nWidth;
|
||||
p_img_msg->height = pFrameInfo->nHeight;
|
||||
|
||||
RCLCPP_INFO_THROTTLE(
|
||||
node->get_logger(),
|
||||
*node->get_clock(),
|
||||
2000,
|
||||
"pixel format=%d (0x%08X), width=%u, height=%u, frame_len=%u",
|
||||
static_cast<int>(pFrameInfo->enPixelType),
|
||||
static_cast<unsigned int>(pFrameInfo->enPixelType),
|
||||
pFrameInfo->nWidth,
|
||||
pFrameInfo->nHeight,
|
||||
pFrameInfo->nFrameLen);
|
||||
|
||||
auto direct_copy = [&](const std::string &encoding, size_t bytes_per_pixel) -> bool {
|
||||
const size_t expected_size =
|
||||
static_cast<size_t>(pFrameInfo->nWidth) *
|
||||
static_cast<size_t>(pFrameInfo->nHeight) *
|
||||
bytes_per_pixel;
|
||||
|
||||
if (pFrameInfo->nFrameLen < expected_size) {
|
||||
RCLCPP_ERROR(
|
||||
node->get_logger(),
|
||||
"frame too small: nFrameLen=%u < expected=%zu",
|
||||
pFrameInfo->nFrameLen,
|
||||
expected_size);
|
||||
return false;
|
||||
}
|
||||
|
||||
p_img_msg->encoding = encoding;
|
||||
p_img_msg->step = static_cast<sensor_msgs::msg::Image::_step_type>(
|
||||
pFrameInfo->nWidth * bytes_per_pixel);
|
||||
p_img_msg->data.resize(expected_size);
|
||||
std::copy_n(pData, expected_size, p_img_msg->data.data());
|
||||
return true;
|
||||
};
|
||||
|
||||
auto convert_to_bgr8 = [&]() -> bool {
|
||||
const size_t dst_size =
|
||||
static_cast<size_t>(pFrameInfo->nWidth) *
|
||||
static_cast<size_t>(pFrameInfo->nHeight) * 3u;
|
||||
|
||||
std::vector<uint8_t> bgr_buf(dst_size);
|
||||
|
||||
MV_CC_PIXEL_CONVERT_PARAM stConvertParam;
|
||||
memset(&stConvertParam, 0, sizeof(stConvertParam));
|
||||
stConvertParam.nWidth = pFrameInfo->nWidth;
|
||||
stConvertParam.nHeight = pFrameInfo->nHeight;
|
||||
stConvertParam.pSrcData = pData;
|
||||
stConvertParam.nSrcDataLen = pFrameInfo->nFrameLen;
|
||||
stConvertParam.enSrcPixelType = pFrameInfo->enPixelType;
|
||||
stConvertParam.enDstPixelType = PixelType_Gvsp_BGR8_Packed;
|
||||
stConvertParam.pDstBuffer = bgr_buf.data();
|
||||
stConvertParam.nDstBufferSize = static_cast<unsigned int>(bgr_buf.size());
|
||||
|
||||
const int nRet = MV_CC_ConvertPixelType(node->pImpl->handle, &stConvertParam);
|
||||
if (nRet != MV_OK) {
|
||||
RCLCPP_ERROR(
|
||||
node->get_logger(),
|
||||
"MV_CC_ConvertPixelType failed: %d, src pixel=%d (0x%08X)",
|
||||
nRet,
|
||||
static_cast<int>(pFrameInfo->enPixelType),
|
||||
static_cast<unsigned int>(pFrameInfo->enPixelType));
|
||||
return false;
|
||||
}
|
||||
|
||||
p_img_msg->encoding = sensor_msgs::image_encodings::BGR8;
|
||||
p_img_msg->step =
|
||||
static_cast<sensor_msgs::msg::Image::_step_type>(pFrameInfo->nWidth * 3u);
|
||||
p_img_msg->data = std::move(bgr_buf);
|
||||
return true;
|
||||
};
|
||||
|
||||
bool ok = false;
|
||||
switch (pFrameInfo->enPixelType) {
|
||||
case PixelType_Gvsp_Mono8:
|
||||
ok = direct_copy(sensor_msgs::image_encodings::MONO8, 1u);
|
||||
break;
|
||||
|
||||
case PixelType_Gvsp_RGB8_Packed:
|
||||
ok = direct_copy(sensor_msgs::image_encodings::RGB8, 3u);
|
||||
break;
|
||||
|
||||
case PixelType_Gvsp_BGR8_Packed:
|
||||
ok = direct_copy(sensor_msgs::image_encodings::BGR8, 3u);
|
||||
break;
|
||||
|
||||
// 8-bit Bayer：直接转 BGR8，避免下游再 debayer
|
||||
case PixelType_Gvsp_BayerRG8:
|
||||
case PixelType_Gvsp_BayerBG8:
|
||||
case PixelType_Gvsp_BayerGR8:
|
||||
case PixelType_Gvsp_BayerGB8:
|
||||
ok = convert_to_bgr8();
|
||||
break;
|
||||
|
||||
// 10-bit packed Bayer：也统一转 BGR8
|
||||
case PixelType_Gvsp_BayerRG10_Packed:
|
||||
case PixelType_Gvsp_BayerBG10_Packed:
|
||||
case PixelType_Gvsp_BayerGR10_Packed:
|
||||
case PixelType_Gvsp_BayerGB10_Packed:
|
||||
ok = convert_to_bgr8();
|
||||
break;
|
||||
|
||||
default:
|
||||
RCLCPP_ERROR_ONCE(
|
||||
node->get_logger(),
|
||||
"unsupported pixel format: %d (0x%08X)",
|
||||
static_cast<int>(pFrameInfo->enPixelType),
|
||||
static_cast<unsigned int>(pFrameInfo->enPixelType));
|
||||
return;
|
||||
}
|
||||
|
||||
if (!ok) {
|
||||
return;
|
||||
}
|
||||
|
||||
auto p_info_msg = std::make_unique<hikvision_interface::msg::HikImageInfo>();
|
||||
p_info_msg->header.frame_id = node->pImpl->camera_name;
|
||||
p_info_msg->header.stamp = ToBuiltinTime(header_stamp);
|
||||
if (const auto device_stamp = node->pImpl->convert_device_stamp_to_ros_time(raw_dev_tick)) {
|
||||
p_info_msg->dev_stamp = ToBuiltinTime(*device_stamp);
|
||||
}
|
||||
p_info_msg->frame_num = pFrameInfo->nFrameNum;
|
||||
p_info_msg->gain = pFrameInfo->fGain;
|
||||
p_info_msg->exposure = pFrameInfo->fExposureTime;
|
||||
p_info_msg->red = pFrameInfo->nRed;
|
||||
p_info_msg->green = pFrameInfo->nGreen;
|
||||
p_info_msg->blue = pFrameInfo->nBlue;
|
||||
|
||||
node->pImpl->img_pub.publish(std::move(p_img_msg));
|
||||
node->pImpl->p_info_pub->publish(std::move(p_info_msg));
|
||||
}
|
||||
|
||||
HikvisionDriver::HikvisionDriver(const rclcpp::NodeOptions &options)
|
||||
: rclcpp::Node("hikvision_ros2_driver_node", options), pImpl(std::make_unique<Impl>()) {
|
||||
auto logger = get_logger();
|
||||
pImpl->logger = std::make_unique<rclcpp::Logger>(logger);
|
||||
|
||||
declare_parameter<std::string>("camera_name", "");
|
||||
declare_parameter<std::string>("sync_mode", "freerun");
|
||||
declare_parameter<std::string>("trigger_activation", "rising_edge");
|
||||
declare_parameter<double>("trigger_delay_us", 0.0);
|
||||
declare_parameter<double>("device_timestamp_tick_hz", 0.0);
|
||||
declare_parameter<bool>("use_timestamp_sync", true);
|
||||
declare_parameter<double>("time_offset_sec", 0.0);
|
||||
declare_parameter<double>("bias_alpha", 0.01);
|
||||
declare_parameter<int>("jump_warn_threshold_ms", 1000);
|
||||
declare_parameter<bool>("clamp_backward_time", true);
|
||||
declare_parameter<bool>("timestamp_diagnostics", false);
|
||||
declare_parameter<int>("timestamp_diagnostics_throttle_ms", 2000);
|
||||
|
||||
pImpl->camera_name = get_parameter("camera_name").as_string();
|
||||
pImpl->sync_mode = get_parameter("sync_mode").as_string();
|
||||
pImpl->trigger_activation = get_parameter("trigger_activation").as_string();
|
||||
pImpl->trigger_delay_us = get_parameter("trigger_delay_us").as_double();
|
||||
pImpl->device_timestamp_tick_hz = get_parameter("device_timestamp_tick_hz").as_double();
|
||||
pImpl->use_timestamp_sync = get_parameter("use_timestamp_sync").as_bool();
|
||||
pImpl->timestamp_diagnostics = get_parameter("timestamp_diagnostics").as_bool();
|
||||
pImpl->timestamp_diagnostics_throttle_ms = get_parameter("timestamp_diagnostics_throttle_ms").as_int();
|
||||
|
||||
driver_timestamp_sync::TimestampSyncConfig config;
|
||||
config.time_offset_sec = get_parameter("time_offset_sec").as_double();
|
||||
config.bias_alpha = get_parameter("bias_alpha").as_double();
|
||||
config.jump_warn_threshold_ms = static_cast<uint32_t>(get_parameter("jump_warn_threshold_ms").as_int());
|
||||
config.clamp_backward_time = get_parameter("clamp_backward_time").as_bool();
|
||||
pImpl->timestamp_sync.set_config(config);
|
||||
|
||||
if (pImpl->camera_name.empty()) {
|
||||
throw std::runtime_error("camera_name parameter must not be empty");
|
||||
}
|
||||
|
||||
RCLCPP_INFO(logger, "trying to open camera: '%s'", pImpl->camera_name.c_str());
|
||||
RCLCPP_INFO(
|
||||
logger,
|
||||
"timestamp_sync: enabled=%s offset=%.6f alpha=%.3f jump_warn_ms=%u clamp_backward=%s diagnostics=%s throttle_ms=%d",
|
||||
pImpl->use_timestamp_sync ? "true" : "false",
|
||||
config.time_offset_sec,
|
||||
config.bias_alpha,
|
||||
config.jump_warn_threshold_ms,
|
||||
config.clamp_backward_time ? "true" : "false",
|
||||
pImpl->timestamp_diagnostics ? "true" : "false",
|
||||
pImpl->timestamp_diagnostics_throttle_ms);
|
||||
|
||||
rclcpp::QoS qos(1);
|
||||
pImpl->img_pub = image_transport::create_publisher(this, "raw/image", qos.get_rmw_qos_profile());
|
||||
pImpl->p_info_pub = create_publisher<HikImageInfo>("info", qos);
|
||||
|
||||
MV_CC_DEVICE_INFO_LIST stDeviceList;
|
||||
memset(&stDeviceList, 0, sizeof(MV_CC_DEVICE_INFO_LIST));
|
||||
MV_CHECK_THROW(logger, MV_CC_EnumDevices, MV_GIGE_DEVICE | MV_USB_DEVICE, &stDeviceList);
|
||||
|
||||
MV_CC_DEVICE_INFO * matched_device = nullptr;
|
||||
for (uint32_t nDeviceId = 0; nDeviceId < stDeviceList.nDeviceNum; nDeviceId++) {
|
||||
auto *pDeviceInfo = stDeviceList.pDeviceInfo[nDeviceId];
|
||||
std::string user_defined_name;
|
||||
if (pDeviceInfo->nTLayerType == MV_GIGE_DEVICE) {
|
||||
user_defined_name = reinterpret_cast<const char *>(pDeviceInfo->SpecialInfo.stGigEInfo.chUserDefinedName);
|
||||
if (user_defined_name == pImpl->camera_name) {
|
||||
int nIp1 = ((pDeviceInfo->SpecialInfo.stGigEInfo.nCurrentIp & 0xff000000) >> 24);
|
||||
int nIp2 = ((pDeviceInfo->SpecialInfo.stGigEInfo.nCurrentIp & 0x00ff0000) >> 16);
|
||||
int nIp3 = ((pDeviceInfo->SpecialInfo.stGigEInfo.nCurrentIp & 0x0000ff00) >> 8);
|
||||
int nIp4 = (pDeviceInfo->SpecialInfo.stGigEInfo.nCurrentIp & 0x000000ff);
|
||||
RCLCPP_INFO(
|
||||
logger,
|
||||
"[%s]: GIGE, %s, %d.%d.%d.%d",
|
||||
user_defined_name.c_str(),
|
||||
pDeviceInfo->SpecialInfo.stGigEInfo.chModelName,
|
||||
nIp1,
|
||||
nIp2,
|
||||
nIp3,
|
||||
nIp4);
|
||||
matched_device = pDeviceInfo;
|
||||
break;
|
||||
}
|
||||
} else if (pDeviceInfo->nTLayerType == MV_USB_DEVICE) {
|
||||
user_defined_name = reinterpret_cast<const char *>(pDeviceInfo->SpecialInfo.stUsb3VInfo.chUserDefinedName);
|
||||
if (user_defined_name == pImpl->camera_name) {
|
||||
RCLCPP_INFO(
|
||||
logger,
|
||||
"[%s]: USB, %s",
|
||||
user_defined_name.c_str(),
|
||||
pDeviceInfo->SpecialInfo.stUsb3VInfo.chModelName);
|
||||
matched_device = pDeviceInfo;
|
||||
break;
|
||||
}
|
||||
} else {
|
||||
RCLCPP_WARN(logger, "type(%d) not support", pDeviceInfo->nTLayerType);
|
||||
}
|
||||
}
|
||||
|
||||
if (matched_device == nullptr) {
|
||||
throw std::runtime_error("camera not found: " + pImpl->camera_name);
|
||||
}
|
||||
|
||||
MV_CHECK_THROW(logger, MV_CC_CreateHandle, &pImpl->handle, matched_device);
|
||||
try {
|
||||
MV_CHECK_THROW(logger, MV_CC_OpenDevice, pImpl->handle);
|
||||
pImpl->device_timestamp_tick_hz = pImpl->resolve_device_timestamp_tick_hz(this, matched_device);
|
||||
pImpl->configure_trigger(this);
|
||||
MV_CHECK_THROW(logger, MV_CC_RegisterImageCallBackEx, pImpl->handle, &HikvisionDriver::Impl::image_callback_ex,
|
||||
this);
|
||||
MV_CHECK_THROW(logger, MV_CC_StartGrabbing, pImpl->handle);
|
||||
} catch (...) {
|
||||
if (pImpl->handle != nullptr) {
|
||||
MV_CC_CloseDevice(pImpl->handle);
|
||||
MV_CC_DestroyHandle(pImpl->handle);
|
||||
pImpl->handle = nullptr;
|
||||
}
|
||||
throw;
|
||||
}
|
||||
}
|
||||
|
||||
HikvisionDriver::~HikvisionDriver() {
|
||||
if (pImpl->handle == nullptr) return;
|
||||
auto logger = get_logger();
|
||||
|
||||
int nRet = MV_CC_StopGrabbing(pImpl->handle);
|
||||
if (MV_OK != nRet) {
|
||||
RCLCPP_ERROR(logger, "hikvision sdk error: MV_CC_StopGrabbing = %d", nRet);
|
||||
}
|
||||
nRet = MV_CC_CloseDevice(pImpl->handle);
|
||||
if (MV_OK != nRet) {
|
||||
RCLCPP_ERROR(logger, "hikvision sdk error: MV_CC_CloseDevice = %d", nRet);
|
||||
}
|
||||
nRet = MV_CC_DestroyHandle(pImpl->handle);
|
||||
if (MV_OK != nRet) {
|
||||
RCLCPP_ERROR(logger, "hikvision sdk error: MV_CC_DestroyHandle = %d", nRet);
|
||||
}
|
||||
pImpl->handle = nullptr;
|
||||
}
|
||||
|
||||
} // namespace hikvision_ros2_driver
|
||||
|
||||
#include "rclcpp_components/register_node_macro.hpp"
|
||||
RCLCPP_COMPONENTS_REGISTER_NODE(hikvision_ros2_driver::HikvisionDriver);
|
||||
Reference in New Issue
Block a user