codac_VIBesFigMap.h

Go to the documentation of this file.

#ifndef __CODAC_VIBESFIGMAP_H__
#define __CODAC_VIBESFIGMAP_H__

#include <map>
#include <vector>

#include <codac_Interval.h>
#include <codac_VIBesFig.h>
#include <codac_TubeVector.h>
#include <codac_TrajectoryVector.h>
#include <codac_Beacon.h>
#include <codac_ColorMap.h>

namespace codac
{
  #define TRAJ_MAX_NB_DISPLAYED_POINTS  2000
  #define TUBE_MAX_NB_DISPLAYED_SLICES  2000

  // HTML color codes:
  #define DEFAULT_BEACON_COLOR                  "#D65A00[#FFB93C]"
  #define DEFAULT_TRAJMAP_COLOR                 "#276279"
  #define DEFAULT_MAPBCKGRND_COLOR              "#d2d2d2[#d2d2d2]"
  #define DEFAULT_OBS_COLOR                     "gray"
  #define DEFAULT_VEHICLE_DISPLAY_ON_EACH_TRAJ  true

  class VIBesFigMap : public VIBesFig
  {
    public:


      VIBesFigMap(const std::string& fig_name);

      ~VIBesFigMap();

      void restrict_tdomain(const Interval& restricted_tdomain);

      void enable_tubes_backgrounds(bool enable = true);

      void show();

      void show(float robot_size);

      void set_tube_max_disp_slices(int max);

      void set_traj_max_disp_points(int max);

      void smooth_tube_drawing(bool smooth);
      

      void add_tube(const TubeVector *tube, const std::string& name, int index_x, int index_y);

      void set_tube_name(const TubeVector *tube, const std::string& name);

      void set_tube_color(const TubeVector *tube, const std::string& color);

      void set_tube_color(const TubeVector *tube, const ColorMap& colormap, const Trajectory *traj_colormap = nullptr);

      void remove_tube(const TubeVector *tube);


      void add_trajectory(const TrajectoryVector *traj, const std::string& name, int index_x, int index_y, const std::string& color = DEFAULT_TRAJMAP_COLOR, bool vehicle_display = DEFAULT_VEHICLE_DISPLAY_ON_EACH_TRAJ);

      void add_trajectory(const TrajectoryVector *traj, const std::string& name, int index_x, int index_y, int index_heading, const std::string& color = DEFAULT_TRAJMAP_COLOR, bool vehicle_display = DEFAULT_VEHICLE_DISPLAY_ON_EACH_TRAJ);

      void set_trajectory_name(const TrajectoryVector *traj, const std::string& name);

      void set_trajectory_color(const TrajectoryVector *traj, const std::string& color);

      void set_trajectory_color(const TrajectoryVector *traj, const ColorMap& colormap, const Trajectory *traj_colormap = nullptr);

      void set_vehicle_display(const TrajectoryVector *traj, bool vehicle_display);

      void remove_trajectory(const TrajectoryVector *traj);


      void draw_vehicle(const Vector& pose, float size = -1);

      void draw_vehicle(const Vector& pose, const vibes::Params& params, float size = -1);

      void draw_vehicle(double t, const TrajectoryVector *traj, float size = -1);

      void draw_vehicle(double t, const TrajectoryVector *traj, const vibes::Params& params, float size = -1);

      void add_beacon(const Beacon& beacon, const std::string& color = DEFAULT_BEACON_COLOR);

      void add_beacon(const IntervalVector& beacon, const std::string& color = DEFAULT_BEACON_COLOR);

      void add_beacon(const Beacon& beacon, double width, const std::string& color = DEFAULT_BEACON_COLOR);

      void add_beacon(const Vector& beacon, double width, const std::string& color = DEFAULT_BEACON_COLOR);

      void add_beacons(const std::vector<Beacon>& v_beacons, const std::string& color = DEFAULT_BEACON_COLOR);

      void add_beacons(const std::vector<Beacon>& v_beacons, double width, const std::string& color = DEFAULT_BEACON_COLOR);

      void add_landmarks(const std::vector<IntervalVector>& v_m, const std::string& color = DEFAULT_BEACON_COLOR);
      
      void add_landmarks(const std::vector<Vector>& v_m, double width, const std::string& color = DEFAULT_BEACON_COLOR);
      
      void add_observation(const IntervalVector& obs, const Vector& pose, const std::string& color = DEFAULT_OBS_COLOR);
      
      void add_observations(const std::vector<IntervalVector>& v_obs, const Vector& pose, const std::string& color = DEFAULT_OBS_COLOR);
      
      void add_observation(const IntervalVector& obs, const TrajectoryVector *traj, const std::string& color = DEFAULT_OBS_COLOR);
      
      void add_observations(const std::vector<IntervalVector>& v_obs, const TrajectoryVector *traj, const std::string& color = DEFAULT_OBS_COLOR);
      
      double heading(double t, const TrajectoryVector *traj) const;
  

    protected:

      const IntervalVector draw_tube(const TubeVector *tube);

      const IntervalVector draw_tube_background(const TubeVector *tube);

      const IntervalVector draw_trajectory(const TrajectoryVector *traj, float points_size = 0.);

      void draw_slices(const TubeVector *tube);

      void draw_beacon(const Beacon& beacon, const std::string& color, const vibes::Params& params);

      void draw_beacon(const Beacon& beacon, double width, const std::string& color, const vibes::Params& params);

      void draw_observation(const IntervalVector& obs, const Vector& pose, const std::string& color = "", const vibes::Params& params = vibes::Params());
      
      void draw_observation(const IntervalVector& obs, const TrajectoryVector *traj, const std::string& color = "", const vibes::Params& params = vibes::Params());


    protected:

      struct FigMapTubeParams
      {
        std::string name; 
        std::string color = ""; 
        std::pair<ColorMap,const Trajectory*> color_map; 
        const Tube *tube_x_copy = nullptr, *tube_y_copy; 
        bool from_first_to_last = !true; 
        int index_x, index_y; 
      };

      struct FigMapTrajParams
      {
        std::string name; 
        std::string color = ""; 
        std::pair<ColorMap,const Trajectory*> color_map; 
        int index_x, index_y, index_heading = -1; 
        bool vehicle_display = DEFAULT_VEHICLE_DISPLAY_ON_EACH_TRAJ; 
      };

      std::map<const TubeVector*,FigMapTubeParams> m_map_tubes; 
      std::map<const TrajectoryVector*,FigMapTrajParams> m_map_trajs; 

      Interval m_restricted_tdomain; 
      bool m_draw_tubes_backgrounds = true; 
      bool m_smooth_drawing = false; 
      float m_robot_size = 5.5; 

      unsigned int m_tube_max_nb_disp_slices = TUBE_MAX_NB_DISPLAYED_SLICES; 
      unsigned int m_traj_max_nb_disp_points = TRAJ_MAX_NB_DISPLAYED_POINTS; 
  };
}

#endif