transformation & downsampling
This commit is contained in:
commit
48697e8583
36
.vscode/c_cpp_properties.json
vendored
Normal file
36
.vscode/c_cpp_properties.json
vendored
Normal file
@ -0,0 +1,36 @@
|
|||||||
|
{
|
||||||
|
"configurations": [
|
||||||
|
{
|
||||||
|
"browse": {
|
||||||
|
"databaseFilename": "${default}",
|
||||||
|
"limitSymbolsToIncludedHeaders": false
|
||||||
|
},
|
||||||
|
"includePath": [
|
||||||
|
"/home/chi/ros1_wss/ant_ws/devel/include/**",
|
||||||
|
"/opt/ros/noetic/include/**",
|
||||||
|
"/home/chi/ros1_wss/ant_ws/src/ant_costmap/include/**",
|
||||||
|
"/home/chi/ros1_wss/ant_ws/src/grid_map/cmap/include/**",
|
||||||
|
"/home/chi/ros1_wss/ant_ws/src/grid_map/grid_map_core/include/**",
|
||||||
|
"/home/chi/ros1_wss/ant_ws/src/grid_map/grid_map_costmap_2d/include/**",
|
||||||
|
"/home/chi/ros1_wss/ant_ws/src/grid_map/grid_map_cv/include/**",
|
||||||
|
"/home/chi/ros1_wss/ant_ws/src/grid_map/grid_map_demos/include/**",
|
||||||
|
"/home/chi/ros1_wss/ant_ws/src/grid_map/grid_map_filters/include/**",
|
||||||
|
"/home/chi/ros1_wss/ant_ws/src/grid_map/grid_map_loader/include/**",
|
||||||
|
"/home/chi/ros1_wss/ant_ws/src/grid_map/grid_map_octomap/include/**",
|
||||||
|
"/home/chi/ros1_wss/ant_ws/src/grid_map/grid_map_pcl/include/**",
|
||||||
|
"/home/chi/ros1_wss/ant_ws/src/grid_map/grid_map_ros/include/**",
|
||||||
|
"/home/chi/ros1_wss/ant_ws/src/grid_map/grid_map_rviz_plugin/include/**",
|
||||||
|
"/home/chi/ros1_wss/ant_ws/src/grid_map/grid_map_sdf/include/**",
|
||||||
|
"/home/chi/ros1_wss/ant_ws/src/grid_map/grid_map_visualization/include/**",
|
||||||
|
"/home/chi/ros1_wss/ant_ws/src/robot_localization/include/**",
|
||||||
|
"/usr/include/**"
|
||||||
|
],
|
||||||
|
"name": "ROS",
|
||||||
|
"intelliSenseMode": "gcc-x64",
|
||||||
|
"compilerPath": "/usr/bin/gcc",
|
||||||
|
"cStandard": "gnu11",
|
||||||
|
"cppStandard": "c++14"
|
||||||
|
}
|
||||||
|
],
|
||||||
|
"version": 4
|
||||||
|
}
|
||||||
7
.vscode/launch.json
vendored
Normal file
7
.vscode/launch.json
vendored
Normal file
@ -0,0 +1,7 @@
|
|||||||
|
{
|
||||||
|
// Use IntelliSense to learn about possible attributes.
|
||||||
|
// Hover to view descriptions of existing attributes.
|
||||||
|
// For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
|
||||||
|
"version": "0.2.0",
|
||||||
|
"configurations": []
|
||||||
|
}
|
||||||
18
.vscode/settings.json
vendored
Normal file
18
.vscode/settings.json
vendored
Normal file
@ -0,0 +1,18 @@
|
|||||||
|
{
|
||||||
|
"python.autoComplete.extraPaths": [
|
||||||
|
"/home/chi/ros1_wss/ant_ws/devel/lib/python3/dist-packages",
|
||||||
|
"/opt/ros/noetic/lib/python3/dist-packages"
|
||||||
|
],
|
||||||
|
"files.associations": {
|
||||||
|
"string": "cpp",
|
||||||
|
"cmath": "cpp",
|
||||||
|
"complex": "cpp",
|
||||||
|
"core": "cpp",
|
||||||
|
"geometry": "cpp",
|
||||||
|
"iostream": "cpp"
|
||||||
|
},
|
||||||
|
"python.analysis.extraPaths": [
|
||||||
|
"/home/chi/ros1_wss/ant_ws/devel/lib/python3/dist-packages",
|
||||||
|
"/opt/ros/noetic/lib/python3/dist-packages"
|
||||||
|
]
|
||||||
|
}
|
||||||
28
.vscode/tasks.json
vendored
Normal file
28
.vscode/tasks.json
vendored
Normal file
@ -0,0 +1,28 @@
|
|||||||
|
{
|
||||||
|
"tasks": [
|
||||||
|
{
|
||||||
|
"type": "cppbuild",
|
||||||
|
"label": "C/C++: g++ build active file",
|
||||||
|
"command": "/usr/bin/g++",
|
||||||
|
"args": [
|
||||||
|
"-fdiagnostics-color=always",
|
||||||
|
"-g",
|
||||||
|
"${file}",
|
||||||
|
"-o",
|
||||||
|
"${fileDirname}/${fileBasenameNoExtension}"
|
||||||
|
],
|
||||||
|
"options": {
|
||||||
|
"cwd": "${fileDirname}"
|
||||||
|
},
|
||||||
|
"problemMatcher": [
|
||||||
|
"$gcc"
|
||||||
|
],
|
||||||
|
"group": {
|
||||||
|
"kind": "build",
|
||||||
|
"isDefault": true
|
||||||
|
},
|
||||||
|
"detail": "Task generated by Debugger."
|
||||||
|
}
|
||||||
|
],
|
||||||
|
"version": "2.0.0"
|
||||||
|
}
|
||||||
264
CMakeLists.txt
Normal file
264
CMakeLists.txt
Normal file
@ -0,0 +1,264 @@
|
|||||||
|
cmake_minimum_required(VERSION 3.0.2)
|
||||||
|
project(cmap)
|
||||||
|
|
||||||
|
## 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
|
||||||
|
roscpp
|
||||||
|
tf
|
||||||
|
tf2_ros
|
||||||
|
pcl_conversions
|
||||||
|
pcl_ros
|
||||||
|
sensor_msgs
|
||||||
|
std_msgs
|
||||||
|
genmsg
|
||||||
|
grid_map_core
|
||||||
|
grid_map_ros
|
||||||
|
grid_map_msgs
|
||||||
|
visualization_msgs
|
||||||
|
)
|
||||||
|
|
||||||
|
#find_package(octomap REQUIRED)
|
||||||
|
#find_package(OpenCV REQUIRED )
|
||||||
|
find_package(PCL REQUIRED)
|
||||||
|
# message(STATUS "Found Octomap (version ${octomap_VERSION}): ${OCTOMAP_INCLUDE_DIRS}")
|
||||||
|
|
||||||
|
## 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
|
||||||
|
# sensor_msgs# 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 cmap
|
||||||
|
# CATKIN_DEPENDS pcl_conversions pcl_ros roscpp sensor_msgs 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}
|
||||||
|
${OpenCV_INCLUDE_DIRS}
|
||||||
|
${PCL_INCLUDE_DIRS}
|
||||||
|
)
|
||||||
|
|
||||||
|
# link_directories(${PCL_LIBRARY_DIRS})
|
||||||
|
# link_directories${OpenCV_INCLUDE_DIRS}
|
||||||
|
## Declare a C++ library
|
||||||
|
# add_library(${PROJECT_NAME}
|
||||||
|
# src/${PROJECT_NAME}/cmap.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/cmap_node.cpp)
|
||||||
|
|
||||||
|
#add_executable(testo
|
||||||
|
# src/testo.cpp
|
||||||
|
#)
|
||||||
|
|
||||||
|
#target_link_libraries(testo ${catkin_LIBRARIES})
|
||||||
|
#target_link_libraries(testo ${OpenCV_LIBS} )
|
||||||
|
|
||||||
|
add_executable(pcd_transformer
|
||||||
|
src/pcd_transformer.cpp
|
||||||
|
)
|
||||||
|
|
||||||
|
target_link_libraries(pcd_transformer ${catkin_LIBRARIES})
|
||||||
|
#target_link_libraries(pcd_transformer ${OpenCV_LIBS} )
|
||||||
|
target_link_libraries(pcd_transformer ${PCL_LIBRARIES})
|
||||||
|
|
||||||
|
# add_executable(pcd_ptf
|
||||||
|
# src/pcd_ptf.cpp
|
||||||
|
# )
|
||||||
|
|
||||||
|
# target_link_libraries(pcd_ptf ${catkin_LIBRARIES})
|
||||||
|
# target_link_libraries(pcd_ptf ${OpenCV_LIBS} )
|
||||||
|
# target_link_libraries(pcd_ptf ${PCL_LIBRARIES})
|
||||||
|
|
||||||
|
add_executable(transforma
|
||||||
|
src/transforma.cpp
|
||||||
|
)
|
||||||
|
|
||||||
|
target_link_libraries(transforma ${catkin_LIBRARIES})
|
||||||
|
#target_link_libraries(transforma ${OpenCV_LIBS} )
|
||||||
|
target_link_libraries(transforma ${PCL_LIBRARIES})
|
||||||
|
|
||||||
|
add_executable(greedy_projection
|
||||||
|
src/greedy_projection.cpp
|
||||||
|
)
|
||||||
|
|
||||||
|
target_link_libraries(greedy_projection ${catkin_LIBRARIES})
|
||||||
|
#target_link_libraries(pcd_transformer ${OpenCV_LIBS} )
|
||||||
|
target_link_libraries(greedy_projection ${PCL_LIBRARIES})
|
||||||
|
|
||||||
|
|
||||||
|
## 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}
|
||||||
|
# )
|
||||||
|
|
||||||
|
#############
|
||||||
|
## Testing ##
|
||||||
|
#############
|
||||||
|
|
||||||
|
## Add gtest based cpp test target and link libraries
|
||||||
|
# catkin_add_gtest(${PROJECT_NAME}-test test/test_cmap.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)
|
||||||
BIN
data/strawberryfields.pcd
Executable file
BIN
data/strawberryfields.pcd
Executable file
Binary file not shown.
18
include/cmap/pcd_ptf.hpp
Normal file
18
include/cmap/pcd_ptf.hpp
Normal file
@ -0,0 +1,18 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
//ROS
|
||||||
|
|
||||||
|
#include <ros/ros.h>
|
||||||
|
#include <sensor_msgs/PointCloud2.h>
|
||||||
|
// #include <pcl/common/transforms.h>
|
||||||
|
// #include <pcl/point_types.h>
|
||||||
|
// #include <pcl/io/pcd_io.h>
|
||||||
|
// #include <pcl/io/ply_io.h>
|
||||||
|
// #include <pcl/point_cloud.h>
|
||||||
|
//Grid map
|
||||||
|
#include <grid_map_ros/grid_map_ros.hpp>
|
||||||
|
|
||||||
|
#include <grid_map_core/GridMap.hpp>
|
||||||
|
// #include <grid_map_costmap_2d/Costmap2DConverter.hpp>
|
||||||
|
#include <string>
|
||||||
|
#include <grid_map_demos/OctomapToGridmapDemo.hpp>
|
||||||
18
include/cmap/testo.hpp
Normal file
18
include/cmap/testo.hpp
Normal file
@ -0,0 +1,18 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
//ROS
|
||||||
|
|
||||||
|
#include <ros/ros.h>
|
||||||
|
#include <sensor_msgs/PointCloud2.h>
|
||||||
|
// #include <pcl/common/transforms.h>
|
||||||
|
// #include <pcl/point_types.h>
|
||||||
|
// #include <pcl/io/pcd_io.h>
|
||||||
|
// #include <pcl/io/ply_io.h>
|
||||||
|
// #include <pcl/point_cloud.h>
|
||||||
|
//Grid map
|
||||||
|
#include <grid_map_ros/grid_map_ros.hpp>
|
||||||
|
|
||||||
|
#include <grid_map_core/GridMap.hpp>
|
||||||
|
// #include <grid_map_costmap_2d/Costmap2DConverter.hpp>
|
||||||
|
#include <string>
|
||||||
|
#include <grid_map_demos/OctomapToGridmapDemo.hpp>
|
||||||
18
include/cmap/transforma.hpp
Normal file
18
include/cmap/transforma.hpp
Normal file
@ -0,0 +1,18 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
//ROS
|
||||||
|
|
||||||
|
#include <ros/ros.h>
|
||||||
|
#include <sensor_msgs/PointCloud2.h>
|
||||||
|
// #include <pcl/common/transforms.h>
|
||||||
|
// #include <pcl/point_types.h>
|
||||||
|
// #include <pcl/io/pcd_io.h>
|
||||||
|
// #include <pcl/io/ply_io.h>
|
||||||
|
// #include <pcl/point_cloud.h>
|
||||||
|
//Grid map
|
||||||
|
#include <grid_map_ros/grid_map_ros.hpp>
|
||||||
|
|
||||||
|
#include <grid_map_core/GridMap.hpp>
|
||||||
|
// #include <grid_map_costmap_2d/Costmap2DConverter.hpp>
|
||||||
|
#include <string>
|
||||||
|
#include <grid_map_demos/OctomapToGridmapDemo.hpp>
|
||||||
7
launch/test.launch
Normal file
7
launch/test.launch
Normal file
@ -0,0 +1,7 @@
|
|||||||
|
<?xml version="1.0" encoding="UTF-8"?>
|
||||||
|
|
||||||
|
|
||||||
|
<launch>
|
||||||
|
<node pkg="tf" type="static_transform_publisher" name="camera_to_base" args=" 1.84 0 -1.0 0 -0.698132 0 camera_color_optical_frame base_link 100" />
|
||||||
|
|
||||||
|
</launch>
|
||||||
9
launch/transform.launch
Executable file
9
launch/transform.launch
Executable file
@ -0,0 +1,9 @@
|
|||||||
|
<?xml version="1.0" encoding="UTF-8"?>
|
||||||
|
|
||||||
|
|
||||||
|
<launch>
|
||||||
|
<node pkg="cmap" type="transforma" name="transforma" />
|
||||||
|
<node pkg="tf" type="static_transform_publisher" name="camera_to_base" args=" -0.55 0 2 -3.14 0.87 1.55 camera_color_optical_frame base_link 100" />
|
||||||
|
<!--node pkg="tf" type="static_transform_publisher" name="camera_to_base" args=" 1.3435 0 -1.3435 0 -0.785398 0 camera_link base_link 100" /-->
|
||||||
|
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find package_name)/rviz/cmap_testing.rviz"/>
|
||||||
|
</launch>
|
||||||
76
package.xml
Normal file
76
package.xml
Normal file
@ -0,0 +1,76 @@
|
|||||||
|
<?xml version="1.0"?>
|
||||||
|
<package format="2">
|
||||||
|
<name>cmap</name>
|
||||||
|
<version>0.0.0</version>
|
||||||
|
<description>The cmap package</description>
|
||||||
|
|
||||||
|
<!-- One maintainer tag required, multiple allowed, one person per tag -->
|
||||||
|
<!-- Example: -->
|
||||||
|
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
|
||||||
|
<maintainer email="chi@todo.todo">chi</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/cmap</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>libpcl-all-dev</build_depend>
|
||||||
|
<!--build_depend>pcl_conversions</build_depend>
|
||||||
|
<build_depend>pcl_ros</build_depend-->
|
||||||
|
<build_depend>roscpp</build_depend>
|
||||||
|
<build_depend>sensor_msgs</build_depend>
|
||||||
|
<build_depend>std_msgs</build_depend>
|
||||||
|
<!--build_export_depend>pcl_conversions</build_export_depend>
|
||||||
|
<build_export_depend>pcl_ros</build_export_depend-->
|
||||||
|
<build_export_depend>roscpp</build_export_depend>
|
||||||
|
<build_export_depend>sensor_msgs</build_export_depend>
|
||||||
|
<build_export_depend>std_msgs</build_export_depend>
|
||||||
|
<!--exec_depend>pcl_conversions</exec_depend>
|
||||||
|
<exec_depend>pcl_ros</exec_depend-->
|
||||||
|
<exec_depend>roscpp</exec_depend>
|
||||||
|
<exec_depend>libpcl-all</exec_depend>
|
||||||
|
<exec_depend>sensor_msgs</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>
|
||||||
233
rviz/cmap_testing.rviz
Normal file
233
rviz/cmap_testing.rviz
Normal file
@ -0,0 +1,233 @@
|
|||||||
|
Panels:
|
||||||
|
- Class: rviz/Displays
|
||||||
|
Help Height: 85
|
||||||
|
Name: Displays
|
||||||
|
Property Tree Widget:
|
||||||
|
Expanded:
|
||||||
|
- /voxel_cloud1
|
||||||
|
- /map_cloud1
|
||||||
|
Splitter Ratio: 0.47749510407447815
|
||||||
|
Tree Height: 628
|
||||||
|
- Class: rviz/Selection
|
||||||
|
Name: Selection
|
||||||
|
- Class: rviz/Tool Properties
|
||||||
|
Expanded:
|
||||||
|
- /2D Pose Estimate1
|
||||||
|
- /2D Nav Goal1
|
||||||
|
- /Publish Point1
|
||||||
|
Name: Tool Properties
|
||||||
|
Splitter Ratio: 0.5886790156364441
|
||||||
|
- Class: rviz/Views
|
||||||
|
Expanded:
|
||||||
|
- /Current View1
|
||||||
|
Name: Views
|
||||||
|
Splitter Ratio: 0.6059479713439941
|
||||||
|
- Class: rviz/Time
|
||||||
|
Name: Time
|
||||||
|
SyncMode: 0
|
||||||
|
SyncSource: voxel_cloud
|
||||||
|
Preferences:
|
||||||
|
PromptSaveOnExit: true
|
||||||
|
Toolbars:
|
||||||
|
toolButtonStyle: 2
|
||||||
|
Visualization Manager:
|
||||||
|
Class: ""
|
||||||
|
Displays:
|
||||||
|
- 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: 10
|
||||||
|
Reference Frame: <Fixed Frame>
|
||||||
|
Value: true
|
||||||
|
- Alpha: 1
|
||||||
|
Autocompute Intensity Bounds: true
|
||||||
|
Autocompute Value Bounds:
|
||||||
|
Max Value: 10
|
||||||
|
Min Value: -10
|
||||||
|
Value: true
|
||||||
|
Axis: Z
|
||||||
|
Channel Name: intensity
|
||||||
|
Class: rviz/PointCloud2
|
||||||
|
Color: 255; 255; 255
|
||||||
|
Color Transformer: RGB8
|
||||||
|
Decay Time: 0
|
||||||
|
Enabled: false
|
||||||
|
Invert Rainbow: false
|
||||||
|
Max Color: 255; 255; 255
|
||||||
|
Min Color: 0; 0; 0
|
||||||
|
Name: voxel_cloud
|
||||||
|
Position Transformer: XYZ
|
||||||
|
Queue Size: 10
|
||||||
|
Selectable: true
|
||||||
|
Size (Pixels): 3
|
||||||
|
Size (m): 0.009999999776482582
|
||||||
|
Style: Flat Squares
|
||||||
|
Topic: /voxel_cloud
|
||||||
|
Unreliable: false
|
||||||
|
Use Fixed Frame: true
|
||||||
|
Use rainbow: true
|
||||||
|
Value: false
|
||||||
|
- Class: rviz/TF
|
||||||
|
Enabled: true
|
||||||
|
Frame Timeout: 15
|
||||||
|
Frames:
|
||||||
|
All Enabled: false
|
||||||
|
camera_accel_frame:
|
||||||
|
Value: false
|
||||||
|
camera_accel_optical_frame:
|
||||||
|
Value: false
|
||||||
|
camera_aligned_depth_to_color_frame:
|
||||||
|
Value: false
|
||||||
|
camera_color_frame:
|
||||||
|
Value: false
|
||||||
|
camera_color_optical_frame:
|
||||||
|
Value: false
|
||||||
|
camera_depth_frame:
|
||||||
|
Value: false
|
||||||
|
camera_depth_optical_frame:
|
||||||
|
Value: false
|
||||||
|
camera_gyro_frame:
|
||||||
|
Value: false
|
||||||
|
camera_imu_optical_frame:
|
||||||
|
Value: false
|
||||||
|
camera_link:
|
||||||
|
Value: true
|
||||||
|
Marker Alpha: 1
|
||||||
|
Marker Scale: 1
|
||||||
|
Name: TF
|
||||||
|
Show Arrows: true
|
||||||
|
Show Axes: true
|
||||||
|
Show Names: true
|
||||||
|
Tree:
|
||||||
|
camera_link:
|
||||||
|
camera_accel_frame:
|
||||||
|
camera_accel_optical_frame:
|
||||||
|
{}
|
||||||
|
camera_aligned_depth_to_color_frame:
|
||||||
|
camera_color_optical_frame:
|
||||||
|
{}
|
||||||
|
camera_color_frame:
|
||||||
|
{}
|
||||||
|
camera_depth_frame:
|
||||||
|
camera_depth_optical_frame:
|
||||||
|
{}
|
||||||
|
camera_gyro_frame:
|
||||||
|
camera_imu_optical_frame:
|
||||||
|
{}
|
||||||
|
Update Interval: 0
|
||||||
|
Value: true
|
||||||
|
- Alpha: 1
|
||||||
|
Class: rviz/Axes
|
||||||
|
Enabled: true
|
||||||
|
Length: 1
|
||||||
|
Name: Axes
|
||||||
|
Radius: 0.10000000149011612
|
||||||
|
Reference Frame: <Fixed Frame>
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
- Alpha: 1
|
||||||
|
Autocompute Intensity Bounds: true
|
||||||
|
Autocompute Value Bounds:
|
||||||
|
Max Value: 10
|
||||||
|
Min Value: -10
|
||||||
|
Value: true
|
||||||
|
Axis: Z
|
||||||
|
Channel Name: intensity
|
||||||
|
Class: rviz/PointCloud2
|
||||||
|
Color: 255; 255; 255
|
||||||
|
Color Transformer: RGB8
|
||||||
|
Decay Time: 0
|
||||||
|
Enabled: true
|
||||||
|
Invert Rainbow: false
|
||||||
|
Max Color: 255; 255; 255
|
||||||
|
Min Color: 0; 0; 0
|
||||||
|
Name: map_cloud
|
||||||
|
Position Transformer: XYZ
|
||||||
|
Queue Size: 10
|
||||||
|
Selectable: true
|
||||||
|
Size (Pixels): 3
|
||||||
|
Size (m): 0.009999999776482582
|
||||||
|
Style: Flat Squares
|
||||||
|
Topic: /map_cloud
|
||||||
|
Unreliable: false
|
||||||
|
Use Fixed Frame: true
|
||||||
|
Use rainbow: true
|
||||||
|
Value: true
|
||||||
|
Enabled: true
|
||||||
|
Global Options:
|
||||||
|
Background Color: 48; 48; 48
|
||||||
|
Default Light: true
|
||||||
|
Fixed Frame: camera_color_optical_frame
|
||||||
|
Frame Rate: 30
|
||||||
|
Name: root
|
||||||
|
Tools:
|
||||||
|
- Class: rviz/Interact
|
||||||
|
Hide Inactive Objects: true
|
||||||
|
- Class: rviz/MoveCamera
|
||||||
|
- Class: rviz/Select
|
||||||
|
- Class: rviz/FocusCamera
|
||||||
|
- Class: rviz/Measure
|
||||||
|
- Class: rviz/SetInitialPose
|
||||||
|
Theta std deviation: 0.2617993950843811
|
||||||
|
Topic: /initialpose
|
||||||
|
X std deviation: 0.5
|
||||||
|
Y std deviation: 0.5
|
||||||
|
- Class: rviz/SetGoal
|
||||||
|
Topic: /move_base_simple/goal
|
||||||
|
- Class: rviz/PublishPoint
|
||||||
|
Single click: true
|
||||||
|
Topic: /clicked_point
|
||||||
|
Value: true
|
||||||
|
Views:
|
||||||
|
Current:
|
||||||
|
Class: rviz/Orbit
|
||||||
|
Distance: 2.104010820388794
|
||||||
|
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: 0.03424595668911934
|
||||||
|
Y: -0.006647967733442783
|
||||||
|
Z: 0.4646419286727905
|
||||||
|
Focal Shape Fixed Size: true
|
||||||
|
Focal Shape Size: 0.05000000074505806
|
||||||
|
Invert Z Axis: false
|
||||||
|
Name: Current View
|
||||||
|
Near Clip Distance: 0.009999999776482582
|
||||||
|
Pitch: 0.7497976422309875
|
||||||
|
Target Frame: camera_color_optical_frame
|
||||||
|
Yaw: 3.0763397216796875
|
||||||
|
Saved: ~
|
||||||
|
Window Geometry:
|
||||||
|
Displays:
|
||||||
|
collapsed: false
|
||||||
|
Height: 959
|
||||||
|
Hide Left Dock: false
|
||||||
|
Hide Right Dock: true
|
||||||
|
QMainWindow State: 000000ff00000000fd00000004000000000000017d00000310fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006e00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004700000310000000f100fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001b800000310fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000004700000310000000c200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000078000000040fc0100000002fb0000000800540069006d00650100000000000007800000045a00fffffffb0000000800540069006d00650100000000000004500000000000000000000005fc0000031000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||||
|
Selection:
|
||||||
|
collapsed: false
|
||||||
|
Time:
|
||||||
|
collapsed: false
|
||||||
|
Tool Properties:
|
||||||
|
collapsed: false
|
||||||
|
Views:
|
||||||
|
collapsed: true
|
||||||
|
Width: 1920
|
||||||
|
X: 2560
|
||||||
|
Y: 34
|
||||||
0
scripts/vtk_visualizer.py
Executable file
0
scripts/vtk_visualizer.py
Executable file
98
src/greedy_projection.cpp
Normal file
98
src/greedy_projection.cpp
Normal file
@ -0,0 +1,98 @@
|
|||||||
|
#include <iostream>
|
||||||
|
#include <pcl/point_types.h>
|
||||||
|
#include <pcl/io/pcd_io.h>
|
||||||
|
#include <pcl/search/kdtree.h>
|
||||||
|
#include <pcl/features/normal_3d.h>
|
||||||
|
#include <pcl/surface/gp3.h>
|
||||||
|
#include <pcl_conversions/pcl_conversions.h>
|
||||||
|
#include <pcl/io/vtk_io.h>
|
||||||
|
// #include <mesh_tools/mesh_msgs/MeshFaceCluster.msg>
|
||||||
|
// #include <grid_map/grid_map_ros/GridMapRosConverter.cpp>
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
// ros::Publisher mesh_pub ;
|
||||||
|
|
||||||
|
|
||||||
|
// void meshing_cb(const sensor_msgs::PointCloud2ConstPtr& cloud_msg) {
|
||||||
|
|
||||||
|
// // Container for original & filtered data
|
||||||
|
|
||||||
|
// pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2;
|
||||||
|
// pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
|
||||||
|
// pcl::PCLPointCloud2 cloud_;
|
||||||
|
|
||||||
|
// // Convert to PCL data type
|
||||||
|
// pcl_conversions::toPCL(*cloud_msg, *cloud);
|
||||||
|
// }
|
||||||
|
|
||||||
|
// int main(int argc, char **argv)
|
||||||
|
// {
|
||||||
|
// ros::init(argc, argv, "greedy_projection");
|
||||||
|
// ros::NodeHandle nh;
|
||||||
|
|
||||||
|
// ros::Subscriber cloud_in = nh.subscribe<sensor_msgs::PointCloud2>("map_cloud", 1, meshing_cb);
|
||||||
|
// mesh_pub = nh.advertise<mesh_msgs::MeshFaceCluster> ("map_cloud", 1);
|
||||||
|
|
||||||
|
|
||||||
|
// ros::spin ();
|
||||||
|
|
||||||
|
// }
|
||||||
|
|
||||||
|
|
||||||
|
int main() {
|
||||||
|
|
||||||
|
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
|
||||||
|
pcl::PCLPointCloud2 cloud_blob;
|
||||||
|
pcl::io::loadPCDFile ("/home/chinmay/ros1_wss/ant_wss/src/grid_map/cmap/data/strawberryfields.pcd", cloud_blob);
|
||||||
|
pcl::fromPCLPointCloud2 (cloud_blob, *cloud);
|
||||||
|
//* the data should be available in cloud
|
||||||
|
|
||||||
|
// Normal estimation*
|
||||||
|
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n;
|
||||||
|
pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
|
||||||
|
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
|
||||||
|
tree->setInputCloud (cloud);
|
||||||
|
n.setInputCloud (cloud);
|
||||||
|
n.setSearchMethod (tree);
|
||||||
|
n.setKSearch (20);
|
||||||
|
n.compute (*normals);
|
||||||
|
//* normals should not contain the point normals + surface curvatures
|
||||||
|
|
||||||
|
// Concatenate the XYZ and normal fields*
|
||||||
|
pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals (new pcl::PointCloud<pcl::PointNormal>);
|
||||||
|
pcl::concatenateFields (*cloud, *normals, *cloud_with_normals);
|
||||||
|
//* cloud_with_normals = cloud + normals
|
||||||
|
|
||||||
|
// Create search tree*
|
||||||
|
pcl::search::KdTree<pcl::PointNormal>::Ptr tree2 (new pcl::search::KdTree<pcl::PointNormal>);
|
||||||
|
tree2->setInputCloud (cloud_with_normals);
|
||||||
|
|
||||||
|
// Initialize objects
|
||||||
|
pcl::GreedyProjectionTriangulation<pcl::PointNormal> gp3;
|
||||||
|
pcl::PolygonMesh triangles;
|
||||||
|
|
||||||
|
// Set the maximum distance between connected points (maximum edge length)
|
||||||
|
gp3.setSearchRadius (0.025);
|
||||||
|
|
||||||
|
// Set typical values for the parameters
|
||||||
|
gp3.setMu (2.5);
|
||||||
|
gp3.setMaximumNearestNeighbors (100);
|
||||||
|
gp3.setMaximumSurfaceAngle(M_PI/4); // 45 degrees
|
||||||
|
gp3.setMinimumAngle(M_PI/18); // 10 degrees
|
||||||
|
gp3.setMaximumAngle(2*M_PI/3); // 120 degrees
|
||||||
|
gp3.setNormalConsistency(false);
|
||||||
|
|
||||||
|
// Get result
|
||||||
|
gp3.setInputCloud (cloud_with_normals);
|
||||||
|
gp3.setSearchMethod (tree2);
|
||||||
|
gp3.reconstruct (triangles);
|
||||||
|
|
||||||
|
// Additional vertex information
|
||||||
|
std::vector<int> parts = gp3.getPartIDs();
|
||||||
|
std::vector<int> states = gp3.getPointStates();
|
||||||
|
|
||||||
|
pcl::io::saveVTKFile ("mesh.vtk", triangles);
|
||||||
|
// Finish
|
||||||
|
return (0);
|
||||||
|
}
|
||||||
88
src/pcd_transformer.cpp
Normal file
88
src/pcd_transformer.cpp
Normal file
@ -0,0 +1,88 @@
|
|||||||
|
#include <iostream>
|
||||||
|
#include <ros/ros.h>
|
||||||
|
#include <sensor_msgs/PointCloud2.h>
|
||||||
|
#include <tf/transform_datatypes.h>
|
||||||
|
#include <tf2_ros/buffer.h>
|
||||||
|
#include <pcl_ros/point_cloud.h>
|
||||||
|
#include <pcl_ros/transforms.h>
|
||||||
|
#include <pcl_conversions/pcl_conversions.h>
|
||||||
|
#include <pcl/common/transforms.h>
|
||||||
|
#include <pcl/point_types.h>
|
||||||
|
#include <pcl/io/pcd_io.h>
|
||||||
|
#include <pcl/io/ply_io.h>
|
||||||
|
#include <pcl/point_cloud.h>
|
||||||
|
#include <pcl/console/parse.h>
|
||||||
|
#include <pcl/common/transforms.h>
|
||||||
|
#include <pcl/filters/voxel_grid.h>
|
||||||
|
#include <Eigen/Geometry>
|
||||||
|
#define foreach BOOST_FOREACH
|
||||||
|
|
||||||
|
tf::Transform trans;
|
||||||
|
// trans.setOrigin( tf::Vector3(1.3435, 0.0, -1.3435) );
|
||||||
|
// tf::Quaternion q;
|
||||||
|
// q.setRPY(0, -0.698132, 0);
|
||||||
|
// trans.setRotation(q);
|
||||||
|
// const std::string target_frame = "base_link";
|
||||||
|
const std::string target_frame = "camera_color_optical_frame";
|
||||||
|
// float pitch = -0.698132;
|
||||||
|
// Eigen::Affine3f trans = Eigen::Affine3f::Identity();
|
||||||
|
// trans.translation() << 1.3435, 0.0, -1.3435;
|
||||||
|
// trans.rotate (Eigen::AngleAxisf (pitch, Eigen::Vector3f::UnitY()));
|
||||||
|
// float pitch = -0.698132;
|
||||||
|
// Eigen::Affine3f trans = Eigen::Affine3f::Identity();
|
||||||
|
// trans.translation() << 1.3435, 0.0, -1.3435;
|
||||||
|
// trans.rotate (Eigen::AngleAxisf (pitch, Eigen::Vector3f::UnitY()));
|
||||||
|
|
||||||
|
ros::Publisher cloud_pub ;
|
||||||
|
|
||||||
|
// https://github.com/methylDragon/pcl-ros-tutorial/blob/master/PCL%20Reference%20with%20ROS.md
|
||||||
|
// http://wiki.ros.org/pcl/Tutorials/hydro?action=AttachFile&do=view&target=example_voxelgrid.cpp
|
||||||
|
// http://docs.ros.org/en/indigo/api/pcl_ros/html/namespacepcl__ros.html#aad20af010640a41f1898e75f6a6121ad
|
||||||
|
// https://answers.ros.org/question/225820/subscribe-to-sensor_msgpointcloud2/
|
||||||
|
|
||||||
|
|
||||||
|
void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_in)
|
||||||
|
{
|
||||||
|
// sensor_msgs::PointCloud2 cloud_out;
|
||||||
|
// pcl_ros::transformPointCloud (target_frame, trans, cloud_in, cloud_out);
|
||||||
|
// transformed_cloud.publish(cloud_out);
|
||||||
|
pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2;
|
||||||
|
pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
|
||||||
|
pcl::PCLPointCloud2 cloud_filtered;
|
||||||
|
|
||||||
|
pcl_conversions::toPCL(*cloud_in , *cloud);
|
||||||
|
|
||||||
|
pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
|
||||||
|
sor.setInputCloud (cloudPtr);
|
||||||
|
sor.setLeafSize (0.1, 0.1, 0.1);
|
||||||
|
sor.filter (cloud_filtered);
|
||||||
|
sensor_msgs::PointCloud2 cloud_o;
|
||||||
|
sensor_msgs::PointCloud2 trans_cloud;
|
||||||
|
pcl_conversions::moveFromPCL(cloud_filtered, cloud_o);
|
||||||
|
|
||||||
|
trans.setOrigin( tf::Vector3(1.3435, 0.0, -1.3435) );
|
||||||
|
tf::Quaternion q;
|
||||||
|
q.setRPY(0, -0.698132, 0);
|
||||||
|
trans.setRotation(q);
|
||||||
|
|
||||||
|
pcl_ros::transformPointCloud(target_frame, trans, cloud_o, trans_cloud);
|
||||||
|
|
||||||
|
cloud_pub.publish (trans_cloud);
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
int main(int argc, char **argv)
|
||||||
|
{
|
||||||
|
ros::init(argc, argv, "pcd_transformer");
|
||||||
|
ros::NodeHandle nh;
|
||||||
|
|
||||||
|
ros::Subscriber cloud_in = nh.subscribe("/voxel_cloud", 1, cloud_cb);
|
||||||
|
|
||||||
|
|
||||||
|
cloud_pub = nh.advertise<sensor_msgs::PointCloud2> ("trans_cloud", 1);
|
||||||
|
|
||||||
|
|
||||||
|
ros::spin ();
|
||||||
|
|
||||||
|
}
|
||||||
BIN
src/strawberryfields.pcd
Executable file
BIN
src/strawberryfields.pcd
Executable file
Binary file not shown.
105
src/transforma.cpp
Normal file
105
src/transforma.cpp
Normal file
@ -0,0 +1,105 @@
|
|||||||
|
#include <iostream>
|
||||||
|
#include <ros/ros.h>
|
||||||
|
#include <sensor_msgs/PointCloud2.h>
|
||||||
|
#include <pcl_conversions/pcl_conversions.h>
|
||||||
|
#include <pcl/common/transforms.h>
|
||||||
|
#include <pcl/point_types.h>
|
||||||
|
#include <pcl/io/pcd_io.h>
|
||||||
|
#include <pcl/io/ply_io.h>
|
||||||
|
#include <pcl/search/kdtree.h>
|
||||||
|
#include <pcl/features/normal_3d.h>
|
||||||
|
#include <pcl/surface/gp3.h>
|
||||||
|
#include <pcl/point_cloud.h>
|
||||||
|
#include <pcl/console/parse.h>
|
||||||
|
#include <pcl/common/io.h>
|
||||||
|
#include <pcl/common/common_headers.h>
|
||||||
|
#include <pcl/common/transforms.h>
|
||||||
|
#include <pcl/filters/voxel_grid.h>
|
||||||
|
#include <Eigen/Geometry>
|
||||||
|
#include <tf/transform_datatypes.h>
|
||||||
|
#include <pcl/io/vtk_io.h>
|
||||||
|
#define foreach BOOST_FOREACH
|
||||||
|
|
||||||
|
|
||||||
|
typedef pcl::PointXYZRGB PointT;
|
||||||
|
|
||||||
|
|
||||||
|
ros::Publisher cloud_pub ;
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
//Transformation
|
||||||
|
|
||||||
|
void transformation(const pcl::PointCloud<PointT>::Ptr &ip,pcl::PointCloud<PointT>::Ptr &op )
|
||||||
|
{
|
||||||
|
float roll = -3.14;
|
||||||
|
float pitch = 0.87;
|
||||||
|
float yaw = 1.55;
|
||||||
|
|
||||||
|
|
||||||
|
Eigen::Affine3f trans = Eigen::Affine3f::Identity();
|
||||||
|
|
||||||
|
|
||||||
|
trans.translation() << -0.55, 0, 2;
|
||||||
|
trans.rotate (Eigen::AngleAxisf (roll, Eigen::Vector3f::UnitX()));
|
||||||
|
trans.rotate (Eigen::AngleAxisf (pitch, Eigen::Vector3f::UnitY()));
|
||||||
|
trans.rotate (Eigen::AngleAxisf (yaw, Eigen::Vector3f::UnitZ()));
|
||||||
|
|
||||||
|
|
||||||
|
pcl::transformPointCloud (*ip, *op, trans);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
//Surface meshing
|
||||||
|
|
||||||
|
// void greedy_projection() {
|
||||||
|
// pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
|
||||||
|
// pcl::PCLPointCloud2 cloud_blob;
|
||||||
|
|
||||||
|
// }
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
//Callback for subscriber
|
||||||
|
|
||||||
|
|
||||||
|
void
|
||||||
|
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg)
|
||||||
|
{
|
||||||
|
|
||||||
|
sensor_msgs::PointCloud2 map_cloud;
|
||||||
|
std::cout << cloud_msg->header.frame_id << std::endl;
|
||||||
|
|
||||||
|
// create pointer to Point Clouds in pcl
|
||||||
|
pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
|
||||||
|
pcl::PointCloud<PointT>::Ptr transform_cloud(new pcl::PointCloud<PointT>);
|
||||||
|
pcl::fromROSMsg(*cloud_msg, *cloud);
|
||||||
|
pcl::fromROSMsg(*cloud_msg, *transform_cloud);
|
||||||
|
|
||||||
|
|
||||||
|
transformation(cloud,transform_cloud);
|
||||||
|
|
||||||
|
|
||||||
|
pcl::toROSMsg(*transform_cloud, map_cloud);
|
||||||
|
|
||||||
|
cloud_pub.publish (map_cloud);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
int main(int argc, char **argv)
|
||||||
|
{
|
||||||
|
ros::init(argc, argv, "pcd_transformer");
|
||||||
|
ros::NodeHandle nh;
|
||||||
|
|
||||||
|
ros::Subscriber cloud_in = nh.subscribe<sensor_msgs::PointCloud2>("/voxel_cloud", 1, cloud_cb);
|
||||||
|
|
||||||
|
|
||||||
|
cloud_pub = nh.advertise<sensor_msgs::PointCloud2> ("map_cloud", 1);
|
||||||
|
|
||||||
|
|
||||||
|
ros::spin ();
|
||||||
|
|
||||||
|
}
|
||||||
Loading…
x
Reference in New Issue
Block a user