forked from pz4kybsvg/Conception
parent
99d52681f2
commit
3edd83c708
@ -1,44 +0,0 @@
|
||||
# compilation and distribution
|
||||
__pycache__
|
||||
_ext
|
||||
*.pyc
|
||||
nano.save
|
||||
.catkin_workspace
|
||||
|
||||
build/
|
||||
devel/
|
||||
dist/
|
||||
.vscode/
|
||||
Modules/object_detection_landing/
|
||||
Modules/object_detection_persistence/
|
||||
Modules/object_detection_yolov5openvino/
|
||||
Modules/object_detection_yolov5tensorrt/
|
||||
Modules/object_detection_circlex/
|
||||
Modules/data/
|
||||
Modules/object_detection_oneshot/
|
||||
|
||||
|
||||
# pytorch/python/numpy formats
|
||||
# *.pth
|
||||
*.pkl
|
||||
*.npy
|
||||
|
||||
# ipython/jupyter notebooks
|
||||
*.ipynb
|
||||
**/.ipynb_checkpoints/
|
||||
|
||||
# Editor temporaries
|
||||
*.swn
|
||||
*.swo
|
||||
*.swp
|
||||
*~
|
||||
|
||||
# Pycharm editor settings
|
||||
.idea
|
||||
|
||||
# project dirs
|
||||
/datasets
|
||||
/models
|
||||
|
||||
ORBvoc.txt
|
||||
|
||||
@ -1,6 +0,0 @@
|
||||
[submodule "Modules/swarm_control"]
|
||||
path = Modules/swarm_control
|
||||
url = https://gitee.com/amovlab/swarm_control.git
|
||||
[submodule "Modules/matlab_bridge"]
|
||||
path = Modules/matlab_bridge
|
||||
url = https://gitee.com/amovlab1/matlab_bridge.git
|
||||
@ -1,118 +0,0 @@
|
||||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(prometheus_experiment)
|
||||
|
||||
## Compile as C++11, supported in ROS Kinetic and newer
|
||||
# add_compile_options(-std=c++11)
|
||||
|
||||
## Find catkin macros and libraries
|
||||
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
|
||||
## is used, also find other catkin packages
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
message_generation
|
||||
roscpp
|
||||
geometry_msgs
|
||||
nav_msgs
|
||||
sensor_msgs
|
||||
mavros
|
||||
std_msgs
|
||||
std_srvs
|
||||
tf2_ros
|
||||
tf2_eigen
|
||||
mavros_msgs
|
||||
prometheus_msgs
|
||||
)
|
||||
|
||||
## System dependencies are found with CMake's conventions
|
||||
find_package(Boost REQUIRED COMPONENTS system)
|
||||
|
||||
|
||||
|
||||
################################################
|
||||
## Declare ROS messages, services and actions ##
|
||||
################################################
|
||||
|
||||
## Generate added messages and services with any dependencies listed here
|
||||
generate_messages(
|
||||
DEPENDENCIES
|
||||
geometry_msgs
|
||||
nav_msgs
|
||||
sensor_msgs
|
||||
std_msgs
|
||||
)
|
||||
|
||||
|
||||
|
||||
###########
|
||||
## Build ##
|
||||
###########
|
||||
|
||||
## Specify additional locations of header files
|
||||
## Your package locations should be listed before other locations
|
||||
include_directories(
|
||||
include
|
||||
${catkin_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
|
||||
###############################
|
||||
## 声明可执行cpp文件 ##
|
||||
###############################
|
||||
|
||||
###### Main File ##########
|
||||
|
||||
##px4_pos_controller.cpp
|
||||
#add_executable(px4_pos_controller src/px4_pos_controller.cpp)
|
||||
#add_dependencies(px4_pos_controller prometheus_control_gencpp)
|
||||
#target_link_libraries(px4_pos_controller ${catkin_LIBRARIES})
|
||||
|
||||
#############
|
||||
## Install ##
|
||||
#############
|
||||
|
||||
# all install targets should use catkin DESTINATION variables
|
||||
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
|
||||
|
||||
## Mark executable scripts (Python etc.) for installation
|
||||
## in contrast to setup.py, you can choose the destination
|
||||
# install(PROGRAMS
|
||||
# scripts/my_python_script
|
||||
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
# )
|
||||
|
||||
## Mark executables and/or libraries for installation
|
||||
# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
|
||||
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
# )
|
||||
|
||||
## Mark cpp header files for installation
|
||||
# install(DIRECTORY include/${PROJECT_NAME}/
|
||||
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
|
||||
# FILES_MATCHING PATTERN "*.h"
|
||||
# PATTERN ".svn" EXCLUDE
|
||||
# )
|
||||
|
||||
## Mark cpp header files for installation
|
||||
install(DIRECTORY include/${PROJECT_NAME}/
|
||||
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
|
||||
FILES_MATCHING PATTERN "*.h"
|
||||
)
|
||||
|
||||
## Mark other files for installation (e.g. launch and bag files, etc.)
|
||||
install(DIRECTORY launch/
|
||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
|
||||
)
|
||||
|
||||
#############
|
||||
## Testing ##
|
||||
#############
|
||||
|
||||
## Add gtest based cpp test target and link libraries
|
||||
# catkin_add_gtest(${PROJECT_NAME}-test test/test_prometheus_control.cpp)
|
||||
# if(TARGET ${PROJECT_NAME}-test)
|
||||
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
|
||||
# endif()
|
||||
|
||||
## Add folders to be run by python nosetests
|
||||
# catkin_add_nosetests(test)
|
||||
@ -1,260 +0,0 @@
|
||||
# Common configuration for PX4 autopilot
|
||||
#
|
||||
# node:
|
||||
startup_px4_usb_quirk: true
|
||||
|
||||
# --- system plugins ---
|
||||
|
||||
# sys_status & sys_time connection options
|
||||
conn:
|
||||
heartbeat_rate: 1.0 # send hertbeat rate in Hertz
|
||||
timeout: 10.0 # hertbeat timeout in seconds
|
||||
timesync_rate: 10.0 # TIMESYNC rate in Hertz (feature disabled if 0.0)
|
||||
system_time_rate: 1.0 # send system time to FCU rate in Hertz (disabled if 0.0)
|
||||
|
||||
# sys_status
|
||||
sys:
|
||||
min_voltage: 10.0 # diagnostics min voltage
|
||||
disable_diag: false # disable all sys_status diagnostics, except heartbeat
|
||||
|
||||
# sys_time
|
||||
time:
|
||||
time_ref_source: "fcu" # time_reference source
|
||||
timesync_mode: MAVLINK
|
||||
timesync_avg_alpha: 0.6 # timesync averaging factor
|
||||
|
||||
# --- mavros plugins (alphabetical order) ---
|
||||
|
||||
# 3dr_radio
|
||||
tdr_radio:
|
||||
low_rssi: 40 # raw rssi lower level for diagnostics
|
||||
|
||||
# actuator_control
|
||||
# None
|
||||
|
||||
# command
|
||||
cmd:
|
||||
use_comp_id_system_control: false # quirk for some old FCUs
|
||||
|
||||
# dummy
|
||||
# None
|
||||
|
||||
# ftp
|
||||
# None
|
||||
|
||||
# global_position
|
||||
global_position:
|
||||
frame_id: "map" # origin frame
|
||||
child_frame_id: "base_link" # body-fixed frame
|
||||
rot_covariance: 99999.0 # covariance for attitude?
|
||||
gps_uere: 1.0 # User Equivalent Range Error (UERE) of GPS sensor (m)
|
||||
use_relative_alt: true # use relative altitude for local coordinates
|
||||
tf:
|
||||
send: false # send TF?
|
||||
frame_id: "map" # TF frame_id
|
||||
global_frame_id: "earth" # TF earth frame_id
|
||||
child_frame_id: "base_link" # TF child_frame_id
|
||||
|
||||
# imu_pub
|
||||
imu:
|
||||
frame_id: "base_link"
|
||||
# need find actual values
|
||||
linear_acceleration_stdev: 0.0003
|
||||
angular_velocity_stdev: 0.0003490659 // 0.02 degrees
|
||||
orientation_stdev: 1.0
|
||||
magnetic_stdev: 0.0
|
||||
|
||||
# local_position
|
||||
local_position:
|
||||
frame_id: "world"
|
||||
tf:
|
||||
send: true
|
||||
frame_id: "world"
|
||||
child_frame_id: "base_link"
|
||||
send_fcu: false
|
||||
|
||||
# param
|
||||
# None, used for FCU params
|
||||
|
||||
# rc_io
|
||||
# None
|
||||
|
||||
# safety_area
|
||||
safety_area:
|
||||
p1: {x: 1.0, y: 1.0, z: 1.0}
|
||||
p2: {x: -1.0, y: -1.0, z: -1.0}
|
||||
|
||||
# setpoint_accel
|
||||
setpoint_accel:
|
||||
send_force: false
|
||||
|
||||
# setpoint_attitude
|
||||
setpoint_attitude:
|
||||
reverse_thrust: false # allow reversed thrust
|
||||
use_quaternion: false # enable PoseStamped topic subscriber
|
||||
tf:
|
||||
listen: false # enable tf listener (disable topic subscribers)
|
||||
frame_id: "map"
|
||||
child_frame_id: "target_attitude"
|
||||
rate_limit: 50.0
|
||||
|
||||
setpoint_raw:
|
||||
thrust_scaling: 1.0 # used in setpoint_raw attitude callback.
|
||||
# Note: PX4 expects normalized thrust values between 0 and 1, which means that
|
||||
# the scaling needs to be unitary and the inputs should be 0..1 as well.
|
||||
|
||||
# setpoint_position
|
||||
setpoint_position:
|
||||
tf:
|
||||
listen: false # enable tf listener (disable topic subscribers)
|
||||
frame_id: "map"
|
||||
child_frame_id: "target_position"
|
||||
rate_limit: 50.0
|
||||
mav_frame: LOCAL_NED
|
||||
|
||||
# setpoint_velocity
|
||||
setpoint_velocity:
|
||||
mav_frame: LOCAL_NED
|
||||
|
||||
# vfr_hud
|
||||
# None
|
||||
|
||||
# waypoint
|
||||
mission:
|
||||
pull_after_gcs: true # update mission if gcs updates
|
||||
|
||||
# --- mavros extras plugins (same order) ---
|
||||
|
||||
# adsb
|
||||
# None
|
||||
|
||||
# debug_value
|
||||
# None
|
||||
|
||||
# distance_sensor
|
||||
## Currently available orientations:
|
||||
# Check http://wiki.ros.org/mavros/Enumerations
|
||||
##
|
||||
distance_sensor:
|
||||
hrlv_ez4_pub:
|
||||
id: 0
|
||||
frame_id: "hrlv_ez4_sonar"
|
||||
orientation: PITCH_270 # RPY:{0.0, 270.0, 0.0} - downward-facing
|
||||
field_of_view: 0.0 # XXX TODO
|
||||
send_tf: true
|
||||
sensor_position: {x: 0.0, y: 0.0, z: -0.1}
|
||||
lidarlite_pub:
|
||||
id: 1
|
||||
frame_id: "lidarlite_laser"
|
||||
orientation: PITCH_270
|
||||
field_of_view: 0.0 # XXX TODO
|
||||
send_tf: true
|
||||
sensor_position: {x: 0.0, y: 0.0, z: -0.1}
|
||||
sonar_1_sub:
|
||||
subscriber: true
|
||||
id: 2
|
||||
orientation: PITCH_270
|
||||
laser_1_sub:
|
||||
subscriber: true
|
||||
id: 3
|
||||
orientation: PITCH_270
|
||||
|
||||
# image_pub
|
||||
image:
|
||||
frame_id: "px4flow"
|
||||
|
||||
# fake_gps
|
||||
fake_gps:
|
||||
# select data source
|
||||
use_mocap: true # ~mocap/pose
|
||||
mocap_transform: true # ~mocap/tf instead of pose
|
||||
use_vision: false # ~vision (pose)
|
||||
# origin (default: Zürich)
|
||||
geo_origin:
|
||||
lat: 47.3667 # latitude [degrees]
|
||||
lon: 8.5500 # longitude [degrees]
|
||||
alt: 408.0 # altitude (height over the WGS-84 ellipsoid) [meters]
|
||||
eph: 2.0
|
||||
epv: 2.0
|
||||
satellites_visible: 5 # virtual number of visible satellites
|
||||
fix_type: 3 # type of GPS fix (default: 3D)
|
||||
tf:
|
||||
listen: false
|
||||
send: false # send TF?
|
||||
frame_id: "map" # TF frame_id
|
||||
child_frame_id: "fix" # TF child_frame_id
|
||||
rate_limit: 10.0 # TF rate
|
||||
gps_rate: 5.0 # GPS data publishing rate
|
||||
|
||||
# landing_target
|
||||
landing_target:
|
||||
listen_lt: false
|
||||
mav_frame: "LOCAL_NED"
|
||||
land_target_type: "VISION_FIDUCIAL"
|
||||
image:
|
||||
width: 640 # [pixels]
|
||||
height: 480
|
||||
camera:
|
||||
fov_x: 2.0071286398 # default: 115 [degrees]
|
||||
fov_y: 2.0071286398
|
||||
tf:
|
||||
send: true
|
||||
listen: false
|
||||
frame_id: "landing_target"
|
||||
child_frame_id: "camera_center"
|
||||
rate_limit: 10.0
|
||||
target_size: {x: 0.3, y: 0.3}
|
||||
|
||||
# mocap_pose_estimate
|
||||
mocap:
|
||||
# select mocap source
|
||||
use_tf: false # ~mocap/tf
|
||||
use_pose: true # ~mocap/pose
|
||||
|
||||
# odom
|
||||
odometry:
|
||||
fcu:
|
||||
odom_parent_id_des: "map" # desired parent frame rotation of the FCU's odometry
|
||||
odom_child_id_des: "base_link" # desired child frame rotation of the FCU's odometry
|
||||
|
||||
# px4flow
|
||||
px4flow:
|
||||
frame_id: "px4flow"
|
||||
ranger_fov: 0.118682 # 6.8 degrees at 5 meters, 31 degrees at 1 meter
|
||||
ranger_min_range: 0.3 # meters
|
||||
ranger_max_range: 5.0 # meters
|
||||
|
||||
# vision_pose_estimate
|
||||
vision_pose:
|
||||
tf:
|
||||
listen: false # enable tf listener (disable topic subscribers)
|
||||
frame_id: "map"
|
||||
child_frame_id: "vision_estimate"
|
||||
rate_limit: 10.0
|
||||
|
||||
# vision_speed_estimate
|
||||
vision_speed:
|
||||
listen_twist: true # enable listen to twist topic, else listen to vec3d topic
|
||||
twist_cov: true # enable listen to twist with covariance topic
|
||||
|
||||
# vibration
|
||||
vibration:
|
||||
frame_id: "base_link"
|
||||
|
||||
# wheel_odometry
|
||||
wheel_odometry:
|
||||
count: 2 # number of wheels to compute odometry
|
||||
use_rpm: false # use wheel's RPM instead of cumulative distance to compute odometry
|
||||
wheel0: {x: 0.0, y: -0.15, radius: 0.05} # x-, y-offset (m,NED) and radius (m)
|
||||
wheel1: {x: 0.0, y: 0.15, radius: 0.05} # x-, y-offset (m,NED) and radius (m)
|
||||
send_raw: true # send wheel's RPM and cumulative distance (~/wheel_odometry/rpm, ~/wheel_odometry/distance)
|
||||
send_twist: false # send geometry_msgs/TwistWithCovarianceStamped instead of nav_msgs/Odometry
|
||||
frame_id: "map" # origin frame
|
||||
child_frame_id: "base_link" # body-fixed frame
|
||||
vel_error: 0.1 # wheel velocity measurement error 1-std (m/s)
|
||||
tf:
|
||||
send: false
|
||||
frame_id: "map"
|
||||
child_frame_id: "base_link"
|
||||
|
||||
# vim:set ts=2 sw=2 et:
|
||||
@ -1,33 +0,0 @@
|
||||
plugin_blacklist:
|
||||
- actuator_control
|
||||
- adsb
|
||||
- safety_area
|
||||
- 3dr_radio
|
||||
- cam_imu_sync
|
||||
- altitude
|
||||
- distance_sensor
|
||||
- fake_gps
|
||||
- gps_rtk
|
||||
- hil
|
||||
- home_position
|
||||
- landing_target
|
||||
- mocap_pose_estimate
|
||||
- mount_control
|
||||
- obstacle_distance
|
||||
- rc_io
|
||||
- vfr_hud
|
||||
- waypoint
|
||||
- wind_estimation
|
||||
- px4flow
|
||||
- global_position
|
||||
- companion_process_status
|
||||
- debug_value
|
||||
- wheel_odometry
|
||||
- vibration
|
||||
- odom
|
||||
- setpoint_attitude
|
||||
- setpoint_position
|
||||
- setpoint_accel
|
||||
- setpoint_velocity
|
||||
plugin_whitelist: []
|
||||
#- 'sys_*'
|
||||
@ -1,29 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>prometheus_experiment</name>
|
||||
<version>0.0.0</version>
|
||||
<description>The prometheus_experiment package</description>
|
||||
|
||||
<!-- One maintainer tag required, multiple allowed, one person per tag -->
|
||||
<maintainer email="qiyh8@mail.sysu.edu.cn">Yuhua Qi</maintainer>
|
||||
<author email="fatmoonqyp@126.com">Yuhua Qi</author>
|
||||
<license>TODO</license>
|
||||
|
||||
<url type="website">https://www.amovlab.com</url>
|
||||
<url type="repository">https://github.com/potato77/prometheus_experiment.git</url>
|
||||
|
||||
<build_depend>message_generation</build_depend>
|
||||
<exec_depend>message_runtime</exec_depend>
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<build_depend>std_msgs</build_depend>
|
||||
<build_export_depend>std_msgs</build_export_depend>
|
||||
<exec_depend>std_msgs</exec_depend>
|
||||
|
||||
|
||||
|
||||
|
||||
<export>
|
||||
<!-- Other tools can request additional information be placed here -->
|
||||
</export>
|
||||
</package>
|
||||
@ -1,60 +0,0 @@
|
||||
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
|
||||
|
||||
Copyright 2022 AMOVLAB
|
||||
|
||||
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.
|
||||
@ -1,145 +0,0 @@
|
||||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(prometheus_future_aircraft)
|
||||
|
||||
## Compile as C++11, supported in ROS Kinetic and newer
|
||||
# add_compile_options(-std=c++11)
|
||||
add_compile_options(-std=c++17)
|
||||
|
||||
## Find catkin macros and libraries
|
||||
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
|
||||
## is used, also find other catkin packages
|
||||
find_package(OpenCV REQUIRED)
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
message_generation
|
||||
roscpp
|
||||
rospy
|
||||
geometry_msgs
|
||||
sensor_msgs
|
||||
mavros
|
||||
nav_msgs
|
||||
std_msgs
|
||||
std_srvs
|
||||
tf2_ros
|
||||
tf2_eigen
|
||||
mavros_msgs
|
||||
prometheus_msgs
|
||||
cv_bridge
|
||||
image_transport
|
||||
)
|
||||
|
||||
## System dependencies are found with CMake's conventions
|
||||
find_package(Boost REQUIRED COMPONENTS system)
|
||||
|
||||
|
||||
################################################
|
||||
## Declare ROS messages, services and actions ##
|
||||
################################################
|
||||
|
||||
## Generate added messages and services with any dependencies listed here
|
||||
generate_messages(
|
||||
DEPENDENCIES
|
||||
geometry_msgs
|
||||
sensor_msgs
|
||||
std_msgs
|
||||
)
|
||||
|
||||
catkin_package(
|
||||
CATKIN_DEPENDS message_runtime
|
||||
)
|
||||
|
||||
|
||||
###########
|
||||
## Build ##
|
||||
###########
|
||||
|
||||
## Specify additional locations of header files
|
||||
## Your package locations should be listed before other locations
|
||||
include_directories(
|
||||
include ../tutorial_demo/advanced/autonomous_landing/include
|
||||
${catkin_INCLUDE_DIRS}
|
||||
../common/include
|
||||
)
|
||||
|
||||
|
||||
###############################
|
||||
## 声明可执行cpp文件 ##
|
||||
###############################
|
||||
# add_executable(autonomous_landing autonomous_landing/autonomous_landing.cpp)
|
||||
# target_link_libraries(autonomous_landing ${catkin_LIBRARIES})
|
||||
|
||||
#add_executable(waypoint_tracking waypoint_tracking/waypoint_tracking.cpp)
|
||||
#target_link_libraries(waypoint_tracking ${catkin_LIBRARIES})
|
||||
|
||||
#add_executable(object_tracking object_tracking/object_tracking.cpp)
|
||||
#target_link_libraries(object_tracking ${catkin_LIBRARIES})
|
||||
|
||||
#add_executable(takeoff takeoff/takeoff.cpp)
|
||||
#target_link_libraries(takeoff ${catkin_LIBRARIES})
|
||||
|
||||
#add_executable(formation_control formation_control/formation_control.cpp)
|
||||
#target_link_libraries(formation_control ${catkin_LIBRARIES})
|
||||
add_executable(future_aircraft src/future_aircraft.cpp)
|
||||
target_link_libraries(future_aircraft ${catkin_LIBRARIES})
|
||||
|
||||
add_executable(ellipse_det
|
||||
src/ellipse_det.cpp
|
||||
src/ellipse_detector/ellipse_detector.cpp
|
||||
)
|
||||
target_include_directories(ellipse_det PUBLIC
|
||||
src/ellipse_detector/
|
||||
)
|
||||
target_link_libraries(ellipse_det ${catkin_LIBRARIES} ${OpenCV_LIBS} yaml-cpp)
|
||||
|
||||
|
||||
#############
|
||||
## Install ##
|
||||
#############
|
||||
|
||||
# all install targets should use catkin DESTINATION variables
|
||||
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
|
||||
|
||||
## Mark executable scripts (Python etc.) for installation
|
||||
## in contrast to setup.py, you can choose the destination
|
||||
install(PROGRAMS
|
||||
scripts/locus.py
|
||||
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
)
|
||||
|
||||
## Mark executables and/or libraries for installation
|
||||
# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
|
||||
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
# )
|
||||
|
||||
## Mark cpp header files for installation
|
||||
# install(DIRECTORY include/${PROJECT_NAME}/
|
||||
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
|
||||
# FILES_MATCHING PATTERN "*.h"
|
||||
# PATTERN ".svn" EXCLUDE
|
||||
# )
|
||||
|
||||
## Mark cpp header files for installation
|
||||
install(DIRECTORY include/${PROJECT_NAME}/
|
||||
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
|
||||
FILES_MATCHING PATTERN "*.h"
|
||||
)
|
||||
|
||||
## Mark other files for installation (e.g. launch and bag files, etc.)
|
||||
install(DIRECTORY launch/
|
||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
|
||||
)
|
||||
|
||||
#############
|
||||
## Testing ##
|
||||
#############
|
||||
|
||||
|
||||
## Add gtest based cpp test target and link libraries
|
||||
# catkin_add_gtest(${PROJECT_NAME}-test test/test_prometheus_demo.cpp)
|
||||
# if(TARGET ${PROJECT_NAME}-test)
|
||||
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
|
||||
# endif()
|
||||
|
||||
## Add folders to be run by python nosetests
|
||||
# catkin_add_nosetests(test)
|
||||
@ -1,20 +0,0 @@
|
||||
%YAML:1.0
|
||||
---
|
||||
calibration_time: "Thu 14 Jul 2022 11:46:13 AM CST"
|
||||
image_width: 640
|
||||
image_height: 480
|
||||
flags: 0
|
||||
camera_matrix: !!opencv-matrix
|
||||
rows: 3
|
||||
cols: 3
|
||||
dt: d
|
||||
data: [ 6.7570491275428003e+02, 0., 3.4702214961185257e+02, 0.,
|
||||
6.7652907115648509e+02, 2.5917548194106814e+02, 0., 0., 1. ]
|
||||
distortion_coefficients: !!opencv-matrix
|
||||
rows: 1
|
||||
cols: 5
|
||||
dt: d
|
||||
data: [ -4.3154114726977300e-01, 2.7199122166782413e-01,
|
||||
-1.5471282947090615e-03, -6.6196646287719843e-04,
|
||||
-4.9889322933567892e-01 ]
|
||||
avg_reprojection_error: 4.7592643246496424e-01
|
||||
@ -1,17 +0,0 @@
|
||||
%YAML:1.0
|
||||
---
|
||||
calibration_time: "Thu 14 Jul 2022 11:46:13 AM CST"
|
||||
image_width: 640
|
||||
image_height: 480
|
||||
flags: 0
|
||||
camera_matrix: !!opencv-matrix
|
||||
rows: 3
|
||||
cols: 3
|
||||
dt: d
|
||||
data: [369.502083, 0.0, 640.0, 0.0, 369.502083, 360.0, 0.0, 0.0, 1.0]
|
||||
distortion_coefficients: !!opencv-matrix
|
||||
rows: 1
|
||||
cols: 5
|
||||
dt: d
|
||||
data: [0,0,0,0,0]
|
||||
avg_reprojection_error: 4.7592643246496424e-01
|
||||
@ -1,22 +0,0 @@
|
||||
<launch>
|
||||
<!-- 启动仿真环境 -->
|
||||
<include file="$(find prometheus_gazebo)/launch_basic/sitl_indoor_1uav.launch">
|
||||
<arg name="world" default="$(find prometheus_gazebo)/gazebo_worlds/detection_worlds/future_aircraft.world" />
|
||||
<arg name="sdf" value="$(find prometheus_gazebo)/gazebo_models/uav_models/p230_future_aircraft/p230_future_aircraft.sdf" />
|
||||
</include>
|
||||
|
||||
<!-- 启动prometheus控制器 -->
|
||||
<include file="$(find prometheus_uav_control)/launch/uav_control_main_indoor.launch">
|
||||
<arg name="launch_prefix" default="bash -c 'gnome-terminal --tab -- $0 $@'" />
|
||||
</include>
|
||||
|
||||
<!-- 椭圆识别 -->
|
||||
<node pkg="prometheus_detection" type="ellipse_det" name="ellipse_det" output="screen">
|
||||
<param name="input_image_topic" type="string" value="/prometheus/sensor/monocular_down/image_raw" />
|
||||
<param name="camera_height_topic" type="string" value="/uav/hgt" />
|
||||
<param name="camera_params" type="string" value="$(find prometheus_future_aircraft)/conf/calib_sitl.yaml"/>
|
||||
</node>
|
||||
|
||||
<!-- gazebo 动目标控制 -->
|
||||
<node pkg="prometheus_future_aircraft" type="locus.py" name="future_aircraft_sitle"/>
|
||||
</launch>
|
||||
@ -1,9 +0,0 @@
|
||||
<launch>
|
||||
<!-- 椭圆识别 -->
|
||||
<node pkg="prometheus_detection" type="ellipse_det" name="ellipse_det" output="screen">
|
||||
<param name="input_image_topic" type="string" value="/prometheus/sensor/monocular_down/image_raw" />
|
||||
<param name="camera_height_topic" type="string" value="/uav/hgt" />
|
||||
<param name="camera_params" type="string" value="$(find prometheus_demo)/advanced/future_aircraft/conf/calib_sitl.yaml"/>
|
||||
</node>
|
||||
|
||||
</launch>
|
||||
@ -1,21 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>prometheus_future_aircraft</name>
|
||||
<version>0.0.0</version>
|
||||
<description>The prometheus_demo package</description>
|
||||
|
||||
<maintainer email="qiyh8@mail.sysu.edu.cn">Yuhua Qi</maintainer>
|
||||
|
||||
<license>TODO</license>
|
||||
|
||||
<build_depend>message_generation</build_depend>
|
||||
<exec_depend>message_runtime</exec_depend>
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<build_depend>std_msgs</build_depend>
|
||||
<build_export_depend>std_msgs</build_export_depend>
|
||||
<exec_depend>std_msgs</exec_depend>
|
||||
|
||||
<export>
|
||||
<!-- Other tools can request additional information be placed here -->
|
||||
</export>
|
||||
</package>
|
||||
@ -1,64 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
import rospy
|
||||
import math
|
||||
import time as t
|
||||
from geometry_msgs.msg import Twist
|
||||
from gazebo_msgs.msg import ModelState, ModelStates
|
||||
from gazebo_msgs.srv import SetModelState, SetModelStateRequest, SetModelStateResponse
|
||||
|
||||
|
||||
client = rospy.ServiceProxy("/gazebo/set_model_state", SetModelState)
|
||||
client.wait_for_service()
|
||||
|
||||
def init_pose():
|
||||
req = SetModelStateRequest()
|
||||
req.model_state.model_name = "future_aircraft_car"
|
||||
req.model_state.pose.position.x = 0
|
||||
req.model_state.pose.position.y = 0
|
||||
req.model_state.pose.position.z = 0
|
||||
client.call(req)
|
||||
|
||||
landing_pad = None
|
||||
def get_pos(info: ModelStates):
|
||||
global landing_pad
|
||||
for i , name in enumerate(info.name):
|
||||
if name == "future_aircraft_car":
|
||||
landing_pad = info.pose[i].position
|
||||
# print(landing_pad)
|
||||
return
|
||||
|
||||
|
||||
def pose_publisher_circle():
|
||||
pub = rospy.Publisher('gazebo/set_model_state', ModelState, queue_size=10)
|
||||
pose_msg = ModelState()
|
||||
pose_msg.model_name = 'landing_pad'
|
||||
pose_msg.pose.position.x = 0
|
||||
pose_msg.pose.position.y = 0
|
||||
pose_msg.pose.position.z = 0
|
||||
|
||||
rate = rospy.Rate(30)
|
||||
pub = rospy.Publisher('/wheeltec/cmd_vel', Twist, queue_size=10)
|
||||
pose_msg = Twist()
|
||||
rate = rospy.Rate(60)
|
||||
circle_radius = 1.5
|
||||
linear_vel = 1
|
||||
omega = math.fabs(linear_vel / circle_radius)
|
||||
# cycle = math.pi * 2 / omega
|
||||
past_time = t.time()
|
||||
while not rospy.is_shutdown():
|
||||
pose_msg.linear.x = linear_vel
|
||||
pose_msg.angular.z = omega
|
||||
if landing_pad != None and math.sqrt(landing_pad.x ** 2 + landing_pad.y**2) < 0.3 and t.time() - past_time > 1:
|
||||
pub.publish(pose_msg)
|
||||
init_pose()
|
||||
past_time = t.time()
|
||||
omega = -omega
|
||||
pub.publish(pose_msg)
|
||||
rate.sleep()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
rospy.init_node('locus_8')
|
||||
rospy.Subscriber("/gazebo/model_states", ModelStates, get_pos)
|
||||
init_pose()
|
||||
pose_publisher_circle()
|
||||
@ -1,168 +0,0 @@
|
||||
#include "ellipse_detector.h"
|
||||
#include <set>
|
||||
#include "printf_utils.h"
|
||||
#include <string>
|
||||
#include <image_transport/image_transport.h>
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
#include <prometheus_msgs/MultiDetectionInfo.h>
|
||||
#include <prometheus_msgs/DetectionInfo.h>
|
||||
#include <unistd.h>
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <sensor_msgs/image_encodings.h>
|
||||
#include <shared_mutex>
|
||||
#include <std_msgs/Float32.h>
|
||||
|
||||
std::shared_mutex g_mutex;
|
||||
int frame_width = 640, frame_height = 480;
|
||||
|
||||
cv::Mat g_camera_matrix, g_dist_coeffs;
|
||||
cv::Mat g_frame;
|
||||
void imageCb(const sensor_msgs::ImageConstPtr &msg)
|
||||
{
|
||||
cv_bridge::CvImagePtr cam_image = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
|
||||
|
||||
if (cam_image)
|
||||
{
|
||||
std::unique_lock lock(g_mutex);
|
||||
cv::Mat tmp = cam_image->image.clone();
|
||||
// NOTE: 图像去畸变
|
||||
// g_frame = tmp
|
||||
cv::undistort(tmp, g_frame, g_camera_matrix, g_dist_coeffs);
|
||||
frame_width = cam_image->image.size().width;
|
||||
frame_height = cam_image->image.size().height;
|
||||
}
|
||||
}
|
||||
|
||||
float g_camera_height = 0;
|
||||
void heightCb(const std_msgs::Float32ConstPtr &msg)
|
||||
{
|
||||
g_camera_height = msg->data;
|
||||
}
|
||||
|
||||
bool unique_ell(const std::pair<float, float> &lhs, const std::pair<float, float> &rhs)
|
||||
{
|
||||
float x = lhs.first - rhs.first;
|
||||
float y = lhs.second - rhs.second;
|
||||
if (std::sqrt(std::pow(x, 2) + std::pow(y, 2)) < 20)
|
||||
{
|
||||
return false;
|
||||
};
|
||||
return true;
|
||||
}
|
||||
// bool (*unique_ell_func)(const std::pair<float, float>, const std::<float, float>) = unique_ell;
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
ros::init(argc, argv, "ellipse_det");
|
||||
ros::NodeHandle nh("~");
|
||||
image_transport::ImageTransport it(nh);
|
||||
std::string input_image_topic, output_image_topic, camera_params, det_info_topic, camera_height_topic;
|
||||
nh.param<std::string>("input_image_topic", input_image_topic, "/prometheus/camera/rgb/image_raw");
|
||||
nh.param<std::string>("output_image_topic", output_image_topic, "/prometheus/detection/image_raw");
|
||||
nh.param<std::string>("camera_height_topic", camera_height_topic, "/camera/height");
|
||||
nh.param<std::string>("camera_params", camera_params, "");
|
||||
nh.param<std::string>("det_info_topic", det_info_topic, "/prometheus/ellipse_det");
|
||||
|
||||
if (camera_params.empty() || access(camera_params.c_str(), 0x00) != 0)
|
||||
{
|
||||
throw camera_params + " path does not exist or not provided";
|
||||
return -1;
|
||||
}
|
||||
cv::FileStorage fs(camera_params, cv::FileStorage::READ);
|
||||
fs["camera_matrix"] >> g_camera_matrix;
|
||||
fs["distortion_coefficients"] >> g_dist_coeffs;
|
||||
|
||||
// 订阅图像话题
|
||||
image_transport::Subscriber image_subscriber = it.subscribe(input_image_topic, 1, imageCb);
|
||||
// 发布检测可视化图像话题
|
||||
image_transport::Publisher image_pub = it.advertise(output_image_topic, 1);
|
||||
|
||||
// 发布MultiDetectionInfo话题
|
||||
ros::Publisher det_info_pub = nh.advertise<prometheus_msgs::MultiDetectionInfo>(det_info_topic, 1);
|
||||
|
||||
// 圆检测参数调整
|
||||
EllipseDetector ellipse_detector;
|
||||
// float fMaxCenterDistance = sqrt(float(width * width + height * height)) * 0.05f;
|
||||
// ellipse_detector.SetParameters(cv::Size(5, 5), 1.306, 1.f, fMaxCenterDistance, 9, 2.984, 0.111, 0.511, 0.470, 22, 0.946, 0.121, 0.468, 0.560, 0.202);
|
||||
|
||||
ros::Rate rate(60);
|
||||
while (ros::ok())
|
||||
{
|
||||
prometheus_msgs::MultiDetectionInfo det_info;
|
||||
det_info.detect_or_track = 0;
|
||||
ros::spinOnce();
|
||||
if (g_frame.empty())
|
||||
{
|
||||
PCOUT(-1, YELLOW, "wait for get image data");
|
||||
continue;
|
||||
}
|
||||
cv::Mat frame;
|
||||
{
|
||||
std::unique_lock lock(g_mutex);
|
||||
frame = g_frame.clone();
|
||||
}
|
||||
std::vector<Ellipse> ellsCned;
|
||||
double t0 = (double)cv::getTickCount();
|
||||
ellipse_detector.Detect(frame, ellsCned);
|
||||
|
||||
sensor_msgs::ImagePtr det_output_msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", frame).toImageMsg();
|
||||
|
||||
// cv::cvtColor(frame, frame, cv::COLOR_BGR2HSV);
|
||||
std::set<std::pair<float, float>, decltype(unique_ell) *> ellipse_set(unique_ell);
|
||||
for (int i = 0; i < ellsCned.size(); i++)
|
||||
{
|
||||
const Ellipse &ellipse = ellsCned[i];
|
||||
auto insert_re = ellipse_set.insert(std::make_pair(ellipse.xc_, ellipse.yc_));
|
||||
if (insert_re.first == ellipse_set.end())
|
||||
continue;
|
||||
prometheus_msgs::DetectionInfo info;
|
||||
// NOTE: 如果区分无人机起点和靶标?
|
||||
cv::Mat mask = cv::Mat::zeros(frame.size(), CV_8U);
|
||||
cv::ellipse(mask, cv::Point(cvRound(ellipse.xc_), cvRound(ellipse.yc_)), cv::Size(cvRound(ellipse.a_), cvRound(ellipse.b_)), ellipse.rad_ * 180 / CV_PI, 0, 360, cv::Scalar(255, 255, 255), -1);
|
||||
cv::Mat tmp;
|
||||
frame.copyTo(tmp, mask);
|
||||
// cv::bitwise_and(frame, cv::noArray(), tmp, mask);
|
||||
cv::inRange(tmp, cv::Scalar(150, 150, 150), cv::Scalar(255, 255, 255), tmp);
|
||||
|
||||
int *tmpi = nullptr;
|
||||
if (static_cast<float>(cv::countNonZero(tmp)) / cv::countNonZero(mask) > 0.1)
|
||||
{
|
||||
info.object_name = "S";
|
||||
cv::Size sz = cv::getTextSize(info.object_name, 0, 0.8, 1, tmpi);
|
||||
cv::putText(frame, info.object_name, cv::Point(ellipse.xc_ - sz.width / 2, ellipse.yc_ + sz.height / 2), cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(255, 255, 0), 1);
|
||||
}
|
||||
else
|
||||
{
|
||||
info.object_name = "T";
|
||||
cv::Size sz = cv::getTextSize(info.object_name, 0, 0.8, 1, tmpi);
|
||||
cv::putText(frame, info.object_name, cv::Point(ellipse.xc_ - sz.width / 2, ellipse.yc_ + sz.height / 2), cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(255, 255, 0), 1);
|
||||
}
|
||||
info.frame = 0;
|
||||
info.detected = true;
|
||||
// NOTE: 目标位置估计
|
||||
info.position[0] = g_camera_height * (ellipse.yc_ - frame_height / 2) / g_camera_matrix.at<float>(1, 1);
|
||||
info.position[1] = g_camera_height * (ellipse.xc_ - frame_width / 2) / g_camera_matrix.at<float>(0, 0);
|
||||
info.position[2] = g_camera_height;
|
||||
info.sight_angle[0] = (ellipse.xc_ - frame_width / 2) / (frame_width / 2) * std::atan(frame_width / (2 * g_camera_matrix.at<float>(0, 0)));
|
||||
info.sight_angle[1] = (ellipse.yc_ - frame_height / 2) / (frame_height / 2) * std::atan(frame_height / (2 * g_camera_matrix.at<float>(1, 1)));
|
||||
info.pixel_position[0] = ellipse.xc_;
|
||||
info.pixel_position[1] = ellipse.yc_;
|
||||
det_info.detection_infos.push_back(info);
|
||||
}
|
||||
det_info.num_objs = ellipse_set.size();
|
||||
|
||||
double dt = ((double)cv::getTickCount() - t0) / cv::getTickFrequency();
|
||||
char msg[256];
|
||||
sprintf(msg, "FPS: %f", 1.f / dt);
|
||||
cv::putText(frame, msg, cv::Point(10, 20), cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(0, 0, 255), 1, 4, 0);
|
||||
|
||||
ellipse_detector.DrawDetectedEllipses(frame, ellsCned, 1, 6);
|
||||
image_pub.publish(det_output_msg);
|
||||
cv::imshow("show", frame);
|
||||
cv::waitKey(1);
|
||||
|
||||
det_info_pub.publish(det_info);
|
||||
rate.sleep();
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@ -1 +0,0 @@
|
||||
/opt/ros/melodic/share/catkin/cmake/toplevel.cmake
|
||||
@ -1,58 +0,0 @@
|
||||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(prometheus_global_planner)
|
||||
|
||||
find_package(Eigen3 REQUIRED)
|
||||
find_package(PCL 1.7 REQUIRED)
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
roscpp
|
||||
rospy
|
||||
std_msgs
|
||||
sensor_msgs
|
||||
laser_geometry
|
||||
geometry_msgs
|
||||
nav_msgs
|
||||
pcl_ros
|
||||
visualization_msgs
|
||||
gazebo_msgs
|
||||
prometheus_msgs
|
||||
)
|
||||
|
||||
|
||||
catkin_package(
|
||||
INCLUDE_DIRS include
|
||||
)
|
||||
|
||||
include_directories(
|
||||
SYSTEM
|
||||
include
|
||||
${PROJECT_SOURCE_DIR}/include
|
||||
${catkin_INCLUDE_DIRS}
|
||||
${Eigen3_INCLUDE_DIRS}
|
||||
${PCL_INCLUDE_DIRS}
|
||||
${PROJECT_SOURCE_DIR}/../../common/include
|
||||
)
|
||||
link_directories(${PCL_LIBRARY_DIRS})
|
||||
|
||||
add_library(occupy_map src/occupy_map.cpp)
|
||||
target_link_libraries(occupy_map ${PCL_LIBRARIES})
|
||||
add_library(A_star src/A_star.cpp)
|
||||
target_link_libraries(A_star occupy_map)
|
||||
add_library(global_planner src/global_planner.cpp)
|
||||
target_link_libraries(global_planner A_star)
|
||||
|
||||
add_executable(global_planner_main src/global_planner_node.cpp)
|
||||
add_dependencies(global_planner_main prometheus_global_planner_gencpp)
|
||||
target_link_libraries(global_planner_main ${catkin_LIBRARIES})
|
||||
target_link_libraries(global_planner_main ${PCL_LIBRARIES})
|
||||
target_link_libraries(global_planner_main global_planner)
|
||||
|
||||
## Mark cpp header files for installation
|
||||
install(DIRECTORY include/${PROJECT_NAME}/
|
||||
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
|
||||
FILES_MATCHING PATTERN "*.h"
|
||||
)
|
||||
|
||||
## Mark other files for installation (e.g. launch and bag files, etc.)
|
||||
install(DIRECTORY launch/
|
||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
|
||||
)
|
||||
@ -1,181 +0,0 @@
|
||||
#ifndef _ASTAR_H
|
||||
#define _ASTAR_H
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <Eigen/Eigen>
|
||||
#include <iostream>
|
||||
#include <queue>
|
||||
#include <string>
|
||||
#include <unordered_map>
|
||||
#include <sstream>
|
||||
|
||||
#include <sensor_msgs/PointCloud2.h>
|
||||
#include <nav_msgs/Path.h>
|
||||
|
||||
#include "occupy_map.h"
|
||||
#include "printf_utils.h"
|
||||
#include "global_planner_utils.h"
|
||||
|
||||
using namespace std;
|
||||
|
||||
|
||||
|
||||
#define IN_CLOSE_SET 'a'
|
||||
#define IN_OPEN_SET 'b'
|
||||
#define NOT_EXPAND 'c'
|
||||
#define inf 1 >> 30
|
||||
|
||||
class Node
|
||||
{
|
||||
public:
|
||||
/* -------------------- */
|
||||
Eigen::Vector3i index;
|
||||
Eigen::Vector3d position;
|
||||
double g_score, f_score;
|
||||
Node *parent;
|
||||
char node_state;
|
||||
|
||||
double time; // dyn
|
||||
int time_idx;
|
||||
|
||||
Node()
|
||||
{
|
||||
parent = NULL;
|
||||
node_state = NOT_EXPAND;
|
||||
}
|
||||
~Node(){};
|
||||
};
|
||||
typedef Node *NodePtr;
|
||||
|
||||
// 为什么这么麻烦，不能直接比较吗
|
||||
class NodeComparator0
|
||||
{
|
||||
public:
|
||||
bool operator()(NodePtr node1, NodePtr node2)
|
||||
{
|
||||
return node1->f_score > node2->f_score;
|
||||
}
|
||||
};
|
||||
|
||||
template <typename T>
|
||||
struct matrix_hash0 : std::unary_function<T, size_t>
|
||||
{
|
||||
std::size_t operator()(T const &matrix) const
|
||||
{
|
||||
size_t seed = 0;
|
||||
for (size_t i = 0; i < matrix.size(); ++i)
|
||||
{
|
||||
auto elem = *(matrix.data() + i);
|
||||
seed ^= std::hash<typename T::Scalar>()(elem) + 0x9e3779b9 + (seed << 6) + (seed >> 2);
|
||||
}
|
||||
return seed;
|
||||
}
|
||||
};
|
||||
|
||||
class NodeHashTable0
|
||||
{
|
||||
private:
|
||||
/* data */
|
||||
std::unordered_map<Eigen::Vector3i, NodePtr, matrix_hash0<Eigen::Vector3i>> data_3d_;
|
||||
|
||||
public:
|
||||
NodeHashTable0(/* args */)
|
||||
{
|
||||
}
|
||||
~NodeHashTable0()
|
||||
{
|
||||
}
|
||||
void insert(Eigen::Vector3i idx, NodePtr node)
|
||||
{
|
||||
data_3d_.insert(std::make_pair(idx, node));
|
||||
}
|
||||
|
||||
NodePtr find(Eigen::Vector3i idx)
|
||||
{
|
||||
auto iter = data_3d_.find(idx);
|
||||
return iter == data_3d_.end() ? NULL : iter->second;
|
||||
}
|
||||
|
||||
void clear()
|
||||
{
|
||||
data_3d_.clear();
|
||||
}
|
||||
};
|
||||
|
||||
class Astar
|
||||
{
|
||||
private:
|
||||
// 备选路径点指针容器
|
||||
std::vector<NodePtr> path_node_pool_;
|
||||
// 使用节点计数器、迭代次数计数器
|
||||
int use_node_num_, iter_num_;
|
||||
// 扩展的节点
|
||||
NodeHashTable0 expanded_nodes_;
|
||||
// open set （根据规则已排序好）
|
||||
std::priority_queue<NodePtr, std::vector<NodePtr>, NodeComparator0> open_set_;
|
||||
// 最终路径点容器
|
||||
std::vector<NodePtr> path_nodes_;
|
||||
|
||||
// 参数
|
||||
// 启发式参数
|
||||
double lambda_heu_;
|
||||
double lambda_cost_;
|
||||
// 最大搜索次数
|
||||
int max_search_num;
|
||||
// tie breaker
|
||||
double tie_breaker_;
|
||||
bool is_2D;
|
||||
double fly_height;
|
||||
|
||||
// 目标点
|
||||
Eigen::Vector3d goal_pos;
|
||||
|
||||
// 地图相关
|
||||
std::vector<int> occupancy_buffer_;
|
||||
double resolution_, inv_resolution_;
|
||||
Eigen::Vector3d origin_, map_size_3d_;
|
||||
|
||||
// 辅助函数
|
||||
Eigen::Vector3i posToIndex(Eigen::Vector3d pt);
|
||||
void indexToPos(Eigen::Vector3i id, Eigen::Vector3d &pos);
|
||||
void retrievePath(NodePtr end_node);
|
||||
|
||||
// 搜索启发函数，三种形式，选用其中一种即可
|
||||
double getDiagHeu(Eigen::Vector3d x1, Eigen::Vector3d x2);
|
||||
double getManhHeu(Eigen::Vector3d x1, Eigen::Vector3d x2);
|
||||
double getEuclHeu(Eigen::Vector3d x1, Eigen::Vector3d x2);
|
||||
|
||||
public:
|
||||
Astar(){}
|
||||
~Astar();
|
||||
|
||||
enum
|
||||
{
|
||||
REACH_END = 1,
|
||||
NO_PATH = 2
|
||||
};
|
||||
|
||||
// 占据图类
|
||||
Occupy_map::Ptr Occupy_map_ptr;
|
||||
|
||||
// 重置
|
||||
void reset();
|
||||
// 初始化
|
||||
void init(ros::NodeHandle &nh);
|
||||
// 检查安全性
|
||||
bool check_safety(Eigen::Vector3d &cur_pos, double safe_distance);
|
||||
// 搜索
|
||||
int search(Eigen::Vector3d start_pt, Eigen::Vector3d end_pt);
|
||||
// 返回路径
|
||||
std::vector<Eigen::Vector3d> getPath();
|
||||
// 返回ros消息格式的路径
|
||||
nav_msgs::Path get_ros_path();
|
||||
// 返回访问过的节点
|
||||
std::vector<NodePtr> getVisitedNodes();
|
||||
|
||||
typedef std::shared_ptr<Astar> Ptr;
|
||||
};
|
||||
|
||||
|
||||
|
||||
#endif
|
||||
@ -1,121 +0,0 @@
|
||||
#ifndef GLOBAL_PLANNER
|
||||
#define GLOBAL_PLANNER
|
||||
|
||||
#include <ros/ros.h>
|
||||
|
||||
#include <Eigen/Eigen>
|
||||
#include <iostream>
|
||||
#include <algorithm>
|
||||
|
||||
#include <geometry_msgs/PoseStamped.h>
|
||||
#include <std_msgs/Bool.h>
|
||||
#include <sensor_msgs/PointCloud2.h>
|
||||
#include <sensor_msgs/LaserScan.h>
|
||||
#include <nav_msgs/Odometry.h>
|
||||
#include <nav_msgs/Path.h>
|
||||
#include <laser_geometry/laser_geometry.h>
|
||||
#include <prometheus_msgs/UAVState.h>
|
||||
#include <prometheus_msgs/UAVCommand.h>
|
||||
#include <prometheus_msgs/UAVControlState.h>
|
||||
|
||||
#include "A_star.h"
|
||||
#include "occupy_map.h"
|
||||
#include "printf_utils.h"
|
||||
#include "global_planner_utils.h"
|
||||
using namespace std;
|
||||
|
||||
|
||||
class GlobalPlanner
|
||||
{
|
||||
public:
|
||||
GlobalPlanner(ros::NodeHandle &nh);
|
||||
ros::NodeHandle global_planner_nh;
|
||||
|
||||
private:
|
||||
// 参数
|
||||
int uav_id;
|
||||
bool sim_mode;
|
||||
int map_input_source;
|
||||
double fly_height;
|
||||
double safe_distance;
|
||||
double time_per_path;
|
||||
double replan_time;
|
||||
bool consider_neighbour;
|
||||
string global_pcl_topic_name, local_pcl_topic_name, scan_topic_name;
|
||||
|
||||
// 订阅无人机状态、目标点、传感器数据（生成地图）
|
||||
ros::Subscriber goal_sub;
|
||||
ros::Subscriber uav_state_sub;
|
||||
// 支持2维激光雷达、3维激光雷达、D435i等实体传感器
|
||||
// 支持直接输入全局已知点云
|
||||
ros::Subscriber Gpointcloud_sub;
|
||||
ros::Subscriber Lpointcloud_sub;
|
||||
ros::Subscriber laserscan_sub;
|
||||
ros::Subscriber uav_control_state_sub;
|
||||
// 发布控制指令
|
||||
ros::Publisher uav_cmd_pub;
|
||||
ros::Publisher path_cmd_pub;
|
||||
ros::Timer mainloop_timer;
|
||||
ros::Timer track_path_timer;
|
||||
ros::Timer safety_timer;
|
||||
// A星规划器
|
||||
Astar::Ptr Astar_ptr;
|
||||
|
||||
// laserscan2pointcloud2 投影器
|
||||
laser_geometry::LaserProjection projector_;
|
||||
|
||||
prometheus_msgs::UAVState uav_state; // 无人机状态
|
||||
prometheus_msgs::UAVControlState uav_control_state;
|
||||
nav_msgs::Odometry uav_odom;
|
||||
Eigen::Vector3d uav_pos; // 无人机位置
|
||||
Eigen::Vector3d uav_vel; // 无人机速度
|
||||
Eigen::Quaterniond uav_quat; // 无人机四元数
|
||||
double uav_yaw;
|
||||
// 规划终端状态
|
||||
Eigen::Vector3d goal_pos, goal_vel;
|
||||
prometheus_msgs::UAVCommand uav_command;
|
||||
nav_msgs::Path path_cmd;
|
||||
double distance_to_goal;
|
||||
|
||||
// 规划器状态
|
||||
bool odom_ready;
|
||||
bool drone_ready;
|
||||
bool sensor_ready;
|
||||
bool goal_ready;
|
||||
bool is_safety;
|
||||
bool is_new_path;
|
||||
bool path_ok;
|
||||
int start_point_index;
|
||||
int Num_total_wp;
|
||||
int cur_id;
|
||||
float desired_yaw;
|
||||
|
||||
ros::Time last_replan_time;
|
||||
|
||||
// 五种状态机
|
||||
enum EXEC_STATE
|
||||
{
|
||||
WAIT_GOAL,
|
||||
PLANNING,
|
||||
TRACKING,
|
||||
LANDING,
|
||||
};
|
||||
EXEC_STATE exec_state;
|
||||
|
||||
// 回调函数
|
||||
void goal_cb(const geometry_msgs::PoseStampedConstPtr &msg);
|
||||
void uav_state_cb(const prometheus_msgs::UAVStateConstPtr &msg);
|
||||
void Gpointcloud_cb(const sensor_msgs::PointCloud2ConstPtr &msg);
|
||||
void Lpointcloud_cb(const sensor_msgs::PointCloud2ConstPtr &msg);
|
||||
void laser_cb(const sensor_msgs::LaserScanConstPtr &msg);
|
||||
void uav_control_state_cb(const prometheus_msgs::UAVControlState::ConstPtr &msg);
|
||||
void safety_cb(const ros::TimerEvent &e);
|
||||
void mainloop_cb(const ros::TimerEvent &e);
|
||||
void track_path_cb(const ros::TimerEvent &e);
|
||||
void debug_info();
|
||||
int get_start_point_id(void);
|
||||
};
|
||||
|
||||
|
||||
|
||||
#endif
|
||||
@ -1,8 +0,0 @@
|
||||
#ifndef GLOBAL_PLANNER_UTILS_H
|
||||
#define GLOBAL_PLANNER_UTILS_H
|
||||
|
||||
#define NODE_NAME "GlobalPlanner [Astar] "
|
||||
#define MIN_DIS 0.1
|
||||
|
||||
#endif
|
||||
|
||||
@ -1,40 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>prometheus_global_planner</name>
|
||||
<version>0.0.0</version>
|
||||
<description>The prometheus_global_planner package</description>
|
||||
|
||||
<maintainer email="qiyh8@mail.sysu.edu.cn">Yuhua Qi</maintainer>
|
||||
|
||||
<license>TODO</license>
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<build_depend>roscpp</build_depend>
|
||||
<build_depend>rospy</build_depend>
|
||||
<build_depend>std_msgs</build_depend>
|
||||
<build_depend>sensor_msgs</build_depend>
|
||||
<build_depend>geometry_msgs</build_depend>
|
||||
<build_depend>nav_msgs</build_depend>
|
||||
<build_depend>prometheus_msgs</build_depend>
|
||||
<build_depend>pcl_ros</build_depend>
|
||||
<build_export_depend>roscpp</build_export_depend>
|
||||
<build_export_depend>rospy</build_export_depend>
|
||||
<build_export_depend>std_msgs</build_export_depend>
|
||||
<build_export_depend>sensor_msgs</build_export_depend>
|
||||
<build_export_depend>geometry_msgs</build_export_depend>
|
||||
<build_export_depend>nav_msgs</build_export_depend>
|
||||
<build_export_depend>prometheus_msgs</build_export_depend>
|
||||
<build_export_depend>pcl_ros</build_export_depend>
|
||||
<exec_depend>roscpp</exec_depend>
|
||||
<exec_depend>rospy</exec_depend>
|
||||
<exec_depend>std_msgs</exec_depend>
|
||||
<exec_depend>sensor_msgs</exec_depend>
|
||||
<exec_depend>geometry_msgs</exec_depend>
|
||||
<exec_depend>nav_msgs</exec_depend>
|
||||
<exec_depend>prometheus_msgs</exec_depend>
|
||||
<exec_depend>pcl_ros</exec_depend>
|
||||
|
||||
<export>
|
||||
<!-- Other tools can request additional information be placed here -->
|
||||
</export>
|
||||
</package>
|
||||
@ -1,28 +0,0 @@
|
||||
#include <ros/ros.h>
|
||||
#include <signal.h>
|
||||
|
||||
#include "global_planner.h"
|
||||
|
||||
;
|
||||
|
||||
void mySigintHandler(int sig)
|
||||
{
|
||||
ROS_INFO("[global_planner_node] exit...");
|
||||
ros::shutdown();
|
||||
}
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
ros::init(argc, argv, "global_planner");
|
||||
ros::NodeHandle nh("~");
|
||||
|
||||
signal(SIGINT, mySigintHandler);
|
||||
ros::Duration(1.0).sleep();
|
||||
|
||||
GlobalPlanner global_planner(nh);
|
||||
|
||||
ros::spin();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
@ -1,50 +0,0 @@
|
||||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(prometheus_local_planner)
|
||||
|
||||
find_package(Eigen3 REQUIRED)
|
||||
find_package(PCL 1.7 REQUIRED)
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
roscpp
|
||||
rospy
|
||||
std_msgs
|
||||
sensor_msgs
|
||||
laser_geometry
|
||||
geometry_msgs
|
||||
nav_msgs
|
||||
pcl_ros
|
||||
visualization_msgs
|
||||
prometheus_msgs
|
||||
mavros_msgs
|
||||
)
|
||||
|
||||
|
||||
catkin_package(
|
||||
INCLUDE_DIRS include
|
||||
LIBRARIES LocalPlannerNS
|
||||
)
|
||||
|
||||
include_directories(
|
||||
SYSTEM
|
||||
include
|
||||
${PROJECT_SOURCE_DIR}/include
|
||||
${catkin_INCLUDE_DIRS}
|
||||
${Eigen3_INCLUDE_DIRS}
|
||||
${PCL_INCLUDE_DIRS}
|
||||
${PROJECT_SOURCE_DIR}/../../common/include
|
||||
)
|
||||
link_directories(${PCL_LIBRARY_DIRS})
|
||||
|
||||
|
||||
|
||||
add_library(vfh src/vfh.cpp)
|
||||
target_link_libraries(vfh ${PCL_LIBRARIES})
|
||||
add_library(apf src/apf.cpp)
|
||||
target_link_libraries(apf ${PCL_LIBRARIES})
|
||||
add_library(local_planner src/local_planner.cpp)
|
||||
target_link_libraries(local_planner vfh)
|
||||
target_link_libraries(local_planner apf)
|
||||
|
||||
add_executable(local_planner_main src/local_planner_node.cpp)
|
||||
add_dependencies(local_planner_main prometheus_local_planner_gencpp)
|
||||
target_link_libraries(local_planner_main ${catkin_LIBRARIES})
|
||||
target_link_libraries(local_planner_main local_planner)
|
||||
@ -1,63 +0,0 @@
|
||||
#ifndef APF_H
|
||||
#define APF_H
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <Eigen/Eigen>
|
||||
#include <iostream>
|
||||
#include <algorithm>
|
||||
#include <iostream>
|
||||
|
||||
#include <geometry_msgs/PoseStamped.h>
|
||||
#include <std_msgs/Empty.h>
|
||||
#include <sensor_msgs/PointCloud2.h>
|
||||
#include <nav_msgs/Odometry.h>
|
||||
|
||||
#include <pcl/point_cloud.h>
|
||||
#include <pcl/point_types.h>
|
||||
#include <pcl_conversions/pcl_conversions.h>
|
||||
|
||||
#include "local_planner_alg.h"
|
||||
#include "local_planner_utils.h"
|
||||
|
||||
using namespace std;
|
||||
|
||||
|
||||
class APF : public local_planner_alg
|
||||
{
|
||||
private:
|
||||
// 参数
|
||||
double inflate_distance;
|
||||
double sensor_max_range;
|
||||
double max_att_dist;
|
||||
double k_repulsion;
|
||||
double k_attraction;
|
||||
double min_dist;
|
||||
double ground_height;
|
||||
double ground_safe_height;
|
||||
double safe_distance;
|
||||
|
||||
bool has_local_map_;
|
||||
bool has_odom_;
|
||||
|
||||
Eigen::Vector3d repulsive_force;
|
||||
Eigen::Vector3d attractive_force;
|
||||
|
||||
pcl::PointCloud<pcl::PointXYZ> latest_local_pcl_;
|
||||
sensor_msgs::PointCloud2ConstPtr local_map_ptr_;
|
||||
nav_msgs::Odometry current_odom;
|
||||
|
||||
public:
|
||||
virtual void set_odom(nav_msgs::Odometry cur_odom);
|
||||
virtual void set_local_map(sensor_msgs::PointCloud2ConstPtr &local_map_ptr);
|
||||
virtual void set_local_map_pcl(pcl::PointCloud<pcl::PointXYZ>::Ptr &pcl_ptr);
|
||||
virtual int compute_force(Eigen::Vector3d &goal, Eigen::Vector3d &desired_vel);
|
||||
virtual void init(ros::NodeHandle &nh);
|
||||
APF() {}
|
||||
~APF() {}
|
||||
|
||||
typedef shared_ptr<APF> Ptr;
|
||||
};
|
||||
|
||||
|
||||
|
||||
#endif
|
||||
@ -1,115 +0,0 @@
|
||||
#ifndef LOCAL_PLANNER_H
|
||||
#define LOCAL_PLANNER_H
|
||||
|
||||
#include <ros/ros.h>
|
||||
|
||||
#include <Eigen/Eigen>
|
||||
#include <iostream>
|
||||
#include <algorithm>
|
||||
|
||||
#include <geometry_msgs/PoseStamped.h>
|
||||
#include <std_msgs/Int8.h>
|
||||
#include <std_msgs/Bool.h>
|
||||
#include <sensor_msgs/PointCloud2.h>
|
||||
#include <nav_msgs/Odometry.h>
|
||||
#include <sensor_msgs/LaserScan.h>
|
||||
#include <pcl/point_cloud.h>
|
||||
#include <pcl/point_types.h>
|
||||
#include <pcl_conversions/pcl_conversions.h>
|
||||
#include <visualization_msgs/Marker.h>
|
||||
#include <prometheus_msgs/UAVState.h>
|
||||
#include <prometheus_msgs/UAVCommand.h>
|
||||
#include <prometheus_msgs/UAVControlState.h>
|
||||
|
||||
#include "apf.h"
|
||||
#include "vfh.h"
|
||||
#include "local_planner_utils.h"
|
||||
|
||||
using namespace std;
|
||||
|
||||
|
||||
class LocalPlanner
|
||||
{
|
||||
public:
|
||||
LocalPlanner(ros::NodeHandle &nh);
|
||||
ros::NodeHandle local_planner_nh;
|
||||
|
||||
private:
|
||||
// 参数
|
||||
int uav_id;
|
||||
int algorithm_mode;
|
||||
int map_input_source;
|
||||
double max_planning_vel;
|
||||
double fly_height;
|
||||
double safe_distance;
|
||||
bool sim_mode;
|
||||
bool map_groundtruth;
|
||||
string local_pcl_topic_name;
|
||||
|
||||
// 订阅无人机状态、目标点、传感器数据（生成地图）
|
||||
ros::Subscriber goal_sub;
|
||||
ros::Subscriber uav_state_sub;
|
||||
ros::Subscriber uav_control_state_sub;
|
||||
ros::Subscriber local_point_cloud_sub;
|
||||
|
||||
// 发布控制指令
|
||||
ros::Publisher uav_cmd_pub;
|
||||
ros::Publisher rviz_vel_pub;
|
||||
ros::Timer mainloop_timer;
|
||||
ros::Timer control_timer;
|
||||
|
||||
// 局部避障算法 算子
|
||||
local_planner_alg::Ptr local_alg_ptr;
|
||||
|
||||
prometheus_msgs::UAVState uav_state; // 无人机状态
|
||||
prometheus_msgs::UAVControlState uav_control_state;
|
||||
nav_msgs::Odometry uav_odom;
|
||||
prometheus_msgs::UAVCommand uav_command;
|
||||
|
||||
double distance_to_goal;
|
||||
// 规划器状态
|
||||
bool odom_ready;
|
||||
bool drone_ready;
|
||||
bool sensor_ready;
|
||||
bool goal_ready;
|
||||
bool is_safety;
|
||||
bool path_ok;
|
||||
|
||||
// 规划初始状态及终端状态
|
||||
Eigen::Vector3d uav_pos; // 无人机位置
|
||||
Eigen::Vector3d uav_vel; // 无人机速度
|
||||
Eigen::Quaterniond uav_quat; // 无人机四元数
|
||||
double uav_yaw;
|
||||
// 规划终端状态
|
||||
Eigen::Vector3d goal_pos, goal_vel;
|
||||
|
||||
int planner_state;
|
||||
Eigen::Vector3d desired_vel;
|
||||
float desired_yaw;
|
||||
|
||||
// 五种状态机
|
||||
enum EXEC_STATE
|
||||
{
|
||||
WAIT_GOAL,
|
||||
PLANNING,
|
||||
TRACKING,
|
||||
LANDING,
|
||||
};
|
||||
EXEC_STATE exec_state;
|
||||
|
||||
sensor_msgs::PointCloud2ConstPtr local_map_ptr_;
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_ptr;
|
||||
pcl::PointCloud<pcl::PointXYZ> latest_local_pcl_;
|
||||
|
||||
void goal_cb(const geometry_msgs::PoseStampedConstPtr &msg);
|
||||
void uav_control_state_cb(const prometheus_msgs::UAVControlState::ConstPtr &msg);
|
||||
void uav_state_cb(const prometheus_msgs::UAVStateConstPtr &msg);
|
||||
void pcl_cb(const sensor_msgs::PointCloud2ConstPtr &msg);
|
||||
void laserscan_cb(const sensor_msgs::LaserScanConstPtr &msg);
|
||||
void mainloop_cb(const ros::TimerEvent &e);
|
||||
void control_cb(const ros::TimerEvent &e);
|
||||
void debug_info();
|
||||
};
|
||||
|
||||
|
||||
#endif
|
||||
@ -1,37 +0,0 @@
|
||||
#ifndef LOCAL_PLANNING_ALG
|
||||
#define LOCAL_PLANNING_ALG
|
||||
|
||||
#include <Eigen/Eigen>
|
||||
#include <iostream>
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <visualization_msgs/Marker.h>
|
||||
#include <geometry_msgs/PoseStamped.h>
|
||||
#include <std_msgs/Empty.h>
|
||||
#include <sensor_msgs/PointCloud2.h>
|
||||
#include <nav_msgs/Odometry.h>
|
||||
|
||||
#include <pcl/point_cloud.h>
|
||||
#include <pcl/point_types.h>
|
||||
#include <pcl_conversions/pcl_conversions.h>
|
||||
|
||||
using namespace std;
|
||||
|
||||
|
||||
class local_planner_alg
|
||||
{
|
||||
public:
|
||||
local_planner_alg() {}
|
||||
~local_planner_alg() {}
|
||||
virtual void set_odom(nav_msgs::Odometry cur_odom) = 0;
|
||||
virtual void set_local_map(sensor_msgs::PointCloud2ConstPtr &local_map_ptr) = 0;
|
||||
virtual void set_local_map_pcl(pcl::PointCloud<pcl::PointXYZ>::Ptr &pcl_ptr) = 0;
|
||||
virtual int compute_force(Eigen::Vector3d &goal, Eigen::Vector3d &desired_vel) = 0;
|
||||
virtual void init(ros::NodeHandle &nh) = 0;
|
||||
|
||||
typedef shared_ptr<local_planner_alg> Ptr;
|
||||
};
|
||||
|
||||
|
||||
|
||||
#endif
|
||||
@ -1,10 +0,0 @@
|
||||
#ifndef LOCAL_PLANNER_UTILS_H
|
||||
#define LOCAL_PLANNER_UTILS_H
|
||||
|
||||
#include "printf_utils.h"
|
||||
|
||||
#define NODE_NAME "LocalPlanner"
|
||||
#define MIN_DIS 0.1
|
||||
|
||||
#endif
|
||||
|
||||
@ -1,73 +0,0 @@
|
||||
#ifndef VFH_H
|
||||
#define VFH_H
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <Eigen/Eigen>
|
||||
#include <iostream>
|
||||
#include <algorithm>
|
||||
#include <iostream>
|
||||
|
||||
#include <geometry_msgs/PoseStamped.h>
|
||||
#include <std_msgs/Empty.h>
|
||||
#include <sensor_msgs/PointCloud2.h>
|
||||
#include <nav_msgs/Odometry.h>
|
||||
|
||||
#include <pcl/point_cloud.h>
|
||||
#include <pcl/point_types.h>
|
||||
#include <pcl_conversions/pcl_conversions.h>
|
||||
|
||||
#include "math.h"
|
||||
#include "local_planner_alg.h"
|
||||
#include "local_planner_utils.h"
|
||||
|
||||
using namespace std;
|
||||
|
||||
|
||||
class VFH : public local_planner_alg
|
||||
{
|
||||
private:
|
||||
// 参数
|
||||
double inflate_distance;
|
||||
double sensor_max_range;
|
||||
double safe_distance;
|
||||
bool has_local_map_;
|
||||
bool has_odom_;
|
||||
|
||||
double goalWeight, obstacle_weight;
|
||||
double inflate_and_safe_distance;
|
||||
double velocity;
|
||||
|
||||
double *Hdata;
|
||||
double Hres;
|
||||
int Hcnt; // 直方图个数
|
||||
|
||||
pcl::PointCloud<pcl::PointXYZ> latest_local_pcl_;
|
||||
sensor_msgs::PointCloud2ConstPtr local_map_ptr_;
|
||||
nav_msgs::Odometry current_odom;
|
||||
|
||||
bool isIgnored(float x, float y, float z, float ws);
|
||||
int find_Hcnt(double angle);
|
||||
double find_angle(int cnt);
|
||||
double angle_error(double angle1, double angle2);
|
||||
void generate_voxel_data(double angle_cen, double angle_range, double val);
|
||||
int find_optimization_path(void);
|
||||
|
||||
public:
|
||||
virtual void set_odom(nav_msgs::Odometry cur_odom);
|
||||
virtual void set_local_map(sensor_msgs::PointCloud2ConstPtr &local_map_ptr);
|
||||
virtual void set_local_map_pcl(pcl::PointCloud<pcl::PointXYZ>::Ptr &pcl_ptr);
|
||||
virtual int compute_force(Eigen::Vector3d &goal, Eigen::Vector3d &desired_vel);
|
||||
virtual void init(ros::NodeHandle &nh);
|
||||
|
||||
VFH() {}
|
||||
~VFH()
|
||||
{
|
||||
delete Hdata;
|
||||
}
|
||||
|
||||
typedef shared_ptr<VFH> Ptr;
|
||||
};
|
||||
|
||||
|
||||
|
||||
#endif
|
||||
@ -1,31 +0,0 @@
|
||||
## local_planner
|
||||
|
||||
|
||||
|
||||
#### 情况讨论
|
||||
|
||||
- 局部点云情况
|
||||
- 对应真实情况:D435i等RGBD相机,三维激光雷达
|
||||
- map_generator生成全局点云,模拟得到局部点云信息
|
||||
- 无人机不需要搭载传感器
|
||||
- PX4动力学 & fake_odom模块
|
||||
|
||||
roslaunch prometheus_simulator_utils map_generator_with_fake_odom.launch
|
||||
roslaunch prometheus_local_planner sitl_apf_with_local_point.launch
|
||||
|
||||
- 2dlidar情况
|
||||
- 对应真实情况:二维激光雷达
|
||||
- projector_.projectLaser(*local_point, input_laser_scan)将scan转换为点云,即对应回到局部点云情况
|
||||
- map_generator生成全局点云,模拟得到局部点云信息
|
||||
- 无人机不需要搭载传感器
|
||||
- PX4动力学 & fake_odom模块
|
||||
|
||||
roslaunch prometheus_simulator_utils map_generator_with_fake_odom.launch
|
||||
roslaunch prometheus_local_planner sitl_global_planner_with_2dlidar.launch
|
||||
|
||||
- 多机情况
|
||||
- 建议使用 全局点云情况 + 多个无人机
|
||||
- fake_odom模块
|
||||
|
||||
## 运行
|
||||
|
||||
@ -1,37 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>prometheus_local_planner</name>
|
||||
<version>0.0.0</version>
|
||||
<description>The prometheus_local_planner package</description>
|
||||
|
||||
<maintainer email="qiyh8@mail.sysu.edu.cn">Yuhua Qi</maintainer>
|
||||
|
||||
<license>TODO</license>
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<build_depend>roscpp</build_depend>
|
||||
<build_depend>rospy</build_depend>
|
||||
<build_depend>std_msgs</build_depend>
|
||||
<build_depend>sensor_msgs</build_depend>
|
||||
<build_depend>geometry_msgs</build_depend>
|
||||
<build_depend>nav_msgs</build_depend>
|
||||
<build_depend>prometheus_msgs</build_depend>
|
||||
<build_export_depend>roscpp</build_export_depend>
|
||||
<build_export_depend>rospy</build_export_depend>
|
||||
<build_export_depend>std_msgs</build_export_depend>
|
||||
<build_export_depend>sensor_msgs</build_export_depend>
|
||||
<build_export_depend>geometry_msgs</build_export_depend>
|
||||
<build_export_depend>nav_msgs</build_export_depend>
|
||||
<build_export_depend>prometheus_msgs</build_export_depend>
|
||||
<exec_depend>roscpp</exec_depend>
|
||||
<exec_depend>rospy</exec_depend>
|
||||
<exec_depend>std_msgs</exec_depend>
|
||||
<exec_depend>sensor_msgs</exec_depend>
|
||||
<exec_depend>geometry_msgs</exec_depend>
|
||||
<exec_depend>nav_msgs</exec_depend>
|
||||
<exec_depend>prometheus_msgs</exec_depend>
|
||||
|
||||
<export>
|
||||
<!-- Other tools can request additional information be placed here -->
|
||||
</export>
|
||||
</package>
|
||||
@ -1,27 +0,0 @@
|
||||
#include <ros/ros.h>
|
||||
#include <signal.h>
|
||||
|
||||
#include "local_planner.h"
|
||||
|
||||
|
||||
|
||||
void mySigintHandler(int sig)
|
||||
{
|
||||
ROS_INFO("[local_planner_node] exit...");
|
||||
ros::shutdown();
|
||||
}
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
ros::init(argc, argv, "local_planner_node");
|
||||
ros::NodeHandle nh("~");
|
||||
|
||||
signal(SIGINT, mySigintHandler);
|
||||
ros::Duration(1.0).sleep();
|
||||
|
||||
LocalPlanner local_planner(nh);
|
||||
|
||||
ros::spin();
|
||||
|
||||
return 0;
|
||||
}
|
||||
@ -1,286 +0,0 @@
|
||||
#include "vfh.h"
|
||||
|
||||
|
||||
|
||||
void VFH::init(ros::NodeHandle &nh)
|
||||
{
|
||||
// 【参数】障碍物膨胀距离
|
||||
nh.param("vfh/inflate_distance", inflate_distance, 0.2);
|
||||
// 【参数】感知障碍物距离
|
||||
nh.param("vfh/sensor_max_range", sensor_max_range, 5.0);
|
||||
// 【参数】目标权重
|
||||
nh.param("vfh/goalWeight", goalWeight, 0.2);
|
||||
// 【参数】直方图 个数
|
||||
nh.param("vfh/h_res", Hcnt, 180);
|
||||
// 【参数】障碍物权重
|
||||
nh.param("vfh/obstacle_weight", obstacle_weight, 0.0);
|
||||
// 【参数】安全停止距离
|
||||
nh.param("vfh/safe_distance", safe_distance, 0.2);
|
||||
// 【参数】设定速度
|
||||
nh.param("vfh/velocity", velocity, 1.0);
|
||||
|
||||
inflate_and_safe_distance = safe_distance + inflate_distance;
|
||||
Hres = 2 * M_PI / Hcnt;
|
||||
Hdata = new double[Hcnt]();
|
||||
for (int i(0); i < Hcnt; i++)
|
||||
{
|
||||
Hdata[i] = 0.0;
|
||||
}
|
||||
|
||||
has_local_map_ = false;
|
||||
}
|
||||
|
||||
// get the map
|
||||
void VFH::set_local_map(sensor_msgs::PointCloud2ConstPtr &local_map_ptr)
|
||||
{
|
||||
local_map_ptr_ = local_map_ptr;
|
||||
pcl::fromROSMsg(*local_map_ptr, latest_local_pcl_);
|
||||
has_local_map_ = true;
|
||||
}
|
||||
|
||||
void VFH::set_local_map_pcl(pcl::PointCloud<pcl::PointXYZ>::Ptr &pcl_ptr)
|
||||
{
|
||||
latest_local_pcl_ = *pcl_ptr;
|
||||
has_local_map_ = true;
|
||||
}
|
||||
|
||||
void VFH::set_odom(nav_msgs::Odometry cur_odom)
|
||||
{
|
||||
current_odom = cur_odom;
|
||||
has_odom_ = true;
|
||||
}
|
||||
|
||||
int VFH::compute_force(Eigen::Vector3d &goal, Eigen::Vector3d &desired_vel)
|
||||
{
|
||||
// 0 for not init; 1for safe; 2 for dangerous
|
||||
int local_planner_state = 0;
|
||||
int safe_cnt = 0;
|
||||
|
||||
if (!has_local_map_ || !has_odom_)
|
||||
return 0;
|
||||
|
||||
if ((int)latest_local_pcl_.points.size() == 0)
|
||||
return 0;
|
||||
|
||||
if (isnan(goal(0)) || isnan(goal(1)) || isnan(goal(2)))
|
||||
return 0;
|
||||
|
||||
// clear the Hdata
|
||||
for (int i = 0; i < Hcnt; i++)
|
||||
{
|
||||
Hdata[i] = 0;
|
||||
}
|
||||
|
||||
ros::Time begin_collision = ros::Time::now();
|
||||
|
||||
// 计算障碍物相关cost
|
||||
Eigen::Vector3d p3d;
|
||||
vector<Eigen::Vector3d> obstacles;
|
||||
Eigen::Vector3d p3d_gloabl_rot;
|
||||
|
||||
// 排斥力
|
||||
Eigen::Quaterniond cur_rotation_local_to_global(current_odom.pose.pose.orientation.w, current_odom.pose.pose.orientation.x, current_odom.pose.pose.orientation.y, current_odom.pose.pose.orientation.z);
|
||||
|
||||
Eigen::Matrix<double, 3, 3> rotation_mat_local_to_global = cur_rotation_local_to_global.toRotationMatrix();
|
||||
Eigen::Vector3d eulerAngle_yrp = rotation_mat_local_to_global.eulerAngles(2, 1, 0);
|
||||
rotation_mat_local_to_global = Eigen::AngleAxisd(eulerAngle_yrp(0), Eigen::Vector3d::UnitZ()).toRotationMatrix();
|
||||
|
||||
// 遍历点云中的所有点
|
||||
for (size_t i = 0; i < latest_local_pcl_.points.size(); ++i)
|
||||
{
|
||||
// 提取障碍物点
|
||||
p3d(0) = latest_local_pcl_.points[i].x;
|
||||
p3d(1) = latest_local_pcl_.points[i].y;
|
||||
p3d(2) = latest_local_pcl_.points[i].z;
|
||||
|
||||
// 将本地点云转化为全局点云点(主要是yaw角)
|
||||
p3d_gloabl_rot = rotation_mat_local_to_global * p3d;
|
||||
|
||||
// sensor_max_range为感知距离,只考虑感知距离内的障碍
|
||||
if (isIgnored(p3d_gloabl_rot(0), p3d_gloabl_rot(1), p3d_gloabl_rot(2), sensor_max_range))
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
double obs_dist = p3d_gloabl_rot.norm();
|
||||
double obs_angle = atan2(p3d_gloabl_rot(1), p3d_gloabl_rot(0));
|
||||
double angle_range;
|
||||
if (obs_dist > inflate_and_safe_distance)
|
||||
{
|
||||
angle_range = asin(inflate_and_safe_distance / obs_dist);
|
||||
}
|
||||
else if (obs_dist <= inflate_and_safe_distance)
|
||||
{
|
||||
safe_cnt++; // 非常危险
|
||||
continue;
|
||||
}
|
||||
|
||||
double obstacle_cost = obstacle_weight * (1 / obs_dist - 1 / sensor_max_range) * 1.0 / (obs_dist * obs_dist);
|
||||
generate_voxel_data(obs_angle, angle_range, obstacle_cost);
|
||||
|
||||
obstacles.push_back(p3d);
|
||||
}
|
||||
|
||||
// 与目标点相关cost计算
|
||||
// 当前位置
|
||||
Eigen::Vector3d current_pos;
|
||||
current_pos[0] = current_odom.pose.pose.position.x;
|
||||
current_pos[1] = current_odom.pose.pose.position.y;
|
||||
current_pos[2] = current_odom.pose.pose.position.z;
|
||||
Eigen::Vector3d uav2goal = goal - current_pos;
|
||||
// 不考虑高度影响
|
||||
uav2goal(2) = 0.0;
|
||||
double dist_att = uav2goal.norm();
|
||||
double goal_heading = atan2(uav2goal(1), uav2goal(0));
|
||||
|
||||
for (int i = 0; i < Hcnt; i++)
|
||||
{
|
||||
// Hdata;
|
||||
// angle_i 为当前角度
|
||||
double angle_i = find_angle(i);
|
||||
|
||||
double goal_cost = 0;
|
||||
double angle_er = angle_error(angle_i, goal_heading);
|
||||
float goal_gain;
|
||||
if (dist_att > 3.0)
|
||||
{
|
||||
goal_gain = 3.0;
|
||||
}
|
||||
else if (dist_att < 0.5)
|
||||
{
|
||||
goal_gain = 0.5;
|
||||
}
|
||||
else
|
||||
{
|
||||
goal_gain = dist_att;
|
||||
}
|
||||
// 当前角度与目标角度差的越多,则该代价越大
|
||||
goal_cost = goalWeight * angle_er * goal_gain;
|
||||
|
||||
Hdata[i] += goal_cost;
|
||||
}
|
||||
|
||||
// 寻找cost最小的路径
|
||||
int best_ind = find_optimization_path(); // direction
|
||||
// 提取最优路径的航向角
|
||||
double best_heading = find_angle(best_ind);
|
||||
|
||||
desired_vel(0) = cos(best_heading) * velocity;
|
||||
desired_vel(1) = sin(best_heading) * velocity;
|
||||
// 定高飞行
|
||||
desired_vel(2) = 0.0;
|
||||
|
||||
// 如果不安全的点超出指定数量
|
||||
if (safe_cnt > 5)
|
||||
{
|
||||
local_planner_state = 2; //成功规划，但是飞机不安全
|
||||
}
|
||||
else
|
||||
{
|
||||
local_planner_state = 1; //成功规划， 安全
|
||||
}
|
||||
|
||||
static int exec_num = 0;
|
||||
exec_num++;
|
||||
|
||||
// 此处改为根据循环时间计算的数值
|
||||
if (exec_num == 50)
|
||||
{
|
||||
cout << GREEN << NODE_NAME << "VFH calculate take: " << (ros::Time::now() - begin_collision).toSec() << " [s] " << TAIL << endl;
|
||||
exec_num = 0;
|
||||
}
|
||||
|
||||
return local_planner_state;
|
||||
}
|
||||
|
||||
// 寻找最小
|
||||
int VFH::find_optimization_path(void)
|
||||
{
|
||||
int bset_ind = 10000;
|
||||
double best_cost = 100000;
|
||||
for (int i = 0; i < Hcnt; i++)
|
||||
{
|
||||
if (Hdata[i] < best_cost)
|
||||
{
|
||||
best_cost = Hdata[i];
|
||||
bset_ind = i;
|
||||
}
|
||||
}
|
||||
return bset_ind;
|
||||
}
|
||||
|
||||
bool VFH::isIgnored(float x, float y, float z, float ws)
|
||||
{
|
||||
z = 0;
|
||||
if (isnan(x) || isnan(y) || isnan(z))
|
||||
return true;
|
||||
|
||||
if (x * x + y * y + z * z > ws)
|
||||
return true;
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
void VFH::generate_voxel_data(double angle_cen, double angle_range, double val) // set the map obstacle into the H data
|
||||
{
|
||||
double angle_max = angle_cen + angle_range;
|
||||
double angle_min = angle_cen - angle_range;
|
||||
int cnt_min = find_Hcnt(angle_min);
|
||||
int cnt_max = find_Hcnt(angle_max);
|
||||
if (cnt_min > cnt_max)
|
||||
{
|
||||
for (int i = cnt_min; i < Hcnt; i++)
|
||||
{
|
||||
Hdata[i] = +val;
|
||||
}
|
||||
for (int i = 0; i < cnt_max; i++)
|
||||
{
|
||||
Hdata[i] += val;
|
||||
}
|
||||
}
|
||||
else if (cnt_max >= cnt_min)
|
||||
{
|
||||
for (int i = cnt_min; i <= cnt_max; i++)
|
||||
{
|
||||
Hdata[i] += val;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// angle: deg
|
||||
int VFH::find_Hcnt(double angle)
|
||||
{
|
||||
if (angle < 0)
|
||||
{
|
||||
angle += 2 * M_PI;
|
||||
}
|
||||
if (angle > 2 * M_PI)
|
||||
{
|
||||
angle -= 2 * M_PI;
|
||||
}
|
||||
int cnt = floor(angle / Hres);
|
||||
return cnt;
|
||||
}
|
||||
|
||||
double VFH::find_angle(int cnt)
|
||||
{
|
||||
double angle = (cnt + 0.5) * Hres;
|
||||
return angle;
|
||||
}
|
||||
|
||||
double VFH::angle_error(double angle1, double angle2)
|
||||
{
|
||||
double angle_er = angle1 - angle2;
|
||||
while (angle_er > M_PI)
|
||||
{
|
||||
angle_er = angle_er - 2 * M_PI;
|
||||
}
|
||||
|
||||
while (angle_er < -M_PI)
|
||||
{
|
||||
angle_er = angle_er + 2 * M_PI;
|
||||
}
|
||||
angle_er = fabs(angle_er);
|
||||
return angle_er;
|
||||
}
|
||||
|
||||
@ -1,7 +0,0 @@
|
||||
#roscore & sleep 5;
|
||||
roslaunch min_snap real.launch & sleep 5;
|
||||
roslaunch vicon_bridge vicon.launch & sleep 3;
|
||||
roslaunch mavros px4.launch & sleep 3;
|
||||
roslaunch ekf PX4_vicon.launch & sleep 3;
|
||||
roslaunch px4ctrl run_ctrl.launch
|
||||
#rosbag record /vicon_imu_ekf_odom /debugPx4ctrl
|
||||
@ -1,221 +0,0 @@
|
||||
cmake_minimum_required(VERSION 3.0.2)
|
||||
project(min_snap)
|
||||
|
||||
## Compile as C++11, supported in ROS Kinetic and newer
|
||||
# add_compile_options(-std=c++11)
|
||||
|
||||
## Find catkin macros and libraries
|
||||
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
|
||||
## is used, also find other catkin packages
|
||||
|
||||
find_package(Eigen3 REQUIRED)
|
||||
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
roscpp
|
||||
rospy
|
||||
std_msgs
|
||||
geometry_msgs
|
||||
quadrotor_msgs
|
||||
mini_snap_traj_utils
|
||||
)
|
||||
|
||||
## System dependencies are found with CMake's conventions
|
||||
# find_package(Boost REQUIRED COMPONENTS system)
|
||||
|
||||
|
||||
## Uncomment this if the package has a setup.py. This macro ensures
|
||||
## modules and global scripts declared therein get installed
|
||||
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
|
||||
# catkin_python_setup()
|
||||
|
||||
################################################
|
||||
## Declare ROS messages, services and actions ##
|
||||
################################################
|
||||
|
||||
## To declare and build messages, services or actions from within this
|
||||
## package, follow these steps:
|
||||
## * Let MSG_DEP_SET be the set of packages whose message types you use in
|
||||
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
|
||||
## * In the file package.xml:
|
||||
## * add a build_depend tag for "message_generation"
|
||||
## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
|
||||
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
|
||||
## but can be declared for certainty nonetheless:
|
||||
## * add a exec_depend tag for "message_runtime"
|
||||
## * In this file (CMakeLists.txt):
|
||||
## * add "message_generation" and every package in MSG_DEP_SET to
|
||||
## find_package(catkin REQUIRED COMPONENTS ...)
|
||||
## * add "message_runtime" and every package in MSG_DEP_SET to
|
||||
## catkin_package(CATKIN_DEPENDS ...)
|
||||
## * uncomment the add_*_files sections below as needed
|
||||
## and list every .msg/.srv/.action file to be processed
|
||||
## * uncomment the generate_messages entry below
|
||||
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
|
||||
|
||||
## Generate messages in the 'msg' folder
|
||||
# add_message_files(
|
||||
# FILES
|
||||
# Message1.msg
|
||||
# Message2.msg
|
||||
# )
|
||||
|
||||
## Generate services in the 'srv' folder
|
||||
# add_service_files(
|
||||
# FILES
|
||||
# Service1.srv
|
||||
# Service2.srv
|
||||
# )
|
||||
|
||||
## Generate actions in the 'action' folder
|
||||
# add_action_files(
|
||||
# FILES
|
||||
# Action1.action
|
||||
# Action2.action
|
||||
# )
|
||||
|
||||
## Generate added messages and services with any dependencies listed here
|
||||
# generate_messages(
|
||||
# DEPENDENCIES
|
||||
# std_msgs
|
||||
# )
|
||||
|
||||
################################################
|
||||
## Declare ROS dynamic reconfigure parameters ##
|
||||
################################################
|
||||
|
||||
## To declare and build dynamic reconfigure parameters within this
|
||||
## package, follow these steps:
|
||||
## * In the file package.xml:
|
||||
## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
|
||||
## * In this file (CMakeLists.txt):
|
||||
## * add "dynamic_reconfigure" to
|
||||
## find_package(catkin REQUIRED COMPONENTS ...)
|
||||
## * uncomment the "generate_dynamic_reconfigure_options" section below
|
||||
## and list every .cfg file to be processed
|
||||
|
||||
## Generate dynamic reconfigure parameters in the 'cfg' folder
|
||||
# generate_dynamic_reconfigure_options(
|
||||
# cfg/DynReconf1.cfg
|
||||
# cfg/DynReconf2.cfg
|
||||
# )
|
||||
|
||||
###################################
|
||||
## catkin specific configuration ##
|
||||
###################################
|
||||
## The catkin_package macro generates cmake config files for your package
|
||||
## Declare things to be passed to dependent projects
|
||||
## INCLUDE_DIRS: uncomment this if your package contains header files
|
||||
## LIBRARIES: libraries you create in this project that dependent projects also need
|
||||
## CATKIN_DEPENDS: catkin_packages dependent projects also need
|
||||
## DEPENDS: system dependencies of this project that dependent projects also need
|
||||
catkin_package(
|
||||
INCLUDE_DIRS include
|
||||
# LIBRARIES min_snap
|
||||
CATKIN_DEPENDS roscpp rospy std_msgs mini_snap_traj_utils
|
||||
# DEPENDS system_lib
|
||||
)
|
||||
|
||||
###########
|
||||
## Build ##
|
||||
###########
|
||||
|
||||
## Specify additional locations of header files
|
||||
## Your package locations should be listed before other locations
|
||||
include_directories(
|
||||
include
|
||||
${catkin_INCLUDE_DIRS}
|
||||
${EIGEN3_INCLUDE_DIR}
|
||||
)
|
||||
|
||||
## Declare a C++ library
|
||||
# add_library(${PROJECT_NAME}
|
||||
# src/${PROJECT_NAME}/min_snap.cpp
|
||||
# )
|
||||
|
||||
## Add cmake target dependencies of the library
|
||||
## as an example, code may need to be generated before libraries
|
||||
## either from message generation or dynamic reconfigure
|
||||
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
|
||||
## Declare a C++ executable
|
||||
## With catkin_make all packages are built within a single CMake context
|
||||
## The recommended prefix ensures that target names across packages don't collide
|
||||
# add_executable(${PROJECT_NAME}_node src/min_snap_node.cpp)
|
||||
|
||||
## Rename C++ executable without prefix
|
||||
## The above recommended prefix causes long target names, the following renames the
|
||||
## target back to the shorter version for ease of user use
|
||||
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
|
||||
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
|
||||
|
||||
## Add cmake target dependencies of the executable
|
||||
## same as for the library above
|
||||
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
|
||||
## Specify libraries to link a library or executable target against
|
||||
# target_link_libraries(${PROJECT_NAME}_node
|
||||
# ${catkin_LIBRARIES}
|
||||
# )
|
||||
|
||||
#############
|
||||
## Install ##
|
||||
#############
|
||||
|
||||
# all install targets should use catkin DESTINATION variables
|
||||
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
|
||||
|
||||
## Mark executable scripts (Python etc.) for installation
|
||||
## in contrast to setup.py, you can choose the destination
|
||||
# catkin_install_python(PROGRAMS
|
||||
# scripts/my_python_script
|
||||
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
# )
|
||||
|
||||
## Mark executables for installation
|
||||
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
|
||||
# install(TARGETS ${PROJECT_NAME}_node
|
||||
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
# )
|
||||
|
||||
## Mark libraries for installation
|
||||
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
|
||||
# install(TARGETS ${PROJECT_NAME}
|
||||
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
|
||||
# )
|
||||
|
||||
## Mark cpp header files for installation
|
||||
# install(DIRECTORY include/${PROJECT_NAME}/
|
||||
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
|
||||
# FILES_MATCHING PATTERN "*.h"
|
||||
# PATTERN ".svn" EXCLUDE
|
||||
# )
|
||||
|
||||
## Mark other files for installation (e.g. launch and bag files, etc.)
|
||||
# install(FILES
|
||||
# # myfile1
|
||||
# # myfile2
|
||||
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
|
||||
# )
|
||||
add_library( minsnapCloseform
|
||||
include/min_snap/min_snap_closeform.h
|
||||
src/min_snap_closeform.cpp
|
||||
)
|
||||
target_link_libraries(minsnapCloseform ${catkin_LIBRARIES})
|
||||
|
||||
add_executable(min_snap_generator src/min_snap_generator.cpp)
|
||||
target_link_libraries(min_snap_generator minsnapCloseform ${catkin_LIBRARIES})
|
||||
|
||||
#############
|
||||
## Testing ##
|
||||
#############
|
||||
|
||||
## Add gtest based cpp test target and link libraries
|
||||
# catkin_add_gtest(${PROJECT_NAME}-test test/test_min_snap.cpp)
|
||||
# if(TARGET ${PROJECT_NAME}-test)
|
||||
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
|
||||
# endif()
|
||||
|
||||
## Add folders to be run by python nosetests
|
||||
# catkin_add_nosetests(test)
|
||||
@ -1,42 +0,0 @@
|
||||
#ifndef _MIN_SNAP_CLOSEFORM_H_
|
||||
#define _MIN_SNAP_CLOSEFORM_H_
|
||||
|
||||
#include <Eigen/Eigen>
|
||||
#include <iostream>
|
||||
|
||||
using std::vector;
|
||||
using namespace std;
|
||||
|
||||
namespace my_planner
|
||||
{
|
||||
class minsnapCloseform
|
||||
{
|
||||
private:
|
||||
vector<Eigen::Vector3d> wps;
|
||||
int n_order, n_seg, n_per_seg;
|
||||
double mean_vel;
|
||||
Eigen::VectorXd ts;
|
||||
Eigen::VectorXd poly_coef_x, poly_coef_y, poly_coef_z;
|
||||
Eigen::VectorXd dec_vel_x, dec_vel_y, dec_vel_z;
|
||||
Eigen::MatrixXd Q, M, Ct;
|
||||
|
||||
int fact(int n);
|
||||
void init_ts(int init_type);
|
||||
std::pair<Eigen::VectorXd, Eigen::VectorXd> MinSnapCloseFormServer(const Eigen::VectorXd &wp);
|
||||
Eigen::VectorXd calDecVel(const Eigen::VectorXd decvel);
|
||||
void calQ();
|
||||
void calM();
|
||||
void calCt();
|
||||
|
||||
public:
|
||||
minsnapCloseform(){};
|
||||
~minsnapCloseform(){};
|
||||
minsnapCloseform(const vector<Eigen::Vector3d> &waypoints, double meanvel = 1.0);
|
||||
void Init(const vector<Eigen::Vector3d> &waypoints, double meanvel = 1.0);
|
||||
void calMinsnap_polycoef();
|
||||
Eigen::MatrixXd getPolyCoef();
|
||||
Eigen::MatrixXd getDecVel();
|
||||
Eigen::VectorXd getTime();
|
||||
};
|
||||
}
|
||||
#endif
|
||||
@ -1,312 +0,0 @@
|
||||
Panels:
|
||||
- Class: rviz/Displays
|
||||
Help Height: 0
|
||||
Name: Displays
|
||||
Property Tree Widget:
|
||||
Expanded:
|
||||
- /Planning1
|
||||
- /Planning1/jerk_dir1
|
||||
- /Planning1/vel_dir1
|
||||
- /Planning1/acc_dir1
|
||||
- /Planning1/Goal_point1
|
||||
- /Planning1/plan_traj1
|
||||
- /Mapping1/simulation_map1/Autocompute Value Bounds1
|
||||
- /Simulation1/robot1
|
||||
Splitter Ratio: 0.5
|
||||
Tree Height: 865
|
||||
- Class: rviz/Selection
|
||||
Name: Selection
|
||||
- Class: rviz/Tool Properties
|
||||
Expanded:
|
||||
- /3D Nav Goal1
|
||||
Name: Tool Properties
|
||||
Splitter Ratio: 0.5886790156364441
|
||||
- Class: rviz/Views
|
||||
Expanded:
|
||||
- /Current View1
|
||||
- /ThirdPersonFollower1
|
||||
Name: Views
|
||||
Splitter Ratio: 0.5
|
||||
- Class: rviz/Time
|
||||
Experimental: false
|
||||
Name: Time
|
||||
SyncMode: 0
|
||||
SyncSource: ""
|
||||
Preferences:
|
||||
PromptSaveOnExit: true
|
||||
Toolbars:
|
||||
toolButtonStyle: 2
|
||||
Visualization Manager:
|
||||
Class: ""
|
||||
Displays:
|
||||
- Alpha: 1
|
||||
Class: rviz/Axes
|
||||
Enabled: true
|
||||
Length: 1
|
||||
Name: Axes
|
||||
Radius: 0.10000000149011612
|
||||
Reference Frame: <Fixed Frame>
|
||||
Value: true
|
||||
- Alpha: 0.5
|
||||
Cell Size: 1
|
||||
Class: rviz/Grid
|
||||
Color: 160; 160; 164
|
||||
Enabled: true
|
||||
Line Style:
|
||||
Line Width: 0.029999999329447746
|
||||
Value: Lines
|
||||
Name: Grid
|
||||
Normal Cell Count: 0
|
||||
Offset:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Plane: XY
|
||||
Plane Cell Count: 40
|
||||
Reference Frame: <Fixed Frame>
|
||||
Value: true
|
||||
- Class: rviz/Group
|
||||
Displays:
|
||||
- Class: rviz/Marker
|
||||
Enabled: true
|
||||
Marker Topic: /min_snap_visual/jerk_dir
|
||||
Name: jerk_dir
|
||||
Namespaces:
|
||||
"": true
|
||||
Queue Size: 1
|
||||
Value: true
|
||||
- Class: rviz/Marker
|
||||
Enabled: true
|
||||
Marker Topic: /min_snap_visual/vel_dir
|
||||
Name: vel_dir
|
||||
Namespaces:
|
||||
"": true
|
||||
Queue Size: 1
|
||||
Value: true
|
||||
- Class: rviz/Marker
|
||||
Enabled: true
|
||||
Marker Topic: /min_snap_visual/acc_dir
|
||||
Name: acc_dir
|
||||
Namespaces:
|
||||
"": true
|
||||
Queue Size: 1
|
||||
Value: true
|
||||
- Class: rviz/Marker
|
||||
Enabled: true
|
||||
Marker Topic: /min_snap_visual/goal_point
|
||||
Name: Goal_point
|
||||
Namespaces:
|
||||
"": true
|
||||
Queue Size: 100
|
||||
Value: true
|
||||
- Class: rviz/Marker
|
||||
Enabled: true
|
||||
Marker Topic: /min_snap_visual/poly_traj
|
||||
Name: plan_traj
|
||||
Namespaces:
|
||||
"": true
|
||||
Queue Size: 100
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Buffer Length: 1
|
||||
Class: rviz/Path
|
||||
Color: 25; 255; 0
|
||||
Enabled: true
|
||||
Head Diameter: 0.30000001192092896
|
||||
Head Length: 0.20000000298023224
|
||||
Length: 0.30000001192092896
|
||||
Line Style: Billboards
|
||||
Line Width: 0.029999999329447746
|
||||
Name: drone_path
|
||||
Offset:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Pose Color: 255; 85; 255
|
||||
Pose Style: None
|
||||
Queue Size: 10
|
||||
Radius: 0.029999999329447746
|
||||
Shaft Diameter: 0.10000000149011612
|
||||
Shaft Length: 0.10000000149011612
|
||||
Topic: /odom_visualization/path
|
||||
Unreliable: false
|
||||
Value: true
|
||||
Enabled: true
|
||||
Name: Planning
|
||||
- Class: rviz/Group
|
||||
Displays:
|
||||
- Alpha: 0.20000000298023224
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 2
|
||||
Min Value: -1
|
||||
Value: false
|
||||
Axis: Z
|
||||
Channel Name: intensity
|
||||
Class: rviz/PointCloud2
|
||||
Color: 85; 170; 255
|
||||
Color Transformer: FlatColor
|
||||
Decay Time: 0
|
||||
Enabled: true
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Min Color: 0; 0; 0
|
||||
Name: simulation_map
|
||||
Position Transformer: XYZ
|
||||
Queue Size: 10
|
||||
Selectable: true
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.10000000149011612
|
||||
Style: Boxes
|
||||
Topic: /map_generator/global_cloud
|
||||
Unreliable: false
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 2.3399999141693115
|
||||
Min Value: 0.03999999910593033
|
||||
Value: true
|
||||
Axis: Z
|
||||
Channel Name: intensity
|
||||
Class: rviz/PointCloud2
|
||||
Color: 255; 255; 255
|
||||
Color Transformer: AxisColor
|
||||
Decay Time: 0
|
||||
Enabled: true
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Min Color: 0; 0; 0
|
||||
Name: map inflate
|
||||
Position Transformer: XYZ
|
||||
Queue Size: 10
|
||||
Selectable: true
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.10000000149011612
|
||||
Style: Flat Squares
|
||||
Topic: /grid_map/occupancy_inflate
|
||||
Unreliable: false
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 2.7592508792877197
|
||||
Min Value: -0.9228500127792358
|
||||
Value: false
|
||||
Axis: Z
|
||||
Channel Name: intensity
|
||||
Class: rviz/PointCloud2
|
||||
Color: 187; 187; 187
|
||||
Color Transformer: AxisColor
|
||||
Decay Time: 0
|
||||
Enabled: false
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Min Color: 0; 0; 0
|
||||
Name: real_map
|
||||
Position Transformer: XYZ
|
||||
Queue Size: 10
|
||||
Selectable: true
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.10000000149011612
|
||||
Style: Boxes
|
||||
Topic: /grid_map/occupancy
|
||||
Unreliable: false
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: false
|
||||
Enabled: false
|
||||
Name: Mapping
|
||||
- Class: rviz/Group
|
||||
Displays:
|
||||
- Class: rviz/Marker
|
||||
Enabled: true
|
||||
Marker Topic: /odom_visualization/robot
|
||||
Name: robot
|
||||
Namespaces:
|
||||
mesh: true
|
||||
Queue Size: 100
|
||||
Value: true
|
||||
Enabled: true
|
||||
Name: Simulation
|
||||
Enabled: true
|
||||
Global Options:
|
||||
Background Color: 255; 253; 224
|
||||
Default Light: true
|
||||
Fixed Frame: world
|
||||
Frame Rate: 30
|
||||
Name: root
|
||||
Tools:
|
||||
- Class: rviz/Interact
|
||||
Hide Inactive Objects: true
|
||||
- Class: rviz/Select
|
||||
- Class: rviz/FocusCamera
|
||||
- Class: rviz/Measure
|
||||
- Class: rviz_plugins/Goal3DTool
|
||||
Topic: /rviz/3d_nav_goal
|
||||
Value: true
|
||||
Views:
|
||||
Current:
|
||||
Class: rviz/ThirdPersonFollower
|
||||
Distance: 9.049065589904785
|
||||
Enable Stereo Rendering:
|
||||
Stereo Eye Separation: 0.05999999865889549
|
||||
Stereo Focal Distance: 1
|
||||
Swap Stereo Eyes: false
|
||||
Value: false
|
||||
Field of View: 0.7853981852531433
|
||||
Focal Point:
|
||||
X: 2.164052724838257
|
||||
Y: -1.1675572395324707
|
||||
Z: 2.1457672119140625e-06
|
||||
Focal Shape Fixed Size: true
|
||||
Focal Shape Size: 0.05000000074505806
|
||||
Invert Z Axis: false
|
||||
Name: Current View
|
||||
Near Clip Distance: 0.009999999776482582
|
||||
Pitch: 0.7747965455055237
|
||||
Target Frame: world
|
||||
Yaw: 4.195437908172607
|
||||
Saved:
|
||||
- Class: rviz/ThirdPersonFollower
|
||||
Distance: 17.48538589477539
|
||||
Enable Stereo Rendering:
|
||||
Stereo Eye Separation: 0.05999999865889549
|
||||
Stereo Focal Distance: 1
|
||||
Swap Stereo Eyes: false
|
||||
Value: false
|
||||
Field of View: 0.7853981852531433
|
||||
Focal Point:
|
||||
X: -16.308002471923828
|
||||
Y: 0.4492051601409912
|
||||
Z: 8.589673598180525e-06
|
||||
Focal Shape Fixed Size: true
|
||||
Focal Shape Size: 0.05000000074505806
|
||||
Invert Z Axis: false
|
||||
Name: ThirdPersonFollower
|
||||
Near Clip Distance: 0.009999999776482582
|
||||
Pitch: 1.0347968339920044
|
||||
Target Frame: <Fixed Frame>
|
||||
Yaw: 3.150407314300537
|
||||
Window Geometry:
|
||||
Displays:
|
||||
collapsed: false
|
||||
Height: 1016
|
||||
Hide Left Dock: false
|
||||
Hide Right Dock: false
|
||||
QMainWindow State: 000000ff00000000fd0000000400000000000001560000039efc020000000ffb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000039e000000c900fffffffb0000000a005600690065007700730000000114000001c9000000a400fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006100000001d1000000b50000000000000000fb0000000a0049006d00610067006502000001a2000001e1000000f8000000b5fb0000000a0064006500700074006800000002da000001010000000000000000fb0000000a0049006d0061006700650100000415000000f80000000000000000fb0000000a0049006d00610067006501000003f4000001190000000000000000fb0000000a006400650070007400680100000459000000f50000000000000000000000010000010f00000385fc0200000002fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006100000003bfc0100000002fb0000000800540069006d0065000000000000000610000004f300fffffffb0000000800540069006d00650100000000000004500000000000000000000005dc0000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||
Selection:
|
||||
collapsed: false
|
||||
Time:
|
||||
collapsed: false
|
||||
Tool Properties:
|
||||
collapsed: false
|
||||
Views:
|
||||
collapsed: false
|
||||
Width: 1848
|
||||
X: 72
|
||||
Y: 27
|
||||
@ -1,50 +0,0 @@
|
||||
<launch>
|
||||
<include file="$(find min_snap)/launch/rviz.launch" />
|
||||
|
||||
<!-- size of map, change the size inflate x, y, z according to your application -->
|
||||
<arg name="map_size_x" value="10.0"/>
|
||||
<arg name="map_size_y" value="10.0"/>
|
||||
<arg name="map_size_z" value=" 3.0"/>
|
||||
<arg name="mean_vel" value="5.0" />
|
||||
|
||||
<!-- topic of your odometry such as VIO or LIO -->
|
||||
<arg name="odom_topic" value="/visual_slam/odom" />
|
||||
|
||||
<!-- use simulator -->
|
||||
<include file="$(find traj_server)/launch/my_sim.xml">
|
||||
<arg name="map_size_x_" value="$(arg map_size_x)"/>
|
||||
<arg name="map_size_y_" value="$(arg map_size_y)"/>
|
||||
<arg name="map_size_z_" value="$(arg map_size_z)"/>
|
||||
<arg name="c_num" value="200"/>
|
||||
<arg name="p_num" value="200"/>
|
||||
<arg name="min_dist" value="1.2"/>
|
||||
|
||||
<arg name="odometry_topic" value="$(arg odom_topic)" />
|
||||
</include>
|
||||
|
||||
<!-- minimum snap close_form, pub poly coefs -->
|
||||
<node pkg="min_snap" name="min_snap_generator" type="min_snap_generator" output="screen">
|
||||
<remap from="/rviz_goal" to="/rviz/3d_nav_goal" />
|
||||
<remap from="/goal_list" to="/planning/goal_list" />
|
||||
<remap from="/poly_coefs" to="/planning/poly_coefs" />
|
||||
<remap from="/odom_topic" to="$(arg odom_topic)" />
|
||||
<remap from="/position_cmd" to="/planning/pos_cmd" />
|
||||
<param name="meanvel" value="$(arg mean_vel)" />
|
||||
</node>
|
||||
|
||||
<!-- visualization, display goals, vel, acc, jerk, planning path -->
|
||||
<node pkg="prometheus_simulator_utils" name="min_snap_visual" type="min_snap_visual" output="screen">
|
||||
<remap from="/goal_list" to="/planning/goal_list" />
|
||||
<remap from="/position_cmd" to="/planning/pos_cmd" />
|
||||
<remap from="/traj_pts" to="/planning/traj_pts" />
|
||||
<remap from="/odometry" to="$(arg odom_topic)" />
|
||||
</node>
|
||||
|
||||
<!-- publish position_cmd -->
|
||||
<node pkg="traj_server" name="my_traj_server" type="my_traj_server" output="screen">
|
||||
<remap from="/position_cmd" to="/planning/pos_cmd" />
|
||||
<remap from="/poly_coefs" to="/planning/poly_coefs" />
|
||||
<remap from="/traj_pts" to="/planning/traj_pts" />
|
||||
</node>
|
||||
|
||||
</launch>
|
||||
@ -1,50 +0,0 @@
|
||||
<launch>
|
||||
<include file="$(find min_snap)/launch/rviz.launch" />
|
||||
|
||||
<!-- size of map, change the size inflate x, y, z according to your application -->
|
||||
<arg name="map_size_x" value="10.0"/>
|
||||
<arg name="map_size_y" value="10.0"/>
|
||||
<arg name="map_size_z" value=" 3.0"/>
|
||||
<arg name="mean_vel" value="0.5" />
|
||||
|
||||
<!-- topic of your odometry such as VIO or LIO -->
|
||||
<arg name="odom_topic" value="/vicon_imu_ekf_odom" />
|
||||
|
||||
<!-- use simulator -->
|
||||
<include file="$(find traj_server)/launch/real_traj_server.xml">
|
||||
<arg name="map_size_x_" value="$(arg map_size_x)"/>
|
||||
<arg name="map_size_y_" value="$(arg map_size_y)"/>
|
||||
<arg name="map_size_z_" value="$(arg map_size_z)"/>
|
||||
<arg name="c_num" value="200"/>
|
||||
<arg name="p_num" value="200"/>
|
||||
<arg name="min_dist" value="1.2"/>
|
||||
|
||||
<arg name="odometry_topic" value="$(arg odom_topic)" />
|
||||
</include>
|
||||
|
||||
<!-- minimum snap close_form, pub poly coefs -->
|
||||
<node pkg="min_snap" name="min_snap_generator" type="min_snap_generator" output="screen">
|
||||
<remap from="/rviz_goal" to="/rviz/3d_nav_goal" />
|
||||
<remap from="/goal_list" to="/planning/goal_list" />
|
||||
<remap from="/poly_coefs" to="/planning/poly_coefs" />
|
||||
<remap from="/odom_topic" to="$(arg odom_topic)" />
|
||||
<remap from="/position_cmd" to="/planning/pos_cmd" />
|
||||
<param name="meanvel" value="$(arg mean_vel)" />
|
||||
</node>
|
||||
|
||||
<!-- visualization, display goals, vel, acc, jerk, planning path -->
|
||||
<node pkg="my_visualization" name="min_snap_visual" type="min_snap_visual" output="screen">
|
||||
<remap from="/goal_list" to="/planning/goal_list" />
|
||||
<remap from="/position_cmd" to="/planning/pos_cmd" />
|
||||
<remap from="/traj_pts" to="/planning/traj_pts" />
|
||||
<remap from="/odometry" to="$(arg odom_topic)" />
|
||||
</node>
|
||||
|
||||
<!-- publish position_cmd -->
|
||||
<node pkg="traj_server" name="my_traj_server" type="my_traj_server" output="screen">
|
||||
<remap from="/position_cmd" to="/planning/pos_cmd" />
|
||||
<remap from="/poly_coefs" to="/planning/poly_coefs" />
|
||||
<remap from="/traj_pts" to="/planning/traj_pts" />
|
||||
</node>
|
||||
|
||||
</launch>
|
||||
@ -1,3 +0,0 @@
|
||||
<launch>
|
||||
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find min_snap)/launch/default.rviz" required="true" />
|
||||
</launch>
|
||||
@ -1,71 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>min_snap</name>
|
||||
<version>0.0.0</version>
|
||||
<description>The min_snap package</description>
|
||||
|
||||
<!-- One maintainer tag required, multiple allowed, one person per tag -->
|
||||
<!-- Example: -->
|
||||
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
|
||||
<maintainer email="mywang@todo.todo">mywang</maintainer>
|
||||
|
||||
|
||||
<!-- One license tag required, multiple allowed, one license per tag -->
|
||||
<!-- Commonly used license strings: -->
|
||||
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
|
||||
<license>TODO</license>
|
||||
|
||||
|
||||
<!-- Url tags are optional, but multiple are allowed, one per tag -->
|
||||
<!-- Optional attribute type can be: website, bugtracker, or repository -->
|
||||
<!-- Example: -->
|
||||
<!-- <url type="website">http://wiki.ros.org/min_snap</url> -->
|
||||
|
||||
|
||||
<!-- Author tags are optional, multiple are allowed, one per tag -->
|
||||
<!-- Authors do not have to be maintainers, but could be -->
|
||||
<!-- Example: -->
|
||||
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
|
||||
|
||||
|
||||
<!-- The *depend tags are used to specify dependencies -->
|
||||
<!-- Dependencies can be catkin packages or system dependencies -->
|
||||
<!-- Examples: -->
|
||||
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
|
||||
<!-- <depend>roscpp</depend> -->
|
||||
<!-- Note that this is equivalent to the following: -->
|
||||
<!-- <build_depend>roscpp</build_depend> -->
|
||||
<!-- <exec_depend>roscpp</exec_depend> -->
|
||||
<!-- Use build_depend for packages you need at compile time: -->
|
||||
<!-- <build_depend>message_generation</build_depend> -->
|
||||
<!-- Use build_export_depend for packages you need in order to build against this package: -->
|
||||
<!-- <build_export_depend>message_generation</build_export_depend> -->
|
||||
<!-- Use buildtool_depend for build tool packages: -->
|
||||
<!-- <buildtool_depend>catkin</buildtool_depend> -->
|
||||
<!-- Use exec_depend for packages you need at runtime: -->
|
||||
<!-- <exec_depend>message_runtime</exec_depend> -->
|
||||
<!-- Use test_depend for packages you need only for testing: -->
|
||||
<!-- <test_depend>gtest</test_depend> -->
|
||||
<!-- Use doc_depend for packages you need only for building documentation: -->
|
||||
<!-- <doc_depend>doxygen</doc_depend> -->
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<build_depend>roscpp</build_depend>
|
||||
<build_depend>rospy</build_depend>
|
||||
<build_depend>std_msgs</build_depend>
|
||||
<build_export_depend>roscpp</build_export_depend>
|
||||
<build_export_depend>rospy</build_export_depend>
|
||||
<build_export_depend>std_msgs</build_export_depend>
|
||||
<exec_depend>roscpp</exec_depend>
|
||||
<exec_depend>rospy</exec_depend>
|
||||
<exec_depend>std_msgs</exec_depend>
|
||||
|
||||
<build_depend>mini_snap_traj_utils</build_depend>
|
||||
<exec_depend>mini_snap_traj_utils</exec_depend>
|
||||
|
||||
|
||||
<!-- The export tag contains other, unspecified, tags -->
|
||||
<export>
|
||||
<!-- Other tools can request additional information be placed here -->
|
||||
|
||||
</export>
|
||||
</package>
|
||||
@ -1,237 +0,0 @@
|
||||
#include <min_snap/min_snap_closeform.h>
|
||||
|
||||
namespace my_planner
|
||||
{
|
||||
minsnapCloseform::minsnapCloseform(const vector<Eigen::Vector3d> &waypoints, double meanvel)
|
||||
{
|
||||
n_order = 7;
|
||||
wps = waypoints;
|
||||
n_seg = int(wps.size()) - 1;
|
||||
n_per_seg = n_order + 1;
|
||||
mean_vel = meanvel;
|
||||
}
|
||||
|
||||
void minsnapCloseform::Init(const vector<Eigen::Vector3d> &waypoints, double meanvel)
|
||||
{
|
||||
n_order = 7;
|
||||
wps = waypoints;
|
||||
n_seg = int(wps.size()) - 1;
|
||||
n_per_seg = n_order + 1;
|
||||
mean_vel = meanvel;
|
||||
}
|
||||
|
||||
// 0 means each segment time is one second.
|
||||
void minsnapCloseform::init_ts(int init_type)
|
||||
{
|
||||
const double dist_min = 2.0;
|
||||
ts = Eigen::VectorXd::Zero(n_seg);
|
||||
if (init_type)
|
||||
{
|
||||
Eigen::VectorXd dist(n_seg);
|
||||
double dist_sum = 0, t_sum = 0;
|
||||
for (int i = 0; i < n_seg; i++)
|
||||
{
|
||||
dist(i) = 0;
|
||||
for (int j = 0; j < 3; j++)
|
||||
{
|
||||
dist(i) += pow(wps[i + 1](j) - wps[i](j), 2);
|
||||
}
|
||||
dist(i) = sqrt(dist(i));
|
||||
if ((dist(i)) < dist_min)
|
||||
{
|
||||
dist(i) = sqrt(dist(i)) * 2;
|
||||
}
|
||||
dist_sum += dist(i);
|
||||
}
|
||||
dist(0) += 1;
|
||||
dist(n_seg - 1) += 1;
|
||||
dist_sum += 2;
|
||||
double T = dist_sum / mean_vel;
|
||||
for (int i = 0; i < n_seg - 1; i++)
|
||||
{
|
||||
ts(i) = dist(i) / dist_sum * T;
|
||||
t_sum += ts(i);
|
||||
}
|
||||
ts(n_seg - 1) = T - t_sum;
|
||||
}
|
||||
else
|
||||
{
|
||||
for (int i = 0; i < n_seg; i++)
|
||||
{
|
||||
ts(i) = 1;
|
||||
}
|
||||
}
|
||||
cout << "ts: " << ts.transpose() << endl;
|
||||
}
|
||||
|
||||
int minsnapCloseform::fact(int n)
|
||||
{
|
||||
if (n < 0)
|
||||
{
|
||||
cout << "ERROR fact(" << n << ")" << endl;
|
||||
return 0;
|
||||
}
|
||||
else if (n == 0)
|
||||
{
|
||||
return 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
return n * fact(n - 1);
|
||||
}
|
||||
}
|
||||
|
||||
Eigen::VectorXd minsnapCloseform::calDecVel(const Eigen::VectorXd decvel)
|
||||
{
|
||||
Eigen::VectorXd temp(Eigen::VectorXd::Zero((n_seg + 1) * 4));
|
||||
for (int i = 0; i < (n_seg + 1) / 2; i++)
|
||||
{
|
||||
temp.segment(i * 8, 8) = decvel.segment((i * 2) * 8, 8);
|
||||
}
|
||||
if (n_seg % 2 != 1)
|
||||
{
|
||||
temp.tail(4) = decvel.tail(4);
|
||||
}
|
||||
|
||||
return temp;
|
||||
}
|
||||
|
||||
void minsnapCloseform::calQ()
|
||||
{
|
||||
Q = Eigen::MatrixXd::Zero(n_seg * (n_order + 1), n_seg * (n_order + 1));
|
||||
for (int k = 0; k < n_seg; k++)
|
||||
{
|
||||
Eigen::MatrixXd Q_k(Eigen::MatrixXd::Zero(n_order + 1, n_order + 1));
|
||||
for (int i = 5; i <= n_order + 1; i++)
|
||||
{
|
||||
for (int j = 5; j <= n_order + 1; j++)
|
||||
{
|
||||
Q_k(i - 1, j - 1) = fact(i) / fact(i - 4) *
|
||||
fact(j) / fact(j - 4) /
|
||||
(i + j - 7) * pow(ts(k), i + j - 7);
|
||||
}
|
||||
}
|
||||
Q.block(k * (n_order + 1), k * (n_order + 1), n_order + 1, n_order + 1) = Q_k;
|
||||
}
|
||||
}
|
||||
|
||||
void minsnapCloseform::calM()
|
||||
{
|
||||
M = Eigen::MatrixXd::Zero(n_seg * (n_order + 1), n_seg * (n_order + 1));
|
||||
for (int k = 0; k < n_seg; k++)
|
||||
{
|
||||
Eigen::MatrixXd M_k(Eigen::MatrixXd::Zero(n_order + 1, n_order + 1));
|
||||
M_k(0, 0) = 1;
|
||||
M_k(1, 1) = 1;
|
||||
M_k(2, 2) = 2;
|
||||
M_k(3, 3) = 6;
|
||||
for (int i = 0; i <= n_order; i++)
|
||||
{
|
||||
for (int j = 0; j <= 3; j++)
|
||||
{
|
||||
if (i >= j)
|
||||
{
|
||||
M_k(j + 4, i) = fact(i) / fact(i - j) * pow(ts(k), i - j);
|
||||
}
|
||||
}
|
||||
}
|
||||
M.block(k * (n_order + 1), k * (n_order + 1), n_order + 1, n_order + 1) = M_k;
|
||||
}
|
||||
}
|
||||
|
||||
void minsnapCloseform::calCt()
|
||||
{
|
||||
int m = n_seg * (n_order + 1);
|
||||
int n = 4 * (n_seg + 1);
|
||||
Ct = Eigen::MatrixXd::Zero(m, n);
|
||||
for (int i = 0; i < n_seg; i++)
|
||||
{
|
||||
Ct.block(i * (n_order + 1), i * 4, n_order + 1, n_order + 1) =
|
||||
Eigen::MatrixXd::Identity(n_order + 1, n_order + 1);
|
||||
}
|
||||
|
||||
Eigen::MatrixXd dF_Ct = Eigen::MatrixXd::Zero(m, 8 + (n_seg - 1));
|
||||
Eigen::MatrixXd dP_Ct = Eigen::MatrixXd::Zero(m, (n_seg - 1) * 3);
|
||||
dF_Ct.leftCols(4) = Ct.leftCols(4);
|
||||
for (int i = 0; i < n_seg - 1; i++)
|
||||
{
|
||||
dF_Ct.col(i + 4) = Ct.col(i * 4 + 4);
|
||||
dP_Ct.middleCols(i * 3, 3) = Ct.middleCols(i * 4 + 5, 3);
|
||||
}
|
||||
dF_Ct.rightCols(4) = Ct.rightCols(4);
|
||||
Ct << dF_Ct, dP_Ct;
|
||||
}
|
||||
|
||||
std::pair<Eigen::VectorXd, Eigen::VectorXd> minsnapCloseform::MinSnapCloseFormServer(const Eigen::VectorXd &wp)
|
||||
{
|
||||
std::pair<Eigen::VectorXd, Eigen::MatrixXd> return_vel;
|
||||
Eigen::VectorXd poly_coef, dec_vel;
|
||||
Eigen::VectorXd dF(n_seg + 7), dP(3 * (n_seg - 1));
|
||||
Eigen::VectorXd start_cond(4), end_cond(4), d(4 * n_seg + 4);
|
||||
Eigen::MatrixXd R, R_pp, R_fp;
|
||||
start_cond << wp.head(1), 0, 0, 0;
|
||||
end_cond << wp.tail(1), 0, 0, 0;
|
||||
init_ts(1);
|
||||
|
||||
calQ();
|
||||
calM();
|
||||
calCt();
|
||||
|
||||
|
||||
R = Ct.transpose() * M.inverse().transpose() * Q * M.inverse() * Ct;
|
||||
R_pp = R.bottomRightCorner(3 * (n_seg - 1), 3 * (n_seg - 1));
|
||||
R_fp = R.topRightCorner(n_seg + 7, 3 * (n_seg - 1));
|
||||
|
||||
dF << start_cond, wp.segment(1, n_seg - 1), end_cond;
|
||||
dP = -R_pp.inverse() * R_fp.transpose() * dF;
|
||||
d << dF, dP;
|
||||
|
||||
dec_vel = Ct * d;
|
||||
poly_coef = M.inverse() * dec_vel;
|
||||
|
||||
return_vel.first = poly_coef;
|
||||
return_vel.second = dec_vel;
|
||||
return return_vel;
|
||||
}
|
||||
|
||||
void minsnapCloseform::calMinsnap_polycoef()
|
||||
{
|
||||
Eigen::VectorXd wps_x(n_seg + 1), wps_y(n_seg + 1), wps_z(n_seg + 1);
|
||||
for (int i = 0; i < n_seg + 1; i++)
|
||||
{
|
||||
wps_x(i) = wps[i](0);
|
||||
wps_y(i) = wps[i](1);
|
||||
wps_z(i) = wps[i](2);
|
||||
}
|
||||
std::pair<Eigen::VectorXd, Eigen::VectorXd> return_vel;
|
||||
return_vel = MinSnapCloseFormServer(wps_x);
|
||||
poly_coef_x = return_vel.first;
|
||||
dec_vel_x = calDecVel(return_vel.second);
|
||||
return_vel = MinSnapCloseFormServer(wps_y);
|
||||
poly_coef_y = return_vel.first;
|
||||
dec_vel_y = calDecVel(return_vel.second);
|
||||
return_vel = MinSnapCloseFormServer(wps_z);
|
||||
poly_coef_z = return_vel.first;
|
||||
dec_vel_z = calDecVel(return_vel.second);
|
||||
}
|
||||
|
||||
Eigen::MatrixXd minsnapCloseform::getPolyCoef()
|
||||
{
|
||||
Eigen::MatrixXd poly_coef(poly_coef_x.size(), 3);
|
||||
poly_coef << poly_coef_x, poly_coef_y, poly_coef_z;
|
||||
return poly_coef;
|
||||
}
|
||||
|
||||
Eigen::MatrixXd minsnapCloseform::getDecVel()
|
||||
{
|
||||
Eigen::MatrixXd dec_vel(dec_vel_x.size(), 3);
|
||||
dec_vel << dec_vel_x, dec_vel_y, dec_vel_z;
|
||||
return dec_vel;
|
||||
}
|
||||
|
||||
Eigen::VectorXd minsnapCloseform::getTime()
|
||||
{
|
||||
return ts;
|
||||
}
|
||||
|
||||
}
|
||||
@ -1,137 +0,0 @@
|
||||
#include <ros/ros.h>
|
||||
#include <iostream>
|
||||
#include <geometry_msgs/PoseStamped.h>
|
||||
#include <geometry_msgs/PoseArray.h>
|
||||
#include <nav_msgs/Odometry.h>
|
||||
#include <quadrotor_msgs/PolynomialTrajectory.h>
|
||||
#include <quadrotor_msgs/PositionCommand.h>
|
||||
#include "min_snap/min_snap_closeform.h"
|
||||
|
||||
ros::Publisher goal_list_pub;
|
||||
ros::Publisher poly_coef_pub;
|
||||
ros::Subscriber rviz_goal_sub;
|
||||
ros::Subscriber odom_sub;
|
||||
|
||||
int id = 0;
|
||||
double mean_vel = 0;
|
||||
const int GOAL_HEIGHT = 1;
|
||||
nav_msgs::Odometry odom;
|
||||
geometry_msgs::Pose goal_pt;
|
||||
geometry_msgs::PoseArray goal_list;
|
||||
my_planner::minsnapCloseform minsnap_solver;
|
||||
std::vector<Eigen::Vector3d> waypoints;
|
||||
quadrotor_msgs::PolynomialTrajectory poly_pub_topic;
|
||||
|
||||
void pub_poly_coefs()
|
||||
{
|
||||
Eigen::MatrixXd poly_coef = minsnap_solver.getPolyCoef();
|
||||
Eigen::MatrixXd dec_vel = minsnap_solver.getDecVel();
|
||||
Eigen::VectorXd time = minsnap_solver.getTime();
|
||||
|
||||
poly_pub_topic.num_segment = goal_list.poses.size() - 1;
|
||||
poly_pub_topic.coef_x.clear();
|
||||
poly_pub_topic.coef_y.clear();
|
||||
poly_pub_topic.coef_z.clear();
|
||||
poly_pub_topic.time.clear();
|
||||
poly_pub_topic.trajectory_id = id;
|
||||
|
||||
// display decision variable
|
||||
ROS_WARN("decision variable:");
|
||||
for (int i = 0; i < goal_list.poses.size(); i++)
|
||||
{
|
||||
cout << "Point number = " << i + 1 << endl
|
||||
<< dec_vel.middleRows(i * 4, 4) << endl;
|
||||
}
|
||||
|
||||
for (int i = 0; i < time.size(); i++)
|
||||
{
|
||||
for (int j = (i + 1) * 8 - 1; j >= i * 8; j--)
|
||||
{
|
||||
poly_pub_topic.coef_x.push_back(poly_coef(j, 0));
|
||||
poly_pub_topic.coef_y.push_back(poly_coef(j, 1));
|
||||
poly_pub_topic.coef_z.push_back(poly_coef(j, 2));
|
||||
}
|
||||
poly_pub_topic.time.push_back(time(i));
|
||||
}
|
||||
|
||||
poly_pub_topic.header.frame_id = "world";
|
||||
poly_pub_topic.header.stamp = ros::Time::now();
|
||||
|
||||
poly_coef_pub.publish(poly_pub_topic);
|
||||
}
|
||||
|
||||
void solve_min_snap()
|
||||
{
|
||||
Eigen::Vector3d wp;
|
||||
waypoints.clear();
|
||||
for (int i = 0; i < int(goal_list.poses.size()); i++)
|
||||
{
|
||||
wp << goal_list.poses[i].position.x, goal_list.poses[i].position.y, goal_list.poses[i].position.z;
|
||||
waypoints.push_back(wp);
|
||||
}
|
||||
minsnap_solver.Init(waypoints, mean_vel);
|
||||
ROS_INFO("Init success");
|
||||
minsnap_solver.calMinsnap_polycoef();
|
||||
pub_poly_coefs();
|
||||
}
|
||||
|
||||
void rviz_goal_cb(const geometry_msgs::PoseStamped::ConstPtr &msg)
|
||||
{
|
||||
goal_pt = msg->pose;
|
||||
if (goal_pt.position.z < 0)
|
||||
{
|
||||
goal_pt.position.z = GOAL_HEIGHT;
|
||||
goal_list.poses.push_back(goal_pt);
|
||||
goal_pt.position = odom.pose.pose.position;
|
||||
goal_list.poses.insert(goal_list.poses.begin(), goal_pt);
|
||||
goal_list.header.stamp = ros::Time::now();
|
||||
goal_list.header.frame_id = "world";
|
||||
goal_list.header.seq = id++;
|
||||
goal_list_pub.publish(goal_list);
|
||||
solve_min_snap();
|
||||
ROS_INFO("solver finished");
|
||||
goal_list.poses.clear();
|
||||
}
|
||||
else
|
||||
{
|
||||
goal_pt.position.z = GOAL_HEIGHT;
|
||||
goal_list.poses.push_back(goal_pt);
|
||||
}
|
||||
}
|
||||
|
||||
void odom_goal_cb(const nav_msgs::Odometry::ConstPtr &msg)
|
||||
{
|
||||
odom = *msg;
|
||||
}
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
ros::init(argc, argv, "min_snap_generator");
|
||||
ros::NodeHandle nh("~");
|
||||
ros::Rate rate(10);
|
||||
|
||||
// 【订阅】里程计（此处注意话题名称替换）
|
||||
odom_sub = nh.subscribe("/odom_topic", 10, odom_goal_cb);
|
||||
// 【订阅】RVIZ目标点
|
||||
rviz_goal_sub = nh.subscribe("/rviz_goal", 10, rviz_goal_cb);
|
||||
// 【发布】目标点，用于目标点显示
|
||||
goal_list_pub = nh.advertise<geometry_msgs::PoseArray>("/goal_list", 10);
|
||||
// 发布多项式轨迹
|
||||
poly_coef_pub = nh.advertise<quadrotor_msgs::PolynomialTrajectory>("/poly_coefs", 10);
|
||||
|
||||
poly_pub_topic.num_order = 7;
|
||||
poly_pub_topic.start_yaw = 0;
|
||||
poly_pub_topic.final_yaw = 0;
|
||||
poly_pub_topic.mag_coeff = 0;
|
||||
poly_pub_topic.order.push_back(0);
|
||||
|
||||
ros::param::get("/min_snap_generator/meanvel", mean_vel);
|
||||
|
||||
while (ros::ok())
|
||||
{
|
||||
ros::spinOnce();
|
||||
rate.sleep();
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
Binary file not shown.
@ -1,217 +0,0 @@
|
||||
cmake_minimum_required(VERSION 3.0.2)
|
||||
project(traj_server)
|
||||
|
||||
## Compile as C++11, supported in ROS Kinetic and newer
|
||||
# add_compile_options(-std=c++11)
|
||||
|
||||
## Find catkin macros and libraries
|
||||
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
|
||||
## is used, also find other catkin packages
|
||||
|
||||
find_package(Eigen3 REQUIRED)
|
||||
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
roscpp
|
||||
rospy
|
||||
std_msgs
|
||||
geometry_msgs
|
||||
quadrotor_msgs
|
||||
mini_snap_traj_utils
|
||||
)
|
||||
|
||||
## System dependencies are found with CMake's conventions
|
||||
# find_package(Boost REQUIRED COMPONENTS system)
|
||||
|
||||
|
||||
## Uncomment this if the package has a setup.py. This macro ensures
|
||||
## modules and global scripts declared therein get installed
|
||||
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
|
||||
# catkin_python_setup()
|
||||
|
||||
################################################
|
||||
## Declare ROS messages, services and actions ##
|
||||
################################################
|
||||
|
||||
## To declare and build messages, services or actions from within this
|
||||
## package, follow these steps:
|
||||
## * Let MSG_DEP_SET be the set of packages whose message types you use in
|
||||
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
|
||||
## * In the file package.xml:
|
||||
## * add a build_depend tag for "message_generation"
|
||||
## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
|
||||
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
|
||||
## but can be declared for certainty nonetheless:
|
||||
## * add a exec_depend tag for "message_runtime"
|
||||
## * In this file (CMakeLists.txt):
|
||||
## * add "message_generation" and every package in MSG_DEP_SET to
|
||||
## find_package(catkin REQUIRED COMPONENTS ...)
|
||||
## * add "message_runtime" and every package in MSG_DEP_SET to
|
||||
## catkin_package(CATKIN_DEPENDS ...)
|
||||
## * uncomment the add_*_files sections below as needed
|
||||
## and list every .msg/.srv/.action file to be processed
|
||||
## * uncomment the generate_messages entry below
|
||||
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
|
||||
|
||||
## Generate messages in the 'msg' folder
|
||||
# add_message_files(
|
||||
# FILES
|
||||
# Message1.msg
|
||||
# Message2.msg
|
||||
# )
|
||||
|
||||
## Generate services in the 'srv' folder
|
||||
# add_service_files(
|
||||
# FILES
|
||||
# Service1.srv
|
||||
# Service2.srv
|
||||
# )
|
||||
|
||||
## Generate actions in the 'action' folder
|
||||
# add_action_files(
|
||||
# FILES
|
||||
# Action1.action
|
||||
# Action2.action
|
||||
# )
|
||||
|
||||
## Generate added messages and services with any dependencies listed here
|
||||
# generate_messages(
|
||||
# DEPENDENCIES
|
||||
# std_msgs
|
||||
# )
|
||||
|
||||
################################################
|
||||
## Declare ROS dynamic reconfigure parameters ##
|
||||
################################################
|
||||
|
||||
## To declare and build dynamic reconfigure parameters within this
|
||||
## package, follow these steps:
|
||||
## * In the file package.xml:
|
||||
## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
|
||||
## * In this file (CMakeLists.txt):
|
||||
## * add "dynamic_reconfigure" to
|
||||
## find_package(catkin REQUIRED COMPONENTS ...)
|
||||
## * uncomment the "generate_dynamic_reconfigure_options" section below
|
||||
## and list every .cfg file to be processed
|
||||
|
||||
## Generate dynamic reconfigure parameters in the 'cfg' folder
|
||||
# generate_dynamic_reconfigure_options(
|
||||
# cfg/DynReconf1.cfg
|
||||
# cfg/DynReconf2.cfg
|
||||
# )
|
||||
|
||||
###################################
|
||||
## catkin specific configuration ##
|
||||
###################################
|
||||
## The catkin_package macro generates cmake config files for your package
|
||||
## Declare things to be passed to dependent projects
|
||||
## INCLUDE_DIRS: uncomment this if your package contains header files
|
||||
## LIBRARIES: libraries you create in this project that dependent projects also need
|
||||
## CATKIN_DEPENDS: catkin_packages dependent projects also need
|
||||
## DEPENDS: system dependencies of this project that dependent projects also need
|
||||
catkin_package(
|
||||
# INCLUDE_DIRS include
|
||||
# LIBRARIES traj_server
|
||||
# CATKIN_DEPENDS roscpp rospy std_msgs
|
||||
# DEPENDS system_lib
|
||||
)
|
||||
|
||||
###########
|
||||
## Build ##
|
||||
###########
|
||||
|
||||
## Specify additional locations of header files
|
||||
## Your package locations should be listed before other locations
|
||||
include_directories(
|
||||
# include
|
||||
${catkin_INCLUDE_DIRS}
|
||||
${EIGEN3_INCLUDE_DIR}
|
||||
)
|
||||
|
||||
## Declare a C++ library
|
||||
# add_library(${PROJECT_NAME}
|
||||
# src/${PROJECT_NAME}/traj_server.cpp
|
||||
# )
|
||||
|
||||
## Add cmake target dependencies of the library
|
||||
## as an example, code may need to be generated before libraries
|
||||
## either from message generation or dynamic reconfigure
|
||||
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
|
||||
## Declare a C++ executable
|
||||
## With catkin_make all packages are built within a single CMake context
|
||||
## The recommended prefix ensures that target names across packages don't collide
|
||||
# add_executable(${PROJECT_NAME}_node src/traj_server_node.cpp)
|
||||
|
||||
## Rename C++ executable without prefix
|
||||
## The above recommended prefix causes long target names, the following renames the
|
||||
## target back to the shorter version for ease of user use
|
||||
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
|
||||
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
|
||||
|
||||
## Add cmake target dependencies of the executable
|
||||
## same as for the library above
|
||||
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
|
||||
## Specify libraries to link a library or executable target against
|
||||
# target_link_libraries(${PROJECT_NAME}_node
|
||||
# ${catkin_LIBRARIES}
|
||||
# )
|
||||
|
||||
add_executable(my_traj_server src/my_traj_server.cpp)
|
||||
target_link_libraries(my_traj_server ${catkin_LIBRARIES})
|
||||
# add_dependencies(my_traj_server ${${PROJECT_NAME}_EXPORTED_TARGETS})
|
||||
|
||||
#############
|
||||
## Install ##
|
||||
#############
|
||||
|
||||
# all install targets should use catkin DESTINATION variables
|
||||
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
|
||||
|
||||
## Mark executable scripts (Python etc.) for installation
|
||||
## in contrast to setup.py, you can choose the destination
|
||||
# catkin_install_python(PROGRAMS
|
||||
# scripts/my_python_script
|
||||
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
# )
|
||||
|
||||
## Mark executables for installation
|
||||
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
|
||||
# install(TARGETS ${PROJECT_NAME}_node
|
||||
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
# )
|
||||
|
||||
## Mark libraries for installation
|
||||
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
|
||||
# install(TARGETS ${PROJECT_NAME}
|
||||
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
|
||||
# )
|
||||
|
||||
## Mark cpp header files for installation
|
||||
# install(DIRECTORY include/${PROJECT_NAME}/
|
||||
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
|
||||
# FILES_MATCHING PATTERN "*.h"
|
||||
# PATTERN ".svn" EXCLUDE
|
||||
# )
|
||||
|
||||
## Mark other files for installation (e.g. launch and bag files, etc.)
|
||||
# install(FILES
|
||||
# # myfile1
|
||||
# # myfile2
|
||||
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
|
||||
# )
|
||||
|
||||
#############
|
||||
## Testing ##
|
||||
#############
|
||||
|
||||
## Add gtest based cpp test target and link libraries
|
||||
# catkin_add_gtest(${PROJECT_NAME}-test test/test_traj_server.cpp)
|
||||
# if(TARGET ${PROJECT_NAME}-test)
|
||||
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
|
||||
# endif()
|
||||
|
||||
## Add folders to be run by python nosetests
|
||||
# catkin_add_nosetests(test)
|
||||
@ -1,70 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>traj_server</name>
|
||||
<version>0.0.0</version>
|
||||
<description>The traj_server package</description>
|
||||
|
||||
<!-- One maintainer tag required, multiple allowed, one person per tag -->
|
||||
<!-- Example: -->
|
||||
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
|
||||
<maintainer email="mywang@todo.todo">mywang</maintainer>
|
||||
|
||||
|
||||
<!-- One license tag required, multiple allowed, one license per tag -->
|
||||
<!-- Commonly used license strings: -->
|
||||
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
|
||||
<license>TODO</license>
|
||||
|
||||
|
||||
<!-- Url tags are optional, but multiple are allowed, one per tag -->
|
||||
<!-- Optional attribute type can be: website, bugtracker, or repository -->
|
||||
<!-- Example: -->
|
||||
<!-- <url type="website">http://wiki.ros.org/traj_server</url> -->
|
||||
|
||||
|
||||
<!-- Author tags are optional, multiple are allowed, one per tag -->
|
||||
<!-- Authors do not have to be maintainers, but could be -->
|
||||
<!-- Example: -->
|
||||
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
|
||||
|
||||
|
||||
<!-- The *depend tags are used to specify dependencies -->
|
||||
<!-- Dependencies can be catkin packages or system dependencies -->
|
||||
<!-- Examples: -->
|
||||
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
|
||||
<!-- <depend>roscpp</depend> -->
|
||||
<!-- Note that this is equivalent to the following: -->
|
||||
<!-- <build_depend>roscpp</build_depend> -->
|
||||
<!-- <exec_depend>roscpp</exec_depend> -->
|
||||
<!-- Use build_depend for packages you need at compile time: -->
|
||||
<!-- <build_depend>message_generation</build_depend> -->
|
||||
<!-- Use build_export_depend for packages you need in order to build against this package: -->
|
||||
<!-- <build_export_depend>message_generation</build_export_depend> -->
|
||||
<!-- Use buildtool_depend for build tool packages: -->
|
||||
<!-- <buildtool_depend>catkin</buildtool_depend> -->
|
||||
<!-- Use exec_depend for packages you need at runtime: -->
|
||||
<!-- <exec_depend>message_runtime</exec_depend> -->
|
||||
<!-- Use test_depend for packages you need only for testing: -->
|
||||
<!-- <test_depend>gtest</test_depend> -->
|
||||
<!-- Use doc_depend for packages you need only for building documentation: -->
|
||||
<!-- <doc_depend>doxygen</doc_depend> -->
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<build_depend>roscpp</build_depend>
|
||||
<build_depend>rospy</build_depend>
|
||||
<build_depend>std_msgs</build_depend>
|
||||
<build_export_depend>roscpp</build_export_depend>
|
||||
<build_export_depend>rospy</build_export_depend>
|
||||
<build_export_depend>std_msgs</build_export_depend>
|
||||
<exec_depend>roscpp</exec_depend>
|
||||
<exec_depend>rospy</exec_depend>
|
||||
<exec_depend>std_msgs</exec_depend>
|
||||
|
||||
<build_depend>mini_snap_traj_utils</build_depend>
|
||||
<exec_depend>mini_snap_traj_utils</exec_depend>
|
||||
|
||||
<!-- The export tag contains other, unspecified, tags -->
|
||||
<export>
|
||||
<!-- Other tools can request additional information be placed here -->
|
||||
|
||||
</export>
|
||||
</package>
|
||||
@ -1,264 +0,0 @@
|
||||
#include <ros/ros.h>
|
||||
#include "quadrotor_msgs/PositionCommand.h"
|
||||
#include "quadrotor_msgs/PolynomialTrajectory.h"
|
||||
#include "mini_snap_traj_utils/polynomial_traj.h"
|
||||
#include <geometry_msgs/PoseArray.h>
|
||||
|
||||
ros::Publisher pose_cmd_pub;
|
||||
ros::Publisher traj_pts_pub;
|
||||
ros::Subscriber poly_traj_sub;
|
||||
|
||||
PolynomialTraj Poly_traj;
|
||||
quadrotor_msgs::PositionCommand cmd;
|
||||
ros::Time start_time;
|
||||
double pos_gain[3] = {0, 0, 0};
|
||||
double vel_gain[3] = {0, 0, 0};
|
||||
|
||||
double last_yaw_, last_yaw_dot_;
|
||||
bool receive_traj_ = false;
|
||||
int traj_id_;
|
||||
double traj_duration_, time_forward_;
|
||||
|
||||
std::pair<double, double> calculate_yaw(double t_cur, Eigen::Vector3d &pos, ros::Time &time_now, ros::Time &time_last)
|
||||
{
|
||||
constexpr double PI = 3.1415926;
|
||||
constexpr double YAW_DOT_MAX_PER_SEC = PI;
|
||||
// constexpr double YAW_DOT_DOT_MAX_PER_SEC = PI;
|
||||
std::pair<double, double> yaw_yawdot(0, 0);
|
||||
double yaw = 0;
|
||||
double yawdot = 0;
|
||||
|
||||
// Eigen::Vector3d dir = t_cur + time_forward_ <= traj_duration_ ? traj_[0].evaluateDeBoorT(t_cur + time_forward_) - pos : traj_[0].evaluateDeBoorT(traj_duration_) - pos;
|
||||
Eigen::Vector3d dir = t_cur + time_forward_ <= traj_duration_ ? Poly_traj.evaluate(t_cur + time_forward_) - pos : Poly_traj.evaluate(traj_duration_) - pos;
|
||||
double yaw_temp = dir.norm() > 0.1 ? atan2(dir(1), dir(0)) : last_yaw_;
|
||||
double max_yaw_change = YAW_DOT_MAX_PER_SEC * (time_now - time_last).toSec();
|
||||
if (yaw_temp - last_yaw_ > PI)
|
||||
{
|
||||
if (yaw_temp - last_yaw_ - 2 * PI < -max_yaw_change)
|
||||
{
|
||||
yaw = last_yaw_ - max_yaw_change;
|
||||
if (yaw < -PI)
|
||||
yaw += 2 * PI;
|
||||
|
||||
yawdot = -YAW_DOT_MAX_PER_SEC;
|
||||
}
|
||||
else
|
||||
{
|
||||
yaw = yaw_temp;
|
||||
if (yaw - last_yaw_ > PI)
|
||||
yawdot = -YAW_DOT_MAX_PER_SEC;
|
||||
else
|
||||
yawdot = (yaw_temp - last_yaw_) / (time_now - time_last).toSec();
|
||||
}
|
||||
}
|
||||
else if (yaw_temp - last_yaw_ < -PI)
|
||||
{
|
||||
if (yaw_temp - last_yaw_ + 2 * PI > max_yaw_change)
|
||||
{
|
||||
yaw = last_yaw_ + max_yaw_change;
|
||||
if (yaw > PI)
|
||||
yaw -= 2 * PI;
|
||||
|
||||
yawdot = YAW_DOT_MAX_PER_SEC;
|
||||
}
|
||||
else
|
||||
{
|
||||
yaw = yaw_temp;
|
||||
if (yaw - last_yaw_ < -PI)
|
||||
yawdot = YAW_DOT_MAX_PER_SEC;
|
||||
else
|
||||
yawdot = (yaw_temp - last_yaw_) / (time_now - time_last).toSec();
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (yaw_temp - last_yaw_ < -max_yaw_change)
|
||||
{
|
||||
yaw = last_yaw_ - max_yaw_change;
|
||||
if (yaw < -PI)
|
||||
yaw += 2 * PI;
|
||||
|
||||
yawdot = -YAW_DOT_MAX_PER_SEC;
|
||||
}
|
||||
else if (yaw_temp - last_yaw_ > max_yaw_change)
|
||||
{
|
||||
yaw = last_yaw_ + max_yaw_change;
|
||||
if (yaw > PI)
|
||||
yaw -= 2 * PI;
|
||||
|
||||
yawdot = YAW_DOT_MAX_PER_SEC;
|
||||
}
|
||||
else
|
||||
{
|
||||
yaw = yaw_temp;
|
||||
if (yaw - last_yaw_ > PI)
|
||||
yawdot = -YAW_DOT_MAX_PER_SEC;
|
||||
else if (yaw - last_yaw_ < -PI)
|
||||
yawdot = YAW_DOT_MAX_PER_SEC;
|
||||
else
|
||||
yawdot = (yaw_temp - last_yaw_) / (time_now - time_last).toSec();
|
||||
}
|
||||
}
|
||||
|
||||
if (fabs(yaw - last_yaw_) <= max_yaw_change)
|
||||
yaw = 0.5 * last_yaw_ + 0.5 * yaw; // nieve LPF
|
||||
yawdot = 0.5 * last_yaw_dot_ + 0.5 * yawdot;
|
||||
last_yaw_ = yaw;
|
||||
last_yaw_dot_ = yawdot;
|
||||
|
||||
yaw_yawdot.first = yaw;
|
||||
yaw_yawdot.second = yawdot;
|
||||
|
||||
return yaw_yawdot;
|
||||
}
|
||||
|
||||
void polyTrajCallback(quadrotor_msgs::PolynomialTrajectory::ConstPtr msg)
|
||||
{
|
||||
ROS_INFO("[my Traj server]:receive poly_traj");
|
||||
|
||||
Poly_traj.reset();
|
||||
int idx;
|
||||
for (int i = 0; i < msg->num_segment; i++)
|
||||
{
|
||||
vector<double> cx, cy, cz;
|
||||
for (int j = 0; j < msg->num_order + 1; j++)
|
||||
{
|
||||
idx = i * (msg->num_order + 1) + j;
|
||||
cx.push_back(msg->coef_x[idx]);
|
||||
cy.push_back(msg->coef_y[idx]);
|
||||
cz.push_back(msg->coef_z[idx]);
|
||||
}
|
||||
Poly_traj.addSegment(cx, cy, cz, msg->time[i]);
|
||||
}
|
||||
Poly_traj.init();
|
||||
|
||||
// ROS_INFO("[my traj server]:time=%f", Poly_traj.getTimes().front());
|
||||
|
||||
start_time = ros::Time::now();
|
||||
traj_id_ = msg->trajectory_id;
|
||||
traj_duration_ = Poly_traj.getTimeSum();
|
||||
receive_traj_ = true;
|
||||
}
|
||||
|
||||
void pub_traj(double t_cur)
|
||||
{
|
||||
static int old_cnt = 0;
|
||||
int cnt = (int)((traj_duration_ - t_cur) * 2) + 1;
|
||||
|
||||
if (cnt != old_cnt)
|
||||
{
|
||||
geometry_msgs::PoseArray traj_pts;
|
||||
geometry_msgs::Pose traj_pt;
|
||||
Eigen::Vector3d opt_pt(Eigen::Vector3d::Zero());
|
||||
|
||||
traj_pts.header.stamp = ros::Time::now();
|
||||
for (int i = 0; i < cnt + 1; i++)
|
||||
{
|
||||
opt_pt = Poly_traj.evaluate(std::min(t_cur + i * 0.5, traj_duration_));
|
||||
traj_pt.orientation.w = 1.0;
|
||||
traj_pt.position.x = opt_pt(0);
|
||||
traj_pt.position.y = opt_pt(1);
|
||||
traj_pt.position.z = opt_pt(2);
|
||||
traj_pts.poses.push_back(traj_pt);
|
||||
}
|
||||
traj_pts_pub.publish(traj_pts);
|
||||
}
|
||||
old_cnt = cnt;
|
||||
}
|
||||
|
||||
void cmdCallback(const ros::TimerEvent &e)
|
||||
{
|
||||
if (!receive_traj_)
|
||||
return;
|
||||
|
||||
Eigen::Vector3d pos(Eigen::Vector3d::Zero());
|
||||
Eigen::Vector3d vel(Eigen::Vector3d::Zero());
|
||||
Eigen::Vector3d acc(Eigen::Vector3d::Zero());
|
||||
Eigen::Vector3d jerk(Eigen::Vector3d::Zero());
|
||||
std::pair<double, double> yaw_yawdot(0, 0);
|
||||
|
||||
ros::Time time_now = ros::Time::now();
|
||||
double t_cur = (time_now - start_time).toSec();
|
||||
// ROS_INFO("time = %f", t_cur);
|
||||
static ros::Time time_last = ros::Time::now();
|
||||
|
||||
if (t_cur < traj_duration_ && t_cur >= 0.0)
|
||||
{
|
||||
pos = Poly_traj.evaluate(t_cur);
|
||||
vel = Poly_traj.evaluateVel(t_cur);
|
||||
acc = Poly_traj.evaluateAcc(t_cur);
|
||||
jerk = Poly_traj.evaluateJerk(t_cur);
|
||||
yaw_yawdot = calculate_yaw(t_cur, pos, time_now, time_last);
|
||||
pub_traj(t_cur + 0.5);
|
||||
}
|
||||
else if (t_cur >= traj_duration_)
|
||||
{
|
||||
pos = Poly_traj.evaluate(traj_duration_);
|
||||
yaw_yawdot.first = last_yaw_;
|
||||
yaw_yawdot.second = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_WARN("[my Traj server]: invalid time.");
|
||||
}
|
||||
time_last = time_now;
|
||||
|
||||
cmd.header.stamp = time_now;
|
||||
cmd.header.frame_id = "world";
|
||||
cmd.trajectory_id = traj_id_;
|
||||
|
||||
cmd.position.x = pos(0);
|
||||
cmd.position.y = pos(1);
|
||||
cmd.position.z = pos(2);
|
||||
|
||||
cmd.velocity.x = vel(0);
|
||||
cmd.velocity.y = vel(1);
|
||||
cmd.velocity.z = vel(2);
|
||||
|
||||
cmd.acceleration.x = acc(0);
|
||||
cmd.acceleration.y = acc(1);
|
||||
cmd.acceleration.z = acc(2);
|
||||
|
||||
cmd.jerk.x = jerk(0);
|
||||
cmd.jerk.y = jerk(1);
|
||||
cmd.jerk.z = jerk(2);
|
||||
|
||||
cmd.yaw = yaw_yawdot.first;
|
||||
cmd.yaw_dot = yaw_yawdot.second;
|
||||
|
||||
last_yaw_ = cmd.yaw;
|
||||
|
||||
pose_cmd_pub.publish(cmd);
|
||||
}
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
ros::init(argc, argv, "my_traj_server");
|
||||
ros::NodeHandle nh("~");
|
||||
|
||||
pose_cmd_pub = nh.advertise<quadrotor_msgs::PositionCommand>("/position_cmd", 50);
|
||||
traj_pts_pub = nh.advertise<geometry_msgs::PoseArray>("/traj_pts", 50);
|
||||
poly_traj_sub = nh.subscribe("/poly_coefs", 10, polyTrajCallback);
|
||||
|
||||
ros::Timer cmd_timer = nh.createTimer(ros::Duration(0.01), cmdCallback);
|
||||
|
||||
/* control parameter */
|
||||
cmd.kx[0] = pos_gain[0];
|
||||
cmd.kx[1] = pos_gain[1];
|
||||
cmd.kx[2] = pos_gain[2];
|
||||
|
||||
cmd.kv[0] = vel_gain[0];
|
||||
cmd.kv[1] = vel_gain[1];
|
||||
cmd.kv[2] = vel_gain[2];
|
||||
last_yaw_ = 0.0;
|
||||
last_yaw_dot_ = 0.0;
|
||||
time_forward_ = 1.0;
|
||||
|
||||
ros::Duration(1.0).sleep();
|
||||
|
||||
ROS_INFO("[my Traj server]: ready.");
|
||||
|
||||
ros::spin();
|
||||
|
||||
return 0;
|
||||
}
|
||||
@ -1,39 +0,0 @@
|
||||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(mini_snap_traj_utils)
|
||||
|
||||
set(CMAKE_BUILD_TYPE "Release")
|
||||
set(CMAKE_CXX_FLAGS "-std=c++11")
|
||||
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g")
|
||||
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
roscpp
|
||||
std_msgs
|
||||
cv_bridge
|
||||
)
|
||||
|
||||
find_package(Eigen3 REQUIRED)
|
||||
find_package(PCL 1.7 REQUIRED)
|
||||
|
||||
catkin_package(
|
||||
INCLUDE_DIRS include
|
||||
LIBRARIES mini_snap_traj_utils
|
||||
# DEPENDS system_lib
|
||||
)
|
||||
|
||||
include_directories(
|
||||
SYSTEM
|
||||
include
|
||||
${catkin_INCLUDE_DIRS}
|
||||
${Eigen3_INCLUDE_DIRS}
|
||||
${PCL_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
link_directories(${PCL_LIBRARY_DIRS})
|
||||
|
||||
add_library( mini_snap_traj_utils
|
||||
src/planning_visualization.cpp
|
||||
src/polynomial_traj.cpp
|
||||
)
|
||||
target_link_libraries( mini_snap_traj_utils
|
||||
${catkin_LIBRARIES}
|
||||
)
|
||||
@ -1,54 +0,0 @@
|
||||
#ifndef _PLANNING_VISUALIZATION_H_
|
||||
#define _PLANNING_VISUALIZATION_H_
|
||||
|
||||
#include <Eigen/Eigen>
|
||||
#include <algorithm>
|
||||
#include <iostream>
|
||||
#include <mini_snap_traj_utils/polynomial_traj.h>
|
||||
#include <ros/ros.h>
|
||||
#include <vector>
|
||||
#include <visualization_msgs/Marker.h>
|
||||
#include <visualization_msgs/MarkerArray.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
using std::vector;
|
||||
namespace my_planner
|
||||
{
|
||||
class PlanningVisualization
|
||||
{
|
||||
private:
|
||||
ros::NodeHandle node;
|
||||
|
||||
ros::Publisher goal_point_pub;
|
||||
ros::Publisher global_list_pub;
|
||||
ros::Publisher init_list_pub;
|
||||
ros::Publisher optimal_list_pub;
|
||||
ros::Publisher a_star_list_pub;
|
||||
ros::Publisher guide_vector_pub;
|
||||
ros::Publisher intermediate_state_pub;
|
||||
|
||||
public:
|
||||
PlanningVisualization(/* args */) {}
|
||||
~PlanningVisualization() {}
|
||||
PlanningVisualization(ros::NodeHandle &nh);
|
||||
|
||||
void Init(ros::NodeHandle &nh);
|
||||
typedef std::shared_ptr<PlanningVisualization> Ptr;
|
||||
|
||||
void displayMarkerList(ros::Publisher &pub, const vector<Eigen::Vector3d> &list, double scale,
|
||||
Eigen::Vector4d color, int id);
|
||||
void generatePathDisplayArray(visualization_msgs::MarkerArray &array,
|
||||
const vector<Eigen::Vector3d> &list, double scale, Eigen::Vector4d color, int id);
|
||||
void generateArrowDisplayArray(visualization_msgs::MarkerArray &array,
|
||||
const vector<Eigen::Vector3d> &list, double scale, Eigen::Vector4d color, int id);
|
||||
void displayGoalPoint(Eigen::Vector3d goal_point, Eigen::Vector4d color, const double scale, int id);
|
||||
void displayGlobalPathList(vector<Eigen::Vector3d> global_pts, const double scale, int id);
|
||||
void displayInitPathList(vector<Eigen::Vector3d> init_pts, const double scale, int id);
|
||||
void displayOptimalList(Eigen::MatrixXd optimal_pts, int id);
|
||||
void displayAStarList(std::vector<std::vector<Eigen::Vector3d>> a_star_paths, int id);
|
||||
void displayArrowList(ros::Publisher &pub, const vector<Eigen::Vector3d> &list, double scale, Eigen::Vector4d color, int id);
|
||||
// void displayIntermediateState(ros::Publisher& intermediate_pub, ego_planner::BsplineOptimizer::Ptr optimizer, double sleep_time, const int start_iteration);
|
||||
// void displayNewArrow(ros::Publisher& guide_vector_pub, ego_planner::BsplineOptimizer::Ptr optimizer);
|
||||
};
|
||||
} // namespace ego_planner
|
||||
#endif
|
||||
@ -1,368 +0,0 @@
|
||||
#ifndef _POLYNOMIAL_TRAJ_H
|
||||
#define _POLYNOMIAL_TRAJ_H
|
||||
|
||||
#include <Eigen/Eigen>
|
||||
#include <vector>
|
||||
|
||||
using std::vector;
|
||||
|
||||
class PolynomialTraj
|
||||
{
|
||||
private:
|
||||
vector<double> times; // time of each segment
|
||||
vector<vector<double>> cxs; // coefficient of x of each segment, from n-1 -> 0
|
||||
vector<vector<double>> cys; // coefficient of y of each segment
|
||||
vector<vector<double>> czs; // coefficient of z of each segment
|
||||
|
||||
double time_sum;
|
||||
int num_seg;
|
||||
|
||||
/* evaluation */
|
||||
vector<Eigen::Vector3d> traj_vec3d;
|
||||
double length;
|
||||
|
||||
public:
|
||||
PolynomialTraj(/* args */)
|
||||
{
|
||||
}
|
||||
~PolynomialTraj()
|
||||
{
|
||||
}
|
||||
|
||||
void reset()
|
||||
{
|
||||
times.clear(), cxs.clear(), cys.clear(), czs.clear();
|
||||
time_sum = 0.0, num_seg = 0;
|
||||
}
|
||||
|
||||
void addSegment(vector<double> cx, vector<double> cy, vector<double> cz, double t)
|
||||
{
|
||||
cxs.push_back(cx), cys.push_back(cy), czs.push_back(cz), times.push_back(t);
|
||||
}
|
||||
|
||||
void init()
|
||||
{
|
||||
num_seg = times.size();
|
||||
time_sum = 0.0;
|
||||
for (int i = 0; i < times.size(); ++i)
|
||||
{
|
||||
time_sum += times[i];
|
||||
}
|
||||
}
|
||||
|
||||
vector<double> getTimes()
|
||||
{
|
||||
return times;
|
||||
}
|
||||
|
||||
vector<vector<double>> getCoef(int axis)
|
||||
{
|
||||
switch (axis)
|
||||
{
|
||||
case 0:
|
||||
return cxs;
|
||||
case 1:
|
||||
return cys;
|
||||
case 2:
|
||||
return czs;
|
||||
default:
|
||||
std::cout << "\033[31mIllegal axis!\033[0m" << std::endl;
|
||||
}
|
||||
|
||||
vector<vector<double>> empty;
|
||||
return empty;
|
||||
}
|
||||
|
||||
Eigen::Vector3d evaluate(double t)
|
||||
{
|
||||
/* detetrmine segment num */
|
||||
int idx = 0;
|
||||
while (times[idx] + 1e-4 < t)
|
||||
{
|
||||
t -= times[idx];
|
||||
++idx;
|
||||
}
|
||||
|
||||
/* evaluation */
|
||||
int order = cxs[idx].size();
|
||||
Eigen::VectorXd cx(order), cy(order), cz(order), tv(order);
|
||||
for (int i = 0; i < order; ++i)
|
||||
{
|
||||
cx(i) = cxs[idx][i], cy(i) = cys[idx][i], cz(i) = czs[idx][i];
|
||||
tv(order - 1 - i) = std::pow(t, double(i));
|
||||
}
|
||||
|
||||
Eigen::Vector3d pt;
|
||||
pt(0) = tv.dot(cx), pt(1) = tv.dot(cy), pt(2) = tv.dot(cz);
|
||||
return pt;
|
||||
}
|
||||
|
||||
Eigen::Vector3d evaluateVel(double t)
|
||||
{
|
||||
/* detetrmine segment num */
|
||||
int idx = 0;
|
||||
while (times[idx] + 1e-4 < t)
|
||||
{
|
||||
t -= times[idx];
|
||||
++idx;
|
||||
}
|
||||
|
||||
/* evaluation */
|
||||
int order = cxs[idx].size();
|
||||
Eigen::VectorXd vx(order - 1), vy(order - 1), vz(order - 1);
|
||||
|
||||
/* coef of vel */
|
||||
for (int i = 0; i < order - 1; ++i)
|
||||
{
|
||||
vx(i) = double(i + 1) * cxs[idx][order - 2 - i];
|
||||
vy(i) = double(i + 1) * cys[idx][order - 2 - i];
|
||||
vz(i) = double(i + 1) * czs[idx][order - 2 - i];
|
||||
}
|
||||
double ts = t;
|
||||
Eigen::VectorXd tv(order - 1);
|
||||
for (int i = 0; i < order - 1; ++i)
|
||||
tv(i) = pow(ts, i);
|
||||
|
||||
Eigen::Vector3d vel;
|
||||
vel(0) = tv.dot(vx), vel(1) = tv.dot(vy), vel(2) = tv.dot(vz);
|
||||
return vel;
|
||||
}
|
||||
|
||||
Eigen::Vector3d evaluateAcc(double t)
|
||||
{
|
||||
/* detetrmine segment num */
|
||||
int idx = 0;
|
||||
while (times[idx] + 1e-4 < t)
|
||||
{
|
||||
t -= times[idx];
|
||||
++idx;
|
||||
}
|
||||
|
||||
/* evaluation */
|
||||
int order = cxs[idx].size();
|
||||
Eigen::VectorXd ax(order - 2), ay(order - 2), az(order - 2);
|
||||
|
||||
/* coef of vel */
|
||||
for (int i = 0; i < order - 2; ++i)
|
||||
{
|
||||
ax(i) = double((i + 2) * (i + 1)) * cxs[idx][order - 3 - i];
|
||||
ay(i) = double((i + 2) * (i + 1)) * cys[idx][order - 3 - i];
|
||||
az(i) = double((i + 2) * (i + 1)) * czs[idx][order - 3 - i];
|
||||
}
|
||||
double ts = t;
|
||||
Eigen::VectorXd tv(order - 2);
|
||||
for (int i = 0; i < order - 2; ++i)
|
||||
tv(i) = pow(ts, i);
|
||||
|
||||
Eigen::Vector3d acc;
|
||||
acc(0) = tv.dot(ax), acc(1) = tv.dot(ay), acc(2) = tv.dot(az);
|
||||
return acc;
|
||||
}
|
||||
|
||||
Eigen::Vector3d evaluateJerk(double t)
|
||||
{
|
||||
/* detetrmine segment num */
|
||||
int idx = 0;
|
||||
while (times[idx] + 1e-4 < t)
|
||||
{
|
||||
t -= times[idx];
|
||||
++idx;
|
||||
}
|
||||
|
||||
/* evaluation */
|
||||
int order = cxs[idx].size();
|
||||
Eigen::VectorXd jx(order - 3), jy(order - 3), jz(order - 3);
|
||||
|
||||
/* coef of vel */
|
||||
for (int i = 0; i < order - 3; ++i)
|
||||
{
|
||||
jx(i) = double((i + 3) * (i + 2) * (i + 1)) * cxs[idx][order - 4 - i];
|
||||
jy(i) = double((i + 3) * (i + 2) * (i + 1)) * cys[idx][order - 4 - i];
|
||||
jz(i) = double((i + 3) * (i + 2) * (i + 1)) * czs[idx][order - 4 - i];
|
||||
}
|
||||
double ts = t;
|
||||
Eigen::VectorXd tv(order - 3);
|
||||
for (int i = 0; i < order - 3; ++i)
|
||||
tv(i) = pow(ts, i);
|
||||
|
||||
Eigen::Vector3d jerk;
|
||||
jerk(0) = tv.dot(jx), jerk(1) = tv.dot(jy), jerk(2) = tv.dot(jz);
|
||||
return jerk;
|
||||
}
|
||||
|
||||
/* for evaluating traj, should be called in sequence!!! */
|
||||
double getTimeSum()
|
||||
{
|
||||
return this->time_sum;
|
||||
}
|
||||
|
||||
vector<Eigen::Vector3d> getTraj()
|
||||
{
|
||||
double eval_t = 0.0;
|
||||
traj_vec3d.clear();
|
||||
while (eval_t < time_sum)
|
||||
{
|
||||
Eigen::Vector3d pt = evaluate(eval_t);
|
||||
traj_vec3d.push_back(pt);
|
||||
eval_t += 0.01;
|
||||
}
|
||||
return traj_vec3d;
|
||||
}
|
||||
|
||||
double getLength()
|
||||
{
|
||||
length = 0.0;
|
||||
|
||||
Eigen::Vector3d p_l = traj_vec3d[0], p_n;
|
||||
for (int i = 1; i < traj_vec3d.size(); ++i)
|
||||
{
|
||||
p_n = traj_vec3d[i];
|
||||
length += (p_n - p_l).norm();
|
||||
p_l = p_n;
|
||||
}
|
||||
return length;
|
||||
}
|
||||
|
||||
double getMeanVel()
|
||||
{
|
||||
double mean_vel = length / time_sum;
|
||||
return mean_vel;
|
||||
}
|
||||
|
||||
double getAccCost()
|
||||
{
|
||||
double cost = 0.0;
|
||||
int order = cxs[0].size();
|
||||
|
||||
for (int s = 0; s < times.size(); ++s)
|
||||
{
|
||||
Eigen::Vector3d um;
|
||||
um(0) = 2 * cxs[s][order - 3], um(1) = 2 * cys[s][order - 3], um(2) = 2 * czs[s][order - 3];
|
||||
cost += um.squaredNorm() * times[s];
|
||||
}
|
||||
|
||||
return cost;
|
||||
}
|
||||
|
||||
double getJerk()
|
||||
{
|
||||
double jerk = 0.0;
|
||||
|
||||
/* evaluate jerk */
|
||||
for (int s = 0; s < times.size(); ++s)
|
||||
{
|
||||
Eigen::VectorXd cxv(cxs[s].size()), cyv(cys[s].size()), czv(czs[s].size());
|
||||
/* convert coefficient */
|
||||
int order = cxs[s].size();
|
||||
for (int j = 0; j < order; ++j)
|
||||
{
|
||||
cxv(j) = cxs[s][order - 1 - j], cyv(j) = cys[s][order - 1 - j], czv(j) = czs[s][order - 1 - j];
|
||||
}
|
||||
double ts = times[s];
|
||||
|
||||
/* jerk matrix */
|
||||
Eigen::MatrixXd mat_jerk(order, order);
|
||||
mat_jerk.setZero();
|
||||
for (double i = 3; i < order; i += 1)
|
||||
for (double j = 3; j < order; j += 1)
|
||||
{
|
||||
mat_jerk(i, j) =
|
||||
i * (i - 1) * (i - 2) * j * (j - 1) * (j - 2) * pow(ts, i + j - 5) / (i + j - 5);
|
||||
}
|
||||
|
||||
jerk += (cxv.transpose() * mat_jerk * cxv)(0, 0);
|
||||
jerk += (cyv.transpose() * mat_jerk * cyv)(0, 0);
|
||||
jerk += (czv.transpose() * mat_jerk * czv)(0, 0);
|
||||
}
|
||||
|
||||
return jerk;
|
||||
}
|
||||
|
||||
void getMeanAndMaxVel(double &mean_v, double &max_v)
|
||||
{
|
||||
int num = 0;
|
||||
mean_v = 0.0, max_v = -1.0;
|
||||
for (int s = 0; s < times.size(); ++s)
|
||||
{
|
||||
int order = cxs[s].size();
|
||||
Eigen::VectorXd vx(order - 1), vy(order - 1), vz(order - 1);
|
||||
|
||||
/* coef of vel */
|
||||
for (int i = 0; i < order - 1; ++i)
|
||||
{
|
||||
vx(i) = double(i + 1) * cxs[s][order - 2 - i];
|
||||
vy(i) = double(i + 1) * cys[s][order - 2 - i];
|
||||
vz(i) = double(i + 1) * czs[s][order - 2 - i];
|
||||
}
|
||||
double ts = times[s];
|
||||
|
||||
double eval_t = 0.0;
|
||||
while (eval_t < ts)
|
||||
{
|
||||
Eigen::VectorXd tv(order - 1);
|
||||
for (int i = 0; i < order - 1; ++i)
|
||||
tv(i) = pow(ts, i);
|
||||
Eigen::Vector3d vel;
|
||||
vel(0) = tv.dot(vx), vel(1) = tv.dot(vy), vel(2) = tv.dot(vz);
|
||||
double vn = vel.norm();
|
||||
mean_v += vn;
|
||||
if (vn > max_v)
|
||||
max_v = vn;
|
||||
++num;
|
||||
|
||||
eval_t += 0.01;
|
||||
}
|
||||
}
|
||||
|
||||
mean_v = mean_v / double(num);
|
||||
}
|
||||
|
||||
void getMeanAndMaxAcc(double &mean_a, double &max_a)
|
||||
{
|
||||
int num = 0;
|
||||
mean_a = 0.0, max_a = -1.0;
|
||||
for (int s = 0; s < times.size(); ++s)
|
||||
{
|
||||
int order = cxs[s].size();
|
||||
Eigen::VectorXd ax(order - 2), ay(order - 2), az(order - 2);
|
||||
|
||||
/* coef of acc */
|
||||
for (int i = 0; i < order - 2; ++i)
|
||||
{
|
||||
ax(i) = double((i + 2) * (i + 1)) * cxs[s][order - 3 - i];
|
||||
ay(i) = double((i + 2) * (i + 1)) * cys[s][order - 3 - i];
|
||||
az(i) = double((i + 2) * (i + 1)) * czs[s][order - 3 - i];
|
||||
}
|
||||
double ts = times[s];
|
||||
|
||||
double eval_t = 0.0;
|
||||
while (eval_t < ts)
|
||||
{
|
||||
Eigen::VectorXd tv(order - 2);
|
||||
for (int i = 0; i < order - 2; ++i)
|
||||
tv(i) = pow(ts, i);
|
||||
Eigen::Vector3d acc;
|
||||
acc(0) = tv.dot(ax), acc(1) = tv.dot(ay), acc(2) = tv.dot(az);
|
||||
double an = acc.norm();
|
||||
mean_a += an;
|
||||
if (an > max_a)
|
||||
max_a = an;
|
||||
++num;
|
||||
|
||||
eval_t += 0.01;
|
||||
}
|
||||
}
|
||||
|
||||
mean_a = mean_a / double(num);
|
||||
}
|
||||
|
||||
static PolynomialTraj minSnapTraj(const Eigen::MatrixXd &Pos, const Eigen::Vector3d &start_vel,
|
||||
const Eigen::Vector3d &end_vel, const Eigen::Vector3d &start_acc,
|
||||
const Eigen::Vector3d &end_acc, const Eigen::VectorXd &Time);
|
||||
|
||||
static PolynomialTraj one_segment_traj_gen(const Eigen::Vector3d &start_pt, const Eigen::Vector3d &start_vel, const Eigen::Vector3d &start_acc,
|
||||
const Eigen::Vector3d &end_pt, const Eigen::Vector3d &end_vel, const Eigen::Vector3d &end_acc,
|
||||
double t);
|
||||
};
|
||||
|
||||
#endif
|
||||
@ -1,67 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>mini_snap_traj_utils</name>
|
||||
<version>0.0.0</version>
|
||||
<description>The mini_snap_traj_utils package</description>
|
||||
|
||||
<!-- One maintainer tag required, multiple allowed, one person per tag -->
|
||||
<!-- Example: -->
|
||||
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
|
||||
<maintainer email="bzhouai@todo.todo">bzhouai</maintainer>
|
||||
|
||||
|
||||
<!-- One license tag required, multiple allowed, one license per tag -->
|
||||
<!-- Commonly used license strings: -->
|
||||
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
|
||||
<license>TODO</license>
|
||||
|
||||
|
||||
<!-- Url tags are optional, but multiple are allowed, one per tag -->
|
||||
<!-- Optional attribute type can be: website, bugtracker, or repository -->
|
||||
<!-- Example: -->
|
||||
<!-- <url type="website">http://wiki.ros.org/mini_snap_traj_utils</url> -->
|
||||
|
||||
|
||||
<!-- Author tags are optional, multiple are allowed, one per tag -->
|
||||
<!-- Authors do not have to be maintainers, but could be -->
|
||||
<!-- Example: -->
|
||||
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
|
||||
|
||||
|
||||
<!-- The *depend tags are used to specify dependencies -->
|
||||
<!-- Dependencies can be catkin packages or system dependencies -->
|
||||
<!-- Examples: -->
|
||||
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
|
||||
<!-- <depend>roscpp</depend> -->
|
||||
<!-- Note that this is equivalent to the following: -->
|
||||
<!-- <build_depend>roscpp</build_depend> -->
|
||||
<!-- <exec_depend>roscpp</exec_depend> -->
|
||||
<!-- Use build_depend for packages you need at compile time: -->
|
||||
<!-- <build_depend>message_generation</build_depend> -->
|
||||
<!-- Use build_export_depend for packages you need in order to build against this package: -->
|
||||
<!-- <build_export_depend>message_generation</build_export_depend> -->
|
||||
<!-- Use buildtool_depend for build tool packages: -->
|
||||
<!-- <buildtool_depend>catkin</buildtool_depend> -->
|
||||
<!-- Use exec_depend for packages you need at runtime: -->
|
||||
<!-- <exec_depend>message_runtime</exec_depend> -->
|
||||
<!-- Use test_depend for packages you need only for testing: -->
|
||||
<!-- <test_depend>gtest</test_depend> -->
|
||||
<!-- Use doc_depend for packages you need only for building documentation: -->
|
||||
<!-- <doc_depend>doxygen</doc_depend> -->
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<build_depend>roscpp</build_depend>
|
||||
<build_depend>std_msgs</build_depend>
|
||||
|
||||
<build_export_depend>roscpp</build_export_depend>
|
||||
<build_export_depend>std_msgs</build_export_depend>
|
||||
|
||||
<exec_depend>roscpp</exec_depend>
|
||||
<exec_depend>std_msgs</exec_depend>
|
||||
|
||||
|
||||
<!-- The export tag contains other, unspecified, tags -->
|
||||
<export>
|
||||
<!-- Other tools can request additional information be placed here -->
|
||||
|
||||
</export>
|
||||
</package>
|
||||
@ -1,253 +0,0 @@
|
||||
#include <mini_snap_traj_utils/planning_visualization.h>
|
||||
|
||||
using std::cout;
|
||||
using std::endl;
|
||||
namespace my_planner
|
||||
{
|
||||
PlanningVisualization::PlanningVisualization(ros::NodeHandle &nh)
|
||||
{
|
||||
node = nh;
|
||||
|
||||
goal_point_pub = nh.advertise<visualization_msgs::Marker>("goal_point", 2);
|
||||
global_list_pub = nh.advertise<visualization_msgs::Marker>("global_list", 2);
|
||||
init_list_pub = nh.advertise<visualization_msgs::Marker>("init_list", 2);
|
||||
optimal_list_pub = nh.advertise<visualization_msgs::Marker>("optimal_list", 2);
|
||||
a_star_list_pub = nh.advertise<visualization_msgs::Marker>("a_star_list", 20);
|
||||
}
|
||||
|
||||
void PlanningVisualization::Init(ros::NodeHandle &nh)
|
||||
{
|
||||
node = nh;
|
||||
|
||||
goal_point_pub = nh.advertise<visualization_msgs::Marker>("goal_point", 2);
|
||||
global_list_pub = nh.advertise<visualization_msgs::Marker>("global_list", 2);
|
||||
init_list_pub = nh.advertise<visualization_msgs::Marker>("init_list", 2);
|
||||
optimal_list_pub = nh.advertise<visualization_msgs::Marker>("optimal_list", 2);
|
||||
a_star_list_pub = nh.advertise<visualization_msgs::Marker>("a_star_list", 20);
|
||||
}
|
||||
|
||||
// // real ids used: {id, id+1000}
|
||||
void PlanningVisualization::displayMarkerList(ros::Publisher &pub, const vector<Eigen::Vector3d> &list, double scale,
|
||||
Eigen::Vector4d color, int id)
|
||||
{
|
||||
visualization_msgs::Marker sphere, line_strip;
|
||||
sphere.header.frame_id = line_strip.header.frame_id = "world";
|
||||
sphere.header.stamp = line_strip.header.stamp = ros::Time::now();
|
||||
sphere.type = visualization_msgs::Marker::SPHERE_LIST;
|
||||
line_strip.type = visualization_msgs::Marker::LINE_STRIP;
|
||||
sphere.action = line_strip.action = visualization_msgs::Marker::ADD;
|
||||
sphere.id = id;
|
||||
line_strip.id = id + 1000;
|
||||
|
||||
sphere.pose.orientation.w = line_strip.pose.orientation.w = 1.0;
|
||||
sphere.color.r = line_strip.color.r = color(0);
|
||||
sphere.color.g = line_strip.color.g = color(1);
|
||||
sphere.color.b = line_strip.color.b = color(2);
|
||||
sphere.color.a = line_strip.color.a = color(3) > 1e-5 ? color(3) : 1.0;
|
||||
sphere.scale.x = scale;
|
||||
sphere.scale.y = scale;
|
||||
sphere.scale.z = scale;
|
||||
line_strip.scale.x = scale / 2;
|
||||
geometry_msgs::Point pt;
|
||||
for (int i = 0; i < int(list.size()); i++)
|
||||
{
|
||||
pt.x = list[i](0);
|
||||
pt.y = list[i](1);
|
||||
pt.z = list[i](2);
|
||||
sphere.points.push_back(pt);
|
||||
line_strip.points.push_back(pt);
|
||||
}
|
||||
pub.publish(sphere);
|
||||
pub.publish(line_strip);
|
||||
}
|
||||
|
||||
// real ids used: {id, id+1}
|
||||
void PlanningVisualization::generatePathDisplayArray(visualization_msgs::MarkerArray &array,
|
||||
const vector<Eigen::Vector3d> &list, double scale, Eigen::Vector4d color, int id)
|
||||
{
|
||||
visualization_msgs::Marker sphere, line_strip;
|
||||
sphere.header.frame_id = line_strip.header.frame_id = "map";
|
||||
sphere.header.stamp = line_strip.header.stamp = ros::Time::now();
|
||||
sphere.type = visualization_msgs::Marker::SPHERE_LIST;
|
||||
line_strip.type = visualization_msgs::Marker::LINE_STRIP;
|
||||
sphere.action = line_strip.action = visualization_msgs::Marker::ADD;
|
||||
sphere.id = id;
|
||||
line_strip.id = id + 1;
|
||||
|
||||
sphere.pose.orientation.w = line_strip.pose.orientation.w = 1.0;
|
||||
sphere.color.r = line_strip.color.r = color(0);
|
||||
sphere.color.g = line_strip.color.g = color(1);
|
||||
sphere.color.b = line_strip.color.b = color(2);
|
||||
sphere.color.a = line_strip.color.a = color(3) > 1e-5 ? color(3) : 1.0;
|
||||
sphere.scale.x = scale;
|
||||
sphere.scale.y = scale;
|
||||
sphere.scale.z = scale;
|
||||
line_strip.scale.x = scale / 3;
|
||||
geometry_msgs::Point pt;
|
||||
for (int i = 0; i < int(list.size()); i++)
|
||||
{
|
||||
pt.x = list[i](0);
|
||||
pt.y = list[i](1);
|
||||
pt.z = list[i](2);
|
||||
sphere.points.push_back(pt);
|
||||
line_strip.points.push_back(pt);
|
||||
}
|
||||
array.markers.push_back(sphere);
|
||||
array.markers.push_back(line_strip);
|
||||
}
|
||||
|
||||
// real ids used: {1000*id ~ (arrow nums)+1000*id}
|
||||
void PlanningVisualization::generateArrowDisplayArray(visualization_msgs::MarkerArray &array,
|
||||
const vector<Eigen::Vector3d> &list, double scale, Eigen::Vector4d color, int id)
|
||||
{
|
||||
visualization_msgs::Marker arrow;
|
||||
arrow.header.frame_id = "map";
|
||||
arrow.header.stamp = ros::Time::now();
|
||||
arrow.type = visualization_msgs::Marker::ARROW;
|
||||
arrow.action = visualization_msgs::Marker::ADD;
|
||||
|
||||
// geometry_msgs::Point start, end;
|
||||
// arrow.points
|
||||
|
||||
arrow.color.r = color(0);
|
||||
arrow.color.g = color(1);
|
||||
arrow.color.b = color(2);
|
||||
arrow.color.a = color(3) > 1e-5 ? color(3) : 1.0;
|
||||
arrow.scale.x = scale;
|
||||
arrow.scale.y = 2 * scale;
|
||||
arrow.scale.z = 2 * scale;
|
||||
|
||||
geometry_msgs::Point start, end;
|
||||
for (int i = 0; i < int(list.size() / 2); i++)
|
||||
{
|
||||
// arrow.color.r = color(0) / (1+i);
|
||||
// arrow.color.g = color(1) / (1+i);
|
||||
// arrow.color.b = color(2) / (1+i);
|
||||
|
||||
start.x = list[2 * i](0);
|
||||
start.y = list[2 * i](1);
|
||||
start.z = list[2 * i](2);
|
||||
end.x = list[2 * i + 1](0);
|
||||
end.y = list[2 * i + 1](1);
|
||||
end.z = list[2 * i + 1](2);
|
||||
arrow.points.clear();
|
||||
arrow.points.push_back(start);
|
||||
arrow.points.push_back(end);
|
||||
arrow.id = i + id * 1000;
|
||||
|
||||
array.markers.push_back(arrow);
|
||||
}
|
||||
}
|
||||
|
||||
void PlanningVisualization::displayGoalPoint(Eigen::Vector3d goal_point, Eigen::Vector4d color, const double scale, int id)
|
||||
{
|
||||
visualization_msgs::Marker sphere;
|
||||
sphere.header.frame_id = "world";
|
||||
sphere.header.stamp = ros::Time::now();
|
||||
sphere.type = visualization_msgs::Marker::SPHERE;
|
||||
sphere.action = visualization_msgs::Marker::ADD;
|
||||
sphere.id = id;
|
||||
|
||||
sphere.pose.orientation.w = 1.0;
|
||||
sphere.color.r = color(0);
|
||||
sphere.color.g = color(1);
|
||||
sphere.color.b = color(2);
|
||||
sphere.color.a = color(3);
|
||||
sphere.scale.x = scale;
|
||||
sphere.scale.y = scale;
|
||||
sphere.scale.z = scale;
|
||||
sphere.pose.position.x = goal_point(0);
|
||||
sphere.pose.position.y = goal_point(1);
|
||||
sphere.pose.position.z = goal_point(2);
|
||||
|
||||
goal_point_pub.publish(sphere);
|
||||
}
|
||||
|
||||
void PlanningVisualization::displayGlobalPathList(vector<Eigen::Vector3d> init_pts, const double scale, int id)
|
||||
{
|
||||
|
||||
if (global_list_pub.getNumSubscribers() == 0)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
Eigen::Vector4d color(0, 0.5, 0.5, 1);
|
||||
displayMarkerList(global_list_pub, init_pts, scale, color, id);
|
||||
}
|
||||
|
||||
void PlanningVisualization::displayInitPathList(vector<Eigen::Vector3d> init_pts, const double scale, int id)
|
||||
{
|
||||
|
||||
if (init_list_pub.getNumSubscribers() == 0)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
Eigen::Vector4d color(0, 0, 1, 1);
|
||||
displayMarkerList(init_list_pub, init_pts, scale, color, id);
|
||||
}
|
||||
|
||||
void PlanningVisualization::displayOptimalList(Eigen::MatrixXd optimal_pts, int id)
|
||||
{
|
||||
|
||||
if (optimal_list_pub.getNumSubscribers() == 0)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
vector<Eigen::Vector3d> list;
|
||||
for (int i = 0; i < optimal_pts.cols(); i++)
|
||||
{
|
||||
Eigen::Vector3d pt = optimal_pts.col(i).transpose();
|
||||
list.push_back(pt);
|
||||
}
|
||||
Eigen::Vector4d color(1, 0, 0, 1);
|
||||
displayMarkerList(optimal_list_pub, list, 0.15, color, id);
|
||||
}
|
||||
|
||||
void PlanningVisualization::displayAStarList(std::vector<std::vector<Eigen::Vector3d>> a_star_paths, int id /* = Eigen::Vector4d(0.5,0.5,0,1)*/)
|
||||
{
|
||||
|
||||
if (a_star_list_pub.getNumSubscribers() == 0)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
int i = 0;
|
||||
vector<Eigen::Vector3d> list;
|
||||
|
||||
Eigen::Vector4d color = Eigen::Vector4d(0.5 + ((double)rand() / RAND_MAX / 2), 0.5 + ((double)rand() / RAND_MAX / 2), 0, 1); // make the A star pathes different every time.
|
||||
double scale = 0.05 + (double)rand() / RAND_MAX / 10;
|
||||
|
||||
// for ( int i=0; i<10; i++ )
|
||||
// {
|
||||
// //Eigen::Vector4d color(1,1,0,0);
|
||||
// displayMarkerList(a_star_list_pub, list, scale, color, id+i);
|
||||
// }
|
||||
|
||||
for (auto block : a_star_paths)
|
||||
{
|
||||
list.clear();
|
||||
for (auto pt : block)
|
||||
{
|
||||
list.push_back(pt);
|
||||
}
|
||||
//Eigen::Vector4d color(0.5,0.5,0,1);
|
||||
displayMarkerList(a_star_list_pub, list, scale, color, id + i); // real ids used: [ id ~ id+a_star_paths.size() ]
|
||||
i++;
|
||||
}
|
||||
}
|
||||
|
||||
void PlanningVisualization::displayArrowList(ros::Publisher &pub, const vector<Eigen::Vector3d> &list, double scale, Eigen::Vector4d color, int id)
|
||||
{
|
||||
visualization_msgs::MarkerArray array;
|
||||
// clear
|
||||
pub.publish(array);
|
||||
|
||||
generateArrowDisplayArray(array, list, scale, color, id);
|
||||
|
||||
pub.publish(array);
|
||||
}
|
||||
|
||||
// PlanningVisualization::
|
||||
} // namespace ego_planner
|
||||
@ -1,224 +0,0 @@
|
||||
#include <iostream>
|
||||
#include <mini_snap_traj_utils/polynomial_traj.h>
|
||||
|
||||
PolynomialTraj PolynomialTraj::minSnapTraj(const Eigen::MatrixXd &Pos, const Eigen::Vector3d &start_vel,
|
||||
const Eigen::Vector3d &end_vel, const Eigen::Vector3d &start_acc,
|
||||
const Eigen::Vector3d &end_acc, const Eigen::VectorXd &Time)
|
||||
{
|
||||
int seg_num = Time.size();
|
||||
Eigen::MatrixXd poly_coeff(seg_num, 3 * 6);
|
||||
Eigen::VectorXd Px(6 * seg_num), Py(6 * seg_num), Pz(6 * seg_num);
|
||||
|
||||
int num_f, num_p; // number of fixed and free variables
|
||||
int num_d; // number of all segments' derivatives
|
||||
|
||||
const static auto Factorial = [](int x) {
|
||||
int fac = 1;
|
||||
for (int i = x; i > 0; i--)
|
||||
fac = fac * i;
|
||||
return fac;
|
||||
};
|
||||
|
||||
/* ---------- end point derivative ---------- */
|
||||
Eigen::VectorXd Dx = Eigen::VectorXd::Zero(seg_num * 6);
|
||||
Eigen::VectorXd Dy = Eigen::VectorXd::Zero(seg_num * 6);
|
||||
Eigen::VectorXd Dz = Eigen::VectorXd::Zero(seg_num * 6);
|
||||
|
||||
for (int k = 0; k < seg_num; k++)
|
||||
{
|
||||
/* position to derivative */
|
||||
Dx(k * 6) = Pos(0, k);
|
||||
Dx(k * 6 + 1) = Pos(0, k + 1);
|
||||
Dy(k * 6) = Pos(1, k);
|
||||
Dy(k * 6 + 1) = Pos(1, k + 1);
|
||||
Dz(k * 6) = Pos(2, k);
|
||||
Dz(k * 6 + 1) = Pos(2, k + 1);
|
||||
|
||||
if (k == 0)
|
||||
{
|
||||
Dx(k * 6 + 2) = start_vel(0);
|
||||
Dy(k * 6 + 2) = start_vel(1);
|
||||
Dz(k * 6 + 2) = start_vel(2);
|
||||
|
||||
Dx(k * 6 + 4) = start_acc(0);
|
||||
Dy(k * 6 + 4) = start_acc(1);
|
||||
Dz(k * 6 + 4) = start_acc(2);
|
||||
}
|
||||
else if (k == seg_num - 1)
|
||||
{
|
||||
Dx(k * 6 + 3) = end_vel(0);
|
||||
Dy(k * 6 + 3) = end_vel(1);
|
||||
Dz(k * 6 + 3) = end_vel(2);
|
||||
|
||||
Dx(k * 6 + 5) = end_acc(0);
|
||||
Dy(k * 6 + 5) = end_acc(1);
|
||||
Dz(k * 6 + 5) = end_acc(2);
|
||||
}
|
||||
}
|
||||
|
||||
/* ---------- Mapping Matrix A ---------- */
|
||||
Eigen::MatrixXd Ab;
|
||||
Eigen::MatrixXd A = Eigen::MatrixXd::Zero(seg_num * 6, seg_num * 6);
|
||||
|
||||
for (int k = 0; k < seg_num; k++)
|
||||
{
|
||||
Ab = Eigen::MatrixXd::Zero(6, 6);
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
Ab(2 * i, i) = Factorial(i);
|
||||
for (int j = i; j < 6; j++)
|
||||
Ab(2 * i + 1, j) = Factorial(j) / Factorial(j - i) * pow(Time(k), j - i);
|
||||
}
|
||||
A.block(k * 6, k * 6, 6, 6) = Ab;
|
||||
}
|
||||
|
||||
/* ---------- Produce Selection Matrix C' ---------- */
|
||||
Eigen::MatrixXd Ct, C;
|
||||
|
||||
num_f = 2 * seg_num + 4; // 3 + 3 + (seg_num - 1) * 2 = 2m + 4
|
||||
num_p = 2 * seg_num - 2; //(seg_num - 1) * 2 = 2m - 2
|
||||
num_d = 6 * seg_num;
|
||||
Ct = Eigen::MatrixXd::Zero(num_d, num_f + num_p);
|
||||
Ct(0, 0) = 1;
|
||||
Ct(2, 1) = 1;
|
||||
Ct(4, 2) = 1; // stack the start point
|
||||
Ct(1, 3) = 1;
|
||||
Ct(3, 2 * seg_num + 4) = 1;
|
||||
Ct(5, 2 * seg_num + 5) = 1;
|
||||
|
||||
Ct(6 * (seg_num - 1) + 0, 2 * seg_num + 0) = 1;
|
||||
Ct(6 * (seg_num - 1) + 1, 2 * seg_num + 1) = 1; // Stack the end point
|
||||
Ct(6 * (seg_num - 1) + 2, 4 * seg_num + 0) = 1;
|
||||
Ct(6 * (seg_num - 1) + 3, 2 * seg_num + 2) = 1; // Stack the end point
|
||||
Ct(6 * (seg_num - 1) + 4, 4 * seg_num + 1) = 1;
|
||||
Ct(6 * (seg_num - 1) + 5, 2 * seg_num + 3) = 1; // Stack the end point
|
||||
|
||||
for (int j = 2; j < seg_num; j++)
|
||||
{
|
||||
Ct(6 * (j - 1) + 0, 2 + 2 * (j - 1) + 0) = 1;
|
||||
Ct(6 * (j - 1) + 1, 2 + 2 * (j - 1) + 1) = 1;
|
||||
Ct(6 * (j - 1) + 2, 2 * seg_num + 4 + 2 * (j - 2) + 0) = 1;
|
||||
Ct(6 * (j - 1) + 3, 2 * seg_num + 4 + 2 * (j - 1) + 0) = 1;
|
||||
Ct(6 * (j - 1) + 4, 2 * seg_num + 4 + 2 * (j - 2) + 1) = 1;
|
||||
Ct(6 * (j - 1) + 5, 2 * seg_num + 4 + 2 * (j - 1) + 1) = 1;
|
||||
}
|
||||
|
||||
C = Ct.transpose();
|
||||
|
||||
Eigen::VectorXd Dx1 = C * Dx;
|
||||
Eigen::VectorXd Dy1 = C * Dy;
|
||||
Eigen::VectorXd Dz1 = C * Dz;
|
||||
|
||||
/* ---------- minimum snap matrix ---------- */
|
||||
Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(seg_num * 6, seg_num * 6);
|
||||
|
||||
for (int k = 0; k < seg_num; k++)
|
||||
{
|
||||
for (int i = 3; i < 6; i++)
|
||||
{
|
||||
for (int j = 3; j < 6; j++)
|
||||
{
|
||||
Q(k * 6 + i, k * 6 + j) =
|
||||
i * (i - 1) * (i - 2) * j * (j - 1) * (j - 2) / (i + j - 5) * pow(Time(k), (i + j - 5));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* ---------- R matrix ---------- */
|
||||
Eigen::MatrixXd R = C * A.transpose().inverse() * Q * A.inverse() * Ct;
|
||||
|
||||
Eigen::VectorXd Dxf(2 * seg_num + 4), Dyf(2 * seg_num + 4), Dzf(2 * seg_num + 4);
|
||||
|
||||
Dxf = Dx1.segment(0, 2 * seg_num + 4);
|
||||
Dyf = Dy1.segment(0, 2 * seg_num + 4);
|
||||
Dzf = Dz1.segment(0, 2 * seg_num + 4);
|
||||
|
||||
Eigen::MatrixXd Rff(2 * seg_num + 4, 2 * seg_num + 4);
|
||||
Eigen::MatrixXd Rfp(2 * seg_num + 4, 2 * seg_num - 2);
|
||||
Eigen::MatrixXd Rpf(2 * seg_num - 2, 2 * seg_num + 4);
|
||||
Eigen::MatrixXd Rpp(2 * seg_num - 2, 2 * seg_num - 2);
|
||||
|
||||
Rff = R.block(0, 0, 2 * seg_num + 4, 2 * seg_num + 4);
|
||||
Rfp = R.block(0, 2 * seg_num + 4, 2 * seg_num + 4, 2 * seg_num - 2);
|
||||
Rpf = R.block(2 * seg_num + 4, 0, 2 * seg_num - 2, 2 * seg_num + 4);
|
||||
Rpp = R.block(2 * seg_num + 4, 2 * seg_num + 4, 2 * seg_num - 2, 2 * seg_num - 2);
|
||||
|
||||
/* ---------- close form solution ---------- */
|
||||
|
||||
Eigen::VectorXd Dxp(2 * seg_num - 2), Dyp(2 * seg_num - 2), Dzp(2 * seg_num - 2);
|
||||
Dxp = -(Rpp.inverse() * Rfp.transpose()) * Dxf;
|
||||
Dyp = -(Rpp.inverse() * Rfp.transpose()) * Dyf;
|
||||
Dzp = -(Rpp.inverse() * Rfp.transpose()) * Dzf;
|
||||
|
||||
Dx1.segment(2 * seg_num + 4, 2 * seg_num - 2) = Dxp;
|
||||
Dy1.segment(2 * seg_num + 4, 2 * seg_num - 2) = Dyp;
|
||||
Dz1.segment(2 * seg_num + 4, 2 * seg_num - 2) = Dzp;
|
||||
|
||||
Px = (A.inverse() * Ct) * Dx1;
|
||||
Py = (A.inverse() * Ct) * Dy1;
|
||||
Pz = (A.inverse() * Ct) * Dz1;
|
||||
|
||||
for (int i = 0; i < seg_num; i++)
|
||||
{
|
||||
poly_coeff.block(i, 0, 1, 6) = Px.segment(i * 6, 6).transpose();
|
||||
poly_coeff.block(i, 6, 1, 6) = Py.segment(i * 6, 6).transpose();
|
||||
poly_coeff.block(i, 12, 1, 6) = Pz.segment(i * 6, 6).transpose();
|
||||
}
|
||||
|
||||
/* ---------- use polynomials ---------- */
|
||||
PolynomialTraj poly_traj;
|
||||
for (int i = 0; i < poly_coeff.rows(); ++i)
|
||||
{
|
||||
vector<double> cx(6), cy(6), cz(6);
|
||||
for (int j = 0; j < 6; ++j)
|
||||
{
|
||||
cx[j] = poly_coeff(i, j), cy[j] = poly_coeff(i, j + 6), cz[j] = poly_coeff(i, j + 12);
|
||||
}
|
||||
reverse(cx.begin(), cx.end());
|
||||
reverse(cy.begin(), cy.end());
|
||||
reverse(cz.begin(), cz.end());
|
||||
double ts = Time(i);
|
||||
poly_traj.addSegment(cx, cy, cz, ts);
|
||||
}
|
||||
|
||||
return poly_traj;
|
||||
}
|
||||
|
||||
PolynomialTraj PolynomialTraj::one_segment_traj_gen(const Eigen::Vector3d &start_pt, const Eigen::Vector3d &start_vel, const Eigen::Vector3d &start_acc,
|
||||
const Eigen::Vector3d &end_pt, const Eigen::Vector3d &end_vel, const Eigen::Vector3d &end_acc,
|
||||
double t)
|
||||
{
|
||||
Eigen::MatrixXd C = Eigen::MatrixXd::Zero(6, 6), Crow(1, 6);
|
||||
Eigen::VectorXd Bx(6), By(6), Bz(6);
|
||||
|
||||
C(0, 5) = 1;
|
||||
C(1, 4) = 1;
|
||||
C(2, 3) = 2;
|
||||
Crow << pow(t, 5), pow(t, 4), pow(t, 3), pow(t, 2), t, 1;
|
||||
C.row(3) = Crow;
|
||||
Crow << 5 * pow(t, 4), 4 * pow(t, 3), 3 * pow(t, 2), 2 * t, 1, 0;
|
||||
C.row(4) = Crow;
|
||||
Crow << 20 * pow(t, 3), 12 * pow(t, 2), 6 * t, 2, 0, 0;
|
||||
C.row(5) = Crow;
|
||||
|
||||
Bx << start_pt(0), start_vel(0), start_acc(0), end_pt(0), end_vel(0), end_acc(0);
|
||||
By << start_pt(1), start_vel(1), start_acc(1), end_pt(1), end_vel(1), end_acc(1);
|
||||
Bz << start_pt(2), start_vel(2), start_acc(2), end_pt(2), end_vel(2), end_acc(2);
|
||||
|
||||
Eigen::VectorXd Cofx = C.colPivHouseholderQr().solve(Bx);
|
||||
Eigen::VectorXd Cofy = C.colPivHouseholderQr().solve(By);
|
||||
Eigen::VectorXd Cofz = C.colPivHouseholderQr().solve(Bz);
|
||||
|
||||
vector<double> cx(6), cy(6), cz(6);
|
||||
for (int i = 0; i < 6; i++)
|
||||
{
|
||||
cx[i] = Cofx(i);
|
||||
cy[i] = Cofy(i);
|
||||
cz[i] = Cofz(i);
|
||||
}
|
||||
|
||||
PolynomialTraj poly_traj;
|
||||
poly_traj.addSegment(cx, cy, cz, t);
|
||||
|
||||
return poly_traj;
|
||||
}
|
||||
@ -1,13 +0,0 @@
|
||||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(cmake_utils)
|
||||
|
||||
find_package(catkin REQUIRED COMPONENTS roscpp)
|
||||
|
||||
set(CMAKE_MODULE_PATH ${CMAKE_CURRENT_LIST_DIR}/cmake)
|
||||
|
||||
file(GLOB CMAKE_UILTS_FILES cmake/*.cmake)
|
||||
|
||||
catkin_package(
|
||||
CFG_EXTRAS ${CMAKE_UILTS_FILES}
|
||||
)
|
||||
|
||||
@ -1,126 +0,0 @@
|
||||
set(archdetect_c_code "
|
||||
#if defined(__arm__) || defined(__TARGET_ARCH_ARM)
|
||||
#if defined(__ARM_ARCH_7__) \\
|
||||
|| defined(__ARM_ARCH_7A__) \\
|
||||
|| defined(__ARM_ARCH_7R__) \\
|
||||
|| defined(__ARM_ARCH_7M__) \\
|
||||
|| (defined(__TARGET_ARCH_ARM) && __TARGET_ARCH_ARM-0 >= 7)
|
||||
#error cmake_ARCH armv7
|
||||
#elif defined(__ARM_ARCH_6__) \\
|
||||
|| defined(__ARM_ARCH_6J__) \\
|
||||
|| defined(__ARM_ARCH_6T2__) \\
|
||||
|| defined(__ARM_ARCH_6Z__) \\
|
||||
|| defined(__ARM_ARCH_6K__) \\
|
||||
|| defined(__ARM_ARCH_6ZK__) \\
|
||||
|| defined(__ARM_ARCH_6M__) \\
|
||||
|| (defined(__TARGET_ARCH_ARM) && __TARGET_ARCH_ARM-0 >= 6)
|
||||
#error cmake_ARCH armv6
|
||||
#elif defined(__ARM_ARCH_5TEJ__) \\
|
||||
|| (defined(__TARGET_ARCH_ARM) && __TARGET_ARCH_ARM-0 >= 5)
|
||||
#error cmake_ARCH armv5
|
||||
#else
|
||||
#error cmake_ARCH arm
|
||||
#endif
|
||||
#elif defined(__i386) || defined(__i386__) || defined(_M_IX86)
|
||||
#error cmake_ARCH i386
|
||||
#elif defined(__x86_64) || defined(__x86_64__) || defined(__amd64) || defined(_M_X64)
|
||||
#error cmake_ARCH x86_64
|
||||
#elif defined(__ia64) || defined(__ia64__) || defined(_M_IA64)
|
||||
#error cmake_ARCH ia64
|
||||
#elif defined(__ppc__) || defined(__ppc) || defined(__powerpc__) \\
|
||||
|| defined(_ARCH_COM) || defined(_ARCH_PWR) || defined(_ARCH_PPC) \\
|
||||
|| defined(_M_MPPC) || defined(_M_PPC)
|
||||
#if defined(__ppc64__) || defined(__powerpc64__) || defined(__64BIT__)
|
||||
#error cmake_ARCH ppc64
|
||||
#else
|
||||
#error cmake_ARCH ppc
|
||||
#endif
|
||||
#endif
|
||||
#error cmake_ARCH unknown
|
||||
")
|
||||
|
||||
# Set ppc_support to TRUE before including this file or ppc and ppc64
|
||||
# will be treated as invalid architectures since they are no longer supported by Apple
|
||||
|
||||
function(target_architecture output_var)
|
||||
if(APPLE AND CMAKE_OSX_ARCHITECTURES)
|
||||
# On OS X we use CMAKE_OSX_ARCHITECTURES *if* it was set
|
||||
# First let's normalize the order of the values
|
||||
|
||||
# Note that it's not possible to compile PowerPC applications if you are using
|
||||
# the OS X SDK version 10.6 or later - you'll need 10.4/10.5 for that, so we
|
||||
# disable it by default
|
||||
# See this page for more information:
|
||||
# http://stackoverflow.com/questions/5333490/how-can-we-restore-ppc-ppc64-as-well-as-full-10-4-10-5-sdk-support-to-xcode-4
|
||||
|
||||
# Architecture defaults to i386 or ppc on OS X 10.5 and earlier, depending on the CPU type detected at runtime.
|
||||
# On OS X 10.6+ the default is x86_64 if the CPU supports it, i386 otherwise.
|
||||
|
||||
foreach(osx_arch ${CMAKE_OSX_ARCHITECTURES})
|
||||
if("${osx_arch}" STREQUAL "ppc" AND ppc_support)
|
||||
set(osx_arch_ppc TRUE)
|
||||
elseif("${osx_arch}" STREQUAL "i386")
|
||||
set(osx_arch_i386 TRUE)
|
||||
elseif("${osx_arch}" STREQUAL "x86_64")
|
||||
set(osx_arch_x86_64 TRUE)
|
||||
elseif("${osx_arch}" STREQUAL "ppc64" AND ppc_support)
|
||||
set(osx_arch_ppc64 TRUE)
|
||||
else()
|
||||
message(FATAL_ERROR "Invalid OS X arch name: ${osx_arch}")
|
||||
endif()
|
||||
endforeach()
|
||||
|
||||
# Now add all the architectures in our normalized order
|
||||
if(osx_arch_ppc)
|
||||
list(APPEND ARCH ppc)
|
||||
endif()
|
||||
|
||||
if(osx_arch_i386)
|
||||
list(APPEND ARCH i386)
|
||||
endif()
|
||||
|
||||
if(osx_arch_x86_64)
|
||||
list(APPEND ARCH x86_64)
|
||||
endif()
|
||||
|
||||
if(osx_arch_ppc64)
|
||||
list(APPEND ARCH ppc64)
|
||||
endif()
|
||||
else()
|
||||
file(WRITE "${CMAKE_BINARY_DIR}/arch.c" "${archdetect_c_code}")
|
||||
|
||||
enable_language(C)
|
||||
|
||||
# Detect the architecture in a rather creative way...
|
||||
# This compiles a small C program which is a series of ifdefs that selects a
|
||||
# particular #error preprocessor directive whose message string contains the
|
||||
# target architecture. The program will always fail to compile (both because
|
||||
# file is not a valid C program, and obviously because of the presence of the
|
||||
# #error preprocessor directives... but by exploiting the preprocessor in this
|
||||
# way, we can detect the correct target architecture even when cross-compiling,
|
||||
# since the program itself never needs to be run (only the compiler/preprocessor)
|
||||
try_run(
|
||||
run_result_unused
|
||||
compile_result_unused
|
||||
"${CMAKE_BINARY_DIR}"
|
||||
"${CMAKE_BINARY_DIR}/arch.c"
|
||||
COMPILE_OUTPUT_VARIABLE ARCH
|
||||
CMAKE_FLAGS CMAKE_OSX_ARCHITECTURES=${CMAKE_OSX_ARCHITECTURES}
|
||||
)
|
||||
|
||||
# Parse the architecture name from the compiler output
|
||||
string(REGEX MATCH "cmake_ARCH ([a-zA-Z0-9_]+)" ARCH "${ARCH}")
|
||||
|
||||
# Get rid of the value marker leaving just the architecture name
|
||||
string(REPLACE "cmake_ARCH " "" ARCH "${ARCH}")
|
||||
|
||||
# If we are compiling with an unknown architecture this variable should
|
||||
# already be set to "unknown" but in the case that it's empty (i.e. due
|
||||
# to a typo in the code), then set it to unknown
|
||||
if (NOT ARCH)
|
||||
set(ARCH unknown)
|
||||
endif()
|
||||
endif()
|
||||
|
||||
set(${output_var} "${ARCH}" PARENT_SCOPE)
|
||||
endfunction()
|
||||
@ -1,2 +0,0 @@
|
||||
list(APPEND CMAKE_MODULE_PATH "${cmake_utils_SOURCE_PREFIX}/cmake_modules")
|
||||
link_directories( ${cmake_utils_SOURCE_PREFIX}/lib/mosek8 )
|
||||
@ -1,17 +0,0 @@
|
||||
string(ASCII 27 Esc)
|
||||
set(ColourReset "${Esc}[m")
|
||||
set(ColourBold "${Esc}[1m")
|
||||
set(Red "${Esc}[31m")
|
||||
set(Green "${Esc}[32m")
|
||||
set(Yellow "${Esc}[33m")
|
||||
set(Blue "${Esc}[34m")
|
||||
set(Magenta "${Esc}[35m")
|
||||
set(Cyan "${Esc}[36m")
|
||||
set(White "${Esc}[37m")
|
||||
set(BoldRed "${Esc}[1;31m")
|
||||
set(BoldGreen "${Esc}[1;32m")
|
||||
set(BoldYellow "${Esc}[1;33m")
|
||||
set(BoldBlue "${Esc}[1;34m")
|
||||
set(BoldMagenta "${Esc}[1;35m")
|
||||
set(BoldCyan "${Esc}[1;36m")
|
||||
set(BoldWhite "${Esc}[1;37m")
|
||||
@ -1,173 +0,0 @@
|
||||
# Ceres Solver - A fast non-linear least squares minimizer
|
||||
# Copyright 2015 Google Inc. All rights reserved.
|
||||
# http://ceres-solver.org/
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions are met:
|
||||
#
|
||||
# * Redistributions of source code must retain the above copyright notice,
|
||||
# this list of conditions and the following disclaimer.
|
||||
# * Redistributions in binary form must reproduce the above copyright notice,
|
||||
# this list of conditions and the following disclaimer in the documentation
|
||||
# and/or other materials provided with the distribution.
|
||||
# * Neither the name of Google Inc. nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software without
|
||||
# specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
|
||||
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
# Author: alexs.mac@gmail.com (Alex Stewart)
|
||||
#
|
||||
|
||||
# FindEigen.cmake - Find Eigen library, version >= 3.
|
||||
#
|
||||
# This module defines the following variables:
|
||||
#
|
||||
# EIGEN_FOUND: TRUE iff Eigen is found.
|
||||
# EIGEN_INCLUDE_DIRS: Include directories for Eigen.
|
||||
#
|
||||
# EIGEN_VERSION: Extracted from Eigen/src/Core/util/Macros.h
|
||||
# EIGEN_WORLD_VERSION: Equal to 3 if EIGEN_VERSION = 3.2.0
|
||||
# EIGEN_MAJOR_VERSION: Equal to 2 if EIGEN_VERSION = 3.2.0
|
||||
# EIGEN_MINOR_VERSION: Equal to 0 if EIGEN_VERSION = 3.2.0
|
||||
#
|
||||
# The following variables control the behaviour of this module:
|
||||
#
|
||||
# EIGEN_INCLUDE_DIR_HINTS: List of additional directories in which to
|
||||
# search for eigen includes, e.g: /timbuktu/eigen3.
|
||||
#
|
||||
# The following variables are also defined by this module, but in line with
|
||||
# CMake recommended FindPackage() module style should NOT be referenced directly
|
||||
# by callers (use the plural variables detailed above instead). These variables
|
||||
# do however affect the behaviour of the module via FIND_[PATH/LIBRARY]() which
|
||||
# are NOT re-called (i.e. search for library is not repeated) if these variables
|
||||
# are set with valid values _in the CMake cache_. This means that if these
|
||||
# variables are set directly in the cache, either by the user in the CMake GUI,
|
||||
# or by the user passing -DVAR=VALUE directives to CMake when called (which
|
||||
# explicitly defines a cache variable), then they will be used verbatim,
|
||||
# bypassing the HINTS variables and other hard-coded search locations.
|
||||
#
|
||||
# EIGEN_INCLUDE_DIR: Include directory for CXSparse, not including the
|
||||
# include directory of any dependencies.
|
||||
|
||||
# Called if we failed to find Eigen or any of it's required dependencies,
|
||||
# unsets all public (designed to be used externally) variables and reports
|
||||
# error message at priority depending upon [REQUIRED/QUIET/<NONE>] argument.
|
||||
macro(EIGEN_REPORT_NOT_FOUND REASON_MSG)
|
||||
unset(EIGEN_FOUND)
|
||||
unset(EIGEN_INCLUDE_DIRS)
|
||||
# Make results of search visible in the CMake GUI if Eigen has not
|
||||
# been found so that user does not have to toggle to advanced view.
|
||||
mark_as_advanced(CLEAR EIGEN_INCLUDE_DIR)
|
||||
# Note <package>_FIND_[REQUIRED/QUIETLY] variables defined by FindPackage()
|
||||
# use the camelcase library name, not uppercase.
|
||||
if (Eigen_FIND_QUIETLY)
|
||||
message(STATUS "Failed to find Eigen - " ${REASON_MSG} ${ARGN})
|
||||
elseif (Eigen_FIND_REQUIRED)
|
||||
message(FATAL_ERROR "Failed to find Eigen - " ${REASON_MSG} ${ARGN})
|
||||
else()
|
||||
# Neither QUIETLY nor REQUIRED, use no priority which emits a message
|
||||
# but continues configuration and allows generation.
|
||||
message("-- Failed to find Eigen - " ${REASON_MSG} ${ARGN})
|
||||
endif ()
|
||||
return()
|
||||
endmacro(EIGEN_REPORT_NOT_FOUND)
|
||||
|
||||
# Protect against any alternative find_package scripts for this library having
|
||||
# been called previously (in a client project) which set EIGEN_FOUND, but not
|
||||
# the other variables we require / set here which could cause the search logic
|
||||
# here to fail.
|
||||
unset(EIGEN_FOUND)
|
||||
|
||||
# Search user-installed locations first, so that we prefer user installs
|
||||
# to system installs where both exist.
|
||||
#
|
||||
# TODO: Add standard Windows search locations for Eigen.
|
||||
list(APPEND EIGEN_CHECK_INCLUDE_DIRS
|
||||
/usr/local/include
|
||||
/usr/local/homebrew/include # Mac OS X
|
||||
/opt/local/var/macports/software # Mac OS X.
|
||||
/opt/local/include
|
||||
/usr/include)
|
||||
# Additional suffixes to try appending to each search path.
|
||||
list(APPEND EIGEN_CHECK_PATH_SUFFIXES
|
||||
eigen3 # Default root directory for Eigen.
|
||||
Eigen/include/eigen3 ) # Windows (for C:/Program Files prefix).
|
||||
|
||||
# Search supplied hint directories first if supplied.
|
||||
find_path(EIGEN_INCLUDE_DIR
|
||||
NAMES Eigen/Core
|
||||
PATHS ${EIGEN_INCLUDE_DIR_HINTS}
|
||||
${EIGEN_CHECK_INCLUDE_DIRS}
|
||||
PATH_SUFFIXES ${EIGEN_CHECK_PATH_SUFFIXES})
|
||||
|
||||
if (NOT EIGEN_INCLUDE_DIR OR
|
||||
NOT EXISTS ${EIGEN_INCLUDE_DIR})
|
||||
eigen_report_not_found(
|
||||
"Could not find eigen3 include directory, set EIGEN_INCLUDE_DIR to "
|
||||
"path to eigen3 include directory, e.g. /usr/local/include/eigen3.")
|
||||
endif (NOT EIGEN_INCLUDE_DIR OR
|
||||
NOT EXISTS ${EIGEN_INCLUDE_DIR})
|
||||
|
||||
# Mark internally as found, then verify. EIGEN_REPORT_NOT_FOUND() unsets
|
||||
# if called.
|
||||
set(EIGEN_FOUND TRUE)
|
||||
|
||||
# Extract Eigen version from Eigen/src/Core/util/Macros.h
|
||||
if (EIGEN_INCLUDE_DIR)
|
||||
set(EIGEN_VERSION_FILE ${EIGEN_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h)
|
||||
if (NOT EXISTS ${EIGEN_VERSION_FILE})
|
||||
eigen_report_not_found(
|
||||
"Could not find file: ${EIGEN_VERSION_FILE} "
|
||||
"containing version information in Eigen install located at: "
|
||||
"${EIGEN_INCLUDE_DIR}.")
|
||||
else (NOT EXISTS ${EIGEN_VERSION_FILE})
|
||||
file(READ ${EIGEN_VERSION_FILE} EIGEN_VERSION_FILE_CONTENTS)
|
||||
|
||||
string(REGEX MATCH "#define EIGEN_WORLD_VERSION [0-9]+"
|
||||
EIGEN_WORLD_VERSION "${EIGEN_VERSION_FILE_CONTENTS}")
|
||||
string(REGEX REPLACE "#define EIGEN_WORLD_VERSION ([0-9]+)" "\\1"
|
||||
EIGEN_WORLD_VERSION "${EIGEN_WORLD_VERSION}")
|
||||
|
||||
string(REGEX MATCH "#define EIGEN_MAJOR_VERSION [0-9]+"
|
||||
EIGEN_MAJOR_VERSION "${EIGEN_VERSION_FILE_CONTENTS}")
|
||||
string(REGEX REPLACE "#define EIGEN_MAJOR_VERSION ([0-9]+)" "\\1"
|
||||
EIGEN_MAJOR_VERSION "${EIGEN_MAJOR_VERSION}")
|
||||
|
||||
string(REGEX MATCH "#define EIGEN_MINOR_VERSION [0-9]+"
|
||||
EIGEN_MINOR_VERSION "${EIGEN_VERSION_FILE_CONTENTS}")
|
||||
string(REGEX REPLACE "#define EIGEN_MINOR_VERSION ([0-9]+)" "\\1"
|
||||
EIGEN_MINOR_VERSION "${EIGEN_MINOR_VERSION}")
|
||||
|
||||
# This is on a single line s/t CMake does not interpret it as a list of
|
||||
# elements and insert ';' separators which would result in 3.;2.;0 nonsense.
|
||||
set(EIGEN_VERSION "${EIGEN_WORLD_VERSION}.${EIGEN_MAJOR_VERSION}.${EIGEN_MINOR_VERSION}")
|
||||
endif (NOT EXISTS ${EIGEN_VERSION_FILE})
|
||||
endif (EIGEN_INCLUDE_DIR)
|
||||
|
||||
# Set standard CMake FindPackage variables if found.
|
||||
if (EIGEN_FOUND)
|
||||
set(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR})
|
||||
endif (EIGEN_FOUND)
|
||||
|
||||
# Handle REQUIRED / QUIET optional arguments and version.
|
||||
include(FindPackageHandleStandardArgs)
|
||||
find_package_handle_standard_args(Eigen
|
||||
REQUIRED_VARS EIGEN_INCLUDE_DIRS
|
||||
VERSION_VAR EIGEN_VERSION)
|
||||
|
||||
# Only mark internal variables as advanced if we found Eigen, otherwise
|
||||
# leave it visible in the standard GUI for the user to set manually.
|
||||
if (EIGEN_FOUND)
|
||||
mark_as_advanced(FORCE EIGEN_INCLUDE_DIR)
|
||||
endif (EIGEN_FOUND)
|
||||
@ -1,135 +0,0 @@
|
||||
# Try to find gnu scientific library GSL
|
||||
# See
|
||||
# http://www.gnu.org/software/gsl/ and
|
||||
# http://gnuwin32.sourceforge.net/packages/gsl.htm
|
||||
#
|
||||
# Based on a script of Felix Woelk and Jan Woetzel
|
||||
# (www.mip.informatik.uni-kiel.de)
|
||||
#
|
||||
# It defines the following variables:
|
||||
# GSL_FOUND - system has GSL lib
|
||||
# GSL_INCLUDE_DIRS - where to find headers
|
||||
# GSL_LIBRARIES - full path to the libraries
|
||||
# GSL_LIBRARY_DIRS, the directory where the PLplot library is found.
|
||||
|
||||
# CMAKE_GSL_CXX_FLAGS = Unix compiler flags for GSL, essentially "`gsl-config --cxxflags`"
|
||||
# GSL_LINK_DIRECTORIES = link directories, useful for rpath on Unix
|
||||
# GSL_EXE_LINKER_FLAGS = rpath on Unix
|
||||
|
||||
set( GSL_FOUND OFF )
|
||||
set( GSL_CBLAS_FOUND OFF )
|
||||
|
||||
# Windows, but not for Cygwin and MSys where gsl-config is available
|
||||
if( WIN32 AND NOT CYGWIN AND NOT MSYS )
|
||||
# look for headers
|
||||
find_path( GSL_INCLUDE_DIR
|
||||
NAMES gsl/gsl_cdf.h gsl/gsl_randist.h
|
||||
)
|
||||
if( GSL_INCLUDE_DIR )
|
||||
# look for gsl library
|
||||
find_library( GSL_LIBRARY
|
||||
NAMES gsl
|
||||
)
|
||||
if( GSL_LIBRARY )
|
||||
set( GSL_INCLUDE_DIRS ${GSL_INCLUDE_DIR} )
|
||||
get_filename_component( GSL_LIBRARY_DIRS ${GSL_LIBRARY} PATH )
|
||||
set( GSL_FOUND ON )
|
||||
endif( GSL_LIBRARY )
|
||||
|
||||
# look for gsl cblas library
|
||||
find_library( GSL_CBLAS_LIBRARY
|
||||
NAMES gslcblas
|
||||
)
|
||||
if( GSL_CBLAS_LIBRARY )
|
||||
set( GSL_CBLAS_FOUND ON )
|
||||
endif( GSL_CBLAS_LIBRARY )
|
||||
|
||||
set( GSL_LIBRARIES ${GSL_LIBRARY} ${GSL_CBLAS_LIBRARY} )
|
||||
endif( GSL_INCLUDE_DIR )
|
||||
|
||||
mark_as_advanced(
|
||||
GSL_INCLUDE_DIR
|
||||
GSL_LIBRARY
|
||||
GSL_CBLAS_LIBRARY
|
||||
)
|
||||
else( WIN32 AND NOT CYGWIN AND NOT MSYS )
|
||||
if( UNIX OR MSYS )
|
||||
find_program( GSL_CONFIG_EXECUTABLE gsl-config
|
||||
/usr/bin/
|
||||
/usr/local/bin
|
||||
)
|
||||
|
||||
if( GSL_CONFIG_EXECUTABLE )
|
||||
set( GSL_FOUND ON )
|
||||
|
||||
# run the gsl-config program to get cxxflags
|
||||
execute_process(
|
||||
COMMAND sh "${GSL_CONFIG_EXECUTABLE}" --cflags
|
||||
OUTPUT_VARIABLE GSL_CFLAGS
|
||||
RESULT_VARIABLE RET
|
||||
ERROR_QUIET
|
||||
)
|
||||
if( RET EQUAL 0 )
|
||||
string( STRIP "${GSL_CFLAGS}" GSL_CFLAGS )
|
||||
separate_arguments( GSL_CFLAGS )
|
||||
|
||||
# parse definitions from cflags; drop -D* from CFLAGS
|
||||
string( REGEX MATCHALL "-D[^;]+"
|
||||
GSL_DEFINITIONS "${GSL_CFLAGS}" )
|
||||
string( REGEX REPLACE "-D[^;]+;" ""
|
||||
GSL_CFLAGS "${GSL_CFLAGS}" )
|
||||
|
||||
# parse include dirs from cflags; drop -I prefix
|
||||
string( REGEX MATCHALL "-I[^;]+"
|
||||
GSL_INCLUDE_DIRS "${GSL_CFLAGS}" )
|
||||
string( REPLACE "-I" ""
|
||||
GSL_INCLUDE_DIRS "${GSL_INCLUDE_DIRS}")
|
||||
string( REGEX REPLACE "-I[^;]+;" ""
|
||||
GSL_CFLAGS "${GSL_CFLAGS}")
|
||||
|
||||
message("GSL_DEFINITIONS=${GSL_DEFINITIONS}")
|
||||
message("GSL_INCLUDE_DIRS=${GSL_INCLUDE_DIRS}")
|
||||
message("GSL_CFLAGS=${GSL_CFLAGS}")
|
||||
else( RET EQUAL 0 )
|
||||
set( GSL_FOUND FALSE )
|
||||
endif( RET EQUAL 0 )
|
||||
|
||||
# run the gsl-config program to get the libs
|
||||
execute_process(
|
||||
COMMAND sh "${GSL_CONFIG_EXECUTABLE}" --libs
|
||||
OUTPUT_VARIABLE GSL_LIBRARIES
|
||||
RESULT_VARIABLE RET
|
||||
ERROR_QUIET
|
||||
)
|
||||
if( RET EQUAL 0 )
|
||||
string(STRIP "${GSL_LIBRARIES}" GSL_LIBRARIES )
|
||||
separate_arguments( GSL_LIBRARIES )
|
||||
|
||||
# extract linkdirs (-L) for rpath (i.e., LINK_DIRECTORIES)
|
||||
string( REGEX MATCHALL "-L[^;]+"
|
||||
GSL_LIBRARY_DIRS "${GSL_LIBRARIES}" )
|
||||
string( REPLACE "-L" ""
|
||||
GSL_LIBRARY_DIRS "${GSL_LIBRARY_DIRS}" )
|
||||
else( RET EQUAL 0 )
|
||||
set( GSL_FOUND FALSE )
|
||||
endif( RET EQUAL 0 )
|
||||
|
||||
MARK_AS_ADVANCED(
|
||||
GSL_CFLAGS
|
||||
)
|
||||
message( STATUS "Using GSL from ${GSL_PREFIX}" )
|
||||
else( GSL_CONFIG_EXECUTABLE )
|
||||
message( STATUS "FindGSL: gsl-config not found.")
|
||||
endif( GSL_CONFIG_EXECUTABLE )
|
||||
endif( UNIX OR MSYS )
|
||||
endif( WIN32 AND NOT CYGWIN AND NOT MSYS )
|
||||
|
||||
if( GSL_FOUND )
|
||||
if( NOT GSL_FIND_QUIETLY )
|
||||
message( STATUS "FindGSL: Found both GSL headers and library" )
|
||||
endif( NOT GSL_FIND_QUIETLY )
|
||||
else( GSL_FOUND )
|
||||
if( GSL_FIND_REQUIRED )
|
||||
message( FATAL_ERROR "FindGSL: Could not find GSL headers or library" )
|
||||
endif( GSL_FIND_REQUIRED )
|
||||
endif( GSL_FOUND )
|
||||
@ -1,124 +0,0 @@
|
||||
macro(mvIMPACT_REPORT_NOT_FOUND REASON_MSG)
|
||||
unset(mvIMPACT_FOUND)
|
||||
unset(mvIMPACT_INCLUDE_DIRS)
|
||||
unset(mvIMPACT_LIBRARIES)
|
||||
unset(mvIMPACT_WORLD_VERSION)
|
||||
unset(mvIMPACT_MAJOR_VERSION)
|
||||
unset(mvIMPACT_MINOR_VERSION)
|
||||
# Make results of search visible in the CMake GUI if mvimpact has not
|
||||
# been found so that user does not have to toggle to advanced view.
|
||||
mark_as_advanced(CLEAR mvIMPACT_INCLUDE_DIR)
|
||||
# Note <package>_FIND_[REQUIRED/QUIETLY] variables defined by FindPackage()
|
||||
# use the camelcase library name, not uppercase.
|
||||
if(Mvimpact_FIND_QUIETLY)
|
||||
message(STATUS "Failed to find mvimpact - " ${REASON_MSG} ${ARGN})
|
||||
elseif(Mvimpact_FIND_REQUIRED)
|
||||
message(FATAL_ERROR "Failed to find mvimpact - " ${REASON_MSG} ${ARGN})
|
||||
else()
|
||||
# Neither QUIETLY nor REQUIRED, use no priority which emits a message
|
||||
# but continues configuration and allows generation.
|
||||
message("-- Failed to find mvimpact - " ${REASON_MSG} ${ARGN})
|
||||
endif()
|
||||
endmacro(mvIMPACT_REPORT_NOT_FOUND)
|
||||
|
||||
# Search user-installed locations first, so that we prefer user installs
|
||||
# to system installs where both exist.
|
||||
get_filename_component(BLUEFOX2_DIR ${CMAKE_CURRENT_SOURCE_DIR} REALPATH)
|
||||
list(APPEND mvIMPACT_CHECK_INCLUDE_DIRS
|
||||
/opt/mvIMPACT_acquire
|
||||
/opt/mvIMPACT_Acquire
|
||||
)
|
||||
execute_process(COMMAND uname -m COMMAND tr -d '\n' OUTPUT_VARIABLE ARCH)
|
||||
|
||||
if(NOT ARCH)
|
||||
set(ARCH "arm64")
|
||||
elseif(${ARCH} STREQUAL "aarch64")
|
||||
set(ARCH "arm64")
|
||||
endif()
|
||||
|
||||
list(APPEND mvIMPACT_CHECK_LIBRARY_DIRS
|
||||
/opt/mvIMPACT_acquire/lib/${ARCH}
|
||||
/opt/mvIMPACT_Acquire/lib/${ARCH}
|
||||
)
|
||||
|
||||
# Check general hints
|
||||
if(mvIMPACT_HINTS AND EXISTS ${mvIMPACT_HINTS})
|
||||
set(mvIMPACT_INCLUDE_DIR_HINTS ${mvIMPACT_HINTS}/include)
|
||||
set(mvIMPACT_LIBRARY_DIR_HINTS ${mvIMPACT_HINTS}/lib)
|
||||
endif()
|
||||
|
||||
# Search supplied hint directories first if supplied.
|
||||
# Find include directory for mvimpact
|
||||
find_path(mvIMPACT_INCLUDE_DIR
|
||||
NAMES mvIMPACT_CPP/mvIMPACT_acquire.h
|
||||
PATHS ${mvIMPACT_INCLUDE_DIR_HINTS}
|
||||
${mvIMPACT_CHECK_INCLUDE_DIRS}
|
||||
NO_DEFAULT_PATH)
|
||||
if(NOT mvIMPACT_INCLUDE_DIR OR NOT EXISTS ${mvIMPACT_INCLUDE_DIR})
|
||||
mvIMPACT_REPORT_NOT_FOUND(
|
||||
"Could not find mvimpact include directory, set mvIMPACT_INCLUDE_DIR to "
|
||||
"path to mvimpact include directory,"
|
||||
"e.g. /opt/mvIMPACT_acquire.")
|
||||
else()
|
||||
message(STATUS "mvimpact include dir found: " ${mvIMPACT_INCLUDE_DIR})
|
||||
endif()
|
||||
|
||||
# Find library directory for mvimpact
|
||||
find_library(mvIMPACT_LIBRARY
|
||||
NAMES libmvBlueFOX.so
|
||||
PATHS ${mvIMPACT_LIBRARY_DIR_HINTS}
|
||||
${mvIMPACT_CHECK_LIBRARY_DIRS}
|
||||
NO_DEFAULT_PATH)
|
||||
if(NOT mvIMPACT_LIBRARY OR NOT EXISTS ${mvIMPACT_LIBRARY})
|
||||
mvIMPACT_REPORT_NOT_FOUND(
|
||||
"Could not find mvimpact library, set mvIMPACT_LIBRARY "
|
||||
"to full path to mvimpact library direcotory.${mvIMPACT_CHECK_LIBRARY_DIRS}~~")
|
||||
else()
|
||||
# TODO: need to fix this hacky solution for getting mvIMPACT_LIBRARY_DIR
|
||||
string(REGEX MATCH ".*/" mvIMPACT_LIBRARY_DIR ${mvIMPACT_LIBRARY})
|
||||
message(STATUS "mvimpact library dir found: " ${mvIMPACT_LIBRARY_DIR})
|
||||
|
||||
endif()
|
||||
|
||||
# Mark internally as found, then verify. mvIMPACT_REPORT_NOT_FOUND() unsets if
|
||||
# called.
|
||||
set(mvIMPACT_FOUND TRUE)
|
||||
|
||||
# Extract mvimpact version
|
||||
if(mvIMPACT_LIBRARY_DIR)
|
||||
file(GLOB mvIMPACT_LIBS
|
||||
RELATIVE ${mvIMPACT_LIBRARY_DIR}
|
||||
${mvIMPACT_LIBRARY_DIR}/libmvBlueFOX.so.[0-9].[0-9].[0-9])
|
||||
# TODO: add version support
|
||||
# string(REGEX MATCH ""
|
||||
# mvIMPACT_WORLD_VERSION ${mvIMPACT_PVBASE})
|
||||
# message(STATUS "mvimpact world version: " ${mvIMPACT_WORLD_VERSION})
|
||||
endif()
|
||||
|
||||
# Catch case when caller has set mvIMPACT_INCLUDE_DIR in the cache / GUI and
|
||||
# thus FIND_[PATH/LIBRARY] are not called, but specified locations are
|
||||
# invalid, otherwise we would report the library as found.
|
||||
if(NOT mvIMPACT_INCLUDE_DIR AND NOT EXISTS ${mvIMPACT_INCLUDE_DIR}/mvIMPACT_CPP/mvIMPACT_acquire.h)
|
||||
mvIMPACT_REPORT_NOT_FOUND("Caller defined mvIMPACT_INCLUDE_DIR: "
|
||||
${mvIMPACT_INCLUDE_DIR}
|
||||
" does not contain mvIMPACT_CPP/mvIMPACT_acquire.h header.")
|
||||
endif()
|
||||
|
||||
# Set standard CMake FindPackage variables if found.
|
||||
if(mvIMPACT_FOUND)
|
||||
set(mvIMPACT_INCLUDE_DIRS ${mvIMPACT_INCLUDE_DIR})
|
||||
file(GLOB mvIMPACT_LIBRARIES ${mvIMPACT_LIBRARY_DIR}lib*.so)
|
||||
endif()
|
||||
|
||||
# Handle REQUIRED / QUIET optional arguments.
|
||||
include(FindPackageHandleStandardArgs)
|
||||
if(mvIMPACT_FOUND)
|
||||
FIND_PACKAGE_HANDLE_STANDARD_ARGS(Mvimpact DEFAULT_MSG
|
||||
mvIMPACT_INCLUDE_DIRS mvIMPACT_LIBRARIES)
|
||||
endif()
|
||||
|
||||
# Only mark internal variables as advanced if we found mvimpact, otherwise
|
||||
# leave it visible in the standard GUI for the user to set manually.
|
||||
if(mvIMPACT_FOUND)
|
||||
mark_as_advanced(FORCE mvIMPACT_INCLUDE_DIR mvIMPACT_LIBRARY)
|
||||
endif()
|
||||
@ -1,18 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>cmake_utils</name>
|
||||
<version>0.0.0</version>
|
||||
<description>
|
||||
Once you used this package,
|
||||
then you do not need to copy cmake files among packages
|
||||
</description>
|
||||
|
||||
<maintainer email="william.wu@dji.com"> William.Wu </maintainer>
|
||||
|
||||
<license>LGPLv3</license>
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<depend>roscpp</depend>
|
||||
<depend>cmake_modules</depend>
|
||||
|
||||
</package>
|
||||
@ -1,222 +0,0 @@
|
||||
#cmake_minimum_required(VERSION 2.4.6)
|
||||
#include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
|
||||
|
||||
# Set the build type. Options are:
|
||||
# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
|
||||
# Debug : w/ debug symbols, w/o optimization
|
||||
# Release : w/o debug symbols, w/ optimization
|
||||
# RelWithDebInfo : w/ debug symbols, w/ optimization
|
||||
# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
|
||||
#set(ROS_BUILD_TYPE Release)
|
||||
|
||||
#rosbuild_init()
|
||||
|
||||
#set the default path for built executables to the "bin" directory
|
||||
#set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
|
||||
#set the default path for built libraries to the "lib" directory
|
||||
#set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
|
||||
|
||||
#uncomment if you have defined messages
|
||||
#rosbuild_genmsg()
|
||||
#uncomment if you have defined services
|
||||
#rosbuild_gensrv()
|
||||
|
||||
#common commands for building c++ executables and libraries
|
||||
#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
|
||||
#target_link_libraries(${PROJECT_NAME} another_library)
|
||||
#rosbuild_add_executable(example examples/example.cpp)
|
||||
#target_link_libraries(example ${PROJECT_NAME})
|
||||
#rosbuild_add_boost_directories()
|
||||
|
||||
|
||||
#rosbuild_add_executable(multi_map_server src/multi_map_server.cc)
|
||||
#target_link_libraries(multi_map_server pose_utils)
|
||||
|
||||
#rosbuild_add_executable(multi_map_visualization src/multi_map_visualization.cc)
|
||||
#target_link_libraries(multi_map_visualization pose_utils)
|
||||
|
||||
#----------------------------------
|
||||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(multi_map_server)
|
||||
|
||||
set(CMAKE_BUILD_TYPE "Release")
|
||||
set(CMAKE_CXX_FLAGS "-std=c++11")
|
||||
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g")
|
||||
|
||||
## Find catkin macros and libraries
|
||||
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
|
||||
## is used, also find other catkin packages
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
geometry_msgs
|
||||
nav_msgs
|
||||
pose_utils
|
||||
message_generation
|
||||
roscpp
|
||||
visualization_msgs
|
||||
tf
|
||||
std_srvs
|
||||
laser_geometry
|
||||
pose_utils
|
||||
quadrotor_msgs
|
||||
)
|
||||
|
||||
## System dependencies are found with CMake's conventions
|
||||
# find_package(Boost REQUIRED COMPONENTS system)
|
||||
|
||||
|
||||
## Uncomment this if the package has a setup.py. This macro ensures
|
||||
## modules and global scripts declared therein get installed
|
||||
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
|
||||
# catkin_python_setup()
|
||||
|
||||
################################################
|
||||
## Declare ROS messages, services and actions ##
|
||||
################################################
|
||||
|
||||
## To declare and build messages, services or actions from within this
|
||||
## package, follow these steps:
|
||||
## * Let MSG_DEP_SET be the set of packages whose message types you use in
|
||||
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
|
||||
## * In the file package.xml:
|
||||
## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
|
||||
## * If MSG_DEP_SET isn't empty the following dependencies might have been
|
||||
## pulled in transitively but can be declared for certainty nonetheless:
|
||||
## * add a build_depend tag for "message_generation"
|
||||
## * add a run_depend tag for "message_runtime"
|
||||
## * In this file (CMakeLists.txt):
|
||||
## * add "message_generation" and every package in MSG_DEP_SET to
|
||||
## find_package(catkin REQUIRED COMPONENTS ...)
|
||||
## * add "message_runtime" and every package in MSG_DEP_SET to
|
||||
## catkin_package(CATKIN_DEPENDS ...)
|
||||
## * uncomment the add_*_files sections below as needed
|
||||
## and list every .msg/.srv/.action file to be processed
|
||||
## * uncomment the generate_messages entry below
|
||||
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
|
||||
|
||||
## Generate messages in the 'msg' folder
|
||||
add_message_files(
|
||||
FILES
|
||||
MultiOccupancyGrid.msg
|
||||
MultiSparseMap3D.msg
|
||||
SparseMap3D.msg
|
||||
VerticalOccupancyGridList.msg
|
||||
#plan_cmd.msg
|
||||
)
|
||||
|
||||
## Generate services in the 'srv' folder
|
||||
# add_service_files(
|
||||
# FILES
|
||||
# Service1.srv
|
||||
# Service2.srv
|
||||
# )
|
||||
|
||||
## Generate actions in the 'action' folder
|
||||
# add_action_files(
|
||||
# FILES
|
||||
# Action1.action
|
||||
# Action2.action
|
||||
# )
|
||||
|
||||
## Generate added messages and services with any dependencies listed here
|
||||
generate_messages(
|
||||
DEPENDENCIES
|
||||
geometry_msgs
|
||||
nav_msgs
|
||||
)
|
||||
|
||||
###################################
|
||||
## catkin specific configuration ##
|
||||
###################################
|
||||
## The catkin_package macro generates cmake config files for your package
|
||||
## Declare things to be passed to dependent projects
|
||||
## INCLUDE_DIRS: uncomment this if you package contains header files
|
||||
## LIBRARIES: libraries you create in this project that dependent projects also need
|
||||
## CATKIN_DEPENDS: catkin_packages dependent projects also need
|
||||
## DEPENDS: system dependencies of this project that dependent projects also need
|
||||
catkin_package(
|
||||
# INCLUDE_DIRS include
|
||||
# LIBRARIES irobot_msgs
|
||||
CATKIN_DEPENDS geometry_msgs nav_msgs quadrotor_msgs
|
||||
# DEPENDS system_lib
|
||||
)
|
||||
|
||||
###########
|
||||
## Build ##
|
||||
###########
|
||||
find_package(Armadillo REQUIRED)
|
||||
include_directories(${ARMADILLO_INCLUDE_DIRS})
|
||||
|
||||
|
||||
## Specify additional locations of header files
|
||||
## Your package locations should be listed before other locations
|
||||
include_directories(include)
|
||||
include_directories(
|
||||
${catkin_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
## Declare a cpp library
|
||||
# add_library(irobot_msgs
|
||||
# src/${PROJECT_NAME}/irobot_msgs.cpp
|
||||
# )
|
||||
|
||||
## Declare a cpp executable
|
||||
add_executable(multi_map_visualization src/multi_map_visualization.cc)
|
||||
|
||||
## Add cmake target dependencies of the executable/library
|
||||
## as an example, message headers may need to be generated before nodes
|
||||
add_dependencies(multi_map_visualization multi_map_server_messages_cpp)
|
||||
|
||||
## Specify libraries to link a library or executable target against
|
||||
target_link_libraries(multi_map_visualization
|
||||
${catkin_LIBRARIES}
|
||||
${ARMADILLO_LIBRARIES}
|
||||
pose_utils
|
||||
)
|
||||
|
||||
#############
|
||||
## Install ##
|
||||
#############
|
||||
|
||||
# all install targets should use catkin DESTINATION variables
|
||||
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
|
||||
|
||||
## Mark executable scripts (Python etc.) for installation
|
||||
## in contrast to setup.py, you can choose the destination
|
||||
# install(PROGRAMS
|
||||
# scripts/my_python_script
|
||||
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
# )
|
||||
|
||||
## Mark executables and/or libraries for installation
|
||||
# install(TARGETS irobot_msgs irobot_msgs_node
|
||||
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
# )
|
||||
|
||||
## Mark cpp header files for installation
|
||||
# install(DIRECTORY include/${PROJECT_NAME}/
|
||||
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
|
||||
# FILES_MATCHING PATTERN "*.h"
|
||||
# PATTERN ".svn" EXCLUDE
|
||||
# )
|
||||
|
||||
## Mark other files for installation (e.g. launch and bag files, etc.)
|
||||
# install(FILES
|
||||
# # myfile1
|
||||
# # myfile2
|
||||
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
|
||||
# )
|
||||
|
||||
#############
|
||||
## Testing ##
|
||||
#############
|
||||
|
||||
## Add gtest based cpp test target and link libraries
|
||||
# catkin_add_gtest(${PROJECT_NAME}-test test/test_irobot_msgs.cpp)
|
||||
# if(TARGET ${PROJECT_NAME}-test)
|
||||
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
|
||||
# endif()
|
||||
|
||||
## Add folders to be run by python nosetests
|
||||
# catkin_add_nosetests(test)
|
||||
@ -1 +0,0 @@
|
||||
include $(shell rospack find mk)/cmake.mk
|
||||
@ -1,242 +0,0 @@
|
||||
#ifndef MAP2D_H
|
||||
#define MAP2D_H
|
||||
|
||||
#include <iostream>
|
||||
#include <ros/ros.h>
|
||||
#include <tf/tf.h>
|
||||
#include <nav_msgs/OccupancyGrid.h>
|
||||
|
||||
using namespace std;
|
||||
|
||||
class Map2D
|
||||
{
|
||||
|
||||
private:
|
||||
nav_msgs::OccupancyGrid map;
|
||||
int expandStep;
|
||||
int binning;
|
||||
bool isBinningSet;
|
||||
bool updated;
|
||||
|
||||
public:
|
||||
Map2D()
|
||||
{
|
||||
map.data.resize(0);
|
||||
map.info.origin.orientation = tf::createQuaternionMsgFromYaw(0.0);
|
||||
expandStep = 200;
|
||||
binning = 1;
|
||||
isBinningSet = false;
|
||||
updated = false;
|
||||
}
|
||||
|
||||
Map2D(int _binning)
|
||||
{
|
||||
map.data.resize(0);
|
||||
map.info.origin.orientation = tf::createQuaternionMsgFromYaw(0.0);
|
||||
expandStep = 200;
|
||||
binning = _binning;
|
||||
isBinningSet = true;
|
||||
updated = false;
|
||||
}
|
||||
|
||||
~Map2D() {}
|
||||
|
||||
double GetResolution() { return map.info.resolution; }
|
||||
double GetMinX() { return map.info.origin.position.x; }
|
||||
double GetMinY() { return map.info.origin.position.y; }
|
||||
double GetMaxX() { return map.info.origin.position.x + map.info.width * map.info.resolution; }
|
||||
double GetMaxY() { return map.info.origin.position.y + map.info.height * map.info.resolution; }
|
||||
bool Updated() { return updated; }
|
||||
void Reset() { map = nav_msgs::OccupancyGrid(); }
|
||||
|
||||
void SetBinning(int _binning)
|
||||
{
|
||||
if (!isBinningSet)
|
||||
binning = _binning;
|
||||
}
|
||||
|
||||
// Get occupancy value, 0: unknown; +ve: occupied; -ve: free
|
||||
signed char GetOccupiedFromWorldFrame(double x, double y)
|
||||
{
|
||||
int xm = (x - map.info.origin.position.x) / map.info.resolution;
|
||||
int ym = (y - map.info.origin.position.y) / map.info.resolution;
|
||||
if (xm < 0 || xm > (int)(map.info.width-1) || ym < 0 || ym > (int)(map.info.height-1))
|
||||
return 0;
|
||||
else
|
||||
return map.data[ym*map.info.width+xm];
|
||||
}
|
||||
|
||||
void Replace(nav_msgs::OccupancyGrid m)
|
||||
{
|
||||
// Check data
|
||||
if (m.data.size() == 0)
|
||||
return;
|
||||
isBinningSet = true;
|
||||
// Binning, conservative, take maximum
|
||||
if (binning > 1)
|
||||
{
|
||||
int _width = m.info.width;
|
||||
m.info.width /= binning;
|
||||
m.info.height /= binning;
|
||||
m.info.resolution *= binning;
|
||||
vector<signed char> data(m.info.width * m.info.height);
|
||||
for (uint32_t i = 0; i < m.info.height; i++)
|
||||
{
|
||||
for (uint32_t j = 0; j < m.info.width; j++)
|
||||
{
|
||||
int val = -0xff;
|
||||
for (int _i = 0; _i < binning; _i++)
|
||||
{
|
||||
for (int _j = 0; _j < binning; _j++)
|
||||
{
|
||||
int v = m.data[(i*binning + _i) * _width + (j*binning + _j)];
|
||||
val = (v > val)?v:val;
|
||||
}
|
||||
}
|
||||
data[i * m.info.width + j] = val;
|
||||
}
|
||||
}
|
||||
m.data = data;
|
||||
}
|
||||
// Replace map
|
||||
map = m;
|
||||
updated = true;
|
||||
}
|
||||
|
||||
// Merge submap
|
||||
void Update(nav_msgs::OccupancyGrid m)
|
||||
{
|
||||
// Check data
|
||||
if (m.data.size() == 0)
|
||||
return;
|
||||
isBinningSet = true;
|
||||
// Binning, conservative, take maximum
|
||||
if (binning > 1)
|
||||
{
|
||||
int _width = m.info.width;
|
||||
m.info.width /= binning;
|
||||
m.info.height /= binning;
|
||||
m.info.resolution *= binning;
|
||||
vector<signed char> data(m.info.width * m.info.height);
|
||||
for (uint32_t i = 0; i < m.info.height; i++)
|
||||
{
|
||||
for (uint32_t j = 0; j < m.info.width; j++)
|
||||
{
|
||||
int val = -0xff;
|
||||
for (int _i = 0; _i < binning; _i++)
|
||||
{
|
||||
for (int _j = 0; _j < binning; _j++)
|
||||
{
|
||||
int v = m.data[(i*binning + _i) * _width + (j*binning + _j)];
|
||||
val = (v > val)?v:val;
|
||||
}
|
||||
}
|
||||
data[i * m.info.width + j] = val;
|
||||
}
|
||||
}
|
||||
m.data = data;
|
||||
}
|
||||
// Initialize and check resolution
|
||||
if (map.info.resolution == 0)
|
||||
map.info.resolution = m.info.resolution;
|
||||
else if (m.info.resolution != map.info.resolution)
|
||||
return;
|
||||
// Get Info
|
||||
double ox = m.info.origin.position.x;
|
||||
double oy = m.info.origin.position.y;
|
||||
double oyaw = tf::getYaw(m.info.origin.orientation);
|
||||
double syaw = sin(oyaw);
|
||||
double cyaw = cos(oyaw);
|
||||
int mx = m.info.width;
|
||||
int my = m.info.height;
|
||||
double xs[4];
|
||||
double ys[4];
|
||||
xs[0] = cyaw * (0) - syaw * (0) + ox;
|
||||
xs[1] = cyaw * (mx*map.info.resolution) - syaw * (0) + ox;
|
||||
xs[2] = cyaw * (0) - syaw * (mx*map.info.resolution) + ox;
|
||||
xs[3] = cyaw * (mx*map.info.resolution) - syaw * (my*map.info.resolution) + ox;
|
||||
ys[0] = syaw * (0) + cyaw * (0) + oy;
|
||||
ys[1] = syaw * (mx*map.info.resolution) + cyaw * (0) + oy;
|
||||
ys[2] = syaw * (0) + cyaw * (my*map.info.resolution) + oy;
|
||||
ys[3] = syaw * (mx*map.info.resolution) + cyaw * (my*map.info.resolution) + oy;
|
||||
double minx = xs[0];
|
||||
double maxx = xs[0];
|
||||
double miny = ys[0];
|
||||
double maxy = ys[0];
|
||||
for (int k = 0; k < 4; k++)
|
||||
{
|
||||
minx = (xs[k] < minx)?xs[k]:minx;
|
||||
miny = (ys[k] < miny)?ys[k]:miny;
|
||||
maxx = (xs[k] > maxx)?xs[k]:maxx;
|
||||
maxy = (ys[k] > maxy)?ys[k]:maxy;
|
||||
}
|
||||
// Check whether resize map is needed
|
||||
bool mapResetFlag = false;
|
||||
double prevOriginX = map.info.origin.position.x;
|
||||
double prevOriginY = map.info.origin.position.y;
|
||||
int prevMapX = map.info.width;
|
||||
int prevMapY = map.info.height;
|
||||
while (map.info.origin.position.x > minx)
|
||||
{
|
||||
map.info.width += expandStep;
|
||||
map.info.origin.position.x -= expandStep*map.info.resolution;
|
||||
mapResetFlag = true;
|
||||
}
|
||||
while (map.info.origin.position.y > miny)
|
||||
{
|
||||
map.info.height += expandStep;
|
||||
map.info.origin.position.y -= expandStep*map.info.resolution;
|
||||
mapResetFlag = true;
|
||||
}
|
||||
while (map.info.origin.position.x + map.info.width*map.info.resolution < maxx)
|
||||
{
|
||||
map.info.width += expandStep;
|
||||
mapResetFlag = true;
|
||||
}
|
||||
while (map.info.origin.position.y + map.info.height*map.info.resolution < maxy)
|
||||
{
|
||||
map.info.height += expandStep;
|
||||
mapResetFlag = true;
|
||||
}
|
||||
// Resize map
|
||||
if (mapResetFlag)
|
||||
{
|
||||
mapResetFlag = false;
|
||||
vector<signed char> _data = map.data;
|
||||
map.data.clear();
|
||||
map.data.resize(map.info.width*map.info.height,0);
|
||||
for (int x = 0; x < prevMapX; x++)
|
||||
{
|
||||
for(int y = 0; y < prevMapY; y++)
|
||||
{
|
||||
int xn = x + round((prevOriginX - map.info.origin.position.x) / map.info.resolution);
|
||||
int yn = y + round((prevOriginY - map.info.origin.position.y) / map.info.resolution);
|
||||
map.data[yn*map.info.width+xn] = _data[y*prevMapX+x];
|
||||
}
|
||||
}
|
||||
}
|
||||
// Merge map
|
||||
for (int x = 0; x < mx; x++)
|
||||
{
|
||||
for(int y = 0; y < my; y++)
|
||||
{
|
||||
int xn = (cyaw*(x*map.info.resolution)-syaw*(y*map.info.resolution)+ox-map.info.origin.position.x) / map.info.resolution;
|
||||
int yn = (syaw*(x*map.info.resolution)+cyaw*(y*map.info.resolution)+oy-map.info.origin.position.y) / map.info.resolution;
|
||||
if (abs((int)(map.data[yn*map.info.width+xn]) + (int)(m.data[y*mx+x])) <= 127)
|
||||
map.data[yn*map.info.width+xn] += m.data[y*mx+x];
|
||||
}
|
||||
}
|
||||
updated = true;
|
||||
}
|
||||
|
||||
const nav_msgs::OccupancyGrid& GetMap()
|
||||
{
|
||||
map.header.stamp = ros::Time::now();
|
||||
map.info.map_load_time = ros::Time::now();
|
||||
map.header.frame_id = string("/map");
|
||||
updated = false;
|
||||
return map;
|
||||
}
|
||||
};
|
||||
|
||||
#endif
|
||||
@ -1,608 +0,0 @@
|
||||
#ifndef MAP3D_H
|
||||
#define MAP3D_H
|
||||
|
||||
#include <iostream>
|
||||
#include <ros/ros.h>
|
||||
#include <tf/tf.h>
|
||||
#include <armadillo>
|
||||
#include <multi_map_server/SparseMap3D.h>
|
||||
|
||||
using namespace std;
|
||||
|
||||
// Occupancy probability of a sensor
|
||||
#define PROB_OCCUPIED 0.95
|
||||
|
||||
// Free space probability of a sensor
|
||||
#define PROB_FREE 0.01
|
||||
|
||||
// Threshold that determine a cell is occupied
|
||||
#define PROB_OCCUPIED_THRESHOLD 0.75
|
||||
|
||||
// If beyond this threshold, the occupancy(occupied) of this cell is fixed, no decay
|
||||
#define PROB_OCCUPIED_FIXED_THRESHOLD 0.95
|
||||
|
||||
// Threshold that determine a cell is free
|
||||
#define PROB_FREE_THRESHOLD 0.25
|
||||
|
||||
// If bwlow this threshold, the occupancy(free) of this cell is fixed, no decay
|
||||
#define PROB_FREE_FIXED_THRESHOLD 0.05
|
||||
|
||||
// Used integer value to store occupancy, create a large scale factor to enable small scale decay
|
||||
#define LOG_ODD_SCALE_FACTOR 10000
|
||||
|
||||
// Decay factor per second
|
||||
#define LOG_ODD_DECAY_RATE 1.00
|
||||
|
||||
// Binary value of the occupancy output
|
||||
#define OCCUPIED 1
|
||||
#define FREE -1
|
||||
|
||||
// Cell Struct -----------------------------------------
|
||||
struct OccupancyGrid
|
||||
{
|
||||
int upper;
|
||||
int lower;
|
||||
int mass;
|
||||
};
|
||||
|
||||
// Occupancy Grids List --------------------------------
|
||||
class OccupancyGridList
|
||||
{
|
||||
public:
|
||||
|
||||
OccupancyGridList() { updateCounter = 0; }
|
||||
|
||||
~OccupancyGridList() { }
|
||||
|
||||
void PackMsg(multi_map_server::VerticalOccupancyGridList &msg)
|
||||
{
|
||||
msg.x = x;
|
||||
msg.y = y;
|
||||
for (list<OccupancyGrid>::iterator k = grids.begin(); k != grids.end(); k++)
|
||||
{
|
||||
msg.upper.push_back(k->upper);
|
||||
msg.lower.push_back(k->lower);
|
||||
msg.mass.push_back(k->mass);
|
||||
}
|
||||
}
|
||||
|
||||
void UnpackMsg(const multi_map_server::VerticalOccupancyGridList &msg)
|
||||
{
|
||||
x = msg.x;
|
||||
y = msg.y;
|
||||
updateCounter = 0;
|
||||
grids.clear();
|
||||
for (unsigned int k = 0; k < msg.mass.size(); k++)
|
||||
{
|
||||
OccupancyGrid c;
|
||||
c.upper = msg.upper[k];
|
||||
c.lower = msg.lower[k];
|
||||
c.mass = msg.mass[k];
|
||||
grids.push_back(c);
|
||||
}
|
||||
}
|
||||
|
||||
void GetOccupancyGrids(vector<OccupancyGrid>& _grids)
|
||||
{
|
||||
_grids.clear();
|
||||
for (list<OccupancyGrid>::iterator k = grids.begin(); k != grids.end(); k++)
|
||||
_grids.push_back((*k));
|
||||
}
|
||||
|
||||
inline int GetUpdateCounter() { return updateCounter; }
|
||||
|
||||
inline void SetUpdateCounterXY(int _updateCounter, double _x, double _y)
|
||||
{
|
||||
updateCounter = _updateCounter;
|
||||
x = _x;
|
||||
y = _y;
|
||||
}
|
||||
|
||||
inline int GetOccupancyValue(int mz)
|
||||
{
|
||||
for (list<OccupancyGrid>::iterator k = grids.begin(); k != grids.end(); k++)
|
||||
if (mz <= k->upper && mz >= k->lower)
|
||||
return k->mass / (k->upper - k->lower + 1);
|
||||
return 0;
|
||||
}
|
||||
|
||||
inline void DeleteOccupancyGrid(int mz)
|
||||
{
|
||||
for (list<OccupancyGrid>::iterator k = grids.begin(); k != grids.end(); k++)
|
||||
{
|
||||
if (mz <= k->upper && mz >= k->lower)
|
||||
{
|
||||
grids.erase(k);
|
||||
return;
|
||||
}
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
inline void SetOccupancyValue(int mz, int value)
|
||||
{
|
||||
OccupancyGrid grid;
|
||||
grid.upper = mz;
|
||||
grid.lower = mz;
|
||||
grid.mass = value;
|
||||
|
||||
list<OccupancyGrid>::iterator gend = grids.end();
|
||||
gend--;
|
||||
|
||||
if (grids.size() == 0) // Empty case
|
||||
{
|
||||
grids.push_back(grid);
|
||||
return;
|
||||
}
|
||||
else if (mz - grids.begin()->upper > 1) // Beyond highest
|
||||
{
|
||||
grids.push_front(grid);
|
||||
return;
|
||||
}
|
||||
else if (mz - grids.begin()->upper == 1) // Next to highest
|
||||
{
|
||||
grids.begin()->upper += 1;
|
||||
grids.begin()->mass += value;
|
||||
return;
|
||||
}
|
||||
else if (gend->lower - mz > 1) // Below lowest
|
||||
{
|
||||
grids.push_back(grid);
|
||||
return;
|
||||
}
|
||||
else if (gend->lower - mz == 1) // Next to lowest
|
||||
{
|
||||
grids.end()->lower -= 1;
|
||||
grids.end()->mass += value;
|
||||
return;
|
||||
}
|
||||
else // General case
|
||||
{
|
||||
for (list<OccupancyGrid>::iterator k = grids.begin(); k != grids.end(); k++)
|
||||
{
|
||||
if (mz <= k->upper && mz >= k->lower) // Within a grid
|
||||
{
|
||||
k->mass += value;
|
||||
return;
|
||||
}
|
||||
else if (k != gend)
|
||||
{
|
||||
list<OccupancyGrid>::iterator j = k;
|
||||
j++;
|
||||
if (k->lower - mz == 1 && mz - j->upper > 1) // ###*--###
|
||||
{
|
||||
k->lower -= 1;
|
||||
k->mass += value;
|
||||
return;
|
||||
}
|
||||
else if (k->lower - mz > 1 && mz - j->upper == 1) // ###--*###
|
||||
{
|
||||
j->upper += 1;
|
||||
j->mass += value;
|
||||
return;
|
||||
}
|
||||
else if (k->lower - mz == 1 && mz - j->upper == 1) // ###*###
|
||||
{
|
||||
k->lower = j->lower;
|
||||
k->mass += j->mass + value;
|
||||
grids.erase(j);
|
||||
return;
|
||||
}
|
||||
else if (k->lower - mz > 1 && mz - j->upper > 1) // ###-*-###
|
||||
{
|
||||
grids.insert(j, grid);
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Merging two columns, merge the grids in input "gridList" into current column
|
||||
inline void Merge(const OccupancyGridList& gridsIn)
|
||||
{
|
||||
// Create a sorted list containing both upper and lower values
|
||||
list<pair<int, int> > lp;
|
||||
for (list<OccupancyGrid>::const_iterator k = grids.begin(); k != grids.end(); k++)
|
||||
{
|
||||
lp.push_back( make_pair(k->upper, k->mass));
|
||||
lp.push_back( make_pair(k->lower, -1));
|
||||
}
|
||||
list<pair<int, int> > lp2;
|
||||
for (list<OccupancyGrid>::const_iterator k = gridsIn.grids.begin(); k != gridsIn.grids.end(); k++)
|
||||
{
|
||||
lp2.push_back( make_pair(k->upper, k->mass));
|
||||
lp2.push_back( make_pair(k->lower, -1));
|
||||
}
|
||||
lp.merge(lp2, ComparePair());
|
||||
// Manipulate this list to get a minimum size list
|
||||
grids.clear();
|
||||
int currUpper = 0;
|
||||
int currLower = 0;
|
||||
int currMass = 0;
|
||||
int upperCnt = 0;
|
||||
int lowerCnt = 0;
|
||||
list<pair<int, int> >::iterator lend = lp.end();
|
||||
lend--;
|
||||
for (list<pair<int, int> >::iterator k = lp.begin(); k != lp.end(); k++)
|
||||
{
|
||||
if (k->second > 0)
|
||||
{
|
||||
if (upperCnt == 0) currUpper = k->first;
|
||||
currMass = (k->second > currMass)?k->second:currMass;
|
||||
upperCnt++;
|
||||
}
|
||||
if (k->second < 0)
|
||||
{
|
||||
currLower = k->first;
|
||||
lowerCnt++;
|
||||
}
|
||||
if (lowerCnt == upperCnt && k != lend)
|
||||
{
|
||||
list<pair<int, int> >::iterator j = k;
|
||||
if (k->first - (++j)->first == 1) continue;
|
||||
}
|
||||
if (lowerCnt == upperCnt)
|
||||
{
|
||||
OccupancyGrid c;
|
||||
c.upper = currUpper;
|
||||
c.lower = currLower;
|
||||
c.mass = currMass;
|
||||
grids.push_back(c);
|
||||
upperCnt = lowerCnt = currUpper = currLower = currMass = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
inline void Decay(int upThr, int lowThr, double factor)
|
||||
{
|
||||
for (list<OccupancyGrid>::iterator k = grids.begin(); k != grids.end(); k++)
|
||||
{
|
||||
int val = k->mass / (k->upper - k->lower + 1);
|
||||
if (val < upThr && val > lowThr)
|
||||
k->mass *= factor;
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
struct ComparePair
|
||||
{
|
||||
bool operator()(pair<int, int> p1, pair<int, int> p2)
|
||||
{
|
||||
if (p1.first != p2.first)
|
||||
return (p1.first > p2.first);
|
||||
else
|
||||
return (p1.second > p2.second);
|
||||
}
|
||||
};
|
||||
|
||||
// List of vertical occupancy values
|
||||
list<OccupancyGrid> grids;
|
||||
// Location of the list in world frame
|
||||
double x;
|
||||
double y;
|
||||
// Update indicator
|
||||
int updateCounter;
|
||||
|
||||
};
|
||||
|
||||
// 3D Map Object ---------------------------------
|
||||
class Map3D
|
||||
{
|
||||
public:
|
||||
|
||||
Map3D()
|
||||
{
|
||||
resolution = 0.1;
|
||||
decayInterval = -1;
|
||||
originX = -5;
|
||||
originY = -5;
|
||||
originZ = 0;
|
||||
mapX = 200;
|
||||
mapY = 200;
|
||||
expandStep = 200;
|
||||
updated = false;
|
||||
updateCounter = 1;
|
||||
updateList.clear();
|
||||
mapBase.clear();
|
||||
mapBase.resize(mapX*mapY, NULL);
|
||||
logOddOccupied = log(PROB_OCCUPIED/(1.0-PROB_OCCUPIED)) * LOG_ODD_SCALE_FACTOR;
|
||||
logOddFree = log(PROB_FREE/(1.0-PROB_FREE)) * LOG_ODD_SCALE_FACTOR;
|
||||
logOddOccupiedThr = log(1.0/(1.0-PROB_OCCUPIED_THRESHOLD) - 1.0) * LOG_ODD_SCALE_FACTOR;
|
||||
logOddOccupiedFixedThr = log(1.0/(1.0-PROB_OCCUPIED_FIXED_THRESHOLD) - 1.0) * LOG_ODD_SCALE_FACTOR;
|
||||
logOddFreeThr = log(1.0/(1.0-PROB_FREE_THRESHOLD) - 1.0) * LOG_ODD_SCALE_FACTOR;
|
||||
logOddFreeFixedThr = log(1.0/(1.0-PROB_FREE_FIXED_THRESHOLD) - 1.0) * LOG_ODD_SCALE_FACTOR;
|
||||
}
|
||||
|
||||
Map3D(const Map3D& _map3d)
|
||||
{
|
||||
resolution = _map3d.resolution;
|
||||
decayInterval = _map3d.decayInterval;
|
||||
originX = _map3d.originX;
|
||||
originY = _map3d.originY;
|
||||
originZ = _map3d.originZ;
|
||||
mapX = _map3d.mapX;
|
||||
mapY = _map3d.mapY;
|
||||
expandStep = _map3d.expandStep;
|
||||
updated = _map3d.updated;
|
||||
updateCounter = _map3d.updateCounter;
|
||||
updateList = _map3d.updateList;
|
||||
mapBase = _map3d.mapBase;
|
||||
for (unsigned int k = 0; k < mapBase.size(); k++)
|
||||
{
|
||||
if (mapBase[k])
|
||||
{
|
||||
mapBase[k] = new OccupancyGridList;
|
||||
*mapBase[k] = *_map3d.mapBase[k];
|
||||
}
|
||||
}
|
||||
logOddOccupied = log(PROB_OCCUPIED/(1.0-PROB_OCCUPIED)) * LOG_ODD_SCALE_FACTOR;
|
||||
logOddFree = log(PROB_FREE/(1.0-PROB_FREE)) * LOG_ODD_SCALE_FACTOR;
|
||||
logOddOccupiedThr = log(1.0/(1.0-PROB_OCCUPIED_THRESHOLD) - 1.0) * LOG_ODD_SCALE_FACTOR;
|
||||
logOddOccupiedFixedThr = log(1.0/(1.0-PROB_OCCUPIED_FIXED_THRESHOLD) - 1.0) * LOG_ODD_SCALE_FACTOR;
|
||||
logOddFreeThr = log(1.0/(1.0-PROB_FREE_THRESHOLD) - 1.0) * LOG_ODD_SCALE_FACTOR;
|
||||
logOddFreeFixedThr = log(1.0/(1.0-PROB_FREE_FIXED_THRESHOLD) - 1.0) * LOG_ODD_SCALE_FACTOR;
|
||||
}
|
||||
|
||||
~Map3D()
|
||||
{
|
||||
for (unsigned int k = 0; k < mapBase.size(); k++)
|
||||
{
|
||||
if (mapBase[k])
|
||||
{
|
||||
delete mapBase[k];
|
||||
mapBase[k] = NULL;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void PackMsg(multi_map_server::SparseMap3D &msg)
|
||||
{
|
||||
// Basic map info
|
||||
msg.header.stamp = ros::Time::now();
|
||||
msg.header.frame_id = string("/map");
|
||||
msg.info.map_load_time = ros::Time::now();
|
||||
msg.info.resolution = resolution;
|
||||
msg.info.origin.position.x = originX;
|
||||
msg.info.origin.position.y = originY;
|
||||
msg.info.origin.position.z = originZ;
|
||||
msg.info.width = mapX;
|
||||
msg.info.height = mapY;
|
||||
msg.info.origin.orientation = tf::createQuaternionMsgFromYaw(0.0);
|
||||
// Pack columns into message
|
||||
msg.lists.clear();
|
||||
for (unsigned int k = 0; k < updateList.size(); k++)
|
||||
{
|
||||
multi_map_server::VerticalOccupancyGridList c;
|
||||
updateList[k]->PackMsg(c);
|
||||
msg.lists.push_back(c);
|
||||
}
|
||||
updateList.clear();
|
||||
updateCounter++;
|
||||
}
|
||||
|
||||
void UnpackMsg(const multi_map_server::SparseMap3D &msg)
|
||||
{
|
||||
// Unpack column msgs, Replace the whole column
|
||||
for (unsigned int k = 0; k < msg.lists.size(); k++)
|
||||
{
|
||||
int mx, my, mz;
|
||||
WorldFrameToMapFrame(msg.lists[k].x, msg.lists[k].y, 0, mx, my, mz);
|
||||
ResizeMapBase(mx, my);
|
||||
if (mapBase[my*mapX+mx]) delete mapBase[my*mapX+mx];
|
||||
mapBase[my*mapX+mx] = new OccupancyGridList;
|
||||
mapBase[my*mapX+mx]->UnpackMsg(msg.lists[k]);
|
||||
}
|
||||
CheckDecayMap();
|
||||
updated = true;
|
||||
}
|
||||
|
||||
inline void SetOccupancyFromWorldFrame(double x, double y, double z, double p = PROB_OCCUPIED)
|
||||
{
|
||||
// Find occupancy value to be set
|
||||
int value = 0;
|
||||
if (p == PROB_OCCUPIED) value = logOddOccupied;
|
||||
else if (p == PROB_FREE) value = logOddFree;
|
||||
else return;
|
||||
// Update occupancy value, return the column that have been changed
|
||||
int mx, my, mz;
|
||||
WorldFrameToMapFrame(x, y, z, mx, my, mz);
|
||||
ResizeMapBase(mx, my);
|
||||
if (!mapBase[my*mapX+mx])
|
||||
mapBase[my*mapX+mx] = new OccupancyGridList;
|
||||
mapBase[my*mapX+mx]->SetOccupancyValue(mz, value);
|
||||
// Also record the column that have been changed in another list, for publish incremental map
|
||||
if (mapBase[my*mapX+mx]->GetUpdateCounter() != updateCounter)
|
||||
{
|
||||
updateList.push_back(mapBase[my*mapX+mx]);
|
||||
mapBase[my*mapX+mx]->SetUpdateCounterXY(updateCounter, x, y);
|
||||
}
|
||||
updated = true;
|
||||
}
|
||||
|
||||
inline int GetOccupancyFromWorldFrame(double x, double y, double z)
|
||||
{
|
||||
int mx, my, mz;
|
||||
WorldFrameToMapFrame(x, y, z, mx, my, mz);
|
||||
if (mx < 0 || my < 0 || mx >= mapX || my >= mapY)
|
||||
return 0;
|
||||
if (!mapBase[my*mapX+mx])
|
||||
return 0;
|
||||
else
|
||||
{
|
||||
if (mapBase[my*mapX+mx]->GetOccupancyValue(mz) > logOddOccupiedThr)
|
||||
return 1;
|
||||
else if (mapBase[my*mapX+mx]->GetOccupancyValue(mz) < logOddFreeThr)
|
||||
return -1;
|
||||
else
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
inline void DeleteFromWorldFrame(double x, double y, double z)
|
||||
{
|
||||
int mx, my, mz;
|
||||
WorldFrameToMapFrame(x, y, z, mx, my, mz);
|
||||
if (mx < 0 || my < 0 || mx >= mapX || my >= mapY)
|
||||
return;
|
||||
if (mapBase[my*mapX+mx])
|
||||
mapBase[my*mapX+mx]->DeleteOccupancyGrid(mz);
|
||||
}
|
||||
|
||||
vector<arma::colvec>& GetOccupancyWorldFrame(int type = OCCUPIED)
|
||||
{
|
||||
pts.clear();
|
||||
for (int mx = 0; mx < mapX; mx++)
|
||||
{
|
||||
for (int my = 0; my < mapY; my++)
|
||||
{
|
||||
if (mapBase[my*mapX+mx])
|
||||
{
|
||||
vector<OccupancyGrid> grids;
|
||||
mapBase[my*mapX+mx]->GetOccupancyGrids(grids);
|
||||
for (unsigned int k = 0; k < grids.size(); k++)
|
||||
{
|
||||
if ( (grids[k].mass / (grids[k].upper - grids[k].lower + 1) > logOddOccupiedThr && type == OCCUPIED) ||
|
||||
(grids[k].mass / (grids[k].upper - grids[k].lower + 1) < logOddFreeThr && type == FREE) )
|
||||
{
|
||||
for (int mz = grids[k].lower; mz <= grids[k].upper; mz++)
|
||||
{
|
||||
double x, y, z;
|
||||
MapFrameToWorldFrame(mx, my, mz, x, y, z);
|
||||
arma::colvec pt(3);
|
||||
pt(0) = x;
|
||||
pt(1) = y;
|
||||
pt(2) = z;
|
||||
pts.push_back(pt);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
return pts;
|
||||
}
|
||||
|
||||
// Do not allow setting parameters if at least one update received
|
||||
void SetResolution(double _resolution)
|
||||
{
|
||||
if (!updated)
|
||||
resolution = _resolution;
|
||||
}
|
||||
|
||||
void SetDecayInterval(double _decayInterval)
|
||||
{
|
||||
if (!updated && _decayInterval > 0)
|
||||
decayInterval = _decayInterval;
|
||||
}
|
||||
|
||||
inline double GetResolution() { return resolution; }
|
||||
inline double GetMaxX() { return originX + mapX*resolution; }
|
||||
inline double GetMinX() { return originX; }
|
||||
inline double GetMaxY() { return originY + mapY*resolution; }
|
||||
inline double GetMinY() { return originY; }
|
||||
inline bool Updated() { return updated; }
|
||||
|
||||
private:
|
||||
|
||||
inline void WorldFrameToMapFrame(double x, double y, double z, int& mx, int& my, int& mz)
|
||||
{
|
||||
mx = (x - originX) / resolution;
|
||||
my = (y - originY) / resolution;
|
||||
mz = (z - originZ) / resolution;
|
||||
}
|
||||
|
||||
inline void MapFrameToWorldFrame(int mx, int my, int mz, double& x, double& y, double& z)
|
||||
{
|
||||
double r = 0.5*resolution;
|
||||
x = mx * resolution + r + originX;
|
||||
y = my * resolution + r + originY;
|
||||
z = mz * resolution + r + originZ;
|
||||
}
|
||||
|
||||
inline void ResizeMapBase(int& mx, int& my)
|
||||
{
|
||||
if (mx < 0 || my < 0 || mx >= mapX || my >= mapY)
|
||||
{
|
||||
double prevOriginX = originX;
|
||||
double prevOriginY = originY;
|
||||
int prevMapX = mapX;
|
||||
int prevMapY = mapY;
|
||||
while(mx < 0)
|
||||
{
|
||||
mapX += expandStep;
|
||||
mx += expandStep;
|
||||
originX -= expandStep*resolution;
|
||||
}
|
||||
while(my < 0)
|
||||
{
|
||||
mapY += expandStep;
|
||||
my += expandStep;
|
||||
originY -= expandStep*resolution;
|
||||
}
|
||||
while(mx >= mapX)
|
||||
{
|
||||
mapX += expandStep;
|
||||
}
|
||||
while(my >= mapY)
|
||||
{
|
||||
mapY += expandStep;
|
||||
}
|
||||
vector<OccupancyGridList*> _mapBase = mapBase;
|
||||
mapBase.clear();
|
||||
mapBase.resize(mapX*mapY,NULL);
|
||||
for (int _x = 0; _x < prevMapX; _x++)
|
||||
{
|
||||
for(int _y = 0; _y < prevMapY; _y++)
|
||||
{
|
||||
int x = _x + round((prevOriginX - originX) / resolution);
|
||||
int y = _y + round((prevOriginY - originY) / resolution);
|
||||
mapBase[y*mapX+x] = _mapBase[_y*prevMapX+_x];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void CheckDecayMap()
|
||||
{
|
||||
if (decayInterval < 0)
|
||||
return;
|
||||
// Check whether to decay
|
||||
static ros::Time prevDecayT = ros::Time::now();
|
||||
ros::Time t = ros::Time::now();
|
||||
double dt = (t - prevDecayT).toSec();
|
||||
if (dt > decayInterval)
|
||||
{
|
||||
double r = pow(LOG_ODD_DECAY_RATE, dt);
|
||||
for (int mx = 0; mx < mapX; mx++)
|
||||
for (int my = 0; my < mapY; my++)
|
||||
if (mapBase[my*mapX+mx])
|
||||
mapBase[my*mapX+mx]->Decay(logOddOccupiedFixedThr, logOddFreeFixedThr, r);
|
||||
prevDecayT = t;
|
||||
}
|
||||
}
|
||||
|
||||
double resolution;
|
||||
double decayInterval;
|
||||
|
||||
int logOddOccupied;
|
||||
int logOddFree;
|
||||
int logOddOccupiedThr;
|
||||
int logOddOccupiedFixedThr;
|
||||
int logOddFreeThr;
|
||||
int logOddFreeFixedThr;
|
||||
|
||||
bool updated;
|
||||
int updateCounter;
|
||||
vector<OccupancyGridList*> updateList;
|
||||
|
||||
double originX, originY, originZ;
|
||||
int mapX, mapY;
|
||||
int expandStep;
|
||||
vector<OccupancyGridList*> mapBase;
|
||||
|
||||
vector<arma::colvec> pts;
|
||||
|
||||
};
|
||||
#endif
|
||||
@ -1,26 +0,0 @@
|
||||
/**
|
||||
\mainpage
|
||||
\htmlinclude manifest.html
|
||||
|
||||
\b multi_map_server is ...
|
||||
|
||||
<!--
|
||||
Provide an overview of your package.
|
||||
-->
|
||||
|
||||
|
||||
\section codeapi Code API
|
||||
|
||||
<!--
|
||||
Provide links to specific auto-generated API documentation within your
|
||||
package that is of particular interest to a reader. Doxygen will
|
||||
document pretty much every part of your code, so do your best here to
|
||||
point the reader to the actual API.
|
||||
|
||||
If your codebase is fairly large or has different sets of APIs, you
|
||||
should use the doxygen 'group' tag to keep these APIs together. For
|
||||
example, the roscpp documentation has 'libros' group.
|
||||
-->
|
||||
|
||||
|
||||
*/
|
||||
@ -1,2 +0,0 @@
|
||||
nav_msgs/OccupancyGrid[] maps
|
||||
geometry_msgs/Pose[] origins
|
||||
@ -1,2 +0,0 @@
|
||||
SparseMap3D[] maps
|
||||
geometry_msgs/Pose[] origins
|
||||
@ -1,4 +0,0 @@
|
||||
Header header
|
||||
nav_msgs/MapMetaData info
|
||||
VerticalOccupancyGridList[] lists
|
||||
|
||||
@ -1,6 +0,0 @@
|
||||
float32 x
|
||||
float32 y
|
||||
int32[] upper
|
||||
int32[] lower
|
||||
int32[] mass
|
||||
|
||||
@ -1,38 +0,0 @@
|
||||
<package>
|
||||
<description>multi_map_server</description>
|
||||
<name>multi_map_server</name>
|
||||
<version>0.0.0</version>
|
||||
<maintainer email="eeshaojie@todo.todo">Shaojie Shen</maintainer>
|
||||
<license>BSD</license>
|
||||
|
||||
<url>http://ros.org/wiki/multi_map_server</url>
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
|
||||
<build_depend>roscpp</build_depend>
|
||||
<build_depend>visualization_msgs</build_depend>
|
||||
<build_depend>geometry_msgs</build_depend>
|
||||
<build_depend>tf</build_depend>
|
||||
<build_depend>nav_msgs</build_depend>
|
||||
<build_depend>std_srvs</build_depend>
|
||||
<build_depend>laser_geometry</build_depend>
|
||||
<build_depend>pose_utils</build_depend>
|
||||
<build_depend>message_generation</build_depend>
|
||||
<build_depend>quadrotor_msgs</build_depend>
|
||||
|
||||
<run_depend>roscpp</run_depend>
|
||||
<run_depend>visualization_msgs</run_depend>
|
||||
<run_depend>geometry_msgs</run_depend>
|
||||
<run_depend>tf</run_depend>
|
||||
<run_depend>nav_msgs</run_depend>
|
||||
<run_depend>std_srvs</run_depend>
|
||||
<run_depend>laser_geometry</run_depend>
|
||||
<run_depend>pose_utils</run_depend>
|
||||
<run_depend>message_runtime</run_depend>
|
||||
<run_depend>quadrotor_msgs</run_depend>
|
||||
|
||||
<export>
|
||||
<cpp cflags="-I${prefix}/include -I${prefix}/msg/cpp"/>
|
||||
</export>
|
||||
</package>
|
||||
|
||||
@ -1 +0,0 @@
|
||||
#autogenerated by ROS python message generators
|
||||
@ -1,404 +0,0 @@
|
||||
"""autogenerated by genpy from multi_map_server/MultiOccupancyGrid.msg. Do not edit."""
|
||||
import sys
|
||||
python3 = True if sys.hexversion > 0x03000000 else False
|
||||
import genpy
|
||||
import struct
|
||||
|
||||
import geometry_msgs.msg
|
||||
import nav_msgs.msg
|
||||
import genpy
|
||||
import std_msgs.msg
|
||||
|
||||
class MultiOccupancyGrid(genpy.Message):
|
||||
_md5sum = "61e63a291f11a6b1796a1edf79f34f72"
|
||||
_type = "multi_map_server/MultiOccupancyGrid"
|
||||
_has_header = False #flag to mark the presence of a Header object
|
||||
_full_text = """nav_msgs/OccupancyGrid[] maps
|
||||
geometry_msgs/Pose[] origins
|
||||
|
||||
================================================================================
|
||||
MSG: nav_msgs/OccupancyGrid
|
||||
# This represents a 2-D grid map, in which each cell represents the probability of
|
||||
# occupancy.
|
||||
|
||||
Header header
|
||||
|
||||
#MetaData for the map
|
||||
MapMetaData info
|
||||
|
||||
# The map data, in row-major order, starting with (0,0). Occupancy
|
||||
# probabilities are in the range [0,100]. Unknown is -1.
|
||||
int8[] data
|
||||
|
||||
================================================================================
|
||||
MSG: std_msgs/Header
|
||||
# Standard metadata for higher-level stamped data types.
|
||||
# This is generally used to communicate timestamped data
|
||||
# in a particular coordinate frame.
|
||||
#
|
||||
# sequence ID: consecutively increasing ID
|
||||
uint32 seq
|
||||
#Two-integer timestamp that is expressed as:
|
||||
# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')
|
||||
# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')
|
||||
# time-handling sugar is provided by the client library
|
||||
time stamp
|
||||
#Frame this data is associated with
|
||||
# 0: no frame
|
||||
# 1: global frame
|
||||
string frame_id
|
||||
|
||||
================================================================================
|
||||
MSG: nav_msgs/MapMetaData
|
||||
# This hold basic information about the characterists of the OccupancyGrid
|
||||
|
||||
# The time at which the map was loaded
|
||||
time map_load_time
|
||||
# The map resolution [m/cell]
|
||||
float32 resolution
|
||||
# Map width [cells]
|
||||
uint32 width
|
||||
# Map height [cells]
|
||||
uint32 height
|
||||
# The origin of the map [m, m, rad]. This is the real-world pose of the
|
||||
# cell (0,0) in the map.
|
||||
geometry_msgs/Pose origin
|
||||
================================================================================
|
||||
MSG: geometry_msgs/Pose
|
||||
# A representation of pose in free space, composed of postion and orientation.
|
||||
Point position
|
||||
Quaternion orientation
|
||||
|
||||
================================================================================
|
||||
MSG: geometry_msgs/Point
|
||||
# This contains the position of a point in free space
|
||||
float64 x
|
||||
float64 y
|
||||
float64 z
|
||||
|
||||
================================================================================
|
||||
MSG: geometry_msgs/Quaternion
|
||||
# This represents an orientation in free space in quaternion form.
|
||||
|
||||
float64 x
|
||||
float64 y
|
||||
float64 z
|
||||
float64 w
|
||||
|
||||
"""
|
||||
__slots__ = ['maps','origins']
|
||||
_slot_types = ['nav_msgs/OccupancyGrid[]','geometry_msgs/Pose[]']
|
||||
|
||||
def __init__(self, *args, **kwds):
|
||||
"""
|
||||
Constructor. Any message fields that are implicitly/explicitly
|
||||
set to None will be assigned a default value. The recommend
|
||||
use is keyword arguments as this is more robust to future message
|
||||
changes. You cannot mix in-order arguments and keyword arguments.
|
||||
|
||||
The available fields are:
|
||||
maps,origins
|
||||
|
||||
:param args: complete set of field values, in .msg order
|
||||
:param kwds: use keyword arguments corresponding to message field names
|
||||
to set specific fields.
|
||||
"""
|
||||
if args or kwds:
|
||||
super(MultiOccupancyGrid, self).__init__(*args, **kwds)
|
||||
#message fields cannot be None, assign default values for those that are
|
||||
if self.maps is None:
|
||||
self.maps = []
|
||||
if self.origins is None:
|
||||
self.origins = []
|
||||
else:
|
||||
self.maps = []
|
||||
self.origins = []
|
||||
|
||||
def _get_types(self):
|
||||
"""
|
||||
internal API method
|
||||
"""
|
||||
return self._slot_types
|
||||
|
||||
def serialize(self, buff):
|
||||
"""
|
||||
serialize message into buffer
|
||||
:param buff: buffer, ``StringIO``
|
||||
"""
|
||||
try:
|
||||
length = len(self.maps)
|
||||
buff.write(_struct_I.pack(length))
|
||||
for val1 in self.maps:
|
||||
_v1 = val1.header
|
||||
buff.write(_struct_I.pack(_v1.seq))
|
||||
_v2 = _v1.stamp
|
||||
_x = _v2
|
||||
buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
|
||||
_x = _v1.frame_id
|
||||
length = len(_x)
|
||||
if python3 or type(_x) == unicode:
|
||||
_x = _x.encode('utf-8')
|
||||
length = len(_x)
|
||||
if python3:
|
||||
buff.write(struct.pack('<I%sB'%length, length, *_x))
|
||||
else:
|
||||
buff.write(struct.pack('<I%ss'%length, length, _x))
|
||||
_v3 = val1.info
|
||||
_v4 = _v3.map_load_time
|
||||
_x = _v4
|
||||
buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
|
||||
_x = _v3
|
||||
buff.write(_struct_f2I.pack(_x.resolution, _x.width, _x.height))
|
||||
_v5 = _v3.origin
|
||||
_v6 = _v5.position
|
||||
_x = _v6
|
||||
buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
|
||||
_v7 = _v5.orientation
|
||||
_x = _v7
|
||||
buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
|
||||
length = len(val1.data)
|
||||
buff.write(_struct_I.pack(length))
|
||||
pattern = '<%sb'%length
|
||||
buff.write(struct.pack(pattern, *val1.data))
|
||||
length = len(self.origins)
|
||||
buff.write(_struct_I.pack(length))
|
||||
for val1 in self.origins:
|
||||
_v8 = val1.position
|
||||
_x = _v8
|
||||
buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
|
||||
_v9 = val1.orientation
|
||||
_x = _v9
|
||||
buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
|
||||
except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
|
||||
except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
|
||||
|
||||
def deserialize(self, str):
|
||||
"""
|
||||
unpack serialized message in str into this message instance
|
||||
:param str: byte array of serialized message, ``str``
|
||||
"""
|
||||
try:
|
||||
if self.maps is None:
|
||||
self.maps = None
|
||||
if self.origins is None:
|
||||
self.origins = None
|
||||
end = 0
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
self.maps = []
|
||||
for i in range(0, length):
|
||||
val1 = nav_msgs.msg.OccupancyGrid()
|
||||
_v10 = val1.header
|
||||
start = end
|
||||
end += 4
|
||||
(_v10.seq,) = _struct_I.unpack(str[start:end])
|
||||
_v11 = _v10.stamp
|
||||
_x = _v11
|
||||
start = end
|
||||
end += 8
|
||||
(_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
start = end
|
||||
end += length
|
||||
if python3:
|
||||
_v10.frame_id = str[start:end].decode('utf-8')
|
||||
else:
|
||||
_v10.frame_id = str[start:end]
|
||||
_v12 = val1.info
|
||||
_v13 = _v12.map_load_time
|
||||
_x = _v13
|
||||
start = end
|
||||
end += 8
|
||||
(_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
|
||||
_x = _v12
|
||||
start = end
|
||||
end += 12
|
||||
(_x.resolution, _x.width, _x.height,) = _struct_f2I.unpack(str[start:end])
|
||||
_v14 = _v12.origin
|
||||
_v15 = _v14.position
|
||||
_x = _v15
|
||||
start = end
|
||||
end += 24
|
||||
(_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
|
||||
_v16 = _v14.orientation
|
||||
_x = _v16
|
||||
start = end
|
||||
end += 32
|
||||
(_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
pattern = '<%sb'%length
|
||||
start = end
|
||||
end += struct.calcsize(pattern)
|
||||
val1.data = struct.unpack(pattern, str[start:end])
|
||||
self.maps.append(val1)
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
self.origins = []
|
||||
for i in range(0, length):
|
||||
val1 = geometry_msgs.msg.Pose()
|
||||
_v17 = val1.position
|
||||
_x = _v17
|
||||
start = end
|
||||
end += 24
|
||||
(_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
|
||||
_v18 = val1.orientation
|
||||
_x = _v18
|
||||
start = end
|
||||
end += 32
|
||||
(_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
|
||||
self.origins.append(val1)
|
||||
return self
|
||||
except struct.error as e:
|
||||
raise genpy.DeserializationError(e) #most likely buffer underfill
|
||||
|
||||
|
||||
def serialize_numpy(self, buff, numpy):
|
||||
"""
|
||||
serialize message with numpy array types into buffer
|
||||
:param buff: buffer, ``StringIO``
|
||||
:param numpy: numpy python module
|
||||
"""
|
||||
try:
|
||||
length = len(self.maps)
|
||||
buff.write(_struct_I.pack(length))
|
||||
for val1 in self.maps:
|
||||
_v19 = val1.header
|
||||
buff.write(_struct_I.pack(_v19.seq))
|
||||
_v20 = _v19.stamp
|
||||
_x = _v20
|
||||
buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
|
||||
_x = _v19.frame_id
|
||||
length = len(_x)
|
||||
if python3 or type(_x) == unicode:
|
||||
_x = _x.encode('utf-8')
|
||||
length = len(_x)
|
||||
if python3:
|
||||
buff.write(struct.pack('<I%sB'%length, length, *_x))
|
||||
else:
|
||||
buff.write(struct.pack('<I%ss'%length, length, _x))
|
||||
_v21 = val1.info
|
||||
_v22 = _v21.map_load_time
|
||||
_x = _v22
|
||||
buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
|
||||
_x = _v21
|
||||
buff.write(_struct_f2I.pack(_x.resolution, _x.width, _x.height))
|
||||
_v23 = _v21.origin
|
||||
_v24 = _v23.position
|
||||
_x = _v24
|
||||
buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
|
||||
_v25 = _v23.orientation
|
||||
_x = _v25
|
||||
buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
|
||||
length = len(val1.data)
|
||||
buff.write(_struct_I.pack(length))
|
||||
pattern = '<%sb'%length
|
||||
buff.write(val1.data.tostring())
|
||||
length = len(self.origins)
|
||||
buff.write(_struct_I.pack(length))
|
||||
for val1 in self.origins:
|
||||
_v26 = val1.position
|
||||
_x = _v26
|
||||
buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
|
||||
_v27 = val1.orientation
|
||||
_x = _v27
|
||||
buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
|
||||
except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
|
||||
except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
|
||||
|
||||
def deserialize_numpy(self, str, numpy):
|
||||
"""
|
||||
unpack serialized message in str into this message instance using numpy for array types
|
||||
:param str: byte array of serialized message, ``str``
|
||||
:param numpy: numpy python module
|
||||
"""
|
||||
try:
|
||||
if self.maps is None:
|
||||
self.maps = None
|
||||
if self.origins is None:
|
||||
self.origins = None
|
||||
end = 0
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
self.maps = []
|
||||
for i in range(0, length):
|
||||
val1 = nav_msgs.msg.OccupancyGrid()
|
||||
_v28 = val1.header
|
||||
start = end
|
||||
end += 4
|
||||
(_v28.seq,) = _struct_I.unpack(str[start:end])
|
||||
_v29 = _v28.stamp
|
||||
_x = _v29
|
||||
start = end
|
||||
end += 8
|
||||
(_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
start = end
|
||||
end += length
|
||||
if python3:
|
||||
_v28.frame_id = str[start:end].decode('utf-8')
|
||||
else:
|
||||
_v28.frame_id = str[start:end]
|
||||
_v30 = val1.info
|
||||
_v31 = _v30.map_load_time
|
||||
_x = _v31
|
||||
start = end
|
||||
end += 8
|
||||
(_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
|
||||
_x = _v30
|
||||
start = end
|
||||
end += 12
|
||||
(_x.resolution, _x.width, _x.height,) = _struct_f2I.unpack(str[start:end])
|
||||
_v32 = _v30.origin
|
||||
_v33 = _v32.position
|
||||
_x = _v33
|
||||
start = end
|
||||
end += 24
|
||||
(_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
|
||||
_v34 = _v32.orientation
|
||||
_x = _v34
|
||||
start = end
|
||||
end += 32
|
||||
(_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
pattern = '<%sb'%length
|
||||
start = end
|
||||
end += struct.calcsize(pattern)
|
||||
val1.data = numpy.frombuffer(str[start:end], dtype=numpy.int8, count=length)
|
||||
self.maps.append(val1)
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
self.origins = []
|
||||
for i in range(0, length):
|
||||
val1 = geometry_msgs.msg.Pose()
|
||||
_v35 = val1.position
|
||||
_x = _v35
|
||||
start = end
|
||||
end += 24
|
||||
(_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
|
||||
_v36 = val1.orientation
|
||||
_x = _v36
|
||||
start = end
|
||||
end += 32
|
||||
(_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
|
||||
self.origins.append(val1)
|
||||
return self
|
||||
except struct.error as e:
|
||||
raise genpy.DeserializationError(e) #most likely buffer underfill
|
||||
|
||||
_struct_I = genpy.struct_I
|
||||
_struct_4d = struct.Struct("<4d")
|
||||
_struct_f2I = struct.Struct("<f2I")
|
||||
_struct_2I = struct.Struct("<2I")
|
||||
_struct_3d = struct.Struct("<3d")
|
||||
@ -1,484 +0,0 @@
|
||||
"""autogenerated by genpy from multi_map_server/MultiSparseMap3D.msg. Do not edit."""
|
||||
import sys
|
||||
python3 = True if sys.hexversion > 0x03000000 else False
|
||||
import genpy
|
||||
import struct
|
||||
|
||||
import geometry_msgs.msg
|
||||
import multi_map_server.msg
|
||||
import nav_msgs.msg
|
||||
import genpy
|
||||
import std_msgs.msg
|
||||
|
||||
class MultiSparseMap3D(genpy.Message):
|
||||
_md5sum = "2e3d76c98ee3e2b23a422f64965f6418"
|
||||
_type = "multi_map_server/MultiSparseMap3D"
|
||||
_has_header = False #flag to mark the presence of a Header object
|
||||
_full_text = """SparseMap3D[] maps
|
||||
geometry_msgs/Pose[] origins
|
||||
|
||||
================================================================================
|
||||
MSG: multi_map_server/SparseMap3D
|
||||
Header header
|
||||
nav_msgs/MapMetaData info
|
||||
VerticalOccupancyGridList[] lists
|
||||
|
||||
|
||||
================================================================================
|
||||
MSG: std_msgs/Header
|
||||
# Standard metadata for higher-level stamped data types.
|
||||
# This is generally used to communicate timestamped data
|
||||
# in a particular coordinate frame.
|
||||
#
|
||||
# sequence ID: consecutively increasing ID
|
||||
uint32 seq
|
||||
#Two-integer timestamp that is expressed as:
|
||||
# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')
|
||||
# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')
|
||||
# time-handling sugar is provided by the client library
|
||||
time stamp
|
||||
#Frame this data is associated with
|
||||
# 0: no frame
|
||||
# 1: global frame
|
||||
string frame_id
|
||||
|
||||
================================================================================
|
||||
MSG: nav_msgs/MapMetaData
|
||||
# This hold basic information about the characterists of the OccupancyGrid
|
||||
|
||||
# The time at which the map was loaded
|
||||
time map_load_time
|
||||
# The map resolution [m/cell]
|
||||
float32 resolution
|
||||
# Map width [cells]
|
||||
uint32 width
|
||||
# Map height [cells]
|
||||
uint32 height
|
||||
# The origin of the map [m, m, rad]. This is the real-world pose of the
|
||||
# cell (0,0) in the map.
|
||||
geometry_msgs/Pose origin
|
||||
================================================================================
|
||||
MSG: geometry_msgs/Pose
|
||||
# A representation of pose in free space, composed of postion and orientation.
|
||||
Point position
|
||||
Quaternion orientation
|
||||
|
||||
================================================================================
|
||||
MSG: geometry_msgs/Point
|
||||
# This contains the position of a point in free space
|
||||
float64 x
|
||||
float64 y
|
||||
float64 z
|
||||
|
||||
================================================================================
|
||||
MSG: geometry_msgs/Quaternion
|
||||
# This represents an orientation in free space in quaternion form.
|
||||
|
||||
float64 x
|
||||
float64 y
|
||||
float64 z
|
||||
float64 w
|
||||
|
||||
================================================================================
|
||||
MSG: multi_map_server/VerticalOccupancyGridList
|
||||
float32 x
|
||||
float32 y
|
||||
int32[] upper
|
||||
int32[] lower
|
||||
int32[] mass
|
||||
|
||||
|
||||
"""
|
||||
__slots__ = ['maps','origins']
|
||||
_slot_types = ['multi_map_server/SparseMap3D[]','geometry_msgs/Pose[]']
|
||||
|
||||
def __init__(self, *args, **kwds):
|
||||
"""
|
||||
Constructor. Any message fields that are implicitly/explicitly
|
||||
set to None will be assigned a default value. The recommend
|
||||
use is keyword arguments as this is more robust to future message
|
||||
changes. You cannot mix in-order arguments and keyword arguments.
|
||||
|
||||
The available fields are:
|
||||
maps,origins
|
||||
|
||||
:param args: complete set of field values, in .msg order
|
||||
:param kwds: use keyword arguments corresponding to message field names
|
||||
to set specific fields.
|
||||
"""
|
||||
if args or kwds:
|
||||
super(MultiSparseMap3D, self).__init__(*args, **kwds)
|
||||
#message fields cannot be None, assign default values for those that are
|
||||
if self.maps is None:
|
||||
self.maps = []
|
||||
if self.origins is None:
|
||||
self.origins = []
|
||||
else:
|
||||
self.maps = []
|
||||
self.origins = []
|
||||
|
||||
def _get_types(self):
|
||||
"""
|
||||
internal API method
|
||||
"""
|
||||
return self._slot_types
|
||||
|
||||
def serialize(self, buff):
|
||||
"""
|
||||
serialize message into buffer
|
||||
:param buff: buffer, ``StringIO``
|
||||
"""
|
||||
try:
|
||||
length = len(self.maps)
|
||||
buff.write(_struct_I.pack(length))
|
||||
for val1 in self.maps:
|
||||
_v1 = val1.header
|
||||
buff.write(_struct_I.pack(_v1.seq))
|
||||
_v2 = _v1.stamp
|
||||
_x = _v2
|
||||
buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
|
||||
_x = _v1.frame_id
|
||||
length = len(_x)
|
||||
if python3 or type(_x) == unicode:
|
||||
_x = _x.encode('utf-8')
|
||||
length = len(_x)
|
||||
if python3:
|
||||
buff.write(struct.pack('<I%sB'%length, length, *_x))
|
||||
else:
|
||||
buff.write(struct.pack('<I%ss'%length, length, _x))
|
||||
_v3 = val1.info
|
||||
_v4 = _v3.map_load_time
|
||||
_x = _v4
|
||||
buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
|
||||
_x = _v3
|
||||
buff.write(_struct_f2I.pack(_x.resolution, _x.width, _x.height))
|
||||
_v5 = _v3.origin
|
||||
_v6 = _v5.position
|
||||
_x = _v6
|
||||
buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
|
||||
_v7 = _v5.orientation
|
||||
_x = _v7
|
||||
buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
|
||||
length = len(val1.lists)
|
||||
buff.write(_struct_I.pack(length))
|
||||
for val2 in val1.lists:
|
||||
_x = val2
|
||||
buff.write(_struct_2f.pack(_x.x, _x.y))
|
||||
length = len(val2.upper)
|
||||
buff.write(_struct_I.pack(length))
|
||||
pattern = '<%si'%length
|
||||
buff.write(struct.pack(pattern, *val2.upper))
|
||||
length = len(val2.lower)
|
||||
buff.write(_struct_I.pack(length))
|
||||
pattern = '<%si'%length
|
||||
buff.write(struct.pack(pattern, *val2.lower))
|
||||
length = len(val2.mass)
|
||||
buff.write(_struct_I.pack(length))
|
||||
pattern = '<%si'%length
|
||||
buff.write(struct.pack(pattern, *val2.mass))
|
||||
length = len(self.origins)
|
||||
buff.write(_struct_I.pack(length))
|
||||
for val1 in self.origins:
|
||||
_v8 = val1.position
|
||||
_x = _v8
|
||||
buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
|
||||
_v9 = val1.orientation
|
||||
_x = _v9
|
||||
buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
|
||||
except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
|
||||
except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
|
||||
|
||||
def deserialize(self, str):
|
||||
"""
|
||||
unpack serialized message in str into this message instance
|
||||
:param str: byte array of serialized message, ``str``
|
||||
"""
|
||||
try:
|
||||
if self.maps is None:
|
||||
self.maps = None
|
||||
if self.origins is None:
|
||||
self.origins = None
|
||||
end = 0
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
self.maps = []
|
||||
for i in range(0, length):
|
||||
val1 = multi_map_server.msg.SparseMap3D()
|
||||
_v10 = val1.header
|
||||
start = end
|
||||
end += 4
|
||||
(_v10.seq,) = _struct_I.unpack(str[start:end])
|
||||
_v11 = _v10.stamp
|
||||
_x = _v11
|
||||
start = end
|
||||
end += 8
|
||||
(_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
start = end
|
||||
end += length
|
||||
if python3:
|
||||
_v10.frame_id = str[start:end].decode('utf-8')
|
||||
else:
|
||||
_v10.frame_id = str[start:end]
|
||||
_v12 = val1.info
|
||||
_v13 = _v12.map_load_time
|
||||
_x = _v13
|
||||
start = end
|
||||
end += 8
|
||||
(_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
|
||||
_x = _v12
|
||||
start = end
|
||||
end += 12
|
||||
(_x.resolution, _x.width, _x.height,) = _struct_f2I.unpack(str[start:end])
|
||||
_v14 = _v12.origin
|
||||
_v15 = _v14.position
|
||||
_x = _v15
|
||||
start = end
|
||||
end += 24
|
||||
(_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
|
||||
_v16 = _v14.orientation
|
||||
_x = _v16
|
||||
start = end
|
||||
end += 32
|
||||
(_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
val1.lists = []
|
||||
for i in range(0, length):
|
||||
val2 = multi_map_server.msg.VerticalOccupancyGridList()
|
||||
_x = val2
|
||||
start = end
|
||||
end += 8
|
||||
(_x.x, _x.y,) = _struct_2f.unpack(str[start:end])
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
pattern = '<%si'%length
|
||||
start = end
|
||||
end += struct.calcsize(pattern)
|
||||
val2.upper = struct.unpack(pattern, str[start:end])
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
pattern = '<%si'%length
|
||||
start = end
|
||||
end += struct.calcsize(pattern)
|
||||
val2.lower = struct.unpack(pattern, str[start:end])
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
pattern = '<%si'%length
|
||||
start = end
|
||||
end += struct.calcsize(pattern)
|
||||
val2.mass = struct.unpack(pattern, str[start:end])
|
||||
val1.lists.append(val2)
|
||||
self.maps.append(val1)
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
self.origins = []
|
||||
for i in range(0, length):
|
||||
val1 = geometry_msgs.msg.Pose()
|
||||
_v17 = val1.position
|
||||
_x = _v17
|
||||
start = end
|
||||
end += 24
|
||||
(_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
|
||||
_v18 = val1.orientation
|
||||
_x = _v18
|
||||
start = end
|
||||
end += 32
|
||||
(_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
|
||||
self.origins.append(val1)
|
||||
return self
|
||||
except struct.error as e:
|
||||
raise genpy.DeserializationError(e) #most likely buffer underfill
|
||||
|
||||
|
||||
def serialize_numpy(self, buff, numpy):
|
||||
"""
|
||||
serialize message with numpy array types into buffer
|
||||
:param buff: buffer, ``StringIO``
|
||||
:param numpy: numpy python module
|
||||
"""
|
||||
try:
|
||||
length = len(self.maps)
|
||||
buff.write(_struct_I.pack(length))
|
||||
for val1 in self.maps:
|
||||
_v19 = val1.header
|
||||
buff.write(_struct_I.pack(_v19.seq))
|
||||
_v20 = _v19.stamp
|
||||
_x = _v20
|
||||
buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
|
||||
_x = _v19.frame_id
|
||||
length = len(_x)
|
||||
if python3 or type(_x) == unicode:
|
||||
_x = _x.encode('utf-8')
|
||||
length = len(_x)
|
||||
if python3:
|
||||
buff.write(struct.pack('<I%sB'%length, length, *_x))
|
||||
else:
|
||||
buff.write(struct.pack('<I%ss'%length, length, _x))
|
||||
_v21 = val1.info
|
||||
_v22 = _v21.map_load_time
|
||||
_x = _v22
|
||||
buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
|
||||
_x = _v21
|
||||
buff.write(_struct_f2I.pack(_x.resolution, _x.width, _x.height))
|
||||
_v23 = _v21.origin
|
||||
_v24 = _v23.position
|
||||
_x = _v24
|
||||
buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
|
||||
_v25 = _v23.orientation
|
||||
_x = _v25
|
||||
buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
|
||||
length = len(val1.lists)
|
||||
buff.write(_struct_I.pack(length))
|
||||
for val2 in val1.lists:
|
||||
_x = val2
|
||||
buff.write(_struct_2f.pack(_x.x, _x.y))
|
||||
length = len(val2.upper)
|
||||
buff.write(_struct_I.pack(length))
|
||||
pattern = '<%si'%length
|
||||
buff.write(val2.upper.tostring())
|
||||
length = len(val2.lower)
|
||||
buff.write(_struct_I.pack(length))
|
||||
pattern = '<%si'%length
|
||||
buff.write(val2.lower.tostring())
|
||||
length = len(val2.mass)
|
||||
buff.write(_struct_I.pack(length))
|
||||
pattern = '<%si'%length
|
||||
buff.write(val2.mass.tostring())
|
||||
length = len(self.origins)
|
||||
buff.write(_struct_I.pack(length))
|
||||
for val1 in self.origins:
|
||||
_v26 = val1.position
|
||||
_x = _v26
|
||||
buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
|
||||
_v27 = val1.orientation
|
||||
_x = _v27
|
||||
buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
|
||||
except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
|
||||
except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
|
||||
|
||||
def deserialize_numpy(self, str, numpy):
|
||||
"""
|
||||
unpack serialized message in str into this message instance using numpy for array types
|
||||
:param str: byte array of serialized message, ``str``
|
||||
:param numpy: numpy python module
|
||||
"""
|
||||
try:
|
||||
if self.maps is None:
|
||||
self.maps = None
|
||||
if self.origins is None:
|
||||
self.origins = None
|
||||
end = 0
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
self.maps = []
|
||||
for i in range(0, length):
|
||||
val1 = multi_map_server.msg.SparseMap3D()
|
||||
_v28 = val1.header
|
||||
start = end
|
||||
end += 4
|
||||
(_v28.seq,) = _struct_I.unpack(str[start:end])
|
||||
_v29 = _v28.stamp
|
||||
_x = _v29
|
||||
start = end
|
||||
end += 8
|
||||
(_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
start = end
|
||||
end += length
|
||||
if python3:
|
||||
_v28.frame_id = str[start:end].decode('utf-8')
|
||||
else:
|
||||
_v28.frame_id = str[start:end]
|
||||
_v30 = val1.info
|
||||
_v31 = _v30.map_load_time
|
||||
_x = _v31
|
||||
start = end
|
||||
end += 8
|
||||
(_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
|
||||
_x = _v30
|
||||
start = end
|
||||
end += 12
|
||||
(_x.resolution, _x.width, _x.height,) = _struct_f2I.unpack(str[start:end])
|
||||
_v32 = _v30.origin
|
||||
_v33 = _v32.position
|
||||
_x = _v33
|
||||
start = end
|
||||
end += 24
|
||||
(_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
|
||||
_v34 = _v32.orientation
|
||||
_x = _v34
|
||||
start = end
|
||||
end += 32
|
||||
(_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
val1.lists = []
|
||||
for i in range(0, length):
|
||||
val2 = multi_map_server.msg.VerticalOccupancyGridList()
|
||||
_x = val2
|
||||
start = end
|
||||
end += 8
|
||||
(_x.x, _x.y,) = _struct_2f.unpack(str[start:end])
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
pattern = '<%si'%length
|
||||
start = end
|
||||
end += struct.calcsize(pattern)
|
||||
val2.upper = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
pattern = '<%si'%length
|
||||
start = end
|
||||
end += struct.calcsize(pattern)
|
||||
val2.lower = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
pattern = '<%si'%length
|
||||
start = end
|
||||
end += struct.calcsize(pattern)
|
||||
val2.mass = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
|
||||
val1.lists.append(val2)
|
||||
self.maps.append(val1)
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
self.origins = []
|
||||
for i in range(0, length):
|
||||
val1 = geometry_msgs.msg.Pose()
|
||||
_v35 = val1.position
|
||||
_x = _v35
|
||||
start = end
|
||||
end += 24
|
||||
(_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
|
||||
_v36 = val1.orientation
|
||||
_x = _v36
|
||||
start = end
|
||||
end += 32
|
||||
(_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
|
||||
self.origins.append(val1)
|
||||
return self
|
||||
except struct.error as e:
|
||||
raise genpy.DeserializationError(e) #most likely buffer underfill
|
||||
|
||||
_struct_I = genpy.struct_I
|
||||
_struct_f2I = struct.Struct("<f2I")
|
||||
_struct_2f = struct.Struct("<2f")
|
||||
_struct_4d = struct.Struct("<4d")
|
||||
_struct_2I = struct.Struct("<2I")
|
||||
_struct_3d = struct.Struct("<3d")
|
||||
@ -1,340 +0,0 @@
|
||||
"""autogenerated by genpy from multi_map_server/SparseMap3D.msg. Do not edit."""
|
||||
import sys
|
||||
python3 = True if sys.hexversion > 0x03000000 else False
|
||||
import genpy
|
||||
import struct
|
||||
|
||||
import geometry_msgs.msg
|
||||
import multi_map_server.msg
|
||||
import nav_msgs.msg
|
||||
import genpy
|
||||
import std_msgs.msg
|
||||
|
||||
class SparseMap3D(genpy.Message):
|
||||
_md5sum = "a20102f0b3a02e95070dab4140b78fb5"
|
||||
_type = "multi_map_server/SparseMap3D"
|
||||
_has_header = True #flag to mark the presence of a Header object
|
||||
_full_text = """Header header
|
||||
nav_msgs/MapMetaData info
|
||||
VerticalOccupancyGridList[] lists
|
||||
|
||||
|
||||
================================================================================
|
||||
MSG: std_msgs/Header
|
||||
# Standard metadata for higher-level stamped data types.
|
||||
# This is generally used to communicate timestamped data
|
||||
# in a particular coordinate frame.
|
||||
#
|
||||
# sequence ID: consecutively increasing ID
|
||||
uint32 seq
|
||||
#Two-integer timestamp that is expressed as:
|
||||
# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')
|
||||
# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')
|
||||
# time-handling sugar is provided by the client library
|
||||
time stamp
|
||||
#Frame this data is associated with
|
||||
# 0: no frame
|
||||
# 1: global frame
|
||||
string frame_id
|
||||
|
||||
================================================================================
|
||||
MSG: nav_msgs/MapMetaData
|
||||
# This hold basic information about the characterists of the OccupancyGrid
|
||||
|
||||
# The time at which the map was loaded
|
||||
time map_load_time
|
||||
# The map resolution [m/cell]
|
||||
float32 resolution
|
||||
# Map width [cells]
|
||||
uint32 width
|
||||
# Map height [cells]
|
||||
uint32 height
|
||||
# The origin of the map [m, m, rad]. This is the real-world pose of the
|
||||
# cell (0,0) in the map.
|
||||
geometry_msgs/Pose origin
|
||||
================================================================================
|
||||
MSG: geometry_msgs/Pose
|
||||
# A representation of pose in free space, composed of postion and orientation.
|
||||
Point position
|
||||
Quaternion orientation
|
||||
|
||||
================================================================================
|
||||
MSG: geometry_msgs/Point
|
||||
# This contains the position of a point in free space
|
||||
float64 x
|
||||
float64 y
|
||||
float64 z
|
||||
|
||||
================================================================================
|
||||
MSG: geometry_msgs/Quaternion
|
||||
# This represents an orientation in free space in quaternion form.
|
||||
|
||||
float64 x
|
||||
float64 y
|
||||
float64 z
|
||||
float64 w
|
||||
|
||||
================================================================================
|
||||
MSG: multi_map_server/VerticalOccupancyGridList
|
||||
float32 x
|
||||
float32 y
|
||||
int32[] upper
|
||||
int32[] lower
|
||||
int32[] mass
|
||||
|
||||
|
||||
"""
|
||||
__slots__ = ['header','info','lists']
|
||||
_slot_types = ['std_msgs/Header','nav_msgs/MapMetaData','multi_map_server/VerticalOccupancyGridList[]']
|
||||
|
||||
def __init__(self, *args, **kwds):
|
||||
"""
|
||||
Constructor. Any message fields that are implicitly/explicitly
|
||||
set to None will be assigned a default value. The recommend
|
||||
use is keyword arguments as this is more robust to future message
|
||||
changes. You cannot mix in-order arguments and keyword arguments.
|
||||
|
||||
The available fields are:
|
||||
header,info,lists
|
||||
|
||||
:param args: complete set of field values, in .msg order
|
||||
:param kwds: use keyword arguments corresponding to message field names
|
||||
to set specific fields.
|
||||
"""
|
||||
if args or kwds:
|
||||
super(SparseMap3D, self).__init__(*args, **kwds)
|
||||
#message fields cannot be None, assign default values for those that are
|
||||
if self.header is None:
|
||||
self.header = std_msgs.msg.Header()
|
||||
if self.info is None:
|
||||
self.info = nav_msgs.msg.MapMetaData()
|
||||
if self.lists is None:
|
||||
self.lists = []
|
||||
else:
|
||||
self.header = std_msgs.msg.Header()
|
||||
self.info = nav_msgs.msg.MapMetaData()
|
||||
self.lists = []
|
||||
|
||||
def _get_types(self):
|
||||
"""
|
||||
internal API method
|
||||
"""
|
||||
return self._slot_types
|
||||
|
||||
def serialize(self, buff):
|
||||
"""
|
||||
serialize message into buffer
|
||||
:param buff: buffer, ``StringIO``
|
||||
"""
|
||||
try:
|
||||
_x = self
|
||||
buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs))
|
||||
_x = self.header.frame_id
|
||||
length = len(_x)
|
||||
if python3 or type(_x) == unicode:
|
||||
_x = _x.encode('utf-8')
|
||||
length = len(_x)
|
||||
if python3:
|
||||
buff.write(struct.pack('<I%sB'%length, length, *_x))
|
||||
else:
|
||||
buff.write(struct.pack('<I%ss'%length, length, _x))
|
||||
_x = self
|
||||
buff.write(_struct_2If2I7d.pack(_x.info.map_load_time.secs, _x.info.map_load_time.nsecs, _x.info.resolution, _x.info.width, _x.info.height, _x.info.origin.position.x, _x.info.origin.position.y, _x.info.origin.position.z, _x.info.origin.orientation.x, _x.info.origin.orientation.y, _x.info.origin.orientation.z, _x.info.origin.orientation.w))
|
||||
length = len(self.lists)
|
||||
buff.write(_struct_I.pack(length))
|
||||
for val1 in self.lists:
|
||||
_x = val1
|
||||
buff.write(_struct_2f.pack(_x.x, _x.y))
|
||||
length = len(val1.upper)
|
||||
buff.write(_struct_I.pack(length))
|
||||
pattern = '<%si'%length
|
||||
buff.write(struct.pack(pattern, *val1.upper))
|
||||
length = len(val1.lower)
|
||||
buff.write(_struct_I.pack(length))
|
||||
pattern = '<%si'%length
|
||||
buff.write(struct.pack(pattern, *val1.lower))
|
||||
length = len(val1.mass)
|
||||
buff.write(_struct_I.pack(length))
|
||||
pattern = '<%si'%length
|
||||
buff.write(struct.pack(pattern, *val1.mass))
|
||||
except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
|
||||
except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
|
||||
|
||||
def deserialize(self, str):
|
||||
"""
|
||||
unpack serialized message in str into this message instance
|
||||
:param str: byte array of serialized message, ``str``
|
||||
"""
|
||||
try:
|
||||
if self.header is None:
|
||||
self.header = std_msgs.msg.Header()
|
||||
if self.info is None:
|
||||
self.info = nav_msgs.msg.MapMetaData()
|
||||
if self.lists is None:
|
||||
self.lists = None
|
||||
end = 0
|
||||
_x = self
|
||||
start = end
|
||||
end += 12
|
||||
(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
start = end
|
||||
end += length
|
||||
if python3:
|
||||
self.header.frame_id = str[start:end].decode('utf-8')
|
||||
else:
|
||||
self.header.frame_id = str[start:end]
|
||||
_x = self
|
||||
start = end
|
||||
end += 76
|
||||
(_x.info.map_load_time.secs, _x.info.map_load_time.nsecs, _x.info.resolution, _x.info.width, _x.info.height, _x.info.origin.position.x, _x.info.origin.position.y, _x.info.origin.position.z, _x.info.origin.orientation.x, _x.info.origin.orientation.y, _x.info.origin.orientation.z, _x.info.origin.orientation.w,) = _struct_2If2I7d.unpack(str[start:end])
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
self.lists = []
|
||||
for i in range(0, length):
|
||||
val1 = multi_map_server.msg.VerticalOccupancyGridList()
|
||||
_x = val1
|
||||
start = end
|
||||
end += 8
|
||||
(_x.x, _x.y,) = _struct_2f.unpack(str[start:end])
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
pattern = '<%si'%length
|
||||
start = end
|
||||
end += struct.calcsize(pattern)
|
||||
val1.upper = struct.unpack(pattern, str[start:end])
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
pattern = '<%si'%length
|
||||
start = end
|
||||
end += struct.calcsize(pattern)
|
||||
val1.lower = struct.unpack(pattern, str[start:end])
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
pattern = '<%si'%length
|
||||
start = end
|
||||
end += struct.calcsize(pattern)
|
||||
val1.mass = struct.unpack(pattern, str[start:end])
|
||||
self.lists.append(val1)
|
||||
return self
|
||||
except struct.error as e:
|
||||
raise genpy.DeserializationError(e) #most likely buffer underfill
|
||||
|
||||
|
||||
def serialize_numpy(self, buff, numpy):
|
||||
"""
|
||||
serialize message with numpy array types into buffer
|
||||
:param buff: buffer, ``StringIO``
|
||||
:param numpy: numpy python module
|
||||
"""
|
||||
try:
|
||||
_x = self
|
||||
buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs))
|
||||
_x = self.header.frame_id
|
||||
length = len(_x)
|
||||
if python3 or type(_x) == unicode:
|
||||
_x = _x.encode('utf-8')
|
||||
length = len(_x)
|
||||
if python3:
|
||||
buff.write(struct.pack('<I%sB'%length, length, *_x))
|
||||
else:
|
||||
buff.write(struct.pack('<I%ss'%length, length, _x))
|
||||
_x = self
|
||||
buff.write(_struct_2If2I7d.pack(_x.info.map_load_time.secs, _x.info.map_load_time.nsecs, _x.info.resolution, _x.info.width, _x.info.height, _x.info.origin.position.x, _x.info.origin.position.y, _x.info.origin.position.z, _x.info.origin.orientation.x, _x.info.origin.orientation.y, _x.info.origin.orientation.z, _x.info.origin.orientation.w))
|
||||
length = len(self.lists)
|
||||
buff.write(_struct_I.pack(length))
|
||||
for val1 in self.lists:
|
||||
_x = val1
|
||||
buff.write(_struct_2f.pack(_x.x, _x.y))
|
||||
length = len(val1.upper)
|
||||
buff.write(_struct_I.pack(length))
|
||||
pattern = '<%si'%length
|
||||
buff.write(val1.upper.tostring())
|
||||
length = len(val1.lower)
|
||||
buff.write(_struct_I.pack(length))
|
||||
pattern = '<%si'%length
|
||||
buff.write(val1.lower.tostring())
|
||||
length = len(val1.mass)
|
||||
buff.write(_struct_I.pack(length))
|
||||
pattern = '<%si'%length
|
||||
buff.write(val1.mass.tostring())
|
||||
except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
|
||||
except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
|
||||
|
||||
def deserialize_numpy(self, str, numpy):
|
||||
"""
|
||||
unpack serialized message in str into this message instance using numpy for array types
|
||||
:param str: byte array of serialized message, ``str``
|
||||
:param numpy: numpy python module
|
||||
"""
|
||||
try:
|
||||
if self.header is None:
|
||||
self.header = std_msgs.msg.Header()
|
||||
if self.info is None:
|
||||
self.info = nav_msgs.msg.MapMetaData()
|
||||
if self.lists is None:
|
||||
self.lists = None
|
||||
end = 0
|
||||
_x = self
|
||||
start = end
|
||||
end += 12
|
||||
(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
start = end
|
||||
end += length
|
||||
if python3:
|
||||
self.header.frame_id = str[start:end].decode('utf-8')
|
||||
else:
|
||||
self.header.frame_id = str[start:end]
|
||||
_x = self
|
||||
start = end
|
||||
end += 76
|
||||
(_x.info.map_load_time.secs, _x.info.map_load_time.nsecs, _x.info.resolution, _x.info.width, _x.info.height, _x.info.origin.position.x, _x.info.origin.position.y, _x.info.origin.position.z, _x.info.origin.orientation.x, _x.info.origin.orientation.y, _x.info.origin.orientation.z, _x.info.origin.orientation.w,) = _struct_2If2I7d.unpack(str[start:end])
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
self.lists = []
|
||||
for i in range(0, length):
|
||||
val1 = multi_map_server.msg.VerticalOccupancyGridList()
|
||||
_x = val1
|
||||
start = end
|
||||
end += 8
|
||||
(_x.x, _x.y,) = _struct_2f.unpack(str[start:end])
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
pattern = '<%si'%length
|
||||
start = end
|
||||
end += struct.calcsize(pattern)
|
||||
val1.upper = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
pattern = '<%si'%length
|
||||
start = end
|
||||
end += struct.calcsize(pattern)
|
||||
val1.lower = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
pattern = '<%si'%length
|
||||
start = end
|
||||
end += struct.calcsize(pattern)
|
||||
val1.mass = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
|
||||
self.lists.append(val1)
|
||||
return self
|
||||
except struct.error as e:
|
||||
raise genpy.DeserializationError(e) #most likely buffer underfill
|
||||
|
||||
_struct_I = genpy.struct_I
|
||||
_struct_3I = struct.Struct("<3I")
|
||||
_struct_2f = struct.Struct("<2f")
|
||||
_struct_2If2I7d = struct.Struct("<2If2I7d")
|
||||
@ -1,185 +0,0 @@
|
||||
"""autogenerated by genpy from multi_map_server/VerticalOccupancyGridList.msg. Do not edit."""
|
||||
import sys
|
||||
python3 = True if sys.hexversion > 0x03000000 else False
|
||||
import genpy
|
||||
import struct
|
||||
|
||||
|
||||
class VerticalOccupancyGridList(genpy.Message):
|
||||
_md5sum = "7ef85cc95b82747f51eb01a16bd7c795"
|
||||
_type = "multi_map_server/VerticalOccupancyGridList"
|
||||
_has_header = False #flag to mark the presence of a Header object
|
||||
_full_text = """float32 x
|
||||
float32 y
|
||||
int32[] upper
|
||||
int32[] lower
|
||||
int32[] mass
|
||||
|
||||
|
||||
"""
|
||||
__slots__ = ['x','y','upper','lower','mass']
|
||||
_slot_types = ['float32','float32','int32[]','int32[]','int32[]']
|
||||
|
||||
def __init__(self, *args, **kwds):
|
||||
"""
|
||||
Constructor. Any message fields that are implicitly/explicitly
|
||||
set to None will be assigned a default value. The recommend
|
||||
use is keyword arguments as this is more robust to future message
|
||||
changes. You cannot mix in-order arguments and keyword arguments.
|
||||
|
||||
The available fields are:
|
||||
x,y,upper,lower,mass
|
||||
|
||||
:param args: complete set of field values, in .msg order
|
||||
:param kwds: use keyword arguments corresponding to message field names
|
||||
to set specific fields.
|
||||
"""
|
||||
if args or kwds:
|
||||
super(VerticalOccupancyGridList, self).__init__(*args, **kwds)
|
||||
#message fields cannot be None, assign default values for those that are
|
||||
if self.x is None:
|
||||
self.x = 0.
|
||||
if self.y is None:
|
||||
self.y = 0.
|
||||
if self.upper is None:
|
||||
self.upper = []
|
||||
if self.lower is None:
|
||||
self.lower = []
|
||||
if self.mass is None:
|
||||
self.mass = []
|
||||
else:
|
||||
self.x = 0.
|
||||
self.y = 0.
|
||||
self.upper = []
|
||||
self.lower = []
|
||||
self.mass = []
|
||||
|
||||
def _get_types(self):
|
||||
"""
|
||||
internal API method
|
||||
"""
|
||||
return self._slot_types
|
||||
|
||||
def serialize(self, buff):
|
||||
"""
|
||||
serialize message into buffer
|
||||
:param buff: buffer, ``StringIO``
|
||||
"""
|
||||
try:
|
||||
_x = self
|
||||
buff.write(_struct_2f.pack(_x.x, _x.y))
|
||||
length = len(self.upper)
|
||||
buff.write(_struct_I.pack(length))
|
||||
pattern = '<%si'%length
|
||||
buff.write(struct.pack(pattern, *self.upper))
|
||||
length = len(self.lower)
|
||||
buff.write(_struct_I.pack(length))
|
||||
pattern = '<%si'%length
|
||||
buff.write(struct.pack(pattern, *self.lower))
|
||||
length = len(self.mass)
|
||||
buff.write(_struct_I.pack(length))
|
||||
pattern = '<%si'%length
|
||||
buff.write(struct.pack(pattern, *self.mass))
|
||||
except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
|
||||
except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
|
||||
|
||||
def deserialize(self, str):
|
||||
"""
|
||||
unpack serialized message in str into this message instance
|
||||
:param str: byte array of serialized message, ``str``
|
||||
"""
|
||||
try:
|
||||
end = 0
|
||||
_x = self
|
||||
start = end
|
||||
end += 8
|
||||
(_x.x, _x.y,) = _struct_2f.unpack(str[start:end])
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
pattern = '<%si'%length
|
||||
start = end
|
||||
end += struct.calcsize(pattern)
|
||||
self.upper = struct.unpack(pattern, str[start:end])
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
pattern = '<%si'%length
|
||||
start = end
|
||||
end += struct.calcsize(pattern)
|
||||
self.lower = struct.unpack(pattern, str[start:end])
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
pattern = '<%si'%length
|
||||
start = end
|
||||
end += struct.calcsize(pattern)
|
||||
self.mass = struct.unpack(pattern, str[start:end])
|
||||
return self
|
||||
except struct.error as e:
|
||||
raise genpy.DeserializationError(e) #most likely buffer underfill
|
||||
|
||||
|
||||
def serialize_numpy(self, buff, numpy):
|
||||
"""
|
||||
serialize message with numpy array types into buffer
|
||||
:param buff: buffer, ``StringIO``
|
||||
:param numpy: numpy python module
|
||||
"""
|
||||
try:
|
||||
_x = self
|
||||
buff.write(_struct_2f.pack(_x.x, _x.y))
|
||||
length = len(self.upper)
|
||||
buff.write(_struct_I.pack(length))
|
||||
pattern = '<%si'%length
|
||||
buff.write(self.upper.tostring())
|
||||
length = len(self.lower)
|
||||
buff.write(_struct_I.pack(length))
|
||||
pattern = '<%si'%length
|
||||
buff.write(self.lower.tostring())
|
||||
length = len(self.mass)
|
||||
buff.write(_struct_I.pack(length))
|
||||
pattern = '<%si'%length
|
||||
buff.write(self.mass.tostring())
|
||||
except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
|
||||
except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
|
||||
|
||||
def deserialize_numpy(self, str, numpy):
|
||||
"""
|
||||
unpack serialized message in str into this message instance using numpy for array types
|
||||
:param str: byte array of serialized message, ``str``
|
||||
:param numpy: numpy python module
|
||||
"""
|
||||
try:
|
||||
end = 0
|
||||
_x = self
|
||||
start = end
|
||||
end += 8
|
||||
(_x.x, _x.y,) = _struct_2f.unpack(str[start:end])
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
pattern = '<%si'%length
|
||||
start = end
|
||||
end += struct.calcsize(pattern)
|
||||
self.upper = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
pattern = '<%si'%length
|
||||
start = end
|
||||
end += struct.calcsize(pattern)
|
||||
self.lower = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
pattern = '<%si'%length
|
||||
start = end
|
||||
end += struct.calcsize(pattern)
|
||||
self.mass = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
|
||||
return self
|
||||
except struct.error as e:
|
||||
raise genpy.DeserializationError(e) #most likely buffer underfill
|
||||
|
||||
_struct_I = genpy.struct_I
|
||||
_struct_2f = struct.Struct("<2f")
|
||||
@ -1,4 +0,0 @@
|
||||
from ._SparseMap3D import *
|
||||
from ._MultiOccupancyGrid import *
|
||||
from ._MultiSparseMap3D import *
|
||||
from ._VerticalOccupancyGridList import *
|
||||
@ -1,90 +0,0 @@
|
||||
#include <iostream>
|
||||
#include <ros/ros.h>
|
||||
#include <sensor_msgs/PointCloud.h>
|
||||
#include <pose_utils.h>
|
||||
#include <multi_map_server/MultiOccupancyGrid.h>
|
||||
#include <multi_map_server/MultiSparseMap3D.h>
|
||||
#include <multi_map_server/Map2D.h>
|
||||
#include <multi_map_server/Map3D.h>
|
||||
|
||||
ros::Publisher pub1;
|
||||
ros::Publisher pub2;
|
||||
|
||||
vector<Map2D> maps2d;
|
||||
vector<geometry_msgs::Pose> origins2d;
|
||||
vector<Map3D> maps3d;
|
||||
vector<geometry_msgs::Pose> origins3d;
|
||||
|
||||
void maps2d_callback(const multi_map_server::MultiOccupancyGrid::ConstPtr &msg)
|
||||
{
|
||||
// Merge map
|
||||
maps2d.resize(msg->maps.size(), Map2D(4));
|
||||
for (unsigned int k = 0; k < msg->maps.size(); k++)
|
||||
maps2d[k].Replace(msg->maps[k]);
|
||||
origins2d = msg->origins;
|
||||
// Assemble and publish map
|
||||
multi_map_server::MultiOccupancyGrid m;
|
||||
m.maps.resize(maps2d.size());
|
||||
m.origins.resize(maps2d.size());
|
||||
for (unsigned int k = 0; k < maps2d.size(); k++)
|
||||
{
|
||||
m.maps[k] = maps2d[k].GetMap();
|
||||
m.origins[k] = origins2d[k];
|
||||
}
|
||||
pub1.publish(m);
|
||||
}
|
||||
|
||||
void maps3d_callback(const multi_map_server::MultiSparseMap3D::ConstPtr &msg)
|
||||
{
|
||||
// Update incremental map
|
||||
maps3d.resize(msg->maps.size());
|
||||
for (unsigned int k = 0; k < msg->maps.size(); k++)
|
||||
maps3d[k].UnpackMsg(msg->maps[k]);
|
||||
origins3d = msg->origins;
|
||||
// Publish
|
||||
sensor_msgs::PointCloud m;
|
||||
for (unsigned int k = 0; k < msg->maps.size(); k++)
|
||||
{
|
||||
colvec po(6);
|
||||
po(0) = origins3d[k].position.x;
|
||||
po(1) = origins3d[k].position.y;
|
||||
po(2) = origins3d[k].position.z;
|
||||
colvec poq(4);
|
||||
poq(0) = origins3d[k].orientation.w;
|
||||
poq(1) = origins3d[k].orientation.x;
|
||||
poq(2) = origins3d[k].orientation.y;
|
||||
poq(3) = origins3d[k].orientation.z;
|
||||
po.rows(3,5) = R_to_ypr(quaternion_to_R(poq));
|
||||
colvec tpo = po.rows(0,2);
|
||||
mat Rpo = ypr_to_R(po.rows(3,5));
|
||||
vector<colvec> pts = maps3d[k].GetOccupancyWorldFrame(OCCUPIED);
|
||||
for (unsigned int i = 0; i < pts.size(); i++)
|
||||
{
|
||||
colvec pt = Rpo * pts[i] + tpo;
|
||||
geometry_msgs::Point32 _pt;
|
||||
_pt.x = pt(0);
|
||||
_pt.y = pt(1);
|
||||
_pt.z = pt(2);
|
||||
m.points.push_back(_pt);
|
||||
}
|
||||
}
|
||||
// Publish
|
||||
m.header.stamp = ros::Time::now();
|
||||
m.header.frame_id = string("/map");
|
||||
pub2.publish(m);
|
||||
}
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
ros::init(argc, argv, "multi_map_visualization");
|
||||
ros::NodeHandle n("~");
|
||||
|
||||
ros::Subscriber sub1 = n.subscribe("dmaps2d", 1, maps2d_callback);
|
||||
ros::Subscriber sub2 = n.subscribe("dmaps3d", 1, maps3d_callback);
|
||||
pub1 = n.advertise<multi_map_server::MultiOccupancyGrid>("maps2d", 1, true);
|
||||
pub2 = n.advertise<sensor_msgs::PointCloud>("map3d", 1, true);
|
||||
|
||||
ros::spin();
|
||||
return 0;
|
||||
}
|
||||
|
||||
@ -1,192 +0,0 @@
|
||||
#include <iostream>
|
||||
#include <ros/ros.h>
|
||||
#include <sensor_msgs/LaserScan.h>
|
||||
#include <sensor_msgs/PointCloud.h>
|
||||
#include <geometry_msgs/PoseArray.h>
|
||||
#include <laser_geometry/laser_geometry.h>
|
||||
#include <pose_utils.h>
|
||||
#include <multi_map_server/MultiOccupancyGrid.h>
|
||||
#include <multi_map_server/MultiSparseMap3D.h>
|
||||
#include <multi_map_server/Map2D.h>
|
||||
#include <multi_map_server/Map3D.h>
|
||||
|
||||
using namespace arma;
|
||||
using namespace std;
|
||||
#define MAX_MAP_CNT 25
|
||||
|
||||
ros::Publisher pub1;
|
||||
ros::Publisher pub2;
|
||||
|
||||
// 2D Map
|
||||
int maps2dCnt = 0;
|
||||
Map2D maps2d[MAX_MAP_CNT];
|
||||
map_server_3d_new::MultiOccupancyGrid grids2d;
|
||||
// 3D Map
|
||||
int maps3dCnt = 0;
|
||||
Map3D maps3d[MAX_MAP_CNT];
|
||||
// Map origin from UKF
|
||||
vector<geometry_msgs::Pose> maps_origin;
|
||||
|
||||
void map2d_callback(const nav_msgs::OccupancyGrid::ConstPtr &msg)
|
||||
{
|
||||
// Update msg and publish
|
||||
if (grids2d.maps.size() == 0)
|
||||
{
|
||||
// Init msg
|
||||
grids2d.maps.push_back(*msg);
|
||||
maps2dCnt++;
|
||||
}
|
||||
else if (grids2d.maps.back().info.map_load_time != msg->info.map_load_time)
|
||||
{
|
||||
// Add Costmap
|
||||
nav_msgs::OccupancyGrid m;
|
||||
maps2d[maps2dCnt-1].get_map(m);
|
||||
mapMatcher.AddCostMap(m);
|
||||
// Increase msg size
|
||||
grids2d.maps.back().data.clear();
|
||||
grids2d.maps.push_back(*msg);
|
||||
maps2dCnt++;
|
||||
}
|
||||
else
|
||||
{
|
||||
grids2d.maps.back() = *msg;
|
||||
}
|
||||
pub1.publish(grids2d);
|
||||
// Update internval map
|
||||
maps2d[maps2dCnt-1].update(*msg);
|
||||
}
|
||||
|
||||
void scan_callback(const sensor_msgs::LaserScan::ConstPtr &msg)
|
||||
{
|
||||
// Get Map Status
|
||||
static bool isSLAMValid = false;
|
||||
if (msg->intensities[10])
|
||||
{
|
||||
if (!isSLAMValid)
|
||||
{
|
||||
maps3dCnt++;
|
||||
mapLinks.push_back(zeros<colvec>(3));
|
||||
}
|
||||
isSLAMValid = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
isSLAMValid = false;
|
||||
return;
|
||||
}
|
||||
// Scan cnt
|
||||
mapLinks.back()(2)++;
|
||||
// Get Current Pose
|
||||
colvec pose(6);
|
||||
pose(0) = msg->intensities[0];
|
||||
pose(1) = msg->intensities[1];
|
||||
pose(2) = msg->intensities[2];
|
||||
pose(3) = msg->intensities[3];
|
||||
pose(4) = msg->intensities[4];
|
||||
pose(5) = msg->intensities[5];
|
||||
colvec pose2D(3);
|
||||
pose2D(0) = msg->intensities[0];
|
||||
pose2D(1) = msg->intensities[1];
|
||||
pose2D(2) = msg->intensities[3];
|
||||
double currFloor = msg->intensities[7];
|
||||
double currLaserH = msg->intensities[8];
|
||||
// Horizontal laser scans
|
||||
sensor_msgs::LaserScan scan = *msg;
|
||||
scan.intensities.clear();
|
||||
laser_geometry::LaserProjection projector;
|
||||
sensor_msgs::PointCloud scanCloud;
|
||||
projector.projectLaser(scan, scanCloud);
|
||||
mat scanMat(3, scanCloud.points.size());
|
||||
// Get scan in body frame
|
||||
for (int k = 0; k < scanCloud.points.size(); k++)
|
||||
{
|
||||
scanMat(0,k) = scanCloud.points[k].x;
|
||||
scanMat(1,k) = scanCloud.points[k].y;
|
||||
scanMat(2,k) = 0.215 - 0.09;
|
||||
}
|
||||
// Downsample Laser scan to map resolution
|
||||
double resolution = maps3d[maps3dCnt-1].GetResolution();
|
||||
double px = NUM_INF;
|
||||
double py = NUM_INF;
|
||||
int cnt = 0;
|
||||
mat scanMatDown = zeros<mat>(3, scanMat.n_cols);
|
||||
for (int k = 0; k < scanMat.n_cols; k++)
|
||||
{
|
||||
double x = scanMat(0,k);
|
||||
double y = scanMat(1,k);
|
||||
double dist = (x-px)*(x-px) + (y-py)*(y-py);
|
||||
if (dist > resolution * resolution)
|
||||
{
|
||||
scanMatDown.col(cnt) = scanMat.col(k);
|
||||
px = x;
|
||||
py = y;
|
||||
cnt++;
|
||||
}
|
||||
}
|
||||
if (cnt > 0)
|
||||
scanMat = scanMatDown.cols(0, cnt-1);
|
||||
else
|
||||
scanMat = scanMatDown.cols(0,cnt);
|
||||
// Transform to map local frame
|
||||
scanMat = ypr_to_R(pose.rows(3,5)) * scanMat + pose.rows(0,2)*ones<rowvec>(scanMat.n_cols);
|
||||
// Update map
|
||||
for (int k = 0; k < scanMat.n_cols; k++)
|
||||
maps3d[maps3dCnt-1].SetOccupancyFromWorldFrame(scanMat(0,k), scanMat(1,k), scanMat(2,k), PROB_LASER_OCCUPIED);
|
||||
// downward facing laser scans
|
||||
if (currLaserH > 0)
|
||||
{
|
||||
colvec pt(3);
|
||||
pt(0) = 0;
|
||||
pt(1) = 0;
|
||||
pt(2) = -currLaserH;
|
||||
pt = ypr_to_R(pose.rows(3,5)) * pt + pose.rows(0,2);
|
||||
double resolution = maps3d[maps3dCnt-1].GetResolution();
|
||||
for (double x = -0.1; x <= 0.1; x+=resolution)
|
||||
for (double y = -0.1; y <= 0.1; y+=resolution)
|
||||
maps3d[maps3dCnt-1].SetOccupancyFromWorldFrame(pt(0)+x, pt(1)+y, pt(2), PROB_LASER_OCCUPIED);
|
||||
}
|
||||
// Floor Levels
|
||||
maps3d[maps3dCnt-1].SetFloor(currFloor);
|
||||
}
|
||||
|
||||
void maps_origin_callback(const geometry_msgs::PoseArray::ConstPtr &msg)
|
||||
{
|
||||
maps_origin = msg->poses;
|
||||
}
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
ros::init(argc, argv, "multi_map_server_3d");
|
||||
ros::NodeHandle n("~");
|
||||
|
||||
ros::Subscriber sub1 = n.subscribe("dmap2d", 100, map2d_callback);
|
||||
ros::Subscriber sub2 = n.subscribe("scan", 100, scan_callback);
|
||||
ros::Subscriber sub3 = n.subscribe("/maps_origin", 100, maps_origin_callback);
|
||||
pub1 = n.advertise<multi_map_server::MultiOccupancyGrid>("dmaps2d", 10, true);
|
||||
pub2 = n.advertise<multi_map_server::MultiSparseMap3D>( "dmaps3d", 10, true);
|
||||
|
||||
ros::Rate r(100.0);
|
||||
int cnt = 0;
|
||||
while (n.ok())
|
||||
{
|
||||
cnt++;
|
||||
if (cnt > 100)
|
||||
{
|
||||
cnt = 0;
|
||||
map_server_3d_new::MultiSparseMap3D msg;
|
||||
msg.maps_origin = maps_origin;
|
||||
for (int k = 0; k < maps3dCnt; k++)
|
||||
{
|
||||
msg.maps_active.push_back((bool)(!mapLinks[k](0)) && mapLinks[k](2) > 50);
|
||||
map_server_3d_new::SparseMap3D m;
|
||||
maps3d[k].GetSparseMap3DMsg(m);
|
||||
m.header.seq = k;
|
||||
msg.maps.push_back(m);
|
||||
}
|
||||
pub2.publish(msg);
|
||||
}
|
||||
ros::spinOnce();
|
||||
r.sleep();
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
@ -1,211 +0,0 @@
|
||||
#cmake_minimum_required(VERSION 2.4.6)
|
||||
#include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
|
||||
|
||||
# Set the build type. Options are:
|
||||
# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
|
||||
# Debug : w/ debug symbols, w/o optimization
|
||||
# Release : w/o debug symbols, w/ optimization
|
||||
# RelWithDebInfo : w/ debug symbols, w/ optimization
|
||||
# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
|
||||
#set(ROS_BUILD_TYPE RelWithDebInfo)
|
||||
|
||||
#rosbuild_init()
|
||||
|
||||
#set the default path for built executables to the "bin" directory
|
||||
#set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
|
||||
#set the default path for built libraries to the "lib" directory
|
||||
#set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
|
||||
|
||||
#uncomment if you have defined messages
|
||||
#rosbuild_genmsg()
|
||||
#uncomment if you have defined services
|
||||
#rosbuild_gensrv()
|
||||
|
||||
#common commands for building c++ executables and libraries
|
||||
#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
|
||||
#target_link_libraries(${PROJECT_NAME} another_library)
|
||||
#rosbuild_add_boost_directories()
|
||||
#rosbuild_link_boost(${PROJECT_NAME} thread)
|
||||
#rosbuild_add_executable(example examples/example.cpp)
|
||||
#target_link_libraries(example ${PROJECT_NAME})
|
||||
|
||||
#rosbuild_add_executable(odom_visualization src/odom_visualization.cpp)
|
||||
#target_link_libraries(odom_visualization pose_utils)
|
||||
|
||||
#----------------------------------
|
||||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(odom_visualization)
|
||||
|
||||
set(CMAKE_BUILD_TYPE "Release")
|
||||
set(CMAKE_CXX_FLAGS "-O3 -Wall -g")
|
||||
|
||||
## Find catkin macros and libraries
|
||||
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
|
||||
## is used, also find other catkin packages
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
roscpp
|
||||
sensor_msgs
|
||||
nav_msgs
|
||||
visualization_msgs
|
||||
quadrotor_msgs
|
||||
tf
|
||||
pose_utils
|
||||
)
|
||||
|
||||
## System dependencies are found with CMake's conventions
|
||||
# find_package(Boost REQUIRED COMPONENTS system)
|
||||
|
||||
|
||||
## Uncomment this if the package has a setup.py. This macro ensures
|
||||
## modules and global scripts declared therein get installed
|
||||
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
|
||||
# catkin_python_setup()
|
||||
|
||||
################################################
|
||||
## Declare ROS messages, services and actions ##
|
||||
################################################
|
||||
|
||||
## To declare and build messages, services or actions from within this
|
||||
## package, follow these steps:
|
||||
## * Let MSG_DEP_SET be the set of packages whose message types you use in
|
||||
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
|
||||
## * In the file package.xml:
|
||||
## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
|
||||
## * If MSG_DEP_SET isn't empty the following dependencies might have been
|
||||
## pulled in transitively but can be declared for certainty nonetheless:
|
||||
## * add a build_depend tag for "message_generation"
|
||||
## * add a run_depend tag for "message_runtime"
|
||||
## * In this file (CMakeLists.txt):
|
||||
## * add "message_generation" and every package in MSG_DEP_SET to
|
||||
## find_package(catkin REQUIRED COMPONENTS ...)
|
||||
## * add "message_runtime" and every package in MSG_DEP_SET to
|
||||
## catkin_package(CATKIN_DEPENDS ...)
|
||||
## * uncomment the add_*_files sections below as needed
|
||||
## and list every .msg/.srv/.action file to be processed
|
||||
## * uncomment the generate_messages entry below
|
||||
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
|
||||
|
||||
## Generate messages in the 'msg' folder
|
||||
#add_message_files(
|
||||
# FILES
|
||||
# MultiOccupancyGrid.msg
|
||||
# MultiSparseMap3D.msg
|
||||
# SparseMap3D.msg
|
||||
# VerticalOccupancyGridList.msg
|
||||
#plan_cmd.msg
|
||||
#)
|
||||
|
||||
## Generate services in the 'srv' folder
|
||||
# add_service_files(
|
||||
# FILES
|
||||
# Service1.srv
|
||||
# Service2.srv
|
||||
# )
|
||||
|
||||
## Generate actions in the 'action' folder
|
||||
# add_action_files(
|
||||
# FILES
|
||||
# Action1.action
|
||||
# Action2.action
|
||||
# )
|
||||
|
||||
## Generate added messages and services with any dependencies listed here
|
||||
#generate_messages(
|
||||
# DEPENDENCIES
|
||||
# geometry_msgs
|
||||
# nav_msgs
|
||||
#)
|
||||
|
||||
###################################
|
||||
## catkin specific configuration ##
|
||||
###################################
|
||||
## The catkin_package macro generates cmake config files for your package
|
||||
## Declare things to be passed to dependent projects
|
||||
## INCLUDE_DIRS: uncomment this if you package contains header files
|
||||
## LIBRARIES: libraries you create in this project that dependent projects also need
|
||||
## CATKIN_DEPENDS: catkin_packages dependent projects also need
|
||||
## DEPENDS: system dependencies of this project that dependent projects also need
|
||||
catkin_package(
|
||||
# INCLUDE_DIRS include
|
||||
# LIBRARIES irobot_msgs
|
||||
# CATKIN_DEPENDS geometry_msgs nav_msgs
|
||||
# DEPENDS system_lib
|
||||
)
|
||||
|
||||
###########
|
||||
## Build ##
|
||||
###########
|
||||
find_package(Armadillo REQUIRED)
|
||||
include_directories(${ARMADILLO_INCLUDE_DIRS})
|
||||
|
||||
|
||||
## Specify additional locations of header files
|
||||
## Your package locations should be listed before other locations
|
||||
include_directories(include)
|
||||
include_directories(
|
||||
${catkin_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
## Declare a cpp library
|
||||
# add_library(irobot_msgs
|
||||
# src/${PROJECT_NAME}/irobot_msgs.cpp
|
||||
# )
|
||||
|
||||
## Declare a cpp executable
|
||||
add_executable(odom_visualization src/odom_visualization.cpp)
|
||||
|
||||
## Add cmake target dependencies of the executable/library
|
||||
## as an example, message headers may need to be generated before nodes
|
||||
# add_dependencies(multi_map_visualization multi_map_server_messages_cpp)
|
||||
|
||||
## Specify libraries to link a library or executable target against
|
||||
target_link_libraries(odom_visualization
|
||||
${catkin_LIBRARIES}
|
||||
${ARMADILLO_LIBRARIES}
|
||||
pose_utils
|
||||
)
|
||||
|
||||
#############
|
||||
## Install ##
|
||||
#############
|
||||
|
||||
# all install targets should use catkin DESTINATION variables
|
||||
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
|
||||
|
||||
## Mark executable scripts (Python etc.) for installation
|
||||
## in contrast to setup.py, you can choose the destination
|
||||
# install(PROGRAMS
|
||||
# scripts/my_python_script
|
||||
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
# )
|
||||
|
||||
## Mark executables and/or libraries for installation
|
||||
# install(TARGETS irobot_msgs irobot_msgs_node
|
||||
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
# )
|
||||
|
||||
## Mark cpp header files for installation
|
||||
# install(DIRECTORY include/${PROJECT_NAME}/
|
||||
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
|
||||
# FILES_MATCHING PATTERN "*.h"
|
||||
# PATTERN ".svn" EXCLUDE
|
||||
# )
|
||||
|
||||
## Mark other files for installation (e.g. launch and bag files, etc.)
|
||||
# install(FILES
|
||||
# # myfile1
|
||||
# # myfile2
|
||||
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
|
||||
# )
|
||||
|
||||
#############
|
||||
## Testing ##
|
||||
#############
|
||||
|
||||
## Add gtest based cpp test target and link libraries
|
||||
# catkin_add_gtest(${PROJECT_NAME}-test test/test_irobot_msgs.cpp)
|
||||
# if(TARGET ${PROJECT_NAME}-test)
|
||||
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
|
||||
# endif()
|
||||
@ -1 +0,0 @@
|
||||
include $(shell rospack find mk)/cmake.mk
|
||||
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in new issue