hongshaorou/YOPO
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Initial Commit (tested training, testing, and TRT conversion)

Browse Source
This commit is contained in:
Lu Junjie
2024-10-20 17:01:07 +08:00
parent 86d2f311f8
commit 5738088bae
221 changed files with 59249 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

138
flightlib/third_party/sdf_tools/CMakeLists.txt vendored Normal file
View File

@@ -0,0 +1,138 @@
cmake_minimum_required(VERSION 2.8.3)
project(sdf_tools)
## 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 rospy std_msgs sensor_msgs visualization_msgs image_transport cv_bridge arc_utilities message_generation)
find_package(cmake_modules REQUIRED)
find_package(Eigen3 REQUIRED)
set(Eigen3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR})
find_package(OpenCV REQUIRED)
# MoveIt! isn't a required dependency
find_package(moveit_msgs)
find_package(moveit_core)
find_package(moveit_ros_planning)
find_package(PCL 1.10 REQUIRED)
## 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/groovy/api/catkin/html/user_guide/setup_dot_py.html
#catkin_python_setup()
#######################################
## Declare ROS messages and services ##
#######################################
## Generate messages in the 'msg' folder
add_message_files(DIRECTORY msg FILES SDF.msg CollisionMap.msg TaggedObjectCollisionMap.msg)
## Generate services in the 'srv' folder
add_service_files(DIRECTORY srv FILES ComputeSDF.srv)
## Generate added messages and services with any dependencies listed here
generate_messages(DEPENDENCIES geometry_msgs std_msgs)
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## 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
if(moveit_core_FOUND)
catkin_package(INCLUDE_DIRS include LIBRARIES ${PROJECT_NAME} CATKIN_DEPENDS rospy std_msgs sensor_msgs moveit_core moveit_ros_planning visualization_msgs moveit_msgs image_transport cv_bridge arc_utilities message_runtime DEPENDS Eigen3 OpenCV)
else()
catkin_package(INCLUDE_DIRS include LIBRARIES ${PROJECT_NAME} CATKIN_DEPENDS rospy std_msgs sensor_msgs visualization_msgs image_transport cv_bridge arc_utilities message_runtime DEPENDS Eigen3 OpenCV)
endif()
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
if(moveit_core_FOUND)
include_directories(include SYSTEM ${catkin_INCLUDE_DIRS} ${Eigen3_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS} ${moveit_core_INCLUDE_DIRS} ${moveit_msgs_INCLUDE_DIRS} ${moveit_ros_planning_INCLUDE_DIRS})
else()
include_directories(include SYSTEM ${catkin_INCLUDE_DIRS} ${Eigen3_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS})
endif()
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
set(CMAKE_CXX_FLAGS "-std=c++14 ${CMAKE_CXX_FLAGS} -flto -O3 -Wall -Wextra -Werror")
set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib)
if(moveit_core_FOUND)
message(STATUS "MoveIt! found. Building SDF Tools library with MoveIt! integration.")
# SDF library
add_library(${PROJECT_NAME}
include/${PROJECT_NAME}/collision_map.hpp
include/${PROJECT_NAME}/tagged_object_collision_map.hpp
include/${PROJECT_NAME}/dynamic_spatial_hashed_collision_map.hpp
include/${PROJECT_NAME}/sdf.hpp
include/${PROJECT_NAME}/sdf_builder.hpp
src/${PROJECT_NAME}/collision_map.cpp
src/${PROJECT_NAME}/tagged_object_collision_map.cpp
src/${PROJECT_NAME}/dynamic_spatial_hashed_collision_map.cpp
src/${PROJECT_NAME}/sdf.cpp
src/${PROJECT_NAME}/sdf_builder.cpp)
add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencpp)
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${moveit_core_LIBRARIES} ${moveit_msgs_LIBRARIES} ${moveit_ros_planning_LIBRARIES})
# SDF generation node
add_executable(sdf_generation_node src/sdf_generation_node.cpp)
add_dependencies(sdf_generation_node ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencpp)
target_link_libraries(sdf_generation_node ${catkin_LIBRARIES} ${PROJECT_NAME} ${moveit_core_LIBRARIES} ${moveit_msgs_LIBRARIES} ${moveit_ros_planning_LIBRARIES})
else()
message(STATUS "MoveIt! not found. Building SDF Tools library without MoveIt! integration.")
# SDF library
add_library(${PROJECT_NAME}
include/${PROJECT_NAME}/collision_map.hpp
include/${PROJECT_NAME}/tagged_object_collision_map.hpp
include/${PROJECT_NAME}/dynamic_spatial_hashed_collision_map.hpp
include/${PROJECT_NAME}/sdf.hpp
src/${PROJECT_NAME}/collision_map.cpp
src/${PROJECT_NAME}/tagged_object_collision_map.cpp
src/${PROJECT_NAME}/dynamic_spatial_hashed_collision_map.cpp
src/${PROJECT_NAME}/sdf.cpp)
add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencpp)
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
endif()
# Simple test node for voxel_grid
# add_executable(voxel_grid_test src/test_voxel_grid.cpp)
# add_dependencies(voxel_grid_test ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencpp)
# target_link_libraries(voxel_grid_test ${catkin_LIBRARIES} ${PROJECT_NAME})
# Image to SDF node
# add_executable(image_2d_sdf_node src/image_2d_sdf_node.cpp)
# add_dependencies(image_2d_sdf_node ${catkin_EXPORTED_TARGETS})
# target_link_libraries(image_2d_sdf_node ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})
# Tutorial on how to use the components of the sdf_tools library
# add_executable(sdf_tools_tutorial src/sdf_tools_tutorial.cpp)
# add_dependencies(sdf_tools_tutorial ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencpp)
# target_link_libraries(sdf_tools_tutorial ${catkin_LIBRARIES} ${PROJECT_NAME})
#############
## Install ##
#############
## Mark library for installation
install(TARGETS ${PROJECT_NAME}
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
ARCHIVE DESTINATION ${PROJECT_SOURCE_DIR}/lib
RUNTIME DESTINATION ${PROJECT_SOURCE_DIR}/lib
LIBRARY DESTINATION ${PROJECT_SOURCE_DIR}/lib
)
## Mark cpp header files for installation
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${PROJECT_SOURCE_DIR}/include/sdf_tools
PATTERN ".svn" EXCLUDE
)

23
flightlib/third_party/sdf_tools/LICENSE vendored Normal file
View File

@@ -0,0 +1,23 @@
Copyright (c) 2014, Autonomous Robotic Collaboration Laboratory
All rights reserved.
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.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

6
flightlib/third_party/sdf_tools/README.md vendored Normal file
View File

@@ -0,0 +1,6 @@
sdf_tools
=========
Builds 2D signed distance fields from images, 3D signed distance fields from pointclouds, 3D signed distance fields from Octomap, provides a lightweight signed distance field library, message types for signed distance fields, and tools to compress signed distance fields for transport.
A simple example/tutorial is provided, see the [Wiki](https://github.com/WPI-ARC/sdf_tools/wiki).

323
flightlib/third_party/sdf_tools/include/sdf_tools/CollisionMap.h vendored Normal file
View File

@@ -0,0 +1,323 @@
// Generated by gencpp from file sdf_tools/CollisionMap.msg
// DO NOT EDIT!
#ifndef SDF_TOOLS_MESSAGE_COLLISIONMAP_H
#define SDF_TOOLS_MESSAGE_COLLISIONMAP_H
#include <string>
#include <vector>
#include <map>
#include <ros/types.h>
#include <ros/serialization.h>
#include <ros/builtin_message_traits.h>
#include <ros/message_operations.h>
#include <std_msgs/Header.h>
#include <geometry_msgs/Transform.h>
#include <geometry_msgs/Vector3.h>
namespace sdf_tools
{
template <class ContainerAllocator>
struct CollisionMap_
{
typedef CollisionMap_<ContainerAllocator> Type;
CollisionMap_()
: header()
, origin_transform()
, dimensions()
, cell_size(0.0)
, OOB_occupancy_value(0.0)
, OOB_component_value(0)
, number_of_components(0)
, components_valid(false)
, initialized(false)
, data() {
}
CollisionMap_(const ContainerAllocator& _alloc)
: header(_alloc)
, origin_transform(_alloc)
, dimensions(_alloc)
, cell_size(0.0)
, OOB_occupancy_value(0.0)
, OOB_component_value(0)
, number_of_components(0)
, components_valid(false)
, initialized(false)
, data(_alloc) {
(void)_alloc;
}
typedef ::std_msgs::Header_<ContainerAllocator> _header_type;
_header_type header;
typedef ::geometry_msgs::Transform_<ContainerAllocator> _origin_transform_type;
_origin_transform_type origin_transform;
typedef ::geometry_msgs::Vector3_<ContainerAllocator> _dimensions_type;
_dimensions_type dimensions;
typedef double _cell_size_type;
_cell_size_type cell_size;
typedef float _OOB_occupancy_value_type;
_OOB_occupancy_value_type OOB_occupancy_value;
typedef uint32_t _OOB_component_value_type;
_OOB_component_value_type OOB_component_value;
typedef uint32_t _number_of_components_type;
_number_of_components_type number_of_components;
typedef uint8_t _components_valid_type;
_components_valid_type components_valid;
typedef uint8_t _initialized_type;
_initialized_type initialized;
typedef std::vector<uint8_t, typename ContainerAllocator::template rebind<uint8_t>::other > _data_type;
_data_type data;
typedef boost::shared_ptr< ::sdf_tools::CollisionMap_<ContainerAllocator> > Ptr;
typedef boost::shared_ptr< ::sdf_tools::CollisionMap_<ContainerAllocator> const> ConstPtr;
}; // struct CollisionMap_
typedef ::sdf_tools::CollisionMap_<std::allocator<void> > CollisionMap;
typedef boost::shared_ptr< ::sdf_tools::CollisionMap > CollisionMapPtr;
typedef boost::shared_ptr< ::sdf_tools::CollisionMap const> CollisionMapConstPtr;
// constants requiring out of line definition
template<typename ContainerAllocator>
std::ostream& operator<<(std::ostream& s, const ::sdf_tools::CollisionMap_<ContainerAllocator> & v)
{
ros::message_operations::Printer< ::sdf_tools::CollisionMap_<ContainerAllocator> >::stream(s, "", v);
return s;
}
} // namespace sdf_tools
namespace ros
{
namespace message_traits
{
// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': True}
// {'std_msgs': ['/opt/ros/indigo/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/opt/ros/indigo/share/geometry_msgs/cmake/../msg'], 'sdf_tools': ['/home/zby/workspaces/temp_ws/src/sdf_tools/msg']}
// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types']
template <class ContainerAllocator>
struct IsFixedSize< ::sdf_tools::CollisionMap_<ContainerAllocator> >
: FalseType
{ };
template <class ContainerAllocator>
struct IsFixedSize< ::sdf_tools::CollisionMap_<ContainerAllocator> const>
: FalseType
{ };
template <class ContainerAllocator>
struct IsMessage< ::sdf_tools::CollisionMap_<ContainerAllocator> >
: TrueType
{ };
template <class ContainerAllocator>
struct IsMessage< ::sdf_tools::CollisionMap_<ContainerAllocator> const>
: TrueType
{ };
template <class ContainerAllocator>
struct HasHeader< ::sdf_tools::CollisionMap_<ContainerAllocator> >
: TrueType
{ };
template <class ContainerAllocator>
struct HasHeader< ::sdf_tools::CollisionMap_<ContainerAllocator> const>
: TrueType
{ };
template<class ContainerAllocator>
struct MD5Sum< ::sdf_tools::CollisionMap_<ContainerAllocator> >
{
static const char* value()
{
return "69b7e5097be57c5900575a10000bd373";
}
static const char* value(const ::sdf_tools::CollisionMap_<ContainerAllocator>&) { return value(); }
static const uint64_t static_value1 = 0x69b7e5097be57c59ULL;
static const uint64_t static_value2 = 0x00575a10000bd373ULL;
};
template<class ContainerAllocator>
struct DataType< ::sdf_tools::CollisionMap_<ContainerAllocator> >
{
static const char* value()
{
return "sdf_tools/CollisionMap";
}
static const char* value(const ::sdf_tools::CollisionMap_<ContainerAllocator>&) { return value(); }
};
template<class ContainerAllocator>
struct Definition< ::sdf_tools::CollisionMap_<ContainerAllocator> >
{
static const char* value()
{
return "std_msgs/Header header\n\
geometry_msgs/Transform origin_transform\n\
geometry_msgs/Vector3 dimensions\n\
float64 cell_size\n\
float32 OOB_occupancy_value\n\
uint32 OOB_component_value\n\
uint32 number_of_components\n\
bool components_valid\n\
bool initialized\n\
uint8[] data\n\
\n\
================================================================================\n\
MSG: std_msgs/Header\n\
# Standard metadata for higher-level stamped data types.\n\
# This is generally used to communicate timestamped data \n\
# in a particular coordinate frame.\n\
# \n\
# sequence ID: consecutively increasing ID \n\
uint32 seq\n\
#Two-integer timestamp that is expressed as:\n\
# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n\
# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n\
# time-handling sugar is provided by the client library\n\
time stamp\n\
#Frame this data is associated with\n\
# 0: no frame\n\
# 1: global frame\n\
string frame_id\n\
\n\
================================================================================\n\
MSG: geometry_msgs/Transform\n\
# This represents the transform between two coordinate frames in free space.\n\
\n\
Vector3 translation\n\
Quaternion rotation\n\
\n\
================================================================================\n\
MSG: geometry_msgs/Vector3\n\
# This represents a vector in free space. \n\
# It is only meant to represent a direction. Therefore, it does not\n\
# make sense to apply a translation to it (e.g., when applying a \n\
# generic rigid transformation to a Vector3, tf2 will only apply the\n\
# rotation). If you want your data to be translatable too, use the\n\
# geometry_msgs/Point message instead.\n\
\n\
float64 x\n\
float64 y\n\
float64 z\n\
================================================================================\n\
MSG: geometry_msgs/Quaternion\n\
# This represents an orientation in free space in quaternion form.\n\
\n\
float64 x\n\
float64 y\n\
float64 z\n\
float64 w\n\
";
}
static const char* value(const ::sdf_tools::CollisionMap_<ContainerAllocator>&) { return value(); }
};
} // namespace message_traits
} // namespace ros
namespace ros
{
namespace serialization
{
template<class ContainerAllocator> struct Serializer< ::sdf_tools::CollisionMap_<ContainerAllocator> >
{
template<typename Stream, typename T> inline static void allInOne(Stream& stream, T m)
{
stream.next(m.header);
stream.next(m.origin_transform);
stream.next(m.dimensions);
stream.next(m.cell_size);
stream.next(m.OOB_occupancy_value);
stream.next(m.OOB_component_value);
stream.next(m.number_of_components);
stream.next(m.components_valid);
stream.next(m.initialized);
stream.next(m.data);
}
ROS_DECLARE_ALLINONE_SERIALIZER
}; // struct CollisionMap_
} // namespace serialization
} // namespace ros
namespace ros
{
namespace message_operations
{
template<class ContainerAllocator>
struct Printer< ::sdf_tools::CollisionMap_<ContainerAllocator> >
{
template<typename Stream> static void stream(Stream& s, const std::string& indent, const ::sdf_tools::CollisionMap_<ContainerAllocator>& v)
{
s << indent << "header: ";
s << std::endl;
Printer< ::std_msgs::Header_<ContainerAllocator> >::stream(s, indent + " ", v.header);
s << indent << "origin_transform: ";
s << std::endl;
Printer< ::geometry_msgs::Transform_<ContainerAllocator> >::stream(s, indent + " ", v.origin_transform);
s << indent << "dimensions: ";
s << std::endl;
Printer< ::geometry_msgs::Vector3_<ContainerAllocator> >::stream(s, indent + " ", v.dimensions);
s << indent << "cell_size: ";
Printer<double>::stream(s, indent + " ", v.cell_size);
s << indent << "OOB_occupancy_value: ";
Printer<float>::stream(s, indent + " ", v.OOB_occupancy_value);
s << indent << "OOB_component_value: ";
Printer<uint32_t>::stream(s, indent + " ", v.OOB_component_value);
s << indent << "number_of_components: ";
Printer<uint32_t>::stream(s, indent + " ", v.number_of_components);
s << indent << "components_valid: ";
Printer<uint8_t>::stream(s, indent + " ", v.components_valid);
s << indent << "initialized: ";
Printer<uint8_t>::stream(s, indent + " ", v.initialized);
s << indent << "data[]" << std::endl;
for (size_t i = 0; i < v.data.size(); ++i)
{
s << indent << " data[" << i << "]: ";
Printer<uint8_t>::stream(s, indent + " ", v.data[i]);
}
}
};
} // namespace message_operations
} // namespace ros
#endif // SDF_TOOLS_MESSAGE_COLLISIONMAP_H

123
flightlib/third_party/sdf_tools/include/sdf_tools/ComputeSDF.h vendored Normal file
View File

@@ -0,0 +1,123 @@
// Generated by gencpp from file sdf_tools/ComputeSDF.msg
// DO NOT EDIT!
#ifndef SDF_TOOLS_MESSAGE_COMPUTESDF_H
#define SDF_TOOLS_MESSAGE_COMPUTESDF_H
#include <ros/service_traits.h>
#include <sdf_tools/ComputeSDFRequest.h>
#include <sdf_tools/ComputeSDFResponse.h>
namespace sdf_tools
{
struct ComputeSDF
{
typedef ComputeSDFRequest Request;
typedef ComputeSDFResponse Response;
Request request;
Response response;
typedef Request RequestType;
typedef Response ResponseType;
}; // struct ComputeSDF
} // namespace sdf_tools
namespace ros
{
namespace service_traits
{
template<>
struct MD5Sum< ::sdf_tools::ComputeSDF > {
static const char* value()
{
return "567be5b04fd66c34e03a51193aff2d4a";
}
static const char* value(const ::sdf_tools::ComputeSDF&) { return value(); }
};
template<>
struct DataType< ::sdf_tools::ComputeSDF > {
static const char* value()
{
return "sdf_tools/ComputeSDF";
}
static const char* value(const ::sdf_tools::ComputeSDF&) { return value(); }
};
// service_traits::MD5Sum< ::sdf_tools::ComputeSDFRequest> should match
// service_traits::MD5Sum< ::sdf_tools::ComputeSDF >
template<>
struct MD5Sum< ::sdf_tools::ComputeSDFRequest>
{
static const char* value()
{
return MD5Sum< ::sdf_tools::ComputeSDF >::value();
}
static const char* value(const ::sdf_tools::ComputeSDFRequest&)
{
return value();
}
};
// service_traits::DataType< ::sdf_tools::ComputeSDFRequest> should match
// service_traits::DataType< ::sdf_tools::ComputeSDF >
template<>
struct DataType< ::sdf_tools::ComputeSDFRequest>
{
static const char* value()
{
return DataType< ::sdf_tools::ComputeSDF >::value();
}
static const char* value(const ::sdf_tools::ComputeSDFRequest&)
{
return value();
}
};
// service_traits::MD5Sum< ::sdf_tools::ComputeSDFResponse> should match
// service_traits::MD5Sum< ::sdf_tools::ComputeSDF >
template<>
struct MD5Sum< ::sdf_tools::ComputeSDFResponse>
{
static const char* value()
{
return MD5Sum< ::sdf_tools::ComputeSDF >::value();
}
static const char* value(const ::sdf_tools::ComputeSDFResponse&)
{
return value();
}
};
// service_traits::DataType< ::sdf_tools::ComputeSDFResponse> should match
// service_traits::DataType< ::sdf_tools::ComputeSDF >
template<>
struct DataType< ::sdf_tools::ComputeSDFResponse>
{
static const char* value()
{
return DataType< ::sdf_tools::ComputeSDF >::value();
}
static const char* value(const ::sdf_tools::ComputeSDFResponse&)
{
return value();
}
};
} // namespace service_traits
} // namespace ros
#endif // SDF_TOOLS_MESSAGE_COMPUTESDF_H

186
flightlib/third_party/sdf_tools/include/sdf_tools/ComputeSDFRequest.h vendored Normal file
View File

@@ -0,0 +1,186 @@
// Generated by gencpp from file sdf_tools/ComputeSDFRequest.msg
// DO NOT EDIT!
#ifndef SDF_TOOLS_MESSAGE_COMPUTESDFREQUEST_H
#define SDF_TOOLS_MESSAGE_COMPUTESDFREQUEST_H
#include <string>
#include <vector>
#include <map>
#include <ros/types.h>
#include <ros/serialization.h>
#include <ros/builtin_message_traits.h>
#include <ros/message_operations.h>
namespace sdf_tools
{
template <class ContainerAllocator>
struct ComputeSDFRequest_
{
typedef ComputeSDFRequest_<ContainerAllocator> Type;
ComputeSDFRequest_()
: request_new(false) {
}
ComputeSDFRequest_(const ContainerAllocator& _alloc)
: request_new(false) {
(void)_alloc;
}
typedef uint8_t _request_new_type;
_request_new_type request_new;
typedef boost::shared_ptr< ::sdf_tools::ComputeSDFRequest_<ContainerAllocator> > Ptr;
typedef boost::shared_ptr< ::sdf_tools::ComputeSDFRequest_<ContainerAllocator> const> ConstPtr;
}; // struct ComputeSDFRequest_
typedef ::sdf_tools::ComputeSDFRequest_<std::allocator<void> > ComputeSDFRequest;
typedef boost::shared_ptr< ::sdf_tools::ComputeSDFRequest > ComputeSDFRequestPtr;
typedef boost::shared_ptr< ::sdf_tools::ComputeSDFRequest const> ComputeSDFRequestConstPtr;
// constants requiring out of line definition
template<typename ContainerAllocator>
std::ostream& operator<<(std::ostream& s, const ::sdf_tools::ComputeSDFRequest_<ContainerAllocator> & v)
{
ros::message_operations::Printer< ::sdf_tools::ComputeSDFRequest_<ContainerAllocator> >::stream(s, "", v);
return s;
}
} // namespace sdf_tools
namespace ros
{
namespace message_traits
{
// BOOLTRAITS {'IsFixedSize': True, 'IsMessage': True, 'HasHeader': False}
// {'std_msgs': ['/opt/ros/indigo/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/opt/ros/indigo/share/geometry_msgs/cmake/../msg'], 'sdf_tools': ['/home/zby/workspaces/temp_ws/src/sdf_tools/msg']}
// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types']
template <class ContainerAllocator>
struct IsFixedSize< ::sdf_tools::ComputeSDFRequest_<ContainerAllocator> >
: TrueType
{ };
template <class ContainerAllocator>
struct IsFixedSize< ::sdf_tools::ComputeSDFRequest_<ContainerAllocator> const>
: TrueType
{ };
template <class ContainerAllocator>
struct IsMessage< ::sdf_tools::ComputeSDFRequest_<ContainerAllocator> >
: TrueType
{ };
template <class ContainerAllocator>
struct IsMessage< ::sdf_tools::ComputeSDFRequest_<ContainerAllocator> const>
: TrueType
{ };
template <class ContainerAllocator>
struct HasHeader< ::sdf_tools::ComputeSDFRequest_<ContainerAllocator> >
: FalseType
{ };
template <class ContainerAllocator>
struct HasHeader< ::sdf_tools::ComputeSDFRequest_<ContainerAllocator> const>
: FalseType
{ };
template<class ContainerAllocator>
struct MD5Sum< ::sdf_tools::ComputeSDFRequest_<ContainerAllocator> >
{
static const char* value()
{
return "70082b15c0185876dcce41c4eb98be14";
}
static const char* value(const ::sdf_tools::ComputeSDFRequest_<ContainerAllocator>&) { return value(); }
static const uint64_t static_value1 = 0x70082b15c0185876ULL;
static const uint64_t static_value2 = 0xdcce41c4eb98be14ULL;
};
template<class ContainerAllocator>
struct DataType< ::sdf_tools::ComputeSDFRequest_<ContainerAllocator> >
{
static const char* value()
{
return "sdf_tools/ComputeSDFRequest";
}
static const char* value(const ::sdf_tools::ComputeSDFRequest_<ContainerAllocator>&) { return value(); }
};
template<class ContainerAllocator>
struct Definition< ::sdf_tools::ComputeSDFRequest_<ContainerAllocator> >
{
static const char* value()
{
return "bool request_new\n\
";
}
static const char* value(const ::sdf_tools::ComputeSDFRequest_<ContainerAllocator>&) { return value(); }
};
} // namespace message_traits
} // namespace ros
namespace ros
{
namespace serialization
{
template<class ContainerAllocator> struct Serializer< ::sdf_tools::ComputeSDFRequest_<ContainerAllocator> >
{
template<typename Stream, typename T> inline static void allInOne(Stream& stream, T m)
{
stream.next(m.request_new);
}
ROS_DECLARE_ALLINONE_SERIALIZER
}; // struct ComputeSDFRequest_
} // namespace serialization
} // namespace ros
namespace ros
{
namespace message_operations
{
template<class ContainerAllocator>
struct Printer< ::sdf_tools::ComputeSDFRequest_<ContainerAllocator> >
{
template<typename Stream> static void stream(Stream& s, const std::string& indent, const ::sdf_tools::ComputeSDFRequest_<ContainerAllocator>& v)
{
s << indent << "request_new: ";
Printer<uint8_t>::stream(s, indent + " ", v.request_new);
}
};
} // namespace message_operations
} // namespace ros
#endif // SDF_TOOLS_MESSAGE_COMPUTESDFREQUEST_H

255
flightlib/third_party/sdf_tools/include/sdf_tools/ComputeSDFResponse.h vendored Normal file
View File

@@ -0,0 +1,255 @@
// Generated by gencpp from file sdf_tools/ComputeSDFResponse.msg
// DO NOT EDIT!
#ifndef SDF_TOOLS_MESSAGE_COMPUTESDFRESPONSE_H
#define SDF_TOOLS_MESSAGE_COMPUTESDFRESPONSE_H
#include <string>
#include <vector>
#include <map>
#include <ros/types.h>
#include <ros/serialization.h>
#include <ros/builtin_message_traits.h>
#include <ros/message_operations.h>
#include <sdf_tools/SDF.h>
namespace sdf_tools
{
template <class ContainerAllocator>
struct ComputeSDFResponse_
{
typedef ComputeSDFResponse_<ContainerAllocator> Type;
ComputeSDFResponse_()
: is_valid(false)
, sdf() {
}
ComputeSDFResponse_(const ContainerAllocator& _alloc)
: is_valid(false)
, sdf(_alloc) {
(void)_alloc;
}
typedef uint8_t _is_valid_type;
_is_valid_type is_valid;
typedef ::sdf_tools::SDF_<ContainerAllocator> _sdf_type;
_sdf_type sdf;
typedef boost::shared_ptr< ::sdf_tools::ComputeSDFResponse_<ContainerAllocator> > Ptr;
typedef boost::shared_ptr< ::sdf_tools::ComputeSDFResponse_<ContainerAllocator> const> ConstPtr;
}; // struct ComputeSDFResponse_
typedef ::sdf_tools::ComputeSDFResponse_<std::allocator<void> > ComputeSDFResponse;
typedef boost::shared_ptr< ::sdf_tools::ComputeSDFResponse > ComputeSDFResponsePtr;
typedef boost::shared_ptr< ::sdf_tools::ComputeSDFResponse const> ComputeSDFResponseConstPtr;
// constants requiring out of line definition
template<typename ContainerAllocator>
std::ostream& operator<<(std::ostream& s, const ::sdf_tools::ComputeSDFResponse_<ContainerAllocator> & v)
{
ros::message_operations::Printer< ::sdf_tools::ComputeSDFResponse_<ContainerAllocator> >::stream(s, "", v);
return s;
}
} // namespace sdf_tools
namespace ros
{
namespace message_traits
{
// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': False}
// {'std_msgs': ['/opt/ros/indigo/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/opt/ros/indigo/share/geometry_msgs/cmake/../msg'], 'sdf_tools': ['/home/zby/workspaces/temp_ws/src/sdf_tools/msg']}
// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types']
template <class ContainerAllocator>
struct IsFixedSize< ::sdf_tools::ComputeSDFResponse_<ContainerAllocator> >
: FalseType
{ };
template <class ContainerAllocator>
struct IsFixedSize< ::sdf_tools::ComputeSDFResponse_<ContainerAllocator> const>
: FalseType
{ };
template <class ContainerAllocator>
struct IsMessage< ::sdf_tools::ComputeSDFResponse_<ContainerAllocator> >
: TrueType
{ };
template <class ContainerAllocator>
struct IsMessage< ::sdf_tools::ComputeSDFResponse_<ContainerAllocator> const>
: TrueType
{ };
template <class ContainerAllocator>
struct HasHeader< ::sdf_tools::ComputeSDFResponse_<ContainerAllocator> >
: FalseType
{ };
template <class ContainerAllocator>
struct HasHeader< ::sdf_tools::ComputeSDFResponse_<ContainerAllocator> const>
: FalseType
{ };
template<class ContainerAllocator>
struct MD5Sum< ::sdf_tools::ComputeSDFResponse_<ContainerAllocator> >
{
static const char* value()
{
return "fbf70ecbf2634799341a7255b0c416e3";
}
static const char* value(const ::sdf_tools::ComputeSDFResponse_<ContainerAllocator>&) { return value(); }
static const uint64_t static_value1 = 0xfbf70ecbf2634799ULL;
static const uint64_t static_value2 = 0x341a7255b0c416e3ULL;
};
template<class ContainerAllocator>
struct DataType< ::sdf_tools::ComputeSDFResponse_<ContainerAllocator> >
{
static const char* value()
{
return "sdf_tools/ComputeSDFResponse";
}
static const char* value(const ::sdf_tools::ComputeSDFResponse_<ContainerAllocator>&) { return value(); }
};
template<class ContainerAllocator>
struct Definition< ::sdf_tools::ComputeSDFResponse_<ContainerAllocator> >
{
static const char* value()
{
return "bool is_valid\n\
sdf_tools/SDF sdf\n\
\n\
\n\
================================================================================\n\
MSG: sdf_tools/SDF\n\
std_msgs/Header header\n\
geometry_msgs/Transform origin_transform\n\
geometry_msgs/Vector3 dimensions\n\
float64 sdf_cell_size\n\
float32 OOB_value\n\
bool initialized\n\
bool locked\n\
uint8[] data\n\
\n\
================================================================================\n\
MSG: std_msgs/Header\n\
# Standard metadata for higher-level stamped data types.\n\
# This is generally used to communicate timestamped data \n\
# in a particular coordinate frame.\n\
# \n\
# sequence ID: consecutively increasing ID \n\
uint32 seq\n\
#Two-integer timestamp that is expressed as:\n\
# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n\
# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n\
# time-handling sugar is provided by the client library\n\
time stamp\n\
#Frame this data is associated with\n\
# 0: no frame\n\
# 1: global frame\n\
string frame_id\n\
\n\
================================================================================\n\
MSG: geometry_msgs/Transform\n\
# This represents the transform between two coordinate frames in free space.\n\
\n\
Vector3 translation\n\
Quaternion rotation\n\
\n\
================================================================================\n\
MSG: geometry_msgs/Vector3\n\
# This represents a vector in free space. \n\
# It is only meant to represent a direction. Therefore, it does not\n\
# make sense to apply a translation to it (e.g., when applying a \n\
# generic rigid transformation to a Vector3, tf2 will only apply the\n\
# rotation). If you want your data to be translatable too, use the\n\
# geometry_msgs/Point message instead.\n\
\n\
float64 x\n\
float64 y\n\
float64 z\n\
================================================================================\n\
MSG: geometry_msgs/Quaternion\n\
# This represents an orientation in free space in quaternion form.\n\
\n\
float64 x\n\
float64 y\n\
float64 z\n\
float64 w\n\
";
}
static const char* value(const ::sdf_tools::ComputeSDFResponse_<ContainerAllocator>&) { return value(); }
};
} // namespace message_traits
} // namespace ros
namespace ros
{
namespace serialization
{
template<class ContainerAllocator> struct Serializer< ::sdf_tools::ComputeSDFResponse_<ContainerAllocator> >
{
template<typename Stream, typename T> inline static void allInOne(Stream& stream, T m)
{
stream.next(m.is_valid);
stream.next(m.sdf);
}
ROS_DECLARE_ALLINONE_SERIALIZER
}; // struct ComputeSDFResponse_
} // namespace serialization
} // namespace ros
namespace ros
{
namespace message_operations
{
template<class ContainerAllocator>
struct Printer< ::sdf_tools::ComputeSDFResponse_<ContainerAllocator> >
{
template<typename Stream> static void stream(Stream& s, const std::string& indent, const ::sdf_tools::ComputeSDFResponse_<ContainerAllocator>& v)
{
s << indent << "is_valid: ";
Printer<uint8_t>::stream(s, indent + " ", v.is_valid);
s << indent << "sdf: ";
s << std::endl;
Printer< ::sdf_tools::SDF_<ContainerAllocator> >::stream(s, indent + " ", v.sdf);
}
};
} // namespace message_operations
} // namespace ros
#endif // SDF_TOOLS_MESSAGE_COMPUTESDFRESPONSE_H

305
flightlib/third_party/sdf_tools/include/sdf_tools/SDF.h vendored Normal file
View File

@@ -0,0 +1,305 @@
// Generated by gencpp from file sdf_tools/SDF.msg
// DO NOT EDIT!
#ifndef SDF_TOOLS_MESSAGE_SDF_H
#define SDF_TOOLS_MESSAGE_SDF_H
#include <string>
#include <vector>
#include <map>
#include <ros/types.h>
#include <ros/serialization.h>
#include <ros/builtin_message_traits.h>
#include <ros/message_operations.h>
#include <std_msgs/Header.h>
#include <geometry_msgs/Transform.h>
#include <geometry_msgs/Vector3.h>
namespace sdf_tools
{
template <class ContainerAllocator>
struct SDF_
{
typedef SDF_<ContainerAllocator> Type;
SDF_()
: header()
, origin_transform()
, dimensions()
, sdf_cell_size(0.0)
, OOB_value(0.0)
, initialized(false)
, locked(false)
, data() {
}
SDF_(const ContainerAllocator& _alloc)
: header(_alloc)
, origin_transform(_alloc)
, dimensions(_alloc)
, sdf_cell_size(0.0)
, OOB_value(0.0)
, initialized(false)
, locked(false)
, data(_alloc) {
(void)_alloc;
}
typedef ::std_msgs::Header_<ContainerAllocator> _header_type;
_header_type header;
typedef ::geometry_msgs::Transform_<ContainerAllocator> _origin_transform_type;
_origin_transform_type origin_transform;
typedef ::geometry_msgs::Vector3_<ContainerAllocator> _dimensions_type;
_dimensions_type dimensions;
typedef double _sdf_cell_size_type;
_sdf_cell_size_type sdf_cell_size;
typedef float _OOB_value_type;
_OOB_value_type OOB_value;
typedef uint8_t _initialized_type;
_initialized_type initialized;
typedef uint8_t _locked_type;
_locked_type locked;
typedef std::vector<uint8_t, typename ContainerAllocator::template rebind<uint8_t>::other > _data_type;
_data_type data;
typedef boost::shared_ptr< ::sdf_tools::SDF_<ContainerAllocator> > Ptr;
typedef boost::shared_ptr< ::sdf_tools::SDF_<ContainerAllocator> const> ConstPtr;
}; // struct SDF_
typedef ::sdf_tools::SDF_<std::allocator<void> > SDF;
typedef boost::shared_ptr< ::sdf_tools::SDF > SDFPtr;
typedef boost::shared_ptr< ::sdf_tools::SDF const> SDFConstPtr;
// constants requiring out of line definition
template<typename ContainerAllocator>
std::ostream& operator<<(std::ostream& s, const ::sdf_tools::SDF_<ContainerAllocator> & v)
{
ros::message_operations::Printer< ::sdf_tools::SDF_<ContainerAllocator> >::stream(s, "", v);
return s;
}
} // namespace sdf_tools
namespace ros
{
namespace message_traits
{
// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': True}
// {'std_msgs': ['/opt/ros/indigo/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/opt/ros/indigo/share/geometry_msgs/cmake/../msg'], 'sdf_tools': ['/home/zby/workspaces/temp_ws/src/sdf_tools/msg']}
// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types']
template <class ContainerAllocator>
struct IsFixedSize< ::sdf_tools::SDF_<ContainerAllocator> >
: FalseType
{ };
template <class ContainerAllocator>
struct IsFixedSize< ::sdf_tools::SDF_<ContainerAllocator> const>
: FalseType
{ };
template <class ContainerAllocator>
struct IsMessage< ::sdf_tools::SDF_<ContainerAllocator> >
: TrueType
{ };
template <class ContainerAllocator>
struct IsMessage< ::sdf_tools::SDF_<ContainerAllocator> const>
: TrueType
{ };
template <class ContainerAllocator>
struct HasHeader< ::sdf_tools::SDF_<ContainerAllocator> >
: TrueType
{ };
template <class ContainerAllocator>
struct HasHeader< ::sdf_tools::SDF_<ContainerAllocator> const>
: TrueType
{ };
template<class ContainerAllocator>
struct MD5Sum< ::sdf_tools::SDF_<ContainerAllocator> >
{
static const char* value()
{
return "a7c1a3fc5ebce39f4d97049e22031ffc";
}
static const char* value(const ::sdf_tools::SDF_<ContainerAllocator>&) { return value(); }
static const uint64_t static_value1 = 0xa7c1a3fc5ebce39fULL;
static const uint64_t static_value2 = 0x4d97049e22031ffcULL;
};
template<class ContainerAllocator>
struct DataType< ::sdf_tools::SDF_<ContainerAllocator> >
{
static const char* value()
{
return "sdf_tools/SDF";
}
static const char* value(const ::sdf_tools::SDF_<ContainerAllocator>&) { return value(); }
};
template<class ContainerAllocator>
struct Definition< ::sdf_tools::SDF_<ContainerAllocator> >
{
static const char* value()
{
return "std_msgs/Header header\n\
geometry_msgs/Transform origin_transform\n\
geometry_msgs/Vector3 dimensions\n\
float64 sdf_cell_size\n\
float32 OOB_value\n\
bool initialized\n\
bool locked\n\
uint8[] data\n\
\n\
================================================================================\n\
MSG: std_msgs/Header\n\
# Standard metadata for higher-level stamped data types.\n\
# This is generally used to communicate timestamped data \n\
# in a particular coordinate frame.\n\
# \n\
# sequence ID: consecutively increasing ID \n\
uint32 seq\n\
#Two-integer timestamp that is expressed as:\n\
# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n\
# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n\
# time-handling sugar is provided by the client library\n\
time stamp\n\
#Frame this data is associated with\n\
# 0: no frame\n\
# 1: global frame\n\
string frame_id\n\
\n\
================================================================================\n\
MSG: geometry_msgs/Transform\n\
# This represents the transform between two coordinate frames in free space.\n\
\n\
Vector3 translation\n\
Quaternion rotation\n\
\n\
================================================================================\n\
MSG: geometry_msgs/Vector3\n\
# This represents a vector in free space. \n\
# It is only meant to represent a direction. Therefore, it does not\n\
# make sense to apply a translation to it (e.g., when applying a \n\
# generic rigid transformation to a Vector3, tf2 will only apply the\n\
# rotation). If you want your data to be translatable too, use the\n\
# geometry_msgs/Point message instead.\n\
\n\
float64 x\n\
float64 y\n\
float64 z\n\
================================================================================\n\
MSG: geometry_msgs/Quaternion\n\
# This represents an orientation in free space in quaternion form.\n\
\n\
float64 x\n\
float64 y\n\
float64 z\n\
float64 w\n\
";
}
static const char* value(const ::sdf_tools::SDF_<ContainerAllocator>&) { return value(); }
};
} // namespace message_traits
} // namespace ros
namespace ros
{
namespace serialization
{
template<class ContainerAllocator> struct Serializer< ::sdf_tools::SDF_<ContainerAllocator> >
{
template<typename Stream, typename T> inline static void allInOne(Stream& stream, T m)
{
stream.next(m.header);
stream.next(m.origin_transform);
stream.next(m.dimensions);
stream.next(m.sdf_cell_size);
stream.next(m.OOB_value);
stream.next(m.initialized);
stream.next(m.locked);
stream.next(m.data);
}
ROS_DECLARE_ALLINONE_SERIALIZER
}; // struct SDF_
} // namespace serialization
} // namespace ros
namespace ros
{
namespace message_operations
{
template<class ContainerAllocator>
struct Printer< ::sdf_tools::SDF_<ContainerAllocator> >
{
template<typename Stream> static void stream(Stream& s, const std::string& indent, const ::sdf_tools::SDF_<ContainerAllocator>& v)
{
s << indent << "header: ";
s << std::endl;
Printer< ::std_msgs::Header_<ContainerAllocator> >::stream(s, indent + " ", v.header);
s << indent << "origin_transform: ";
s << std::endl;
Printer< ::geometry_msgs::Transform_<ContainerAllocator> >::stream(s, indent + " ", v.origin_transform);
s << indent << "dimensions: ";
s << std::endl;
Printer< ::geometry_msgs::Vector3_<ContainerAllocator> >::stream(s, indent + " ", v.dimensions);
s << indent << "sdf_cell_size: ";
Printer<double>::stream(s, indent + " ", v.sdf_cell_size);
s << indent << "OOB_value: ";
Printer<float>::stream(s, indent + " ", v.OOB_value);
s << indent << "initialized: ";
Printer<uint8_t>::stream(s, indent + " ", v.initialized);
s << indent << "locked: ";
Printer<uint8_t>::stream(s, indent + " ", v.locked);
s << indent << "data[]" << std::endl;
for (size_t i = 0; i < v.data.size(); ++i)
{
s << indent << " data[" << i << "]: ";
Printer<uint8_t>::stream(s, indent + " ", v.data[i]);
}
}
};
} // namespace message_operations
} // namespace ros
#endif // SDF_TOOLS_MESSAGE_SDF_H

327
flightlib/third_party/sdf_tools/include/sdf_tools/TaggedObjectCollisionMap.h vendored Normal file
View File

@@ -0,0 +1,327 @@
// Generated by gencpp from file sdf_tools/TaggedObjectCollisionMap.msg
// DO NOT EDIT!
#ifndef SDF_TOOLS_MESSAGE_TAGGEDOBJECTCOLLISIONMAP_H
#define SDF_TOOLS_MESSAGE_TAGGEDOBJECTCOLLISIONMAP_H
#include <string>
#include <vector>
#include <map>
#include <ros/types.h>
#include <ros/serialization.h>
#include <ros/builtin_message_traits.h>
#include <ros/message_operations.h>
#include <std_msgs/Header.h>
#include <geometry_msgs/Transform.h>
#include <geometry_msgs/Vector3.h>
namespace sdf_tools
{
template <class ContainerAllocator>
struct TaggedObjectCollisionMap_
{
typedef TaggedObjectCollisionMap_<ContainerAllocator> Type;
TaggedObjectCollisionMap_()
: header()
, origin_transform()
, dimensions()
, cell_size(0.0)
, number_of_components(0)
, components_valid(false)
, convex_segments_valid(false)
, initialized(false)
, OOB_value()
, data() {
}
TaggedObjectCollisionMap_(const ContainerAllocator& _alloc)
: header(_alloc)
, origin_transform(_alloc)
, dimensions(_alloc)
, cell_size(0.0)
, number_of_components(0)
, components_valid(false)
, convex_segments_valid(false)
, initialized(false)
, OOB_value(_alloc)
, data(_alloc) {
(void)_alloc;
}
typedef ::std_msgs::Header_<ContainerAllocator> _header_type;
_header_type header;
typedef ::geometry_msgs::Transform_<ContainerAllocator> _origin_transform_type;
_origin_transform_type origin_transform;
typedef ::geometry_msgs::Vector3_<ContainerAllocator> _dimensions_type;
_dimensions_type dimensions;
typedef double _cell_size_type;
_cell_size_type cell_size;
typedef uint32_t _number_of_components_type;
_number_of_components_type number_of_components;
typedef uint8_t _components_valid_type;
_components_valid_type components_valid;
typedef uint8_t _convex_segments_valid_type;
_convex_segments_valid_type convex_segments_valid;
typedef uint8_t _initialized_type;
_initialized_type initialized;
typedef std::vector<uint8_t, typename ContainerAllocator::template rebind<uint8_t>::other > _OOB_value_type;
_OOB_value_type OOB_value;
typedef std::vector<uint8_t, typename ContainerAllocator::template rebind<uint8_t>::other > _data_type;
_data_type data;
typedef boost::shared_ptr< ::sdf_tools::TaggedObjectCollisionMap_<ContainerAllocator> > Ptr;
typedef boost::shared_ptr< ::sdf_tools::TaggedObjectCollisionMap_<ContainerAllocator> const> ConstPtr;
}; // struct TaggedObjectCollisionMap_
typedef ::sdf_tools::TaggedObjectCollisionMap_<std::allocator<void> > TaggedObjectCollisionMap;
typedef boost::shared_ptr< ::sdf_tools::TaggedObjectCollisionMap > TaggedObjectCollisionMapPtr;
typedef boost::shared_ptr< ::sdf_tools::TaggedObjectCollisionMap const> TaggedObjectCollisionMapConstPtr;
// constants requiring out of line definition
template<typename ContainerAllocator>
std::ostream& operator<<(std::ostream& s, const ::sdf_tools::TaggedObjectCollisionMap_<ContainerAllocator> & v)
{
ros::message_operations::Printer< ::sdf_tools::TaggedObjectCollisionMap_<ContainerAllocator> >::stream(s, "", v);
return s;
}
} // namespace sdf_tools
namespace ros
{
namespace message_traits
{
// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': True}
// {'std_msgs': ['/opt/ros/indigo/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/opt/ros/indigo/share/geometry_msgs/cmake/../msg'], 'sdf_tools': ['/home/zby/workspaces/temp_ws/src/sdf_tools/msg']}
// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types']
template <class ContainerAllocator>
struct IsFixedSize< ::sdf_tools::TaggedObjectCollisionMap_<ContainerAllocator> >
: FalseType
{ };
template <class ContainerAllocator>
struct IsFixedSize< ::sdf_tools::TaggedObjectCollisionMap_<ContainerAllocator> const>
: FalseType
{ };
template <class ContainerAllocator>
struct IsMessage< ::sdf_tools::TaggedObjectCollisionMap_<ContainerAllocator> >
: TrueType
{ };
template <class ContainerAllocator>
struct IsMessage< ::sdf_tools::TaggedObjectCollisionMap_<ContainerAllocator> const>
: TrueType
{ };
template <class ContainerAllocator>
struct HasHeader< ::sdf_tools::TaggedObjectCollisionMap_<ContainerAllocator> >
: TrueType
{ };
template <class ContainerAllocator>
struct HasHeader< ::sdf_tools::TaggedObjectCollisionMap_<ContainerAllocator> const>
: TrueType
{ };
template<class ContainerAllocator>
struct MD5Sum< ::sdf_tools::TaggedObjectCollisionMap_<ContainerAllocator> >
{
static const char* value()
{
return "320371317f699b0048968a467deb0a13";
}
static const char* value(const ::sdf_tools::TaggedObjectCollisionMap_<ContainerAllocator>&) { return value(); }
static const uint64_t static_value1 = 0x320371317f699b00ULL;
static const uint64_t static_value2 = 0x48968a467deb0a13ULL;
};
template<class ContainerAllocator>
struct DataType< ::sdf_tools::TaggedObjectCollisionMap_<ContainerAllocator> >
{
static const char* value()
{
return "sdf_tools/TaggedObjectCollisionMap";
}
static const char* value(const ::sdf_tools::TaggedObjectCollisionMap_<ContainerAllocator>&) { return value(); }
};
template<class ContainerAllocator>
struct Definition< ::sdf_tools::TaggedObjectCollisionMap_<ContainerAllocator> >
{
static const char* value()
{
return "std_msgs/Header header\n\
geometry_msgs/Transform origin_transform\n\
geometry_msgs/Vector3 dimensions\n\
float64 cell_size\n\
uint32 number_of_components\n\
bool components_valid\n\
bool convex_segments_valid\n\
bool initialized\n\
uint8[] OOB_value\n\
uint8[] data\n\
\n\
================================================================================\n\
MSG: std_msgs/Header\n\
# Standard metadata for higher-level stamped data types.\n\
# This is generally used to communicate timestamped data \n\
# in a particular coordinate frame.\n\
# \n\
# sequence ID: consecutively increasing ID \n\
uint32 seq\n\
#Two-integer timestamp that is expressed as:\n\
# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n\
# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n\
# time-handling sugar is provided by the client library\n\
time stamp\n\
#Frame this data is associated with\n\
# 0: no frame\n\
# 1: global frame\n\
string frame_id\n\
\n\
================================================================================\n\
MSG: geometry_msgs/Transform\n\
# This represents the transform between two coordinate frames in free space.\n\
\n\
Vector3 translation\n\
Quaternion rotation\n\
\n\
================================================================================\n\
MSG: geometry_msgs/Vector3\n\
# This represents a vector in free space. \n\
# It is only meant to represent a direction. Therefore, it does not\n\
# make sense to apply a translation to it (e.g., when applying a \n\
# generic rigid transformation to a Vector3, tf2 will only apply the\n\
# rotation). If you want your data to be translatable too, use the\n\
# geometry_msgs/Point message instead.\n\
\n\
float64 x\n\
float64 y\n\
float64 z\n\
================================================================================\n\
MSG: geometry_msgs/Quaternion\n\
# This represents an orientation in free space in quaternion form.\n\
\n\
float64 x\n\
float64 y\n\
float64 z\n\
float64 w\n\
";
}
static const char* value(const ::sdf_tools::TaggedObjectCollisionMap_<ContainerAllocator>&) { return value(); }
};
} // namespace message_traits
} // namespace ros
namespace ros
{
namespace serialization
{
template<class ContainerAllocator> struct Serializer< ::sdf_tools::TaggedObjectCollisionMap_<ContainerAllocator> >
{
template<typename Stream, typename T> inline static void allInOne(Stream& stream, T m)
{
stream.next(m.header);
stream.next(m.origin_transform);
stream.next(m.dimensions);
stream.next(m.cell_size);
stream.next(m.number_of_components);
stream.next(m.components_valid);
stream.next(m.convex_segments_valid);
stream.next(m.initialized);
stream.next(m.OOB_value);
stream.next(m.data);
}
ROS_DECLARE_ALLINONE_SERIALIZER
}; // struct TaggedObjectCollisionMap_
} // namespace serialization
} // namespace ros
namespace ros
{
namespace message_operations
{
template<class ContainerAllocator>
struct Printer< ::sdf_tools::TaggedObjectCollisionMap_<ContainerAllocator> >
{
template<typename Stream> static void stream(Stream& s, const std::string& indent, const ::sdf_tools::TaggedObjectCollisionMap_<ContainerAllocator>& v)
{
s << indent << "header: ";
s << std::endl;
Printer< ::std_msgs::Header_<ContainerAllocator> >::stream(s, indent + " ", v.header);
s << indent << "origin_transform: ";
s << std::endl;
Printer< ::geometry_msgs::Transform_<ContainerAllocator> >::stream(s, indent + " ", v.origin_transform);
s << indent << "dimensions: ";
s << std::endl;
Printer< ::geometry_msgs::Vector3_<ContainerAllocator> >::stream(s, indent + " ", v.dimensions);
s << indent << "cell_size: ";
Printer<double>::stream(s, indent + " ", v.cell_size);
s << indent << "number_of_components: ";
Printer<uint32_t>::stream(s, indent + " ", v.number_of_components);
s << indent << "components_valid: ";
Printer<uint8_t>::stream(s, indent + " ", v.components_valid);
s << indent << "convex_segments_valid: ";
Printer<uint8_t>::stream(s, indent + " ", v.convex_segments_valid);
s << indent << "initialized: ";
Printer<uint8_t>::stream(s, indent + " ", v.initialized);
s << indent << "OOB_value[]" << std::endl;
for (size_t i = 0; i < v.OOB_value.size(); ++i)
{
s << indent << " OOB_value[" << i << "]: ";
Printer<uint8_t>::stream(s, indent + " ", v.OOB_value[i]);
}
s << indent << "data[]" << std::endl;
for (size_t i = 0; i < v.data.size(); ++i)
{
s << indent << " data[" << i << "]: ";
Printer<uint8_t>::stream(s, indent + " ", v.data[i]);
}
}
};
} // namespace message_operations
} // namespace ros
#endif // SDF_TOOLS_MESSAGE_TAGGEDOBJECTCOLLISIONMAP_H

168
flightlib/third_party/sdf_tools/include/sdf_tools/collision_map.hpp vendored Normal file
View File

@@ -0,0 +1,168 @@
#include <stdlib.h>
#include <vector>
#include <string>
#include <Eigen/Geometry>
#include <visualization_msgs/Marker.h>
#include <arc_utilities/arc_helpers.hpp>
#include <arc_utilities/voxel_grid.hpp>
#include <sdf_tools/sdf.hpp>
#include <sdf_tools/CollisionMap.h>
#include <pcl/common/common.h>
#ifndef COLLISION_MAP_HPP
#define COLLISION_MAP_HPP
#define ENABLE_UNORDERED_MAP_SIZE_HINTS
namespace sdf_tools
{
struct COLLISION_CELL
{
float occupancy;
uint32_t component;
COLLISION_CELL(const float in_occupancy = 0.0, const uint32_t in_component = 0);
};
std::vector<uint8_t> CollisionCellToBinary(const COLLISION_CELL& value);
COLLISION_CELL CollisionCellFromBinary(const std::vector<uint8_t>& binary);
class CollisionMapGrid
{
protected:
static std_msgs::ColorRGBA GenerateComponentColor(const uint32_t component, const float alpha=1.0f);
bool IsSurfaceIndex(const int64_t x_index, const int64_t y_index, const int64_t z_index) const;
typedef struct
{
uint32_t location[3];
uint32_t closest_point[3];
double distance_square;
int32_t update_direction;
} bucket_cell;
typedef VoxelGrid::VoxelGrid<bucket_cell> DistanceField;
DistanceField BuildDistanceField(const std::vector<VoxelGrid::GRID_INDEX>& points) const;
std::vector<std::vector<std::vector<std::vector<int>>>> MakeNeighborhoods() const;
int GetDirectionNumber(const int dx, const int dy, const int dz) const;
double ComputeDistanceSquared(const int32_t x1, const int32_t y1, const int32_t z1, const int32_t x2, const int32_t y2, const int32_t z2) const;
VoxelGrid::VoxelGrid<COLLISION_CELL> collision_field_;
uint32_t number_of_components_;
std::string frame_;
bool initialized_;
bool components_valid_;
std::vector<uint8_t> PackBinaryRepresentation(std::vector<COLLISION_CELL>& raw);
std::vector<COLLISION_CELL> UnpackBinaryRepresentation(std::vector<uint8_t>& packed);
int64_t MarkConnectedComponent(int64_t x_index, int64_t y_index, int64_t z_index, uint32_t connected_component);
public:
CollisionMapGrid(const std::string& frame, const double resolution, const double x_size, const double y_size, const double z_size, const COLLISION_CELL& default_value, const COLLISION_CELL& OOB_value);
CollisionMapGrid(const Eigen::Isometry3d& origin_transform, const std::string& frame, const double resolution, const double x_size, double y_size, const double z_size, const COLLISION_CELL& default_value, const COLLISION_CELL& OOB_value);
CollisionMapGrid(const std::string& frame, const double resolution, const double x_size, const double y_size, const double z_size, const COLLISION_CELL& OOB_default_value);
CollisionMapGrid(const Eigen::Isometry3d& origin_transform, const std::string& frame, const double resolution, const double x_size, double y_size, const double z_size, const COLLISION_CELL& OOB_default_value);
CollisionMapGrid();
bool IsInitialized() const;
bool AreComponentsValid() const;
std::pair<COLLISION_CELL, bool> Get3d(const Eigen::Vector3d& location) const;
std::pair<COLLISION_CELL, bool> Get4d(const Eigen::Vector4d& location) const;
std::pair<COLLISION_CELL, bool> Get(const double x, const double y, const double z) const;
std::pair<COLLISION_CELL, bool> Get(const VoxelGrid::GRID_INDEX& index) const;
std::pair<COLLISION_CELL, bool> Get(const int64_t x_index, const int64_t y_index, const int64_t z_index) const;
bool Set(const double x, const double y, const double z, COLLISION_CELL value);
bool Set3d(const Eigen::Vector3d& location, COLLISION_CELL value);
bool Set4d(const Eigen::Vector4d& location, COLLISION_CELL value);
bool Set(const int64_t x_index, const int64_t y_index, const int64_t z_index, COLLISION_CELL value);
bool Set(const VoxelGrid::GRID_INDEX& index, COLLISION_CELL value);
double GetXSize() const;
double GetYSize() const;
double GetZSize() const;
double GetResolution() const;
COLLISION_CELL GetDefaultValue() const;
COLLISION_CELL GetOOBValue() const;
int64_t GetNumXCells() const;
int64_t GetNumYCells() const;
int64_t GetNumZCells() const;
const Eigen::Isometry3d& GetOriginTransform() const;
const Eigen::Isometry3d& GetInverseOriginTransform() const;
std::string GetFrame() const;
std::pair<uint32_t, bool> GetNumConnectedComponents() const;
std::vector<int64_t> LocationToGridIndex3d(const Eigen::Vector3d& location) const;
std::vector<int64_t> LocationToGridIndex4d(const Eigen::Vector4d& location) const;
std::vector<int64_t> LocationToGridIndex(double x, double y, double z) const;
std::vector<double> GridIndexToLocation(int64_t x_index, int64_t y_index, int64_t z_index) const;
bool SaveToFile(const std::string& filepath);
bool LoadFromFile(const std::string &filepath);
sdf_tools::CollisionMap GetMessageRepresentation();
bool LoadFromMessageRepresentation(sdf_tools::CollisionMap& message);
uint32_t UpdateConnectedComponents();
std::map<uint32_t, std::pair<int32_t, int32_t>> ComputeComponentTopology(bool ignore_empty_components, bool recompute_connected_components, bool verbose);
std::map<uint32_t, std::unordered_map<VoxelGrid::GRID_INDEX, uint8_t>> ExtractComponentSurfaces(const bool ignore_empty_components) const;
std::pair<int32_t, int32_t> ComputeHolesInSurface(const uint32_t component, const std::unordered_map<VoxelGrid::GRID_INDEX, uint8_t>& surface, const bool verbose) const;
int32_t ComputeConnectivityOfSurfaceVertices(const std::unordered_map<VoxelGrid::GRID_INDEX, uint8_t>& surface_vertex_connectivity) const;
std::pair<sdf_tools::SignedDistanceField, std::pair<double, double>> ExtractSignedDistanceField(const float oob_value) const;
visualization_msgs::Marker ExportForDisplay(const std_msgs::ColorRGBA& collision_color, const std_msgs::ColorRGBA& free_color, const std_msgs::ColorRGBA& unknown_color) const;
void DisplayPCL(pcl::PointCloud<pcl::PointXYZ> &occ_cloud);
void DisplayLocalPCL(pcl::PointCloud<pcl::PointXYZ> &occ_cloud, Eigen::Vector3d pose);
visualization_msgs::Marker ExportConnectedComponentsForDisplay(bool color_unknown_components) const;
};
}
#endif // COLLISION_MAP_HPP

55
flightlib/third_party/sdf_tools/include/sdf_tools/dynamic_spatial_hashed_collision_map.hpp vendored Normal file
View File

@@ -0,0 +1,55 @@
#include <stdlib.h>
#include <vector>
#include <string>
#include <Eigen/Geometry>
#include <visualization_msgs/Marker.h>
#include <arc_utilities/voxel_grid.hpp>
#include <arc_utilities/dynamic_spatial_hashed_voxel_grid.hpp>
#include <sdf_tools/collision_map.hpp>
#ifndef DYNAMIC_SPATIAL_HASHED_COLLISION_MAP_HPP
#define DYNAMIC_SPATIAL_HASHED_COLLISION_MAP_HPP
namespace sdf_tools
{
class DynamicSpatialHashedCollisionMapGrid
{
protected:
VoxelGrid::DynamicSpatialHashedVoxelGrid<COLLISION_CELL> collision_field_;
uint32_t number_of_components_;
std::string frame_;
bool initialized_;
bool components_valid_;
public:
DynamicSpatialHashedCollisionMapGrid(std::string frame, double resolution, int64_t chunk_x_size, int64_t chunk_y_size, int64_t chunk_z_size, COLLISION_CELL OOB_value);
DynamicSpatialHashedCollisionMapGrid(Eigen::Isometry3d origin_transform, std::string frame, double resolution, int64_t chunk_x_size, int64_t chunk_y_size, int64_t chunk_z_size, COLLISION_CELL OOB_value);
DynamicSpatialHashedCollisionMapGrid();
bool IsInitialized() const;
bool AreComponentsValid() const;
std::pair<COLLISION_CELL, VoxelGrid::FOUND_STATUS> Get(const double x, const double y, const double z) const;
std::pair<COLLISION_CELL, VoxelGrid::FOUND_STATUS> Get(const Eigen::Vector3d& location) const;
VoxelGrid::SET_STATUS SetCell(const double x, const double y, const double z, COLLISION_CELL value);
VoxelGrid::SET_STATUS SetCell(const Eigen::Vector3d& location, COLLISION_CELL value);
VoxelGrid::SET_STATUS SetChunk(const double x, const double y, const double z, COLLISION_CELL value);
VoxelGrid::SET_STATUS SetChunk(const Eigen::Vector3d& location, COLLISION_CELL value);
Eigen::Isometry3d GetOriginTransform() const;
std::vector<visualization_msgs::Marker> ExportForDisplay(const std_msgs::ColorRGBA& collision_color, const std_msgs::ColorRGBA& free_color, const std_msgs::ColorRGBA& unknown_color) const;
};
}
#endif // DYNAMIC_SPATIAL_HASHED_COLLISION_MAP_HPP

224
flightlib/third_party/sdf_tools/include/sdf_tools/sdf.hpp vendored Normal file
View File

@@ -0,0 +1,224 @@
#ifndef SDF_HPP
#define SDF_HPP
#include <stdlib.h>
#include <vector>
#include <string>
#include <Eigen/Geometry>
#include <visualization_msgs/Marker.h>
#include <arc_utilities/eigen_helpers.hpp>
#include <arc_utilities/voxel_grid.hpp>
#include <sdf_tools/SDF.h>
#include <pcl/common/common.h>
namespace sdf_tools
{
std::vector<uint8_t> FloatToBinary(float value);
float FloatFromBinary(std::vector<uint8_t>& binary);
class SignedDistanceField
{
protected:
VoxelGrid::VoxelGrid<float> distance_field_;
std::string frame_;
bool initialized_;
bool locked_;
std::vector<uint8_t> GetInternalBinaryRepresentation(const std::vector<float> &field_data);
std::vector<float> UnpackFieldFromBinaryRepresentation(const std::vector<uint8_t>& binary);
/*
* You *MUST* provide valid indices to this function, hence why it is protected (there are safe wrappers available - use them!)
*/
void FollowGradientsToLocalMaximaUnsafe(VoxelGrid::VoxelGrid<Eigen::Vector3d>& watershed_map, const int64_t x_index, const int64_t y_index, const int64_t z_index) const;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
SignedDistanceField(std::string frame, double resolution, double x_size, double y_size, double z_size, float OOB_value);
SignedDistanceField(Eigen::Isometry3d origin_transform, std::string frame, double resolution, double x_size, double y_size, double z_size, float OOB_value);
SignedDistanceField();
bool IsInitialized() const;
bool IsLocked() const;
void Lock();
void Unlock();
float Get(const double x, const double y, const double z) const;
float Get3d(const Eigen::Vector3d& location) const;
float Get4d(const Eigen::Vector4d& location) const;
float Get(const int64_t x_index, const int64_t y_index, const int64_t z_index) const;
std::pair<float, bool> GetSafe(const double x, const double y, const double z) const;
std::pair<float, bool> GetSafe3d(const Eigen::Vector3d& location) const;
std::pair<float, bool> GetSafe4d(const Eigen::Vector4d& location) const;
std::pair<float, bool> GetSafe(const int64_t x_index, const int64_t y_index, const int64_t z_index) const;
/*
* Setter functions MUST be used carefully - If you arbitrarily change SDF values, it is not a proper SDF any more!
*
* Use of these functions can be prevented by calling SignedDistanceField::Lock() on the SDF, at which point these functions
* will fail with a warning printed to std_err.
*/
bool Set(const double x, const double y, const double z, float value);
bool Set3d(const Eigen::Vector3d& location, float value);
bool Set4d(const Eigen::Vector4d& location, float value);
bool Set(const int64_t x_index, const int64_t y_index, const int64_t z_index, const float value);
bool Set(const VoxelGrid::GRID_INDEX& index, const float value);
bool CheckInBounds3d(const Eigen::Vector3d& location) const;
bool CheckInBounds4d(const Eigen::Vector4d& location) const;
bool CheckInBounds(const double x, const double y, const double z) const;
bool CheckInBounds(const VoxelGrid::GRID_INDEX& index) const;
bool CheckInBounds(const int64_t x_index, const int64_t y_index, const int64_t z_index) const;
double GetXSize() const;
double GetYSize() const;
double GetZSize() const;
double GetResolution() const;
float GetOOBValue() const;
int64_t GetNumXCells() const;
int64_t GetNumYCells() const;
int64_t GetNumZCells() const;
protected:
std::pair<Eigen::Vector3d, double> GetPrimaryComponentsVector(const Eigen::Vector3d& raw_vector) const;
double ComputeAxisMatch(const double axis_value, const double check_value) const;
Eigen::Vector3d GetBestMatchSurfaceVector(const Eigen::Vector3d& possible_surfaces_vector, const Eigen::Vector3d& center_to_location_vector) const;
/**
* @brief GetPrimaryEntrySurfaceVector Estimates the real distance of the provided point, comparing it with the cell center location and gradient vector
* @param boundary_direction_vector
* @param center_to_location_vector
* @return vector from center of voxel to primary entry surface, and magnitude of that vector
*/
std::pair<Eigen::Vector3d, double> GetPrimaryEntrySurfaceVector(const Eigen::Vector3d& boundary_direction_vector, const Eigen::Vector3d& center_to_location_vector) const;
double EstimateDistanceInternal(const double x, const double y, const double z, const int64_t x_idx, const int64_t y_idx, const int64_t z_idx) const;
public:
std::pair<double, bool> EstimateDistance(const double x, const double y, const double z) const;
std::pair<double, bool> EstimateDistance3d(const Eigen::Vector3d& location) const;
std::pair<double, bool> EstimateDistance4d(const Eigen::Vector4d& location) const;
// Estimate the distance between the given point and the outer boundary of the SDF
std::pair<double, bool> DistanceToBoundary(const double x, const double y, const double z) const;
std::pair<double, bool> DistanceToBoundary3d(const Eigen::Vector3d& location) const;
std::pair<double, bool> DistanceToBoundary4d(const Eigen::Vector4d& location) const;
std::vector<double> GetGradient(const double x, const double y, const double z, const bool enable_edge_gradients = false) const;
std::vector<double> GetGradient3d(const Eigen::Vector3d& location, const bool enable_edge_gradients = false) const;
std::vector<double> GetGradient4d(const Eigen::Vector4d& location, const bool enable_edge_gradients = false) const;
std::vector<double> GetGradient(const VoxelGrid::GRID_INDEX& index, const bool enable_edge_gradients = false) const;
std::vector<double> GetGradient(const int64_t x_index, const int64_t y_index, const int64_t z_index, const bool enable_edge_gradients = false) const;
Eigen::Vector3d ProjectOutOfCollision(const double x, const double y, const double z, const double stepsize_multiplier = 1.0 / 8.0) const;
Eigen::Vector3d ProjectOutOfCollisionToMinimumDistance(const double x, const double y, const double z, const double minimum_distance, const double stepsize_multiplier = 1.0 / 8.0) const;
Eigen::Vector3d ProjectOutOfCollision3d(const Eigen::Vector3d& location, const double stepsize_multiplier = 1.0 / 8.0) const;
Eigen::Vector3d ProjectOutOfCollisionToMinimumDistance3d(const Eigen::Vector3d& location, const double minimum_distance, const double stepsize_multiplier = 1.0 / 8.0) const;
Eigen::Vector4d ProjectOutOfCollision4d(const Eigen::Vector4d& location, const double stepsize_multiplier = 1.0 / 8.0) const;
Eigen::Vector4d ProjectOutOfCollisionToMinimumDistance4d(const Eigen::Vector4d& location, const double minimum_distance, const double stepsize_multiplier = 1.0 / 8.0) const;
Eigen::Vector3d ProjectIntoValidVolume(const double x, const double y, const double z) const;
Eigen::Vector3d ProjectIntoValidVolumeToMinimumDistance(const double x, const double y, const double z, const double minimum_distance) const;
Eigen::Vector3d ProjectIntoValidVolume3d(const Eigen::Vector3d& location) const;
Eigen::Vector3d ProjectIntoValidVolumeToMinimumDistance3d(const Eigen::Vector3d& location, const double minimum_distance) const;
Eigen::Vector4d ProjectIntoValidVolume4d(const Eigen::Vector4d& location) const;
Eigen::Vector4d ProjectIntoValidVolumeToMinimumDistance4d(const Eigen::Vector4d& location, const double minimum_distance) const;
const Eigen::Isometry3d& GetOriginTransform() const;
const Eigen::Isometry3d& GetInverseOriginTransform() const;
std::string GetFrame() const;
std::vector<int64_t> LocationToGridIndex3d(const Eigen::Vector3d& location) const;
std::vector<int64_t> LocationToGridIndex4d(const Eigen::Vector4d& location) const;
std::vector<int64_t> LocationToGridIndex(const double x, const double y, const double z) const;
std::vector<double> GridIndexToLocation(const VoxelGrid::GRID_INDEX& index) const;
std::vector<double> GridIndexToLocation(const int64_t x_index, const int64_t y_index, const int64_t z_index) const;
bool SaveToFile(const std::string& filepath);
bool LoadFromFile(const std::string& filepath);
sdf_tools::SDF GetMessageRepresentation();
bool LoadFromMessageRepresentation(const sdf_tools::SDF& message);
visualization_msgs::Marker ExportForDisplay(const float alpha = 0.01f, float vis_level = FLT_MAX) const;
visualization_msgs::Marker ExportForDisplayCollisionOnly(const float alpha = 0.01f) const;
void DisplayPCL(pcl::PointCloud<pcl::PointXYZI> &sdf_cloud, float vis_level = 0.0);
visualization_msgs::Marker ExportForDebug(const float alpha = 0.5f) const;
/*
* The following function can be *VERY EXPENSIVE* to compute, since it performs gradient ascent across the SDF
*/
VoxelGrid::VoxelGrid<Eigen::Vector3d> ComputeLocalMaximaMap() const;
bool GradientIsEffectiveFlat(const Eigen::Vector3d& gradient) const;
VoxelGrid::GRID_INDEX GetNextFromGradient(const VoxelGrid::GRID_INDEX& index, const Eigen::Vector3d& gradient) const;
};
}
#endif // SDF_HPP

95
flightlib/third_party/sdf_tools/include/sdf_tools/sdf_builder.hpp vendored Normal file
View File

@@ -0,0 +1,95 @@
#include <stdlib.h>
#include <vector>
#include <string>
#include <Eigen/Geometry>
#include <ros/ros.h>
#include <moveit_msgs/GetPlanningScene.h>
#include <urdf_model/model.h>
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/planning_scene/planning_scene.h>
#include "sdf_tools/sdf.hpp"
#include "sdf_tools/SDF.h"
#ifndef SDF_BUILDER_HPP
#define SDF_BUILDER_HPP
namespace sdf_tools
{
static const uint8_t USE_CACHED = 0x00;
static const uint8_t USE_ONLY_OCTOMAP = 0x01;
static const uint8_t USE_ONLY_COLLISION_OBJECTS = 0x02;
static const uint8_t USE_FULL_PLANNING_SCENE = 0x03;
typedef struct
{
uint32_t location[3];
uint32_t closest_point[3];
double distance_square;
int32_t update_direction;
} bucket_cell;
typedef VoxelGrid::VoxelGrid<bucket_cell> DistanceField;
double ComputeDistanceSquared(int32_t x1, int32_t y1, int32_t z1, int32_t x2, int32_t y2, int32_t z2);
class SDF_Builder
{
protected:
bool initialized_;
bool has_cached_sdf_;
bool has_cached_collmap_;
bool has_planning_scene_;
Eigen::Isometry3d origin_transform_;
std::string frame_;
double x_size_;
double y_size_;
double z_size_;
double resolution_;
float OOB_value_;
SignedDistanceField cached_sdf_;
VoxelGrid::VoxelGrid<uint8_t> cached_collmap_;
std::shared_ptr<planning_scene::PlanningScene> planning_scene_ptr_;
ros::NodeHandle nh_;
ros::ServiceClient planning_scene_client_;
SignedDistanceField UpdateSDFFromPlanningScene();
VoxelGrid::VoxelGrid<uint8_t> UpdateCollisionMapFromPlanningScene();
bool BuildInternalPlanningScene();
DistanceField BuildDistanceField(std::vector<Eigen::Vector3i>& points);
std::vector<std::vector<std::vector<std::vector<int>>>> MakeNeighborhoods();
static int GetDirectionNumber(int dx, int dy, int dz);
std::string GenerateSDFComputeBotURDFString();
std::string GenerateSDFComputeBotSRDFString();
public:
SDF_Builder(ros::NodeHandle& nh, Eigen::Isometry3d origin_transform, std::string frame, double x_size, double y_size, double z_size, double resolution, float OOB_value, std::string planning_scene_service);
SDF_Builder(ros::NodeHandle& nh, std::string frame, double x_size, double y_size, double z_size, double resolution, float OOB_value, std::string planning_scene_service);
SDF_Builder();
void UpdatePlanningSceneFromMessage(moveit_msgs::PlanningScene& planning_scene);
SignedDistanceField UpdateSDF(uint8_t update_mode);
SignedDistanceField GetCachedSDF();
VoxelGrid::VoxelGrid<uint8_t> UpdateCollisionMap(uint8_t update_mode);
VoxelGrid::VoxelGrid<uint8_t> GetCachedCollisionMap();
};
}
#endif // SDF_BUILDER_HPP

270
flightlib/third_party/sdf_tools/include/sdf_tools/tagged_object_collision_map.hpp vendored Normal file
View File

@@ -0,0 +1,270 @@
#include <stdlib.h>
#include <vector>
#include <string>
#include <Eigen/Geometry>
#include <Eigen/Sparse>
#include <visualization_msgs/Marker.h>
#include <arc_utilities/voxel_grid.hpp>
#include <arc_utilities/arc_helpers.hpp>
#include <arc_utilities/eigen_helpers.hpp>
#include <sdf_tools/sdf.hpp>
#include <sdf_tools/TaggedObjectCollisionMap.h>
#ifndef TAGGED_OBJECT_COLLISION_MAP_HPP
#define TAGGED_OBJECT_COLLISION_MAP_HPP
#define ENABLE_UNORDERED_MAP_SIZE_HINTS
namespace sdf_tools
{
struct TAGGED_OBJECT_COLLISION_CELL
{
float occupancy;
uint32_t component;
uint32_t object_id;
uint32_t convex_segment;
TAGGED_OBJECT_COLLISION_CELL();
TAGGED_OBJECT_COLLISION_CELL(const float in_occupancy, const uint32_t in_object_id);
TAGGED_OBJECT_COLLISION_CELL(const float in_occupancy, const uint32_t in_object_id, const uint32_t in_component, const uint32_t in_convex_segment);
bool SharesConvexSegment(const TAGGED_OBJECT_COLLISION_CELL& other) const;
std::vector<uint32_t> GetListOfConvexSegments() const;
bool IsPartOfConvexSegment(const uint32_t segment) const;
void AddToConvexSegment(const uint32_t segment);
void RemoveFromConvexSegment(const uint32_t segment);
};
std::vector<uint8_t> TaggedObjectCollisionCellToBinary(const TAGGED_OBJECT_COLLISION_CELL& value);
TAGGED_OBJECT_COLLISION_CELL TaggedObjectCollisionCellFromBinary(const std::vector<uint8_t>& binary);
class TaggedObjectCollisionMapGrid
{
protected:
static std_msgs::ColorRGBA GenerateComponentColor(const uint32_t component, const float alpha=1.0f);
bool IsSurfaceIndex(const int64_t x_index, const int64_t y_index, const int64_t z_index) const;
struct bucket_cell
{
uint32_t location[3];
uint32_t closest_point[3];
double distance_square;
int32_t update_direction;
};
typedef VoxelGrid::VoxelGrid<bucket_cell> DistanceField;
DistanceField BuildDistanceField(const std::vector<VoxelGrid::GRID_INDEX>& points) const;
std::vector<std::vector<std::vector<std::vector<int>>>> MakeNeighborhoods() const;
int GetDirectionNumber(const int dx, const int dy, const int dz) const;
double ComputeDistanceSquared(const int32_t x1, const int32_t y1, const int32_t z1, const int32_t x2, const int32_t y2, const int32_t z2) const;
VoxelGrid::VoxelGrid<TAGGED_OBJECT_COLLISION_CELL> collision_field_;
uint32_t number_of_components_;
std::string frame_;
bool initialized_;
bool components_valid_;
bool convex_segments_valid_;
std::vector<uint8_t> PackBinaryRepresentation(const std::vector<TAGGED_OBJECT_COLLISION_CELL>& raw) const;
std::vector<TAGGED_OBJECT_COLLISION_CELL> UnpackBinaryRepresentation(const std::vector<uint8_t>& packed) const;
int64_t MarkConnectedComponent(const int64_t x_index, const int64_t y_index, const int64_t z_index, const uint32_t connected_component);
std::vector<VoxelGrid::GRID_INDEX> CheckIfConvex(const VoxelGrid::GRID_INDEX& candidate_index, std::unordered_map<VoxelGrid::GRID_INDEX, int8_t>& explored_indices, const VoxelGrid::VoxelGrid<std::vector<uint32_t>>& region_grid, const uint32_t current_convex_region) const;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
TaggedObjectCollisionMapGrid(const std::string& frame, const double resolution, const double x_size, const double y_size, const double z_size, const TAGGED_OBJECT_COLLISION_CELL& default_value, const TAGGED_OBJECT_COLLISION_CELL& OOB_value);
TaggedObjectCollisionMapGrid(const Eigen::Isometry3d& origin_transform, const std::string& frame, const double resolution, const double x_size, const double y_size, const double z_size, const TAGGED_OBJECT_COLLISION_CELL& default_value, const TAGGED_OBJECT_COLLISION_CELL& OOB_value);
TaggedObjectCollisionMapGrid(const std::string& frame, const double resolution, const double x_size, const double y_size, const double z_size, const TAGGED_OBJECT_COLLISION_CELL& OOB_value);
TaggedObjectCollisionMapGrid(const Eigen::Isometry3d& origin_transform, const std::string& frame, const double resolution, const double x_size, const double y_size, const double z_size, const TAGGED_OBJECT_COLLISION_CELL& OOB_value);
TaggedObjectCollisionMapGrid();
bool IsInitialized() const;
bool AreComponentsValid() const;
bool AreConvexSegmentsValid() const;
std::pair<bool, bool> CheckIfCandidateCorner3d(const Eigen::Vector3d& location) const;
std::pair<bool, bool> CheckIfCandidateCorner4d(const Eigen::Vector4d& location) const;
std::pair<bool, bool> CheckIfCandidateCorner(const double x, const double y, const double z) const;
std::pair<bool, bool> CheckIfCandidateCorner(const VoxelGrid::GRID_INDEX& index) const;
std::pair<bool, bool> CheckIfCandidateCorner(const int64_t x_index, const int64_t y_index, const int64_t z_index) const;
std::pair<const TAGGED_OBJECT_COLLISION_CELL&, bool> GetImmutable3d(const Eigen::Vector3d& location) const;
std::pair<const TAGGED_OBJECT_COLLISION_CELL&, bool> GetImmutable4d(const Eigen::Vector4d& location) const;
std::pair<const TAGGED_OBJECT_COLLISION_CELL&, bool> GetImmutable(const double x, const double y, const double z) const;
std::pair<const TAGGED_OBJECT_COLLISION_CELL&, bool> GetImmutable(const VoxelGrid::GRID_INDEX& index) const;
std::pair<const TAGGED_OBJECT_COLLISION_CELL&, bool> GetImmutable(const int64_t x_index, const int64_t y_index, const int64_t z_index) const;
std::pair<TAGGED_OBJECT_COLLISION_CELL&, bool> GetMutable3d(const Eigen::Vector3d& location);
std::pair<TAGGED_OBJECT_COLLISION_CELL&, bool> GetMutable4d(const Eigen::Vector4d& location);
std::pair<TAGGED_OBJECT_COLLISION_CELL&, bool> GetMutable(const double x, const double y, const double z);
std::pair<TAGGED_OBJECT_COLLISION_CELL&, bool> GetMutable(const VoxelGrid::GRID_INDEX& index);
std::pair<TAGGED_OBJECT_COLLISION_CELL&, bool> GetMutable(const int64_t x_index, const int64_t y_index, const int64_t z_index);
bool Set(const double x, const double y, const double z, const TAGGED_OBJECT_COLLISION_CELL& value);
bool Set3d(const Eigen::Vector3d& location, const TAGGED_OBJECT_COLLISION_CELL& value);
bool Set4d(const Eigen::Vector4d& location, const TAGGED_OBJECT_COLLISION_CELL& value);
bool Set(const int64_t x_index, const int64_t y_index, const int64_t z_index, const TAGGED_OBJECT_COLLISION_CELL& value);
bool Set(const VoxelGrid::GRID_INDEX& index, const TAGGED_OBJECT_COLLISION_CELL& value);
bool Set(const double x, const double y, const double z, TAGGED_OBJECT_COLLISION_CELL&& value);
bool Set3d(const Eigen::Vector3d& location, TAGGED_OBJECT_COLLISION_CELL&& value);
bool Set4d(const Eigen::Vector4d& location, TAGGED_OBJECT_COLLISION_CELL&& value);
bool Set(const int64_t x_index, const int64_t y_index, const int64_t z_index, TAGGED_OBJECT_COLLISION_CELL&& value);
bool Set(const VoxelGrid::GRID_INDEX& index, TAGGED_OBJECT_COLLISION_CELL&& value);
double GetXSize() const;
double GetYSize() const;
double GetZSize() const;
double GetResolution() const;
TAGGED_OBJECT_COLLISION_CELL GetDefaultValue() const;
TAGGED_OBJECT_COLLISION_CELL GetOOBValue() const;
int64_t GetNumXCells() const;
int64_t GetNumYCells() const;
int64_t GetNumZCells() const;
const Eigen::Isometry3d& GetOriginTransform() const;
const Eigen::Isometry3d& GetInverseOriginTransform() const;
std::string GetFrame() const;
std::pair<uint32_t, bool> GetNumConnectedComponents() const;
std::vector<int64_t> LocationToGridIndex3d(const Eigen::Vector3d& location) const;
std::vector<int64_t> LocationToGridIndex4d(const Eigen::Vector4d& location) const;
std::vector<int64_t> LocationToGridIndex(const double x, const double y, const double z) const;
std::vector<double> GridIndexToLocation(const int64_t x_index, const int64_t y_index, const int64_t z_index) const;
bool SaveToFile(const std::string& filepath) const;
bool LoadFromFile(const std::string &filepath);
sdf_tools::TaggedObjectCollisionMap GetMessageRepresentation() const;
bool LoadFromMessageRepresentation(const sdf_tools::TaggedObjectCollisionMap& message);
TaggedObjectCollisionMapGrid Resample(const double new_resolution) const;
uint32_t UpdateConnectedComponents();
std::map<uint32_t, std::pair<int32_t, int32_t>> ComputeComponentTopology(const bool ignore_empty_components, const bool recompute_connected_components, const bool verbose);
std::map<uint32_t, std::unordered_map<VoxelGrid::GRID_INDEX, uint8_t>> ExtractComponentSurfaces(const bool ignore_empty_components) const;
/* Extracts the active indices from a surface map as a vector, which is useful in contexts where a 1-dimensional index into the surface is needed
*/
std::vector<VoxelGrid::GRID_INDEX> ExtractStaticSurface(const std::unordered_map<VoxelGrid::GRID_INDEX, uint8_t>& raw_surface) const;
std::unordered_map<VoxelGrid::GRID_INDEX, uint8_t> ConvertToDynamicSurface(const std::vector<VoxelGrid::GRID_INDEX>& static_surface) const;
std::unordered_map<VoxelGrid::GRID_INDEX, size_t> BuildSurfaceIndexMap(const std::vector<VoxelGrid::GRID_INDEX>& static_surface) const;
std::pair<int32_t, int32_t> ComputeHolesInSurface(const uint32_t component, const std::unordered_map<VoxelGrid::GRID_INDEX, uint8_t>& surface, const bool verbose) const;
int32_t ComputeConnectivityOfSurfaceVertices(const std::unordered_map<VoxelGrid::GRID_INDEX, uint8_t>& surface_vertex_connectivity) const;
std::pair<sdf_tools::SignedDistanceField, std::pair<double, double>> ExtractSignedDistanceField(const float oob_value, const std::vector<uint32_t>& objects_to_use) const;
VoxelGrid::VoxelGrid<std::vector<uint32_t>> ComputeConvexRegions(const double max_check_radius) const;
void GrowConvexRegion(const VoxelGrid::GRID_INDEX& start_index, VoxelGrid::VoxelGrid<std::vector<uint32_t>>& region_grid, const double max_check_radius, const uint32_t current_convex_region) const;
EigenHelpers::VectorVector3d GenerateRayPrimitiveVectors(const uint32_t number_of_rays, const double cone_angle) const;
std::pair<std::vector<size_t>, std::vector<size_t>> CastSingleRay(const std::unordered_map<VoxelGrid::GRID_INDEX, size_t>& surface_index_map, const VoxelGrid::GRID_INDEX& current_surface_index, const Eigen::Vector3d& ray_unit_vector) const;
std::pair<std::vector<size_t>, std::vector<size_t>> PerformRayCasting(const sdf_tools::SignedDistanceField& sdf, const std::unordered_map<VoxelGrid::GRID_INDEX, size_t>& surface_index_map, const VoxelGrid::GRID_INDEX& current_surface_index, const EigenHelpers::VectorVector3d& ray_primitive_vectors) const;
//std::pair<Eigen::MatrixXd, std::pair<Eigen::SparseMatrix<double>, Eigen::SparseMatrix<double>>> ComputeSparseLineOfSight(const std::vector<VoxelGrid::GRID_INDEX>& static_surface, const uint32_t number_of_rays, const double cone_angle) const
Eigen::MatrixXd ComputeSparseLineOfSight(const std::vector<VoxelGrid::GRID_INDEX>& static_surface, const uint32_t number_of_rays, const double cone_angle) const;
std::pair<Eigen::VectorXd, Eigen::MatrixXd> ExtractKLargestEigenvaluesAndEigenvectors(const Eigen::EigenSolver<Eigen::MatrixXd>::EigenvalueType& raw_eigenvalues, const Eigen::EigenSolver<Eigen::MatrixXd>::EigenvectorsType& raw_eigenvectors, const uint32_t num_values) const;
std::vector<uint32_t> PerformKMeansSpectralClustering(const Eigen::EigenSolver<Eigen::MatrixXd>::EigenvalueType& raw_eigenvalues, const Eigen::EigenSolver<Eigen::MatrixXd>::EigenvectorsType& raw_eigenvectors, const uint32_t num_clusters) const;
double ComputeConvexityMetric(const Eigen::MatrixXd& los_matrix, const std::vector<uint32_t>& cluster_labels) const;
std::vector<std::vector<size_t>> ClusterSurfaceFromLOSMatrix(const Eigen::MatrixXd& los_matrix, const uint32_t max_num_clusters) const;
std::vector<std::vector<VoxelGrid::GRID_INDEX>> ComputeWeaklyConvexSurfaceSegments(const std::unordered_map<VoxelGrid::GRID_INDEX, uint8_t>& surface, const uint32_t max_num_clusters) const;
std::map<uint32_t, uint32_t> UpdateConvexSegments();
std::map<uint32_t, sdf_tools::SignedDistanceField> MakeObjectSDFs(const std::vector<uint32_t>& object_ids) const;
std::map<uint32_t, sdf_tools::SignedDistanceField> MakeObjectSDFs() const;
visualization_msgs::Marker ExportForDisplay(const float alpha, const std::vector<uint32_t>& objects_to_draw=std::vector<uint32_t>()) const;
visualization_msgs::Marker ExportForDisplay(const std::map<uint32_t, std_msgs::ColorRGBA>& object_color_map=std::map<uint32_t, std_msgs::ColorRGBA>()) const;
visualization_msgs::Marker ExportContourOnlyForDisplay(const float alpha, const std::vector<uint32_t>& objects_to_draw=std::vector<uint32_t>()) const;
visualization_msgs::Marker ExportContourOnlyForDisplay(const std::map<uint32_t, std_msgs::ColorRGBA>& object_color_map=std::map<uint32_t, std_msgs::ColorRGBA>()) const;
visualization_msgs::Marker ExportForDisplayOccupancyOnly(const std_msgs::ColorRGBA& collision_color, const std_msgs::ColorRGBA& free_color, const std_msgs::ColorRGBA& unknown_color) const;
visualization_msgs::Marker ExportConnectedComponentsForDisplay(bool color_unknown_components) const;
visualization_msgs::Marker ExportConvexSegmentForDisplay(const uint32_t object_id, const uint32_t convex_segment) const;
visualization_msgs::Marker ExportSurfaceForDisplay(const std::unordered_map<VoxelGrid::GRID_INDEX, uint8_t>& surface, const std_msgs::ColorRGBA& surface_color) const;
};
}
#endif // TAGGED_OBJECT_COLLISION_MAP_HPP

10
flightlib/third_party/sdf_tools/msg/CollisionMap.msg vendored Normal file
View File

@@ -0,0 +1,10 @@
std_msgs/Header header
geometry_msgs/Transform origin_transform
geometry_msgs/Vector3 dimensions
float64 cell_size
float32 OOB_occupancy_value
uint32 OOB_component_value
uint32 number_of_components
bool components_valid
bool initialized
uint8[] data

8
flightlib/third_party/sdf_tools/msg/SDF.msg vendored Normal file
View File

@@ -0,0 +1,8 @@
std_msgs/Header header
geometry_msgs/Transform origin_transform
geometry_msgs/Vector3 dimensions
float64 sdf_cell_size
float32 OOB_value
bool initialized
bool locked
uint8[] data

10
flightlib/third_party/sdf_tools/msg/TaggedObjectCollisionMap.msg vendored Normal file
View File

@@ -0,0 +1,10 @@
std_msgs/Header header
geometry_msgs/Transform origin_transform
geometry_msgs/Vector3 dimensions
float64 cell_size
uint32 number_of_components
bool components_valid
bool convex_segments_valid
bool initialized
uint8[] OOB_value
uint8[] data

46
flightlib/third_party/sdf_tools/package.xml vendored Normal file
View File

@@ -0,0 +1,46 @@
<?xml version="1.0"?>
<package>
<name>sdf_tools</name>
<version>0.0.1</version>
<description>Builds 2D signed distance fields from images, 3D signed distance fields from MoveIt PlanningScene/Octomap, provides a lightweight signed distance field library, message types for signed distance fields, and tools to compress signed distance fields for transport and storage.</description>
<maintainer email="calder.pg@gmail.com">Calder Phillips-Grafflin</maintainer>
<license>BSD</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>image_transport</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>moveit_msgs</build_depend>
<build_depend>visualization_msgs</build_depend>
<build_depend>cv_bridge</build_depend>
<build_depend>moveit_core</build_depend>
<build_depend>moveit_ros_planning</build_depend>
<build_depend>arc_utilities</build_depend>
<build_depend>message_generation</build_depend>
<run_depend>image_transport</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>rospy</run_depend>
<run_depend>sensor_msgs</run_depend>
<run_depend>std_msgs</run_depend>
<run_depend>moveit_msgs</run_depend>
<run_depend>visualization_msgs</run_depend>
<run_depend>cv_bridge</run_depend>
<run_depend>moveit_core</run_depend>
<run_depend>moveit_ros_planning</run_depend>
<run_depend>arc_utilities</run_depend>
<run_depend>message_runtime</run_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- You can specify that this package is a metapackage here: -->
<!-- <metapackage/> -->
<!-- Other tools can request additional information be placed here -->
</export>
</package>

327
flightlib/third_party/sdf_tools/src/image_2d_sdf_node.cpp vendored Normal file
View File

@@ -0,0 +1,327 @@
#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv/cv.h>
#include <sensor_msgs/image_encodings.h>
#define snap(x) ((x)>=0?(int)((x)+0.5):(int)((x)-0.5))
typedef struct sdf_cell {
double dx;
double dy;
} sdf_cell_t;
class ImageSDF
{
protected:
ros::NodeHandle nh_;
image_transport::ImageTransport it_;
image_transport::Subscriber binary_sub_;
image_transport::Publisher sdf_preview_pub_;
image_transport::Publisher sdf_raw_pub_;
std::vector< std::vector<double> > distance_field_;
double max_distance_;
double min_distance_;
public:
ImageSDF(ros::NodeHandle &n, std::string binary_base_topic, std::string sdf_preview_topic, std::string sdf_raw_topic) : nh_(n), it_(n)
{
max_distance_ = 0.0;
min_distance_ = 0.0;
binary_sub_ = it_.subscribe(binary_base_topic, 1, &ImageSDF::camera_cb, this);
sdf_preview_pub_ = it_.advertise(sdf_preview_topic, 1, true);
sdf_raw_pub_ = it_.advertise(sdf_raw_topic, 1, true);
std::string transport_in = binary_sub_.getTransport();
ROS_INFO("Subscribed using %s for transport", transport_in.c_str());
}
void loop()
{
while (ros::ok())
{
ros::spinOnce();
}
}
void update_sdf(cv::Mat& image)
{
ROS_DEBUG("Making intermediate containers for SDF");
std::vector< std::vector<sdf_cell_t> > empty_cells;
std::vector< std::vector<sdf_cell_t> > filled_cells;
// Resize the fields
int width = image.cols;
int height = image.rows;
empty_cells.resize(height);
filled_cells.resize(height);
distance_field_.resize(height);
for (int i = 0; i < height; i++)
{
empty_cells[i].resize(width);
filled_cells[i].resize(width);
distance_field_[i].resize(width);
}
ROS_DEBUG("Marking filled/empty pixels for SDF");
// Go through the image and add filled/empty pixels to the corresponding fields
sdf_cell_t empty_cell;
empty_cell.dx = INFINITY;
empty_cell.dy = INFINITY;
sdf_cell_t filled_cell;
filled_cell.dx = 0.0;
filled_cell.dy = 0.0;
for (int i = 0; i < height; i++)
{
for (int j = 0; j < width; j++)
{
uint8_t pixel = image.at<uint8_t>(i,j);
if (pixel != 0)
{
filled_cells[i][j] = filled_cell;
empty_cells[i][j] = empty_cell;
}
else
{
filled_cells[i][j] = empty_cell;
empty_cells[i][j] = filled_cell;
}
}
}
ROS_DEBUG("Running 8SSEDT on intermediate containers");
// Run the 8SSEDT algorithm to compute the partial SDFs
update_partial_sdf(filled_cells);
update_partial_sdf(empty_cells);
ROS_DEBUG("Computing the final SDF");
// Combine the partial fields to form the SDF
max_distance_ = 0.0;
min_distance_ = 0.0;
for (int i = 0; i < height; i++)
{
for (int j = 0; j < width; j++)
{
double filled_distance = sqrt(pow(filled_cells[i][j].dx, 2) + pow(filled_cells[i][j].dy, 2));
double empty_distance = sqrt(pow(empty_cells[i][j].dx, 2) + pow(empty_cells[i][j].dy, 2));
double new_distance = filled_distance - empty_distance;
distance_field_[i][j] = new_distance;
if (new_distance > max_distance_)
{
max_distance_ = new_distance;
}
if (new_distance < min_distance_)
{
min_distance_ = new_distance;
}
}
}
}
bool update_partial_sdf(std::vector< std::vector<sdf_cell_t> >& partial_field)
{
if (partial_field.size() > 0)
{
// Pass 1.1.0
for (size_t y = 0; y < partial_field[0].size(); y++)
{
// Pass 1.1.1
for (size_t x = 0; x < partial_field.size(); x++)
{
// Get value from grid
sdf_cell_t cell = get(partial_field, x, y);
// Do work
compare(partial_field, cell, x, y, -1, 0);
compare(partial_field, cell, x, y, 0, -1);
compare(partial_field, cell, x, y, -1, -1);
compare(partial_field, cell, x, y, 1, -1);
// Store back into grid
put(partial_field, cell, x, y);
}
// Pass 1.1.2
for (int x = (partial_field.size() - 1); x >= 0; x--)
{
// Get value from grid
sdf_cell_t cell = get(partial_field, x, y);
// Do work
compare(partial_field, cell, x, y, 1, 0);
// Store back into grid
put(partial_field, cell, x, y);
}
}
// Pass 1.2.0
for (int y = (partial_field[0].size() - 1); y >= 0; y--)
{
// Pass 1.1.1
for (int x = (partial_field.size() - 1); x >= 0; x--)
{
// Get value from grid
sdf_cell_t cell = get(partial_field, x, y);
// Do work
compare(partial_field, cell, x, y, 1, 0);
compare(partial_field, cell, x, y, 0, 1);
compare(partial_field, cell, x, y, -1, 1);
compare(partial_field, cell, x, y, 1, 1);
// Store back into grid
put(partial_field, cell, x, y);
}
// Pass 1.1.2
for (size_t x = 0; x < partial_field.size(); x++)
{
// Get value from grid
sdf_cell_t cell = get(partial_field, x, y);
// Do work
compare(partial_field, cell, x, y, -1, 0);
// Store back into grid
put(partial_field, cell, x, y);
}
}
return true;
}
else
{
return false;
}
}
inline sdf_cell_t get(std::vector< std::vector<sdf_cell_t> >& partial_field, size_t x, size_t y)
{
if (x < partial_field.size())
{
if (y < partial_field[x].size())
{
return partial_field[x][y];
}
}
sdf_cell_t empty;
empty.dx = INFINITY;
empty.dy = INFINITY;
return empty;
}
inline void put(std::vector< std::vector<sdf_cell_t> >& partial_field, sdf_cell_t& cell, size_t x, size_t y)
{
if (x < partial_field.size())
{
if (y < partial_field[x].size())
{
partial_field[x][y] = cell;
}
}
}
inline double distance_squared(sdf_cell_t& cell)
{
return ((cell.dx * cell.dx) + (cell.dy * cell.dy));
}
inline void compare(std::vector< std::vector<sdf_cell_t> >& partial_field, sdf_cell_t& cell, int x, int y, int x_offset, int y_offset)
{
sdf_cell_t other = get(partial_field, x + x_offset, y + y_offset);
other.dx += x_offset;
other.dy += y_offset;
if (distance_squared(other) < distance_squared(cell))
{
cell = other;
}
}
void camera_cb(const sensor_msgs::ImageConstPtr& image)
{
ROS_DEBUG("Got new image to resize and SDF");
// Convert to OpenCV
cv_bridge::CvImagePtr cv_ptr;
try
{
cv_ptr = cv_bridge::toCvCopy(image);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
// Make destination
cv::Mat binary; //(cv::Size(resized_width_, resized_height_), CV_8UC1);
binary = cv_ptr->image;
// Compute the SDF for the image
ROS_DEBUG("Attempting to compute SDF of image...");
update_sdf(binary);
ROS_DEBUG("...SDF compute finished");
// Publish the raw SDF
cv::Mat raw_sdf_image(cv::Size(binary.cols, binary.rows), CV_32FC2);
for (int i = 0; i < binary.rows; i++)
{
for (int j = 0; j < binary.cols; j++)
{
double current_distance = distance_field_[i][j];
if (current_distance >= 0.0)
{
raw_sdf_image.at<cv::Vec2f>(i,j)[0] = fabs(current_distance);
raw_sdf_image.at<cv::Vec2f>(i,j)[1] = 0.0;
}
else
{
raw_sdf_image.at<cv::Vec2f>(i,j)[0] = 0.0;
raw_sdf_image.at<cv::Vec2f>(i,j)[1] = fabs(current_distance);
}
}
}
// Convert back to ROS
sensor_msgs::Image sdf_raw_image;
cv_bridge::CvImage sdf_raw_converted(image->header, sensor_msgs::image_encodings::TYPE_32FC2, raw_sdf_image);
sdf_raw_converted.toImageMsg(sdf_raw_image);
// Republish
sdf_raw_pub_.publish(sdf_raw_image);
// Convert SDF to false-color image
cv::Mat false_color_sdf(cv::Size(binary.cols, binary.rows), CV_8UC3);
for (int i = 0; i < binary.rows; i++)
{
for (int j = 0; j < binary.cols; j++)
{
double current_distance = distance_field_[i][j];
uint8_t blue_channel = 0x00;
uint8_t red_channel = 0x00;
uint8_t green_channel = 0x00;
if (current_distance > 0.0)
{
red_channel = (uint8_t)(64.0 + (64.0 * fabs(current_distance / max_distance_)));
}
else if (current_distance == 0.0 || current_distance == -0.0)
{
green_channel = 0xff;
}
else
{
blue_channel = (uint8_t)(64.0 + (64.0 * fabs(current_distance / min_distance_)));
}
false_color_sdf.at<cv::Vec3b>(i, j)[0] = blue_channel;
false_color_sdf.at<cv::Vec3b>(i, j)[1] = green_channel;
false_color_sdf.at<cv::Vec3b>(i, j)[2] = red_channel;
}
}
// Convert back to ROS
sensor_msgs::Image sdf_preview_image;
cv_bridge::CvImage sdf_preview_converted(image->header, sensor_msgs::image_encodings::BGR8, false_color_sdf);
sdf_preview_converted.toImageMsg(sdf_preview_image);
// Republish
sdf_preview_pub_.publish(sdf_preview_image);
ROS_DEBUG("Resize + SDF finished");
}
};
int main(int argc, char** argv)
{
ros::init(argc, argv, "image_sdf");
ROS_INFO("Starting SDF from image generator...");
ros::NodeHandle nh;
ros::NodeHandle nhp("~");
std::string binary_base_topic;
std::string sdf_preview_topic;
std::string sdf_raw_topic;
nhp.param(std::string("binary_base_topic"), binary_base_topic, std::string("camera/rgb/binary"));
nhp.param(std::string("sdf_preview_topic"), sdf_preview_topic, std::string("camera/rgb/sdf"));
nhp.param(std::string("sdf_raw_topic"), sdf_raw_topic, std::string("camera/rgb/sdf_raw"));
ImageSDF processor(nh, binary_base_topic, sdf_preview_topic, sdf_raw_topic);
ROS_INFO("...startup complete");
processor.loop();
return 0;
}

107
flightlib/third_party/sdf_tools/src/sdf_generation_node.cpp vendored Normal file
View File

@@ -0,0 +1,107 @@
#include <ros/ros.h>
#include "sdf_tools/SDF.h"
#include "sdf_tools/sdf_builder.hpp"
#include <arc_utilities/eigen_helpers_conversions.hpp>
#include <time.h>
visualization_msgs::Marker ExportCollisionMapForDisplay(VoxelGrid::VoxelGrid<uint8_t>& collision_map, std::string frame, float alpha)
{
// Assemble a visualization_markers::Marker representation of the SDF to display in RViz
visualization_msgs::Marker display_rep;
// Populate the header
display_rep.header.frame_id = frame;
// Populate the options
display_rep.ns = "collision_map_display";
display_rep.id = 1;
display_rep.type = visualization_msgs::Marker::CUBE_LIST;
display_rep.action = visualization_msgs::Marker::ADD;
display_rep.lifetime = ros::Duration(0.0);
display_rep.frame_locked = false;
const Eigen::Isometry3d base_transform = Eigen::Isometry3d::Identity();
display_rep.pose = EigenHelpersConversions::EigenIsometry3dToGeometryPose(base_transform);
display_rep.scale.x = collision_map.GetCellSizes()[0];
display_rep.scale.y = collision_map.GetCellSizes()[1];
display_rep.scale.z = collision_map.GetCellSizes()[2];
// Add all cells in collision
for (int64_t x_index = 0; x_index < collision_map.GetNumXCells(); x_index++)
{
for (int64_t y_index = 0; y_index < collision_map.GetNumYCells(); y_index++)
{
for (int64_t z_index = 0; z_index < collision_map.GetNumZCells(); z_index++)
{
// Check if the current cell is in collision
uint8_t status = collision_map.GetImmutable(x_index, y_index, z_index).first;
if (status == 1)
{
// Convert cell indices into a real-world location
std::vector<double> location = collision_map.GridIndexToLocation(x_index, y_index, z_index);
geometry_msgs::Point new_point;
new_point.x = location[0];
new_point.y = location[1];
new_point.z = location[2];
display_rep.points.push_back(new_point);
// Color it
std_msgs::ColorRGBA new_color;
new_color.a = alpha;
new_color.b = 0.0;
new_color.g = 0.0;
new_color.r = 1.0;
display_rep.colors.push_back(new_color);
}
else
{
assert(status == 0);
}
}
}
}
return display_rep;
}
int main(int argc, char** argv)
{
//test_voxel_grid();
ros::init(argc, argv, "planning_scene_SDF_generator");
ROS_INFO("Starting SDF from planning scene generator...");
ros::NodeHandle nh;
ros::NodeHandle nhp("~");
std::string frame;
double resolution;
double x_size;
double y_size;
double z_size;
nhp.param(std::string("sdf_origin_frame"), frame, std::string("base"));
nhp.param(std::string("resolution"), resolution, 0.05);
nhp.param(std::string("x_size"), x_size, 1.0);
nhp.param(std::string("y_size"), y_size, 2.0);
nhp.param(std::string("z_size"), z_size, 1.0);
Eigen::Translation3d origin_translation(0.1, -1.0, -0.3);
Eigen::Quaterniond origin_rotation;
origin_rotation.setIdentity();
Eigen::Isometry3d origin_transform = origin_translation * origin_rotation;
sdf_tools::SDF_Builder sdf_builder(nh, origin_transform, frame, x_size, y_size, z_size, resolution, INFINITY, "get_planning_scene");
ROS_INFO("...startup complete");
////////////////////
///// Display! /////
////////////////////
ros::Publisher viz_pub = nh.advertise<visualization_msgs::Marker>("sdf_markers", 1, true);
ros::Rate spin_rate(10);
while (ros::ok())
{
/* Collision map visualization message publish */
std::cout << "Generating a new Collision Map..." << std::endl;
VoxelGrid::VoxelGrid<uint8_t> coll_map = sdf_builder.UpdateCollisionMap(sdf_tools::USE_FULL_PLANNING_SCENE);
std::cout << "...Collision Map with " << (coll_map.GetNumXCells() * coll_map.GetNumYCells() * coll_map.GetNumZCells()) << " cells generated - sending to RVIZ" << std::endl;
viz_pub.publish(ExportCollisionMapForDisplay(coll_map, "base", 1.0));
/* SDF visualization message publish */
clock_t st, et;
st = std::clock();
sdf_tools::SignedDistanceField sdf = sdf_builder.UpdateSDF(sdf_tools::USE_CACHED);
et = std::clock();
std::cout << "SDF with " << (sdf.GetNumXCells() * sdf.GetNumYCells() * sdf.GetNumZCells()) << " cells generated - took " << (((float)(et - st)) / CLOCKS_PER_SEC) << " seconds to compute" << std::endl;
viz_pub.publish(sdf.ExportForDisplay(0.1));
ros::spinOnce();
spin_rate.sleep();
}
return 0;
}

1420
flightlib/third_party/sdf_tools/src/sdf_tools/collision_map.cpp vendored Normal file
View File

File diff suppressed because it is too large Load Diff

200
flightlib/third_party/sdf_tools/src/sdf_tools/dynamic_spatial_hashed_collision_map.cpp vendored Normal file
View File

@@ -0,0 +1,200 @@
#include <stdlib.h>
#include <stdio.h>
#include <vector>
#include <string>
#include <functional>
#include <unordered_map>
#include <sstream>
#include <iostream>
#include <stdexcept>
#include <Eigen/Geometry>
#include <visualization_msgs/Marker.h>
#include <arc_utilities/eigen_helpers_conversions.hpp>
#include <arc_utilities/voxel_grid.hpp>
#include <arc_utilities/dynamic_spatial_hashed_voxel_grid.hpp>
#include <sdf_tools/collision_map.hpp>
#include <sdf_tools/dynamic_spatial_hashed_collision_map.hpp>
namespace sdf_tools
{
DynamicSpatialHashedCollisionMapGrid::DynamicSpatialHashedCollisionMapGrid(std::string frame, double resolution, int64_t chunk_x_size, int64_t chunk_y_size, int64_t chunk_z_size, COLLISION_CELL OOB_value)
{
frame_ = frame;
VoxelGrid::DynamicSpatialHashedVoxelGrid<COLLISION_CELL> new_field(resolution, chunk_x_size, chunk_y_size, chunk_z_size, OOB_value);
collision_field_ = new_field;
number_of_components_ = 0;
components_valid_ = false;
}
DynamicSpatialHashedCollisionMapGrid::DynamicSpatialHashedCollisionMapGrid(Eigen::Isometry3d origin_transform, std::string frame, double resolution, int64_t chunk_x_size, int64_t chunk_y_size, int64_t chunk_z_size, COLLISION_CELL OOB_value)
{
frame_ = frame;
VoxelGrid::DynamicSpatialHashedVoxelGrid<COLLISION_CELL> new_field(origin_transform, resolution, chunk_x_size, chunk_y_size, chunk_z_size, OOB_value);
collision_field_ = new_field;
number_of_components_ = 0;
components_valid_ = false;
}
DynamicSpatialHashedCollisionMapGrid::DynamicSpatialHashedCollisionMapGrid() : number_of_components_(0), initialized_(false), components_valid_(false) {}
bool DynamicSpatialHashedCollisionMapGrid::IsInitialized() const
{
return initialized_;
}
bool DynamicSpatialHashedCollisionMapGrid::AreComponentsValid() const
{
return components_valid_;
}
std::pair<COLLISION_CELL, VoxelGrid::FOUND_STATUS> DynamicSpatialHashedCollisionMapGrid::Get(const double x, const double y, const double z) const
{
return collision_field_.GetImmutable(x, y, z);
}
std::pair<COLLISION_CELL, VoxelGrid::FOUND_STATUS> DynamicSpatialHashedCollisionMapGrid::Get(const Eigen::Vector3d& location) const
{
return collision_field_.GetImmutable(location);
}
VoxelGrid::SET_STATUS DynamicSpatialHashedCollisionMapGrid::SetCell(const double x, const double y, const double z, COLLISION_CELL value)
{
return collision_field_.SetCellValue(x, y, z, value);
}
VoxelGrid::SET_STATUS DynamicSpatialHashedCollisionMapGrid::SetCell(const Eigen::Vector3d& location, COLLISION_CELL value)
{
return collision_field_.SetCellValue(location, value);
}
VoxelGrid::SET_STATUS DynamicSpatialHashedCollisionMapGrid::SetChunk(const double x, const double y, const double z, COLLISION_CELL value)
{
return collision_field_.SetChunkValue(x, y, z, value);
}
VoxelGrid::SET_STATUS DynamicSpatialHashedCollisionMapGrid::SetChunk(const Eigen::Vector3d& location, COLLISION_CELL value)
{
return collision_field_.SetChunkValue(location, value);
}
Eigen::Isometry3d DynamicSpatialHashedCollisionMapGrid::GetOriginTransform() const
{
return collision_field_.GetOriginTransform();
}
std::vector<visualization_msgs::Marker> DynamicSpatialHashedCollisionMapGrid::ExportForDisplay(const std_msgs::ColorRGBA& collision_color, const std_msgs::ColorRGBA& free_color, const std_msgs::ColorRGBA& unknown_color) const
{
// Assemble a visualization_markers::Marker representation of the SDF to display in RViz
// We make one marker for the individual cells, and another for the chunks, but we return 0, 1, or 2
// markers depending on if any chenks or cells can be drawn.
// First, the chunks
visualization_msgs::Marker chunks_display_rep;
// Populate the header
chunks_display_rep.header.frame_id = frame_;
// Populate the options
chunks_display_rep.ns = "dynamic_spatial_hashed_collision_map_chunks_display";
chunks_display_rep.id = 1;
chunks_display_rep.type = visualization_msgs::Marker::CUBE_LIST;
chunks_display_rep.action = visualization_msgs::Marker::ADD;
chunks_display_rep.lifetime = ros::Duration(0.0);
chunks_display_rep.frame_locked = false;
const Eigen::Isometry3d base_transform = Eigen::Isometry3d::Identity();
chunks_display_rep.pose = EigenHelpersConversions::EigenIsometry3dToGeometryPose(base_transform);
std::vector<double> chunk_sizes = collision_field_.GetChunkSizes();
chunks_display_rep.scale.x = chunk_sizes[0];
chunks_display_rep.scale.y = chunk_sizes[1];
chunks_display_rep.scale.z = chunk_sizes[2];
// Second, the cells
visualization_msgs::Marker cells_display_rep;
// Populate the header
cells_display_rep.header.frame_id = frame_;
// Populate the options
cells_display_rep.ns = "dynamic_spatial_hashed_collision_map_cells_display";
cells_display_rep.id = 1;
cells_display_rep.type = visualization_msgs::Marker::CUBE_LIST;
cells_display_rep.action = visualization_msgs::Marker::ADD;
cells_display_rep.lifetime = ros::Duration(0.0);
cells_display_rep.frame_locked = false;
cells_display_rep.pose = EigenHelpersConversions::EigenIsometry3dToGeometryPose(base_transform);
std::vector<double> cell_sizes = collision_field_.GetCellSizes();
cells_display_rep.scale.x = cell_sizes[0];
cells_display_rep.scale.y = cell_sizes[1];
cells_display_rep.scale.z = cell_sizes[2];
// Now, go through the chunks and add everything to the message
const Eigen::Isometry3d& grid_transform = GetOriginTransform();
const std::unordered_map<VoxelGrid::CHUNK_REGION, VoxelGrid::DynamicSpatialHashedVoxelGridChunk<COLLISION_CELL>>& raw_chunks = collision_field_.GetInternalChunks();
std::unordered_map<VoxelGrid::CHUNK_REGION, VoxelGrid::DynamicSpatialHashedVoxelGridChunk<COLLISION_CELL>>::const_iterator raw_chunks_itr;
for (raw_chunks_itr = raw_chunks.begin(); raw_chunks_itr != raw_chunks.end(); ++raw_chunks_itr)
{
const VoxelGrid::DynamicSpatialHashedVoxelGridChunk<COLLISION_CELL>& current_chunk = raw_chunks_itr->second;
if (current_chunk.IsChunkInitialized())
{
const COLLISION_CELL& current_cell = current_chunk.GetChunkImmutable();
std::vector<double> cell_location_in_grid = current_chunk.GetIndexLocationInGrid(0, 0, 0);
Eigen::Vector3d grid_location(cell_location_in_grid[0], cell_location_in_grid[1], cell_location_in_grid[2]);
Eigen::Vector3d location = grid_transform * grid_location;
geometry_msgs::Point new_point;
new_point.x = location.x();
new_point.y = location.y();
new_point.z = location.z();
chunks_display_rep.points.push_back(new_point);
if (current_cell.occupancy > 0.5)
{
chunks_display_rep.colors.push_back(collision_color);
}
else if (current_cell.occupancy < 0.5)
{
chunks_display_rep.colors.push_back(free_color);
}
else
{
chunks_display_rep.colors.push_back(unknown_color);
}
}
else if (current_chunk.IsCellInitialized())
{
for (int64_t x_index = 0; x_index < current_chunk.GetNumXCells(); x_index++)
{
for (int64_t y_index = 0; y_index < current_chunk.GetNumYCells(); y_index++)
{
for (int64_t z_index = 0; z_index < current_chunk.GetNumZCells(); z_index++)
{
const COLLISION_CELL& current_cell = current_chunk.GetImmutableByIndex(x_index, y_index, z_index).first;
std::vector<double> cell_location_in_grid = current_chunk.GetIndexLocationInGrid(x_index, y_index, z_index);
Eigen::Vector3d grid_location(cell_location_in_grid[0], cell_location_in_grid[1], cell_location_in_grid[2]);
Eigen::Vector3d location = grid_transform * grid_location;
geometry_msgs::Point new_point;
new_point.x = location.x();
new_point.y = location.y();
new_point.z = location.z();
cells_display_rep.points.push_back(new_point);
if (current_cell.occupancy > 0.5)
{
cells_display_rep.colors.push_back(collision_color);
}
else if (current_cell.occupancy < 0.5)
{
cells_display_rep.colors.push_back(free_color);
}
else
{
cells_display_rep.colors.push_back(unknown_color);
}
}
}
}
}
}
// Assemble the data to return
std::vector<visualization_msgs::Marker> display_markers;
if (chunks_display_rep.points.size() > 0)
{
display_markers.push_back(chunks_display_rep);
}
if (cells_display_rep.points.size() > 0)
{
display_markers.push_back(cells_display_rep);
}
return display_markers;
}
}

1330
flightlib/third_party/sdf_tools/src/sdf_tools/sdf.cpp vendored Normal file
View File

File diff suppressed because it is too large Load Diff

584
flightlib/third_party/sdf_tools/src/sdf_tools/sdf_builder.cpp vendored Normal file
View File

@@ -0,0 +1,584 @@
#include <stdlib.h>
#include <stdio.h>
#include <vector>
#include <string>
#include <sstream>
#include <iostream>
#include <fstream>
#include <stdexcept>
#include <zlib.h>
#include <octomap_msgs/conversions.h>
#include "sdf_tools/sdf.hpp"
#include "sdf_tools/sdf_builder.hpp"
#include "sdf_tools/SDF.h"
namespace sdf_tools
{
double ComputeDistanceSquared(int32_t x1, int32_t y1, int32_t z1, int32_t x2, int32_t y2, int32_t z2)
{
int32_t dx = x1 - x2;
int32_t dy = y1 - y2;
int32_t dz = z1 - z2;
return double((dx * dx) + (dy * dy) + (dz * dz));
}
SDF_Builder::SDF_Builder(ros::NodeHandle& nh, Eigen::Isometry3d origin_transform, std::string frame, double x_size, double y_size, double z_size, double resolution, float OOB_value, std::string planning_scene_service) : nh_(nh)
{
origin_transform_ = origin_transform;
frame_ = frame;
x_size_ = x_size;
y_size_ = y_size;
z_size_ = z_size;
resolution_ = resolution;
OOB_value_ = OOB_value;
if (!BuildInternalPlanningScene())
{
throw std::invalid_argument("Unable to construct internal planning scene");
}
initialized_ = true;
has_cached_sdf_ = false;
has_cached_collmap_ = false;
has_planning_scene_ = false;
planning_scene_client_ = nh.serviceClient<moveit_msgs::GetPlanningScene>(planning_scene_service);
}
SDF_Builder::SDF_Builder(ros::NodeHandle& nh, std::string frame, double x_size, double y_size, double z_size, double resolution, float OOB_value, std::string planning_scene_service) : nh_(nh)
{
Eigen::Translation3d origin_translation(-x_size * 0.5, -y_size * 0.5, -z_size * 0.5);
Eigen::Quaterniond origin_rotation;
origin_rotation.setIdentity();
origin_transform_ = origin_translation * origin_rotation;
frame_ = frame;
x_size_ = x_size;
y_size_ = y_size;
z_size_ = z_size;
resolution_ = resolution;
OOB_value_ = OOB_value;
if (!BuildInternalPlanningScene())
{
throw std::invalid_argument("Unable to construct internal planning scene");
}
initialized_ = true;
has_cached_sdf_ = false;
has_cached_collmap_ = false;
has_planning_scene_ = false;
planning_scene_client_ = nh.serviceClient<moveit_msgs::GetPlanningScene>(planning_scene_service);
}
SDF_Builder::SDF_Builder()
{
initialized_ = false;
has_cached_sdf_ = false;
has_cached_collmap_ = false;
has_planning_scene_ = false;
}
bool SDF_Builder::BuildInternalPlanningScene()
{
/* Builds a planning scene from XML string urdf and srdf descriptions */
// Make the URDF model
boost::shared_ptr<urdf::Model> urdf_model(new urdf::Model());
urdf_model->initString(GenerateSDFComputeBotURDFString());
// Make the SRDF model
boost::shared_ptr<srdf::Model> srdf_model(new srdf::Model());
srdf_model->initString(*urdf_model, GenerateSDFComputeBotSRDFString());
// Make the planning scene
planning_scene_ptr_.reset();
planning_scene_ptr_ = std::shared_ptr<planning_scene::PlanningScene>(new planning_scene::PlanningScene(urdf_model, srdf_model));
return true;
}
std::string SDF_Builder::GenerateSDFComputeBotURDFString()
{
// Figure out the minimum+maximum X,Y,Z values (for safety, we pad them out to make sure)
double min_x_limit = origin_transform_.translation().x() - fabs(2 * x_size_);
double max_x_limit = origin_transform_.translation().x() + fabs(2 * x_size_);
double min_y_limit = origin_transform_.translation().y() - fabs(2 * y_size_);
double max_y_limit = origin_transform_.translation().y() + fabs(2 * y_size_);
double min_z_limit = origin_transform_.translation().z() - fabs(2 * z_size_);
double max_z_limit = origin_transform_.translation().z() + fabs(2 * z_size_);
// Make the limits into strings
std::ostringstream mnxls_strm;
mnxls_strm << min_x_limit;
std::string min_x_limit_str = mnxls_strm.str();
std::ostringstream mxxls_strm;
mxxls_strm << max_x_limit;
std::string max_x_limit_str = mxxls_strm.str();
std::ostringstream mnyls_strm;
mnyls_strm << min_y_limit;
std::string min_y_limit_str = mnyls_strm.str();
std::ostringstream mxyls_strm;
mxyls_strm << max_y_limit;
std::string max_y_limit_str = mxyls_strm.str();
std::ostringstream mnzls_strm;
mnzls_strm << min_z_limit;
std::string min_z_limit_str = mnzls_strm.str();
std::ostringstream mxzls_strm;
mxzls_strm << max_z_limit;
std::string max_z_limit_str = mxzls_strm.str();
// Figure out the cell resolution
std::ostringstream crs_strm;
crs_strm << resolution_;
std::string cell_resolution_str = crs_strm.str();
// Make the URDF xml string
std::string urdf_string;
urdf_string = "<?xml version=\"1.0\" ?>\n<robot name=\"sdf_compute_bot\">\n<link name=\"" + frame_ + "\">\n</link>\n<joint name=\"virtual_x\" type=\"prismatic\">\n<parent link=\"" + frame_ + "\"/>\n<child link=\"x_stage\"/>\n<origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>\n<axis xyz=\"1 0 0\"/>\n<limit effort=\"10.0\" lower=\"" + min_x_limit_str + "\" upper=\"10.0\" velocity=\"" + max_x_limit_str + "\"/>\n</joint>\n<link name=\"x_stage\">\n</link>\n<joint name=\"virtual_y\" type=\"prismatic\">\n<parent link=\"x_stage\"/>\n<child link=\"y_stage\"/>\n<origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>\n<axis xyz=\"0 1 0\"/>\n<limit effort=\"10.0\" lower=\"" + min_y_limit_str + "\" upper=\"" + max_y_limit_str + "\" velocity=\"10.0\"/>\n</joint>\n<link name=\"y_stage\">\n</link>\n<joint name=\"virtual_z\" type=\"prismatic\">\n<parent link=\"y_stage\"/>\n<child link=\"sdf_bot\"/>\n<origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>\n<axis xyz=\"0 0 1\"/>\n<limit effort=\"10.0\" lower=\"" + min_z_limit_str + "\" upper=\"" + max_z_limit_str + "\" velocity=\"10.0\"/>\n</joint>\n<link name=\"sdf_bot\">\n<visual>\n<geometry>\n<box size=\"" + cell_resolution_str + " " + cell_resolution_str + " " + cell_resolution_str + "\"/>\n</geometry>\n<origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>\n<material name=\"\">\n<color rgba=\"0.3 0.3 0.3 1.0\"/>\n</material>\n</visual>\n</link>\n</robot>";
return urdf_string;
}
std::string SDF_Builder::GenerateSDFComputeBotSRDFString()
{
std::string srdf_string;
srdf_string = "<?xml version=\"1.0\" ?>\n<robot name=\"sdf_compute_bot\">\n<group name=\"platform\">\n<joint name=\"virtual_x\" />\n<joint name=\"virtual_y\" />\n<joint name=\"virtual_z\" />\n</group>\n<disable_collisions link1=\"" + frame_ + "\" link2=\"x_stage\" reason=\"Adjacent\" />\n<disable_collisions link1=\"" + frame_ + "\" link2=\"y_stage\" reason=\"Adjacent\" />\n<disable_collisions link1=\"" + frame_ + "\" link2=\"sdf_bot\" reason=\"Adjacent\" />\n<disable_collisions link1=\"x_stage\" link2=\"y_stage\" reason=\"Adjacent\" />\n<disable_collisions link1=\"x_stage\" link2=\"sdf_bot\" reason=\"Adjacent\" />\n<disable_collisions link1=\"y_stage\" link2=\"sdf_bot\" reason=\"Adjacent\" />\n</robot>";
return srdf_string;
}
SignedDistanceField SDF_Builder::UpdateSDF(uint8_t update_mode)
{
if (!initialized_)
{
throw std::invalid_argument("SDF Builder has not been initialized");
}
if (update_mode == USE_CACHED)
{
if (has_planning_scene_)
{
// Build the SDF
return UpdateSDFFromPlanningScene();
}
else
{
ROS_ERROR("No planning scene available");
throw std::invalid_argument("No planning scene available");
}
}
else
{
if (update_mode == USE_ONLY_COLLISION_OBJECTS)
{
// Update the planning scene
moveit_msgs::GetPlanningSceneRequest ps_req;
ps_req.components.components = moveit_msgs::PlanningSceneComponents::WORLD_OBJECT_NAMES | moveit_msgs::PlanningSceneComponents::WORLD_OBJECT_GEOMETRY;
moveit_msgs::GetPlanningSceneResponse ps_res;
planning_scene_client_.call(ps_req, ps_res);
moveit_msgs::PlanningScene& planning_scene_state = ps_res.scene;
planning_scene_ptr_->usePlanningSceneMsg(planning_scene_state);
has_planning_scene_ = true;
// Build the SDF
return UpdateSDFFromPlanningScene();
}
else if (update_mode == USE_ONLY_OCTOMAP)
{
// Update the planning scene
moveit_msgs::GetPlanningSceneRequest ps_req;
ps_req.components.components = moveit_msgs::PlanningSceneComponents::OCTOMAP;
moveit_msgs::GetPlanningSceneResponse ps_res;
planning_scene_client_.call(ps_req, ps_res);
moveit_msgs::PlanningScene& planning_scene_state = ps_res.scene;
planning_scene_ptr_->usePlanningSceneMsg(planning_scene_state);
has_planning_scene_ = true;
// Build the SDF
return UpdateSDFFromPlanningScene();
}
else if (update_mode == USE_FULL_PLANNING_SCENE)
{
// Update the planning scene
moveit_msgs::GetPlanningSceneRequest ps_req;
ps_req.components.components = moveit_msgs::PlanningSceneComponents::WORLD_OBJECT_NAMES | moveit_msgs::PlanningSceneComponents::WORLD_OBJECT_GEOMETRY | moveit_msgs::PlanningSceneComponents::OCTOMAP;
moveit_msgs::GetPlanningSceneResponse ps_res;
planning_scene_client_.call(ps_req, ps_res);
moveit_msgs::PlanningScene& planning_scene_state = ps_res.scene;
planning_scene_ptr_->usePlanningSceneMsg(planning_scene_state);
has_planning_scene_ = true;
// Build the SDF
return UpdateSDFFromPlanningScene();
}
else
{
ROS_ERROR("Invalid update mode (mode not recognized)");
throw std::invalid_argument("Invalid update mode (mode not recognized)");
}
}
}
SignedDistanceField SDF_Builder::GetCachedSDF()
{
if (has_cached_sdf_)
{
return cached_sdf_;
}
else
{
ROS_ERROR("No cached SDF available");
throw std::invalid_argument("No cached SDF available");
}
}
VoxelGrid::VoxelGrid<uint8_t> SDF_Builder::UpdateCollisionMap(uint8_t update_mode)
{
if (!initialized_)
{
throw std::invalid_argument("SDF Builder has not been initialized");
}
if (update_mode == USE_CACHED)
{
if (has_planning_scene_)
{
// Build the collision map
return UpdateCollisionMapFromPlanningScene();
}
else
{
ROS_ERROR("No planning scene available");
throw std::invalid_argument("No planning scene available");
}
}
else
{
if (update_mode == USE_ONLY_COLLISION_OBJECTS)
{
// Update the planning scene
moveit_msgs::GetPlanningSceneRequest ps_req;
ps_req.components.components = moveit_msgs::PlanningSceneComponents::WORLD_OBJECT_NAMES | moveit_msgs::PlanningSceneComponents::WORLD_OBJECT_GEOMETRY;
moveit_msgs::GetPlanningSceneResponse ps_res;
planning_scene_client_.call(ps_req, ps_res);
moveit_msgs::PlanningScene& planning_scene_state = ps_res.scene;
planning_scene_ptr_->usePlanningSceneMsg(planning_scene_state);
has_planning_scene_ = true;
// Build the collision map
return UpdateCollisionMapFromPlanningScene();
}
else if (update_mode == USE_ONLY_OCTOMAP)
{
// Update the planning scene
moveit_msgs::GetPlanningSceneRequest ps_req;
ps_req.components.components = moveit_msgs::PlanningSceneComponents::OCTOMAP;
moveit_msgs::GetPlanningSceneResponse ps_res;
planning_scene_client_.call(ps_req, ps_res);
moveit_msgs::PlanningScene& planning_scene_state = ps_res.scene;
planning_scene_ptr_->usePlanningSceneMsg(planning_scene_state);
has_planning_scene_ = true;
// Build the collision map
return UpdateCollisionMapFromPlanningScene();
}
else if (update_mode == USE_FULL_PLANNING_SCENE)
{
// Update the planning scene
moveit_msgs::GetPlanningSceneRequest ps_req;
ps_req.components.components = moveit_msgs::PlanningSceneComponents::WORLD_OBJECT_NAMES | moveit_msgs::PlanningSceneComponents::WORLD_OBJECT_GEOMETRY | moveit_msgs::PlanningSceneComponents::OCTOMAP;
moveit_msgs::GetPlanningSceneResponse ps_res;
planning_scene_client_.call(ps_req, ps_res);
moveit_msgs::PlanningScene& planning_scene_state = ps_res.scene;
planning_scene_ptr_->usePlanningSceneMsg(planning_scene_state);
has_planning_scene_ = true;
// Build the collision map
return UpdateCollisionMapFromPlanningScene();
}
else
{
ROS_ERROR("Invalid update mode (mode not recognized)");
throw std::invalid_argument("Invalid update mode (mode not recognized)");
}
}
}
VoxelGrid::VoxelGrid<uint8_t> SDF_Builder::GetCachedCollisionMap()
{
if (has_cached_collmap_)
{
return cached_collmap_;
}
else
{
ROS_ERROR("No cached Collision Map available");
throw std::invalid_argument("No cached Collision Map available");
}
}
VoxelGrid::VoxelGrid<uint8_t> SDF_Builder::UpdateCollisionMapFromPlanningScene()
{
// Make a collision field for debugging
VoxelGrid::VoxelGrid<uint8_t> collision_field(origin_transform_, resolution_, x_size_, y_size_, z_size_, 0);
// Loop through the planning scene to populate the voxel grids
std::string x_joint("virtual_x");
std::string y_joint("virtual_y");
std::string z_joint("virtual_z");
collision_detection::CollisionRequest col_req;
collision_detection::CollisionResult col_res;
robot_state::RobotState& sdf_compute_bot_state = planning_scene_ptr_->getCurrentStateNonConst();
for (int64_t x_index = 0; x_index < collision_field.GetNumXCells(); x_index++)
{
for (int64_t y_index = 0; y_index < collision_field.GetNumYCells(); y_index++)
{
for (int64_t z_index = 0; z_index < collision_field.GetNumZCells(); z_index++)
{
// Convert SDF indices into a real-world location
std::vector<double> location = collision_field.GridIndexToLocation(x_index, y_index, z_index);
double x = location[0];
double y = location[1];
double z = location[2];
// Set them
sdf_compute_bot_state.setJointPositions(x_joint, &x);
sdf_compute_bot_state.setJointPositions(y_joint, &y);
sdf_compute_bot_state.setJointPositions(z_joint, &z);
col_res.clear();
planning_scene_ptr_->checkCollision(col_req, col_res);
if (col_res.collision)
{
// Mark as filled
//std::cout << "Collision" << std::endl;
uint8_t status = 1;
collision_field.SetValue(x_index, y_index, z_index, status);
}
else
{
// Mark as free space
//std::cout << "No collision" << std::endl;
uint8_t status = 0;
collision_field.SetValue(x_index, y_index, z_index, status);
}
}
}
}
// Export the collision map
cached_collmap_ = collision_field;
has_cached_collmap_ = true;
return collision_field;
}
SignedDistanceField SDF_Builder::UpdateSDFFromPlanningScene()
{
// Make the SDF
SignedDistanceField new_sdf(origin_transform_, frame_, resolution_, x_size_, y_size_, z_size_, OOB_value_);
// Loop through the planning scene to populate the voxel grids
std::vector<Eigen::Vector3i> filled;
std::vector<Eigen::Vector3i> free;
std::string x_joint("virtual_x");
std::string y_joint("virtual_y");
std::string z_joint("virtual_z");
collision_detection::CollisionRequest col_req;
collision_detection::CollisionResult col_res;
robot_state::RobotState& sdf_compute_bot_state = planning_scene_ptr_->getCurrentStateNonConst();
for (uint32_t x_index = 0; x_index < new_sdf.GetNumXCells(); x_index++)
{
for (uint32_t y_index = 0; y_index < new_sdf.GetNumYCells(); y_index++)
{
for (uint32_t z_index = 0; z_index < new_sdf.GetNumZCells(); z_index++)
{
// Convert SDF indices into a real-world location
std::vector<double> location = new_sdf.GridIndexToLocation(x_index, y_index, z_index);
double x = location[0];
double y = location[1];
double z = location[2];
sdf_compute_bot_state.setJointPositions(x_joint, &x);
sdf_compute_bot_state.setJointPositions(y_joint, &y);
sdf_compute_bot_state.setJointPositions(z_joint, &z);
col_res.clear();
planning_scene_ptr_->checkCollision(col_req, col_res);
if (col_res.collision)
{
// Mark as filled
filled.push_back(Eigen::Vector3i(x_index, y_index, z_index));
}
else
{
// Mark as free space
free.push_back(Eigen::Vector3i(x_index, y_index, z_index));
}
}
}
}
// Make two distance fields (one for distance to filled voxels, one for distance to free voxels
DistanceField filled_distance_field = BuildDistanceField(filled);
DistanceField free_distance_field = BuildDistanceField(free);
// Generate the SDF
for (int64_t x_index = 0; x_index < filled_distance_field.GetNumXCells(); x_index++)
{
for (int64_t y_index = 0; y_index < filled_distance_field.GetNumYCells(); y_index++)
{
for (int64_t z_index = 0; z_index < filled_distance_field.GetNumZCells(); z_index++)
{
double distance1 = sqrt(filled_distance_field.GetImmutable(x_index, y_index, z_index).first.distance_square) * resolution_;
double distance2 = sqrt(free_distance_field.GetImmutable(x_index, y_index, z_index).first.distance_square) * resolution_;
new_sdf.Set(x_index, y_index, z_index, (distance1 - distance2));
}
}
}
// Export the SDF
cached_sdf_ = new_sdf;
has_cached_sdf_ = true;
return new_sdf;
}
std::vector<std::vector<std::vector<std::vector<int>>>> SDF_Builder::MakeNeighborhoods()
{
std::vector<std::vector<std::vector<std::vector<int>>>> neighborhoods;
neighborhoods.resize(2);
for (size_t n = 0; n < neighborhoods.size(); n++)
{
neighborhoods[n].resize(27);
// Loop through the source directions
for (int dx = -1; dx <= 1; dx++)
{
for (int dy = -1; dy <= 1; dy++)
{
for (int dz = -1; dz <= 1; dz++)
{
int direction_number = GetDirectionNumber(dx, dy, dz);
// Loop through the target directions
for (int tdx = -1; tdx <= 1; tdx++)
{
for (int tdy = -1; tdy <= 1; tdy++)
{
for (int tdz = -1; tdz <= 1; tdz++)
{
if (tdx == 0 && tdy == 0 && tdz == 0)
{
continue;
}
if (n >= 1)
{
if ((abs(tdx) + abs(tdy) + abs(tdz)) != 1)
{
continue;
}
if ((dx * tdx) < 0 || (dy * tdy) < 0 || (dz * tdz) < 0)
{
continue;
}
}
std::vector<int> new_point;
new_point.resize(3);
new_point[0] = tdx;
new_point[1] = tdy;
new_point[2] = tdz;
neighborhoods[n][direction_number].push_back(new_point);
}
}
}
}
}
}
}
return neighborhoods;
}
int SDF_Builder::GetDirectionNumber(int dx, int dy, int dz)
{
return ((dx + 1) * 9) + ((dy + 1) * 3) + (dz + 1);
}
DistanceField SDF_Builder::BuildDistanceField(std::vector<Eigen::Vector3i>& points)
{
// Make the DistanceField container
bucket_cell default_cell;
default_cell.distance_square = INFINITY;
DistanceField distance_field(origin_transform_, resolution_, x_size_, y_size_, z_size_, default_cell);
// Compute maximum distance square
long max_distance_square = (distance_field.GetNumXCells() * distance_field.GetNumXCells()) + (distance_field.GetNumYCells() * distance_field.GetNumYCells()) + (distance_field.GetNumZCells() * distance_field.GetNumZCells());
// Make bucket queue
std::vector<std::vector<bucket_cell>> bucket_queue(max_distance_square + 1);
bucket_queue[0].reserve(points.size());
// Set initial update direction
int initial_update_direction = GetDirectionNumber(0, 0, 0);
// Mark all points with distance zero and add to the bucket queue
for (size_t index = 0; index < points.size(); index++)
{
std::pair<bucket_cell&, bool> query = distance_field.GetMutable((int64_t)points[index].x(), (int64_t)points[index].y(), (int64_t)points[index].z());
if (query.second)
{
query.first.location[0] = points[index].x();
query.first.location[1] = points[index].y();
query.first.location[2] = points[index].z();
query.first.closest_point[0] = points[index].x();
query.first.closest_point[1] = points[index].y();
query.first.closest_point[2] = points[index].z();
query.first.distance_square = 0.0;
query.first.update_direction = initial_update_direction;
bucket_queue[0].push_back(query.first);
}
// If the point is outside the bounds of the SDF, skip
else
{
continue;
}
}
// Process the bucket queue
std::vector<std::vector<std::vector<std::vector<int>>>> neighborhoods = MakeNeighborhoods();
for (size_t bq_idx = 0; bq_idx < bucket_queue.size(); bq_idx++)
{
std::vector<bucket_cell>::iterator queue_itr = bucket_queue[bq_idx].begin();
while (queue_itr != bucket_queue[bq_idx].end())
{
// Get the current location
bucket_cell& cur_cell = *queue_itr;
double x = cur_cell.location[0];
double y = cur_cell.location[1];
double z = cur_cell.location[2];
// Pick the update direction
int D = bq_idx;
if (D > 1)
{
D = 1;
}
// Make sure the update direction is valid
if (cur_cell.update_direction < 0 || cur_cell.update_direction > 26)
{
++queue_itr;
continue;
}
// Get the current neighborhood list
std::vector<std::vector<int>>& neighborhood = neighborhoods[D][cur_cell.update_direction];
// Update the distance from the neighboring cells
for (size_t nh_idx = 0; nh_idx < neighborhood.size(); nh_idx++)
{
// Get the direction to check
int dx = neighborhood[nh_idx][0];
int dy = neighborhood[nh_idx][1];
int dz = neighborhood[nh_idx][2];
int nx = x + dx;
int ny = y + dy;
int nz = z + dz;
std::pair<bucket_cell&, bool> neighbor_query = distance_field.GetMutable((int64_t)nx, (int64_t)ny, (int64_t)nz);
if (!neighbor_query.second)
{
// "Neighbor" is outside the bounds of the SDF
continue;
}
// Update the neighbor's distance based on the current
int new_distance_square = ComputeDistanceSquared(nx, ny, nz, cur_cell.closest_point[0], cur_cell.closest_point[1], cur_cell.closest_point[2]);
if (new_distance_square > max_distance_square)
{
// Skip these cases
continue;
}
if (new_distance_square < neighbor_query.first.distance_square)
{
// If the distance is better, time to update the neighbor
neighbor_query.first.distance_square = new_distance_square;
neighbor_query.first.closest_point[0] = cur_cell.closest_point[0];
neighbor_query.first.closest_point[1] = cur_cell.closest_point[1];
neighbor_query.first.closest_point[2] = cur_cell.closest_point[2];
neighbor_query.first.location[0] = nx;
neighbor_query.first.location[1] = ny;
neighbor_query.first.location[2] = nz;
neighbor_query.first.update_direction = GetDirectionNumber(dx, dy, dz);
// Add the neighbor into the bucket queue
bucket_queue[new_distance_square].push_back(neighbor_query.first);
}
}
// Increment the queue iterator
++queue_itr;
}
// Clear the current queue now that we're done with it
bucket_queue[bq_idx].clear();
}
return distance_field;
}
void SDF_Builder::UpdatePlanningSceneFromMessage(moveit_msgs::PlanningScene& planning_scene)
{
planning_scene_ptr_->usePlanningSceneMsg(planning_scene);
has_planning_scene_ = true;
}
}

2605
flightlib/third_party/sdf_tools/src/sdf_tools/tagged_object_collision_map.cpp vendored Normal file
View File

File diff suppressed because it is too large Load Diff

155
flightlib/third_party/sdf_tools/src/sdf_tools_tutorial.cpp vendored Normal file
View File

@@ -0,0 +1,155 @@
#include <ros/ros.h>
#include <arc_utilities/voxel_grid.hpp>
#include <arc_utilities/pretty_print.hpp>
#include "sdf_tools/collision_map.hpp"
#include "sdf_tools/CollisionMap.h"
#include "sdf_tools/sdf.hpp"
#include "sdf_tools/SDF.h"
int main(int argc, char** argv)
{
// Make a ROS node, which we'll use to publish copies of the data in the CollisionMap and SDF
// and Rviz markers that allow us to visualize them.
ros::init(argc, argv, "sdf_tools_tutorial");
// Get a handle to the current node
ros::NodeHandle nh;
// Make a publisher for visualization messages
ros::Publisher visualization_pub = nh.advertise<visualization_msgs::Marker>("sdf_tools_tutorial_visualization", 1, true);
// Make a publisher for serialized CollisionMaps
ros::Publisher collision_map_pub = nh.advertise<sdf_tools::CollisionMap>("collision_map_pub", 1, true);
// Make a publisher for serialized SDFs
ros::Publisher sdf_pub = nh.advertise<sdf_tools::SDF>("sdf_pub", 1, true);
// In preparation, we want to set a couple common paramters
double resolution = 0.25;
double x_size = 10.0;
double y_size = 10.0;
double z_size = 10.0;
// Let's center the grid around the origin
Eigen::Translation3d origin_translation(-5.0, -5.0, -5.0);
Eigen::Quaterniond origin_rotation(1.0, 0.0, 0.0, 0.0);
Eigen::Isometry3d origin_transform = origin_translation * origin_rotation;
std::string frame = "tutorial_frame";
///////////////////////////////////
//// Let's make a CollisionMap ////
///////////////////////////////////
// We pick a reasonable out-of-bounds value
sdf_tools::COLLISION_CELL oob_cell;
oob_cell.occupancy = 0.0;
oob_cell.component = 0; // This should ALWAYS be zero, unless you know exactly what you're doing
// Instead, we could initialize it like this - the component value is automatically set to 0
sdf_tools::COLLISION_CELL oob_cell_2(0.0);
// First, let's make the container
sdf_tools::CollisionMapGrid collision_map(origin_transform, frame, resolution, x_size, y_size, z_size, oob_cell);
// Let's set some values
// This is how you should iterate through the 3D grid's cells
for (int64_t x_index = 0; x_index < collision_map.GetNumXCells(); x_index++)
{
for (int64_t y_index = 0; y_index < collision_map.GetNumYCells(); y_index++)
{
for (int64_t z_index = 0; z_index < collision_map.GetNumZCells(); z_index++)
{
// Let's make the bottom corner (low x, low y, low z) an object
if ((x_index < (collision_map.GetNumXCells() / 2)) && (y_index < (collision_map.GetNumYCells() / 2)) && (z_index < (collision_map.GetNumZCells() / 2)))
{
sdf_tools::COLLISION_CELL obstacle_cell(1.0); // Occupancy values > 0.5 are obstacles
collision_map.Set(x_index, y_index, z_index, obstacle_cell);
}
}
}
}
// We can also set by location
sdf_tools::COLLISION_CELL obstacle_cell(1.0); // Occupancy values > 0.5 are obstacles
collision_map.Set(0.0, 0.0, 0.0, obstacle_cell);
// Let's get some values
// We can query by index
int64_t x_index = 10;
int64_t y_index = 10;
int64_t z_index = 10;
std::pair<sdf_tools::COLLISION_CELL, bool> index_query = collision_map.Get(x_index, y_index, z_index);
std::cout << "Index query result - stored value " << index_query.first.occupancy << " (occupancy) " << index_query.first.component << " (component) was it in the grid? - " << index_query.second << std::endl;
// Or we can query by location
double x_location = 0.0;
double y_location = 0.0;
double z_location = 0.0;
std::pair<sdf_tools::COLLISION_CELL, bool> location_query = collision_map.Get(x_location, y_location, z_location);
std::cout << "Location query result - stored value " << location_query.first.occupancy << " (occupancy) " << location_query.first.component << " (component) was it in the grid? - " << location_query.second << std::endl;
// Let's compute connected components
uint32_t num_connected_components = collision_map.UpdateConnectedComponents();
std::cout << " There are " << num_connected_components << " connected components in the grid" << std::endl;
// Let's display the results to Rviz
// First, the CollisionMap itself
// We need to provide colors to use
std_msgs::ColorRGBA collision_color;
collision_color.r = 1.0;
collision_color.g = 0.0;
collision_color.b = 0.0;
collision_color.a = 0.5;
std_msgs::ColorRGBA free_color;
free_color.r = 0.0;
free_color.g = 1.0;
free_color.b = 0.0;
free_color.a = 0.5;
std_msgs::ColorRGBA unknown_color;
unknown_color.r = 1.0;
unknown_color.g = 1.0;
unknown_color.b = 0.0;
unknown_color.a = 0.5;
visualization_msgs::Marker collision_map_marker = collision_map.ExportForDisplay(collision_color, free_color, unknown_color);
// To be safe, you'll need to set these yourself. The namespace (ns) value should distinguish between different things being displayed
// while the id value lets you have multiple versions of the same message at once. Always set this to 1 if you only want one copy.
collision_map_marker.ns = "collision_map";
collision_map_marker.id = 1;
// Send it off for display
visualization_pub.publish(collision_map_marker);
// Now, let's draw the connected components
visualization_msgs::Marker connected_components_marker = collision_map.ExportConnectedComponentsForDisplay(false); // Generally, you don't want a special color for unknown [P(occupancy) = 0.5] components
connected_components_marker.ns = "connected_components";
connected_components_marker.id = 1;
visualization_pub.publish(connected_components_marker);
// Let's export the CollisionMap - this is how you can transfer it to another ROS node
collision_map_pub.publish(collision_map.GetMessageRepresentation());
// You can also save it to a file
std::string collision_map_filename = "collision_map.cmg";
collision_map.SaveToFile(collision_map_filename);
// And load it back in
bool loaded = collision_map.LoadFromFile(collision_map_filename);
if (loaded)
{
std::cout << "Reloaded CollisionMap from file" << std::endl;
}
else
{
std::cerr << "Whoa, something broke!" << std::endl;
}
///////////////////////////
//// Let's make an SDF ////
///////////////////////////
// We pick a reasonable out-of-bounds value
float oob_value = INFINITY;
// We start by extracting the SDF from the CollisionMap
std::pair<sdf_tools::SignedDistanceField, std::pair<double, double>> sdf_with_extrema = collision_map.ExtractSignedDistanceField(oob_value);
sdf_tools::SignedDistanceField& sdf = sdf_with_extrema.first;
std::pair<double, double> sdf_extrema = sdf_with_extrema.second;
std::cout << "Maximum distance in the SDF: " << sdf_extrema.first << ", minimum distance in the SDF: " << sdf_extrema.second << std::endl;
// We lock the SDF to prevent unintended changes that would invalidate it
sdf.Lock();
// Let's get some values
std::pair<float, bool> index_sdf_query = sdf.GetSafe(x_index, y_index, z_index);
std::cout << "Index query result - stored distance " << index_sdf_query.first << " was it in the grid? - " << index_sdf_query.second << std::endl;
std::pair<float, bool> location_sdf_query = sdf.GetSafe(x_location, y_location, z_location);
std::cout << "Location query result - stored distance " << location_sdf_query.first << " was it in the grid? - " << location_sdf_query.second << std::endl;
// Let's get some gradients
std::vector<double> index_gradient_query = sdf.GetGradient(x_index, y_index, z_index, true); // Usually, you want to enable 'edge gradients' i.e. gradients for cells on the edge of the grid that don't have 6 neighbors
std::cout << "Index gradient query result - gradient " << PrettyPrint::PrettyPrint(index_gradient_query) << std::endl;
std::vector<double> location_gradient_query = sdf.GetGradient(x_location, y_location, z_location, true); // Usually, you want to enable 'edge gradients' i.e. gradients for cells on the edge of the grid that don't have 6 neighbors
std::cout << "Location gradient query result - gradient " << PrettyPrint::PrettyPrint(location_gradient_query) << std::endl;
// Let's display the results to Rviz
visualization_msgs::Marker sdf_marker = sdf.ExportForDisplay(0.5); // Set the alpha for display
sdf_marker.ns = "sdf";
sdf_marker.id = 1;
visualization_pub.publish(sdf_marker);
// Let's export the SDF
sdf_pub.publish(sdf.GetMessageRepresentation());
std::cout << "...done" << std::endl;
return 0;
}

345
flightlib/third_party/sdf_tools/src/test_voxel_grid.cpp vendored Normal file
View File

@@ -0,0 +1,345 @@
#include "arc_utilities/voxel_grid.hpp"
#include "arc_utilities/pretty_print.hpp"
#include "sdf_tools/collision_map.hpp"
#include "arc_utilities/dynamic_spatial_hashed_voxel_grid.hpp"
#include "sdf_tools/dynamic_spatial_hashed_collision_map.hpp"
#include "sdf_tools/sdf.hpp"
#include "ros/ros.h"
#include "visualization_msgs/MarkerArray.h"
#include <chrono>
#include <random>
void test_voxel_grid_indices()
{
VoxelGrid::VoxelGrid<int> test_grid(1.0, 20.0, 20.0, 20.0, 0);
// Load with special values
int check_val = 1;
std::vector<int> check_vals;
for (int64_t x_index = 0; x_index < test_grid.GetNumXCells(); x_index++)
{
for (int64_t y_index = 0; y_index < test_grid.GetNumYCells(); y_index++)
{
for (int64_t z_index = 0; z_index < test_grid.GetNumZCells(); z_index++)
{
test_grid.SetValue(x_index, y_index, z_index, check_val);
check_vals.push_back(check_val);
check_val++;
}
}
}
// Check the values
int check_index = 0;
bool pass = true;
for (int64_t x_index = 0; x_index < test_grid.GetNumXCells(); x_index++)
{
for (int64_t y_index = 0; y_index < test_grid.GetNumYCells(); y_index++)
{
for (int64_t z_index = 0; z_index < test_grid.GetNumZCells(); z_index++)
{
int ref_val = test_grid.GetImmutable(x_index, y_index, z_index).first;
//std::cout << "Value in grid: " << ref_val << " Value should be: " << check_vals[check_index] << std::endl;
if (ref_val == check_vals[check_index])
{
//std::cout << "Check pass" << std::endl;
}
else
{
std::cout << "Check fail" << std::endl;
pass = false;
}
check_index++;
}
}
}
if (pass)
{
std::cout << "VG-I - All checks pass" << std::endl;
}
else
{
std::cout << "*** VG-I - Checks failed ***" << std::endl;
}
}
void test_voxel_grid_locations()
{
VoxelGrid::VoxelGrid<int> test_grid(1.0, 20.0, 20.0, 20.0, 0);
// Load with special values
int check_val = 1;
std::vector<int> check_vals;
for (double x_pos = -9.5; x_pos <= 9.5; x_pos += 1.0)
{
for (double y_pos = -9.5; y_pos <= 9.5; y_pos += 1.0)
{
for (double z_pos = -9.5; z_pos <= 9.5; z_pos += 1.0)
{
test_grid.SetValue(x_pos, y_pos, z_pos, check_val);
check_vals.push_back(check_val);
check_val++;
}
}
}
// Check the values
int check_index = 0;
bool pass = true;
for (double x_pos = -9.5; x_pos <= 9.5; x_pos += 1.0)
{
for (double y_pos = -9.5; y_pos <= 9.5; y_pos += 1.0)
{
for (double z_pos = -9.5; z_pos <= 9.5; z_pos += 1.0)
{
int ref_val = test_grid.GetImmutable(x_pos, y_pos, z_pos).first;
//std::cout << "Value in grid: " << ref_val << " Value should be: " << check_vals[check_index] << std::endl;
if (ref_val == check_vals[check_index])
{
//std::cout << "Value check pass" << std::endl;
}
else
{
std::cout << "Value check fail" << std::endl;
pass = false;
}
check_index++;
std::vector<double> query_point = {x_pos, y_pos, z_pos};
//std::cout << "Query point - " << PrettyPrint::PrettyPrint(query_point) << std::endl;
std::vector<int64_t> query_index = test_grid.LocationToGridIndex(x_pos, y_pos, z_pos);
//std::cout << "Query index - " << PrettyPrint::PrettyPrint(query_index) << std::endl;
std::vector<double> query_location = test_grid.GridIndexToLocation(query_index[0], query_index[1], query_index[2]);
//std::cout << "Query location - " << PrettyPrint::PrettyPrint(query_location) << std::endl;
std::vector<int64_t> found_query_index = test_grid.LocationToGridIndex(query_location[0], query_location[1], query_location[2]);
//std::cout << "Found query index - " << PrettyPrint::PrettyPrint(found_query_index) << std::endl;
if (query_point[0] == query_location[0] && query_point[1] == query_location[1] && query_point[2] == query_location[2])
{
//std::cout << "Position check pass" << std::endl;
}
else
{
std::cout << "Position check fail" << std::endl;
pass = false;
}
if (query_index[0] == found_query_index[0] && query_index[1] == found_query_index[1] && query_index[2] == found_query_index[2])
{
//std::cout << "Position index check pass" << std::endl;
}
else
{
std::cout << "Position index check fail" << std::endl;
pass = false;
}
}
}
}
if (pass)
{
std::cout << "VG-L - All checks pass" << std::endl;
}
else
{
std::cout << "*** VG-L - Checks failed ***" << std::endl;
}
}
void test_voxel_grid_serialization()
{
VoxelGrid::VoxelGrid<int> test_grid(1.0, 20.0, 20.0, 20.0, 0);
// Load with special values
int check_val = 1;
std::vector<int> check_vals;
for (int64_t x_index = 0; x_index < test_grid.GetNumXCells(); x_index++)
{
for (int64_t y_index = 0; y_index < test_grid.GetNumYCells(); y_index++)
{
for (int64_t z_index = 0; z_index < test_grid.GetNumZCells(); z_index++)
{
test_grid.SetValue(x_index, y_index, z_index, check_val);
check_vals.push_back(check_val);
check_val++;
}
}
}
std::vector<uint8_t> buffer;
VoxelGrid::VoxelGrid<int>::Serialize(test_grid, buffer, arc_utilities::SerializeFixedSizePOD<int>);
const VoxelGrid::VoxelGrid<int> read_grid = VoxelGrid::VoxelGrid<int>::Deserialize(buffer, 0, arc_utilities::DeserializeFixedSizePOD<int>).first;
// Check the values
int check_index = 0;
bool pass = true;
for (int64_t x_index = 0; x_index < read_grid.GetNumXCells(); x_index++)
{
for (int64_t y_index = 0; y_index < read_grid.GetNumYCells(); y_index++)
{
for (int64_t z_index = 0; z_index < read_grid.GetNumZCells(); z_index++)
{
int ref_val = read_grid.GetImmutable(x_index, y_index, z_index).first;
//std::cout << "Value in grid: " << ref_val << " Value should be: " << check_vals[check_index] << std::endl;
if (ref_val == check_vals[check_index])
{
//std::cout << "Check pass" << std::endl;
}
else
{
std::cout << "Check fail" << std::endl;
pass = false;
}
check_index++;
}
}
}
if (pass)
{
std::cout << "VG-I de/serialize - All checks pass" << std::endl;
}
else
{
std::cout << "*** VG-I de/serialize - Checks failed ***" << std::endl;
}
}
void test_dsh_voxel_grid_locations()
{
VoxelGrid::DynamicSpatialHashedVoxelGrid<int> test_grid(1.0, 4, 4, 4, 0);
// Load with special values
int check_val = 1;
std::vector<int> check_vals;
for (double x_pos = -9.5; x_pos <= 9.5; x_pos += 1.0)
{
for (double y_pos = -9.5; y_pos <= 9.5; y_pos += 1.0)
{
for (double z_pos = -9.5; z_pos <= 9.5; z_pos += 1.0)
{
test_grid.SetCellValue(x_pos, y_pos, z_pos, check_val);
check_vals.push_back(check_val);
check_val++;
}
}
}
// Check the values
int check_index = 0;
bool pass = true;
for (double x_pos = -9.5; x_pos <= 9.5; x_pos += 1.0)
{
for (double y_pos = -9.5; y_pos <= 9.5; y_pos += 1.0)
{
for (double z_pos = -9.5; z_pos <= 9.5; z_pos += 1.0)
{
int ref_val = test_grid.GetImmutable(x_pos, y_pos, z_pos).first;
//std::cout << "Value in grid: " << ref_val << " Value should be: " << check_vals[check_index] << std::endl;
if (ref_val == check_vals[check_index])
{
//std::cout << "Value check pass" << std::endl;
}
else
{
std::cout << "Value check fail" << std::endl;
pass = false;
}
check_index++;
}
}
}
if (pass)
{
std::cout << "DSHVG - All checks pass" << std::endl;
}
else
{
std::cout << "*** DSHVG - Checks failed ***" << std::endl;
}
}
void test_float_binary_conversion(float test_val)
{
std::cout << "Initial value " << test_val << std::endl;
std::vector<uint8_t> binary_value = sdf_tools::FloatToBinary(test_val);
float final_val = sdf_tools::FloatFromBinary(binary_value);
std::cout << "Final value " << final_val << std::endl;
}
Eigen::Vector3d get_random_location(std::default_random_engine& generator, const double min_x, const double min_y, const double min_z, const double max_x, const double max_y, const double max_z)
{
std::uniform_real_distribution<double> x_distribution(min_x, max_x);
std::uniform_real_distribution<double> y_distribution(min_y, max_y);
std::uniform_real_distribution<double> z_distribution(min_z, max_z);
double rand_x = x_distribution(generator);
double rand_y = y_distribution(generator);
double rand_z = z_distribution(generator);
return Eigen::Vector3d(rand_x, rand_y, rand_z);
}
bool get_random_bool(std::default_random_engine& generator)
{
std::uniform_int_distribution<int> distribution(0,1);
int rand_int = distribution(generator);
if (rand_int)
{
return true;
}
else
{
return false;
}
}
visualization_msgs::MarkerArray test_dsh_collision_map(std::default_random_engine& generator)
{
sdf_tools::COLLISION_CELL default_cell(0.0);
sdf_tools::COLLISION_CELL filled_cell(1.0);
sdf_tools::DynamicSpatialHashedCollisionMapGrid test_col_map("test_voxel_grid", 1.0, 5, 5, 5, default_cell);
// Add a bunch of random data
for (int idx = 0; idx < 500; idx++)
{
// Get a random location in +-10m
Eigen::Vector3d random_location = get_random_location(generator, -20.0, -20.0, -20.0, 20.0, 20.0, 20.0);
// Get a random bool to choose between cell/chunk
bool use_cell = get_random_bool(generator);
// Update the col map
if (use_cell)
{
test_col_map.SetCell(random_location, filled_cell);
}
else
{
test_col_map.SetChunk(random_location, filled_cell);
}
}
// Get the Rviz markers
std_msgs::ColorRGBA filled_color;
filled_color.a = 1.0;
filled_color.b = 0.0;
filled_color.g = 0.0;
filled_color.r = 1.0;
std_msgs::ColorRGBA free_color;
free_color.a = 0.1;
free_color.b = 0.0;
free_color.g = 1.0;
free_color.r = 0.0;
std_msgs::ColorRGBA unknown_color;
unknown_color.a = 0.5;
unknown_color.b = 1.0;
unknown_color.g = 0.0;
unknown_color.r = 0.0;
std::vector<visualization_msgs::Marker> display_markers = test_col_map.ExportForDisplay(filled_color, free_color, unknown_color);
visualization_msgs::MarkerArray display_rep;
display_rep.markers = display_markers;
return display_rep;
}
int main(int argc, char** argv)
{
// construct a trivial random generator engine from a time-based seed:
unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
std::default_random_engine generator(seed);
ros::init(argc, argv, "test_voxel_grid");
ros::NodeHandle nh;
ros::Publisher display_pub = nh.advertise<visualization_msgs::MarkerArray>("display_test_voxel_grid", 1, true);
test_voxel_grid_indices();
test_voxel_grid_locations();
test_voxel_grid_serialization();
test_dsh_voxel_grid_locations();
test_float_binary_conversion(5280.0);
visualization_msgs::MarkerArray display_rep = test_dsh_collision_map(generator);
display_pub.publish(display_rep);
ros::spin();
return 0;
}

4
flightlib/third_party/sdf_tools/srv/ComputeSDF.srv vendored Normal file
View File

@@ -0,0 +1,4 @@
bool request_new
---
bool is_valid
sdf_tools/SDF sdf