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

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