ControllerClient.h

Go to the documentation of this file.

/*
 * Copyright 2015-2020 CNRS-UM LIRMM, CNRS-AIST JRL
 */
 
#pragma once
 
#include <mc_control/ControllerServer.h>
#include <mc_control/client_api.h>
 
#include <mc_rtc/Configuration.h>
#include <mc_rtc/gui/plot/types.h>
#include <mc_rtc/gui/types.h>
 
#include <string>
#include <thread>
#include <vector>
 
namespace mc_control
{
 
struct MC_CONTROL_CLIENT_DLLAPI ElementId
{
  ElementId() = default;
  ElementId(const ElementId &) = default;
  ElementId(ElementId &&) = default;
  ElementId & operator=(const ElementId &) = default;
  ElementId & operator=(ElementId &&) = default;
 
  ElementId(const std::vector<std::string> & category, const std::string & name, int sid)
  : category(category), name(name), sid(sid)
  {
  }
 
  ElementId(const std::vector<std::string> & category, const std::string & name) : ElementId(category, name, -1) {}
 
  std::vector<std::string> category = {};
  std::string name = {};
  int sid = -1;
};
 
struct MC_CONTROL_CLIENT_DLLAPI ControllerClient
{
 
  ControllerClient();
 
  ControllerClient(const std::string & sub_conn_uri, const std::string & push_conn_uri, double timeout = 0);
 
  ControllerClient(ControllerServer & server, mc_rtc::gui::StateBuilder & gui);
 
  ControllerClient(const ControllerClient &) = delete;
  ControllerClient & operator=(const ControllerClient &) = delete;
 
  ~ControllerClient();
 
  void connect(const std::string & sub_conn_uri, const std::string & push_conn_uri);
 
  void connect(ControllerServer & server, mc_rtc::gui::StateBuilder & gui);
 
  void send_request(const ElementId & id, const mc_rtc::Configuration & data);
 
  template<typename T>
  void send_request(const ElementId & id, const T & data)
  {
    mc_rtc::Configuration c;
    c.add("data", data);
    send_request(id, c("data"));
  }
 
  void send_request(const ElementId & id);
 
  void raw_request(const ElementId & id, const mc_rtc::Configuration & data, std::string & out);
 
  template<typename T>
  void raw_request(const ElementId & id, const T & data, std::string & out)
  {
    mc_rtc::Configuration c;
    c.add("data", data);
    raw_request(id, c("data"), out);
  }
 
  void raw_request(const ElementId & id, std::string & out);
 
  void timeout(double t);
 
  double timeout();
 
  void run(std::vector<char> & buffer, std::chrono::system_clock::time_point & t_last_received);
 
  void run(const char * buffer, size_t bufferSize);
 
protected:
  void start();
 
  void reconnect(const std::string & sub_conn_uri, const std::string & push_conn_uri);
 
  void stop();
 
  void handle_gui_state(mc_rtc::Configuration state);
 
  void handle_category(const std::vector<std::string> & parent,
                       const std::string & category,
                       const mc_rtc::Configuration & data);
 
  void handle_widget(const ElementId & id, const mc_rtc::Configuration & data);
 
  virtual void started() {}
 
  virtual void stopped() {}
 
  virtual void category(const std::vector<std::string> & parent, const std::string & category) {};
 
  inline virtual void label(const ElementId & id, const std::string &) { default_impl("Label", id); }
 
  inline virtual void array_label(const ElementId & id, const std::vector<std::string> &, const Eigen::VectorXd &)
  { default_impl("ArrayLabel", id); }
 
  virtual void button(const ElementId & id) { default_impl("Button", id); }
 
  virtual void checkbox(const ElementId & id, bool /*state */) { default_impl("Checkbox", id); }
 
  virtual void string_input(const ElementId & id, const std::string & /*data*/) { default_impl("StringInput", id); }
 
  virtual void integer_input(const ElementId & id, int /*data*/) { default_impl("IntegerInput", id); }
 
  virtual void number_input(const ElementId & id, double /*data*/) { default_impl("NumberInput", id); }
 
  virtual void number_slider(const ElementId & id, double /*data*/, double /*min*/, double /*max*/)
  { default_impl("NumberSlider", id); }
 
  virtual void array_input(const ElementId & id,
                           const std::vector<std::string> & /*labels*/,
                           const Eigen::VectorXd & /*data*/)
  { default_impl("ArrayInput", id); }
 
  virtual void combo_input(const ElementId & id,
                           const std::vector<std::string> & /*values*/,
                           const std::string & /*data*/)
  { default_impl("ComboInput", id); }
 
  virtual void data_combo_input(const ElementId & id,
                                const std::vector<std::string> & /*data_ref*/,
                                const std::string & /*data*/)
  { default_impl("DataComboInput", id); }
 
  virtual void point3d(const ElementId & id,
                       const ElementId & /*requestId*/,
                       bool /*ro */,
                       const Eigen::Vector3d & /*pos*/,
                       const mc_rtc::gui::PointConfig & /* config */)
  { default_impl("Point3D", id); }
 
  virtual void trajectory(const ElementId & id,
                          const std::vector<Eigen::Vector3d> & /* points */,
                          const mc_rtc::gui::LineConfig & /* config */)
  { default_impl("Point3DTrajectory", id); }
 
  virtual void trajectory(const ElementId & id,
                          const std::vector<sva::PTransformd> & /* points */,
                          const mc_rtc::gui::LineConfig & /* config */)
  { default_impl("PoseTrajectory", id); }
 
  virtual void trajectory(const ElementId & id,
                          const Eigen::Vector3d & /* point */,
                          const mc_rtc::gui::LineConfig & /* config */)
  { default_impl("Point3DRealTimeTrajectory", id); }
 
  virtual void trajectory(const ElementId & id,
                          const sva::PTransformd & /* point */,
                          const mc_rtc::gui::LineConfig & /* config */)
  { default_impl("PoseRealTimeTrajectory", id); }
 
  virtual void polygon(const ElementId & id,
                       const std::vector<std::vector<Eigen::Vector3d>> & /* points */,
                       const mc_rtc::gui::Color & /* color */)
  { default_impl("PolygonArray", id); }
 
  virtual void polygon(const ElementId & id,
                       const std::vector<std::vector<Eigen::Vector3d>> & points,
                       const mc_rtc::gui::LineConfig & config)
  { polygon(id, points, config.color); }
 
  virtual void polyhedron(const ElementId & id,
                          const std::vector<std::array<Eigen::Vector3d, 3>> & triangles,
                          const std::vector<std::array<mc_rtc::gui::Color, 3>> & colors,
                          const mc_rtc::gui::PolyhedronConfig & config);
 
  virtual void polyhedron(const ElementId & id,
                          const std::vector<Eigen::Vector3d> & vertices,
                          const std::vector<std::array<size_t, 3>> & triangles,
                          const std::vector<mc_rtc::gui::Color> & colors,
                          const mc_rtc::gui::PolyhedronConfig & config);
 
  virtual void force(const ElementId & id,
                     const ElementId & /*requestId*/,
                     const sva::ForceVecd & /* force */,
                     const sva::PTransformd & /* application point for the force */,
                     const mc_rtc::gui::ForceConfig & /* forceConfig */,
                     bool /* ro */)
  { default_impl("Force", id); }
 
  virtual void arrow(const ElementId & id,
                     const ElementId & /* requestId */,
                     const Eigen::Vector3d & /* start */,
                     const Eigen::Vector3d & /* end */,
                     const mc_rtc::gui::ArrowConfig & /* config */,
                     bool /* ro */)
  { default_impl("Arrow", id); }
 
  virtual void rotation(const ElementId & id,
                        const ElementId & /*requestId*/,
                        bool /*ro */,
                        const sva::PTransformd & /*pos*/)
  { default_impl("Rotation", id); }
 
  virtual void transform(const ElementId & id,
                         const ElementId & /*requestId*/,
                         bool /*ro */,
                         const sva::PTransformd & /*pos*/)
  { default_impl("Transform", id); }
 
  virtual void xytheta(const ElementId & id,
                       const ElementId & /*requestId*/,
                       bool /*ro */,
                       const Eigen::Vector3d & /*xytheta*/,
                       double /* altitude */)
  { default_impl("XYTheta", id); }
 
  virtual void table_start(const ElementId & id, const std::vector<std::string> & /*header*/)
  { default_impl("Table", id); }
 
  virtual void table_row(const ElementId & /*id*/, const std::vector<std::string> & /*data*/) {}
 
  virtual void table_end(const ElementId & /*id*/) {}
 
  virtual void robot(const ElementId & id,
                     const std::vector<std::string> & /*parameters*/,
                     const std::vector<std::vector<double>> & /*q*/,
                     const sva::PTransformd & /*posW*/)
  { default_impl("Robot", id); }
 
  virtual void robot_msg(const ElementId & id, const mc_rtc::gui::RobotMsgData & /*msg*/)
  { default_impl("RobotMsg", id); }
 
  virtual void visual(const ElementId & id,
                      [[maybe_unused]] const rbd::parsers::Visual & visual,
                      [[maybe_unused]] const sva::PTransformd & pose)
  { default_impl("Visual", id); }
 
  virtual void schema(const ElementId & id, const std::string & /*schema*/) { default_impl("Schema", id); }
 
  virtual void form(const ElementId & id) { default_impl("Form", id); }
 
  virtual void form_checkbox(const ElementId & /*formId*/,
                             const std::string & /*name*/,
                             bool /*required*/,
                             bool /*default*/)
  {
  }
 
  virtual void form_checkbox(const ElementId & formId,
                             const std::string & name,
                             bool required,
                             bool default_,
                             bool /*default_from_user*/)
  { form_checkbox(formId, name, required, default_); }
 
  virtual void form_integer_input(const ElementId & /*formId*/,
                                  const std::string & /*name*/,
                                  bool /*required*/,
                                  int /*default*/)
  {
  }
 
  virtual void form_integer_input(const ElementId & formId,
                                  const std::string & name,
                                  bool required,
                                  int default_,
                                  bool /*default_from_user*/)
  { form_integer_input(formId, name, required, default_); }
 
  virtual void form_number_input(const ElementId & /*formId*/,
                                 const std::string & /*name*/,
                                 bool /*required*/,
                                 double /*default*/)
  {
  }
 
  virtual void form_number_input(const ElementId & formId,
                                 const std::string & name,
                                 bool required,
                                 double default_,
                                 bool /*default_from_user*/)
  { form_number_input(formId, name, required, default_); }
 
  virtual void form_string_input(const ElementId & /*formId*/,
                                 const std::string & /*name*/,
                                 bool /*required*/,
                                 const std::string & /*default*/)
  {
  }
 
  virtual void form_string_input(const ElementId & formId,
                                 const std::string & name,
                                 bool required,
                                 const std::string & default_,
                                 bool /*default_from_user*/)
  { form_string_input(formId, name, required, default_); }
 
  virtual void form_array_input(const ElementId & /*formId*/,
                                const std::string & /*name*/,
                                bool /*required*/,
                                const Eigen::VectorXd & /*default*/,
                                bool /*fixed_size*/)
  {
  }
 
  virtual void form_array_input(const ElementId & formId,
                                const std::string & name,
                                bool required,
                                const Eigen::VectorXd & default_,
                                bool fixed_size,
                                bool /*default_from_user*/)
  { form_array_input(formId, name, required, default_, fixed_size); }
 
  virtual void form_array_input(const ElementId & formId,
                                const std::string & name,
                                bool required,
                                const std::vector<std::string> & /*labels*/,
                                const Eigen::VectorXd & default_,
                                bool fixed_size,
                                bool default_from_user)
  { form_array_input(formId, name, required, default_, fixed_size, default_from_user); }
 
  virtual void form_combo_input(const ElementId & /*formId*/,
                                const std::string & /*name*/,
                                bool /*required*/,
                                const std::vector<std::string> & /*values*/,
                                bool /*send_index*/)
  {
  }
 
  virtual void form_combo_input(const ElementId & formId,
                                const std::string & name,
                                bool required,
                                const std::vector<std::string> & values,
                                bool send_index,
                                int /*def*/)
  { form_combo_input(formId, name, required, values, send_index); }
 
  virtual void form_data_combo_input(const ElementId & /*formId*/,
                                     const std::string & /*name*/,
                                     bool /*required*/,
                                     const std::vector<std::string> & /*ref*/,
                                     bool /*send_index*/)
  {
  }
 
  virtual void form_point3d_input(const ElementId & /*formId*/,
                                  const std::string & /*name*/,
                                  bool /*required*/,
                                  const Eigen::Vector3d & /*default_*/,
                                  bool /*default_from_user*/,
                                  bool /*interactive*/)
  {
  }
 
  virtual void form_rotation_input(const ElementId & /*formId*/,
                                   const std::string & /*name*/,
                                   bool /*required*/,
                                   const sva::PTransformd & /*default_*/,
                                   bool /*default_from_user*/,
                                   bool /*interactive*/)
  {
  }
 
  virtual void form_transform_input(const ElementId & /*formId*/,
                                    const std::string & /*name*/,
                                    bool /*required*/,
                                    const sva::PTransformd & /*default_*/,
                                    bool /*default_from_user*/,
                                    bool /*interactive*/)
  {
  }
 
  virtual void start_form_object_input(const std::string & /*name*/, bool /*required*/) {}
 
  virtual void end_form_object_input() {}
 
  virtual void start_form_generic_array_input(const std::string & /*name*/,
                                              bool /*required*/,
                                              std::optional<std::vector<Configuration>> /*data*/)
  {
  }
 
  virtual void end_form_generic_array_input() {}
 
  virtual void start_form_one_of_input(const std::string & /*name*/,
                                       bool /*required*/,
                                       const std::optional<std::pair<size_t, Configuration>> & /*data*/)
  {
  }
 
  virtual void end_form_one_of_input() {}
 
  virtual void start_plot(uint64_t /*id*/, const std::string & /*title*/) {}
 
  virtual void plot_setup_xaxis(uint64_t /*id*/,
                                const std::string & /*legend*/,
                                const mc_rtc::gui::plot::Range & /*range*/)
  {
  }
 
  virtual void plot_setup_yaxis_left(uint64_t /*id*/,
                                     const std::string & /*legend*/,
                                     const mc_rtc::gui::plot::Range & /*range*/)
  {
  }
 
  virtual void plot_setup_yaxis_right(uint64_t /*id*/,
                                      const std::string & /*legend*/,
                                      const mc_rtc::gui::plot::Range & /*range*/)
  {
  }
 
  virtual void plot_point(uint64_t /* id */,
                          uint64_t /* did */,
                          const std::string & /*legend*/,
                          double /*x*/,
                          double /*y*/,
                          mc_rtc::gui::Color /*color*/,
                          mc_rtc::gui::plot::Style /*style*/,
                          mc_rtc::gui::plot::Side /*side*/)
  {
  }
 
  virtual void plot_polygon(uint64_t /*id*/,
                            uint64_t /* did */,
                            const std::string & /*legend*/,
                            const mc_rtc::gui::plot::PolygonDescription & /*polygon*/,
                            mc_rtc::gui::plot::Side /*side*/)
  {
  }
 
  virtual void plot_polygons(uint64_t /*id*/,
                             uint64_t /* did */,
                             const std::string & /*legend*/,
                             const std::vector<mc_rtc::gui::plot::PolygonDescription> & /*polygons*/,
                             mc_rtc::gui::plot::Side /*side*/)
  {
  }
 
  virtual void end_plot(uint64_t /*id*/) {}
 
  /* Network elements */
  bool run_ = true;
  int sub_socket_ = -1;
  std::thread sub_th_;
  int push_socket_ = -1;
  double timeout_;
 
  /* Hold data from the server */
  mc_rtc::Configuration data_;
 
  /* Pointer to the server if connected in-memory */
  ControllerServer * server_ = nullptr;
  /* Pointer to the GUI if connected in-memory */
  mc_rtc::gui::StateBuilder * gui_ = nullptr;
 
private:
  virtual void default_impl(const std::string & type, const ElementId & id);
 
  void handle_point3d(const ElementId & id, const mc_rtc::Configuration & data);
 
  void handle_trajectory(const ElementId & id, const mc_rtc::Configuration & data);
 
  void handle_polygon(const ElementId & id, const mc_rtc::Configuration & data);
 
  void handle_polyhedron_triangles_list(const ElementId & id, const mc_rtc::Configuration & data_);
 
  void handle_polyhedron_vertices_triangles(const ElementId & id, const mc_rtc::Configuration & data_);
 
  void handle_force(const ElementId & id, const mc_rtc::Configuration & data);
 
  void handle_arrow(const ElementId & id, const mc_rtc::Configuration & data);
 
  void handle_rotation(const ElementId & id, const mc_rtc::Configuration & data);
 
  void handle_transform(const ElementId & id, const mc_rtc::Configuration & data);
 
  void handle_xytheta(const ElementId & id, const mc_rtc::Configuration & data);
 
  void handle_form(const ElementId & id, const mc_rtc::Configuration & data);
 
  void handle_plot(const mc_rtc::Configuration & plot);
 
  void handle_standard_plot(const mc_rtc::Configuration & plot);
 
  void handle_xy_plot(const mc_rtc::Configuration & plot);
 
  void handle_table(const ElementId & id,
                    const std::vector<std::string> & header,
                    const std::vector<std::string> & format,
                    const std::vector<mc_rtc::Configuration> & data);
 
  bool default_polyhedron_triangles_list_ = false;
  bool default_polyhedron_vertices_triangles_ = false;
};
 
} // namespace mc_control