cspace.h

Go to the documentation of this file.

/* Author: Zachary Kingston */

#ifndef SE2EZ_CORE_CSPACE_
#define SE2EZ_CORE_CSPACE_

#include <ompl/util/PPM.h>

#include <queue>
#include <thread>
#include <mutex>
#include <condition_variable>

#include <se2ez/core/class_forward.h>
#include <se2ez/core/robot.h>

namespace se2ez
{
    /** \cond IGNORE */
    SE2EZ_CLASS_FORWARD(State)
    /** \endcond */

    /** \cond IGNORE */
    SE2EZ_CLASS_FORWARD(CSpaceGrid)
    /** \endcond */

    /** \class se2ez::CSpaceGridPtr
        \brief A shared pointer wrapper for se2ez::CSpaceGrid. */

    /** \class se2ez::CSpaceGridConstPtr
        \brief A const shared pointer wrapper for se2ez::CSpaceGrid. */

    /** \brief Helper class to compute and draw C-Space images from arbitrary robots.
     */
    class CSpaceGrid
    {
    public:
        /** \brief Grid coordinates.
         */
        struct GridCoords
        {
            unsigned int x;  ///< x coordinate.
            unsigned int y;  ///< y coordinate.
        };

        /** \brief Line on the grid.
         */
        struct GridLine
        {
            GridCoords a;  ///< Endpoint a.
            GridCoords b;  ///< Endpoint b.
        };

        /** \brief Coordinates for the robot.
         */
        struct RobotCoords
        {
            double x;  ///< x coordinate
            double y;  ///< y coordinate
        };

        /** \brief Drawing mode for the grid.
         */
        enum Mode
        {
            COLLISION,       ///< Draw collisions as black pixels.
            COLOR,           ///< Color obstacles by geometry color.
            DISTANCE,        ///< Use `viridis` color palette for signed distance.
            DISTANCE_COLOR,  ///< Signed distance, but penetrations are colored by geometry.
        };

        /** \brief Function to callback for grid computation. Gives a pixel to color, the state, and whether
         * the state is in collision.
         */
        using GridCallback = std::function<void(ompl::PPM::Color &, const StatePtr &, bool)>;

        /** \brief Constructor.
         *  \param[in] robot Robot to compute grid for.
         *  \param[in] jointA index of configuration to vary.
         *  \param[in] jointB other index of configuration to vary.
         *  \param[in] width Width of image to compute.
         *  \param[in] height Height of image to compute.
         *  \param[in] mode What mode to use for rendering.
         */
        CSpaceGrid(const RobotPtr &robot, unsigned int jointA, unsigned int jointB,  //
                   unsigned int width = 500, unsigned int height = 500, Mode mode = COLLISION);

        /** \brief Constructor.
         *  \param[in] robot Robot to compute grid for.
         *  \param[in] jointA 1-DoF joint to vary.
         *  \param[in] jointB other 1-DoF joint to vary.
         *  \param[in] width Width of image to compute.
         *  \param[in] height Height of image to compute.
         *  \param[in] mode What mode to use for rendering.
         */
        CSpaceGrid(const RobotPtr &robot, const std::string &jointA, const std::string &jointB,
                   unsigned int width = 500, unsigned int height = 500, Mode mode = COLLISION);

        /** \brief Destructor.
         */
        ~CSpaceGrid();

        /** \brief Compute a PPM image of the configuration space of two joints, \e jointA and \e jointB.
         *  \param[in] block Wait for computation to finish. If false, returns immediately.
         *  \param[in] base For robots with more than 2 DoF, set other DoF to this state.
         *  \param[in] callback Extra callback function to draw extra information.
         */
        void compute(bool block = true, const StatePtr &base = nullptr, const GridCallback &callback = {});

        /** \brief Wait for all grid computation to complete.
         */
        void waitToComplete();

        /** \brief Checks if any computation is happening.
         */
        bool isComputing() const;

        /** \brief Cancels any remaining computation.
         */
        void cancel();

        /** \brief Get the pixel coordinate for a configuration.
         *  \param[in] state State to use for configuration values.
         *  \return Reference to pixel to set.
         */
        ompl::PPM::Color &getPixel(const StatePtr &state);

        /** \brief Get the pixel coordinate for a configuration of \e i for jointA and \e j for jointB.
         *  \param[in] i Value for jointA.
         *  \param[in] j Value for jointB.
         *  \return Reference to pixel to set.
         */
        ompl::PPM::Color &getPixel(double i, double j);

        /** \brief Get the pixel coordinate for a configuration.
         *  \param[in] state State to use for configuration values.
         *  \return Pixel coordinates.
         */
        GridCoords getCoords(const StatePtr &state);

        /** \brief Get the pixel coordinate for a configuration.
         *  \param[in] i Configuration for first joint.
         *  \param[in] j Configuration for second joint.
         *  \return Pixel coordinates.
         */
        GridCoords getCoords(double i, double j);

        /** \brief Get the associated robot configuration from image coordinates.
         *  \param[in] i X-coordinate
         *  \param[in] j Y-coordinate
         *  \return Robot configuration.
         */
        RobotCoords getCoords(unsigned int i, unsigned int j);

        /** \brief Get the line segments between two points on the grid. Can have two due to wrapping.
         *  \param[in] a Endpoint a
         *  \param[in] b Endpoint b
         *  \return Grid lines.
         */
        std::vector<GridLine> getLines(const StatePtr &a, const StatePtr &b);

        /** \brief Get the current computed PPM image.
         *  \return A PPM image with black meaning the robot is in collision, white if it is not in
         * collision.
         */
        std::shared_ptr<ompl::PPM> getImage() const;

        /** \brief Returns the distance along the dimensions of the robot not captured by the current C-Space
         * slice.
         *  \param[in] state State to measure distance to.
         *  \return The distance of the state to the slice.
         */
        double distanceToSlice(const StatePtr &state);

        /** \brief Returns a value in [0, 1] of the progress of the current computation.
         *  \return Current progress value.
         */
        double progress() const;

        /** \brief Return width of grid.
         *  \return The width of the grid.
         */
        unsigned int getWidth() const;

        /** \brief Return height of grid.
         *  \return The height of the grid.
         */
        unsigned int getHeight() const;

    private:
        /** \brief Initialize all internal constructs.
         */
        void initialize();

        /** \brief Emplaces all images chunks into work queue.
         */
        void emplaceChunks();

        /** \brief Gets the average color of a frame.
         *  \param[in] frame Frame to get color of.
         *  \return The average color of the frame.
         */
        Eigen::Vector4d getFrameColor(const std::string &frame) const;

        const RobotPtr robot_;           ///< Robot to use.
        const unsigned int xChunk_{25};  ///< Size of X chunk for threading.
        const unsigned int yChunk_{25};  ///< Size of Y chunk for threading.
        const double minDepth_{-1.};     ///< Minimum value for signed distance color map.
        const double maxDepth_{1.};      ///< Maximum value for signed distance color map.

        static const unsigned int cores_;  ///< Number of cores on the machine
        const unsigned int indexA_;        ///< Joint index for X-axis
        const unsigned int indexB_;        ///< Joint index for Y-axis
        const Eigen::Vector2d limitA_;     ///< Joint limits for first joint (1-DoF)
        const Eigen::Vector2d limitB_;     ///< Joint limits for second joint (1-DoF)
        const bool contA_;                 ///< Is joint A continuous?
        const bool contB_;                 ///< Is joint B continuous?

        const unsigned int width_;                   ///< Width of image.
        const unsigned int height_;                  ///< Height of image.
        const Mode mode_;                            ///< Render mode.
        std::shared_ptr<ompl::PPM> image_{nullptr};  ///< Image itself.

        /** \brief Container class for image chunk information to worker threads.
         */
        struct Chunk
        {
            /** \brief Default constructor.
             */
            Chunk() = default;

            /** \brief Constructor.
             *  \param[in] x Starting x coordinate.
             *  \param[in] y Starting y coordinate.
             *  \param[in] w Width of chunk.
             *  \param[in] h Height of chunk.
             */
            Chunk(unsigned int x, unsigned int y, unsigned int w, unsigned int h) : x(x), y(y), w(w), h(h)
            {
            }

            unsigned int x;  ///< Starting x coordinate of chunk.
            unsigned int y;  ///< Starting y coordinate of chunk.
            unsigned int w;  ///< Width of chunk.
            unsigned int h;  ///< Height of chunk.
        };

        unsigned int maxChunks_{0};  ///< Maximum number of chunks in queue.
        StatePtr state_{nullptr};    ///< State provided through compute().
        StatePtr temp_{nullptr};     ///< Temporary state.
        GridCallback callback_{};    ///< Callback function to use for extra drawing.

        bool done_{false};                  ///< Notify threads object is to be destroyed.
        std::mutex lock_;                   ///< Mutex.
        std::mutex statelock_;              ///< Mutex for state.
        std::condition_variable work_;      ///< Condition variable that there is work to do.
        std::condition_variable complete_;  ///< Condition variable to notify when complete.
        std::queue<Chunk> chunks_;          ///< Queue of chunks.

        std::vector<bool> newState_;          ///< Has each thread updated to new base state for computation?
        std::vector<bool> working_;           ///< Which threads are currently working?
        std::vector<std::thread *> threads_;  ///< Threads.
    };
}  // namespace se2ez

#endif