um-arm-lab / arc_utilities Goto Github PK

So I ran into an irritating problem using arc_utilities/eigen_helpers.hpp in conjunction with sdf_tools/sdf_builder.hpp

One of the files included by sdf_tools/sdf_builder.hpp includes <Eigen/StdVector>. Depending on the include order, this causes problems.

The following code does not compile.

#include <arc_utilities/eigen_helpers.hpp>
#include <Eigen/StdVector>

int main(int argc, char** argv)
{
    (void)argc;
    (void)argv;
    return 0;
}

If I switch the include order so that <Eigen/StdVector> is the first include, everything compiles with no problem.

So I've currently tweaked my include order to put sdf_tools/sdf_builder.hpp' before the files that includearc_utilities/eigen_helpers.hpp` (I don't include it directly) as a work around, however this is not ideal.

The internet tells me this is probably a known issue already, but we may want to address it directly in arc_utlities anyway, as the last comment in the linked Eigen bugzilla seems to indicate that while Eigen has addressed this in some fashion in the devel branch, who knows how long till that makes it out to us.

strands-project/v4r#39

That and the instructions for using Eigen types with STL containers does say to include <Eigen/StdVector>

http://eigen.tuxfamily.org/dox-devel/group__TopicStlContainers.html#vector_spec

Right now one frequently needs to cast the lambdas to an appropriate std::function<()> before passing the lambda as a value (de)serializer. It would be far more convenient to use if this was not necessary.

PROFILE_RECORD_DOUBLE("name", val) segfault if called before the timer has been started.
This is unintuitive, since often we dont even care about the timer value

#16 and #17 added in a duplicate A* implementation inside of arc_helpers.hpp alongside the code in simple_astar_planner.hpp

@orthez
We don't want to duplicate code for the planner, since this means that bugs and changes will have to be made to two similar, but separate files. If a simpler interface to simple_rrt_planner.hpp is desired, then it should be built on top of the existing code - for example, a simpler interface that simply includes simple_rrt_planner.hpp and provides wrapper functions, rather than copying the planner directly.

arc_helpers.hpp:

inline Eigen::MatrixXd BuildDistanceMatrix(const std::vector<Datatype, Allocator>& data, const std::function<double(const Datatype&, const Datatype&)>& distance_fn)

eigen_helpers.hpp

template<typename ScalarType, int VectorLength, int NumVectors>
inline Eigen::Matrix<double, NumVectors, NumVectors> CalculateSquaredDistanceMatrix(const Eigen::Matrix<ScalarType, VectorLength, NumVectors>& set)

An "assert with stacktrace" macro could be useful for times when debugging with GDB just isn't practical.

Here is one such possibility (may need tweaking) adapted from https://stackoverflow.com/questions/37473/how-can-i-assert-without-using-abort

#include <execinfo.h>

void print_stack_trace(int fd)
{
    void *array[256];
    size_t size;

    size = backtrace (array, 256);
    backtrace_symbols_fd(array, size, fd);
}

#define ASSERT_STACKTRACE(expr)      do {                           \
 if (!(expr))                                                       \
 {                                                                  \
         fprintf(stderr,                                            \
                "file %s: line %d (%s): precondition `%s' failed.", \
                __FILE__,                                           \
                __LINE__,                                           \
                __PRETTY_FUNCTION__,                                \
                #expr);                                             \
         print_stack_trace(2);                                      \
         assert(expr);                                              \
 };               } while(0)

This duplication should be resolved, likely by moving the functionality from EigenHelpers into the new file and ensuring any code using either file works with the resulting file.

For example, CloseEnough, IsApprox etc. can and should be constexpr.

TravisCI works with mps_shape_completion. This would be a great repo for CI

For places where assert is used for tasks other than debugging/sanity checking, we should use custom assert macro that ignores NDEBUG. See #54 (comment)

Switching to clang briefly generates the following error:

catkin_ws/src/arc_utilities/include/arc_utilities/arc_helpers.hpp:17:10: fatal error: 'omp.h' file not found
#include <omp.h>
         ^
1 error generated.

Presumably this is due to missing compile flags:
https://stackoverflow.com/questions/33400462/omp-h-file-not-found-when-compiling-using-clang
https://cmake.org/cmake/help/v3.0/module/FindOpenMP.html

We also likely do not want to set these flags across the whole library, instead leaving them to be set by each end user library. We ought to find a way to let these headers be used under clang with or without OpenMP.

Might be worth including in arc_utilities:

http://aantron.github.io/better-enums/
https://www.gamedev.net/articles/programming/engines-and-middleware/maintenance-free-enum-to-string-in-pure-c-with-better-enums-r4230/

In short:

eigen_helpers.hpp

template<typename ScalarType, int VectorLength, int NumVectors>              
inline Eigen::Matrix<double, NumVectors, NumVectors> CalculateSquaredDistanceMatrix(const Eigen::Matrix<ScalarType, VectorLength, NumVectors>& set)

arc_helpers.hpp:

template<typename Datatype, typename Allocator = std::allocator<Datatype>>   
inline Eigen::MatrixXd BuildDistanceMatrix(const std::vector<Datatype, Allocator>& data, const std::function<double(const Datatype&, const Datatype&)>& distance_fn)

template<typename FirstDatatype, typename SecondDatatype, typename FirstAllocator = std::allocator<FirstDatatype>, typename SecondAllocator = std::allocator<SecondDatatype>>
inline Eigen::MatrixXd BuildDistanceMatrix(const std::vector<FirstDatatype, FirstAllocator>& data1, const std::vector<SecondDatatype, SecondAllocator>& data2, const std::function<double(const                 FirstDatatype&, const SecondDatatype&)>& distance_fn)

We might want to consider cleaning this up at some point.

For example, right now we would need 4 different functions for the following:

inline geometry_msgs::Pose EigenIsometry3fToGeometryPose(const Eigen::Isometry3f& transform);
inline geometry_msgs::Pose EigenIsometry3dToGeometryPose(const Eigen::Isometry3d& transform);
inline geometry_msgs::Pose EigenAffine3fToGeometryPose(const Eigen::Affine3f& transform);
inline geometry_msgs::Pose EigenAffine3dToGeometryPose(const Eigen::Affine3d& transform);

There are many such functions that would need to be duplicated. I think templates is by far the better way to go for this.

The serialize wrapper would presumably take an object, a filename, and a function pointer to an object serialization function. It would serialize, compress, and then save. Something along these lines:

template<typename T, typename SerializationFn>
void WriteToFile(const std::string& filename, const T& obj, const SerializationFn& serializer)
{
    std::vector<uint8_t> buffer;
    serializer(obj, buffer);
    ...
}

This functionality exists already in ros, we should probably remove this file and replace any usage of it with the ros versions. For example from SDF tools:

ros::serialization::IStream deser_stream(deser_buffer.get(), serialized_size);
sdf_tools::SDF new_message;
ros::serialization::deserialize(deser_stream, new_message);

Why is the function named GetParams (and it does 'get' params from the server), but the logging says "Setting"?

For example Interpolate. The compiler can't decide between the Vector3d and the VectorXd versions when passing in a Isometry3d.translation() object.

There are extra conversion functions for some types of objects. These should be resolved at least to the point of having one call the other.

When using -Werror=unused-variable and -DCMAKE_BUILD_TYPE=Release, assert statements do not count as a variable usage. Code snippets like the following will fail:

const size_t bytes_used = arc_utilities::SerializeEigen(vec, buffer);
const auto deserialized = arc_utilities::DeserializeEigen<EigenType>(buffer, 0);
assert(deserialized.second == bytes_used);

I have a problem on building this project.

error: ‘evaluator’ is not a class template struct evaluator<Aligned4Vector3<Scalar> > ^

Some of the profiler tests rely on timing. The TravisCI servers might be slow, so at least once the build has failed:
https://travis-ci.com/UM-ARM-Lab/arc_utilities/builds/106956715

Solution: Perhaps just remove these tests? Perhaps extend the time limit?

Functions GridIndexToLocation and LocationToGridIndex should change their return types to improve consistency.

GridIndexToLocation should change from std::vector<double> to Eigen::Vector4d

LocationToGridIndex should change from std::vector<int64_t> to GRID_INDEX

All uses in arc_utilities and sdf_tools should already check that these values are inside the grid boundaries already, so having these functions no longer perform bounds checking is not an issue.

A graph node currently stores in_edges and out_edges.
This duplicates each edge.
More importantly, it is possible to make inconsistent graphs where edges have different weights (and other properties) depending if looking from the "in_edges" or "out_edges" perspective.

Entire contents of the function are here:

void CompressAndWriteToFile(const std::vector<uint8_t>& uncompressed, const std::string& path)
{
    const auto compressed = CompressBytes(uncompressed);
    std::ofstream output_file(path, std::ios::out | std::ios::binary);
    output_file.write(reinterpret_cast<const char*>(compressed.data()), (std::streamsize)compressed.size());
    output_file.close();
}

The return values on the open and write functions are not checked. We should do something here, probably throw an exception.

Currently, tests exist in src and test. All tests should be moved to tests.

Additionally, we should consider migrating to the gtest framework for this package

What are arc_utilities dependencies? How do you install it?
Running into a few issues when installing on a fresh docker image:

cannot find Eigen3.cmake

zlib.h: No such file or directory

eigen_helpers.hpp has the following functions:

template <typename DerivedV>
inline Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, 1> GetArbitraryOrthogonalVector(
            const Eigen::MatrixBase<DerivedV>& vector);

template <typename DerivedV>
inline Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, 1> GetArbitraryOrthogonalVectorToPlane(
            const Eigen::MatrixBase<DerivedV>& plane_vector1,
            const Eigen::MatrixBase<DerivedV>& plane_vector2,
            const Eigen::MatrixBase<DerivedV>& vector);

Eigen already has some functionality that may do this already, we should check that the functionality is not duplicated.

https://eigen.tuxfamily.org/dox/group__Geometry__Module.html#title53

When the xbox controller is turned on it triggers a ros message.

In Victor's stack, this causes the grippers to open.

Possible fix: Ignore the first message from the controller