<commit_before>#include <QVBoxLayout>
#include <QHBoxLayout>
#include <QLabel>
#include <QDoubleSpinBox>
#include <QSpinBox>
#include <QPushButton>
#include <QMessageBox>
#include <QFuture>
#include <QtConcurrentRun>
#include <QWidget>
#include <QTabWidget>
#include <QDebug>
#include <QLabel>
#include <QRadioButton>
#include <QLineEdit>
#include <QFileDialog>

#include <string>
#include <iostream>

#include "fanuc_grinding_rviz_plugin.h"

namespace fanuc_grinding_rviz_plugin
{
FanucGrindingRvizPlugin::FanucGrindingRvizPlugin(QWidget* parent) :
    rviz::Panel(parent)
{
  // Create Tabs
  tab_widget_ = new QTabWidget();

  scanning_widget_ = new ScanningWidget();
  alignment_widget_ = new AlignmentWidget();
  comparison_widget_ = new ComparisonWidget();
  path_planning_widget_ = new PathPlanningWidget();
  post_processor_widget_ = new PostProcessorWidget();

  tab_widget_->addTab(scanning_widget_, "Scanning");
  tab_widget_->addTab(alignment_widget_, "Alignment");
  tab_widget_->addTab(comparison_widget_, "Comparison");
  tab_widget_->addTab(path_planning_widget_, "Path planning");
  tab_widget_->addTab(post_processor_widget_, "Post processor");
  tab_widget_->setTabEnabled(0, true);
  tab_widget_->setTabEnabled(1, false);
  tab_widget_->setTabEnabled(2, false);
  tab_widget_->setTabEnabled(3, false);
  tab_widget_->setTabEnabled(4, false);

  // Bottom status layout
  QVBoxLayout *status_layout_ = new QVBoxLayout;
  status_layout_->addWidget(new QLabel("Status:"));

  // Global Layout
  QVBoxLayout *global_layout_ = new QVBoxLayout;
  global_layout_->addWidget(tab_widget_);
  global_layout_->addLayout(status_layout_);
  status_label_ = new QLabel;
  global_layout_->addWidget(status_label_);
  setLayout(global_layout_);

  // Connect handlers
  // SCANNING
  // Will display a status in general status label ( from scanning widget )
  connect(scanning_widget_, SIGNAL(sendStatus(QString)), this, SLOT(displayStatusHandler(QString)));
  connect(scanning_widget_, SIGNAL(sendMsgBox(QString, QString , QString)),
                      this, SLOT(displayMsgBoxHandler(QString, QString, QString)));

  // Call configChanged at each time that scanning_widget_ is modified
  connect(scanning_widget_, SIGNAL(GUIChanged()), this, SLOT(triggerSave()));
  // Enable AlignementPanel when scanning_widget_ will send the SIGNAL
  connect(scanning_widget_, SIGNAL(enablePanelAlignment()), this, SLOT(enablePanelAlignmentHandler()));
  // Enable general panel when scanning_widget_ send the SIGNAL
  connect(scanning_widget_, SIGNAL(enablePanel(bool)), this, SLOT(enablePanelHandler(bool)));

  // Will send information about cad and scan in the other widgets
  connect(scanning_widget_, SIGNAL(sendCADDatas(QString, QString)),
                      this, SLOT(setCADDatas(QString, QString)));
  connect(scanning_widget_, SIGNAL(sendScanDatas(QString, QString)),
                      this, SLOT(setScanDatas(QString, QString)));
  // For the demonstrator, we will skip alignment and comparison parts for the moment
  connect(scanning_widget_, SIGNAL(enablePanelPathPlanning()), this, SLOT(enablePanelPathPlanningHandler()));

  //ALIGNMENT
  // Will display a status in general status label ( from alignment widget )
  connect(alignment_widget_, SIGNAL(sendStatus(QString)), this, SLOT(displayStatusHandler(QString)));
  connect(alignment_widget_, SIGNAL(sendMsgBox(QString, QString , QString)),
                       this, SLOT(displayMsgBoxHandler(QString, QString, QString)));
  // Call configChanged at each time that alignment_widget_ is modified
  connect(alignment_widget_, SIGNAL(GUIChanged()), this, SLOT(triggerSave()));
  // Enable compoarison_panel when alignment_widget_ will send the SIGNAL
  connect(alignment_widget_,SIGNAL(enablePanelComparison()), this, SLOT(enablePanelComparisonHandler()));
  // Enable general panel when alignment_widget_ send the SIGNAL
  connect(alignment_widget_, SIGNAL(enablePanel(bool)), this, SLOT(enablePanelHandler(bool)));
  // Received a signal from alignment widget in order to get CAD and scan params
  connect(alignment_widget_, SIGNAL(getCADAndScanParams()), this, SLOT(sendCADAndScanDatasSlot()));
  // Send a signal to alignment widget in order to give CAD and scan params
  connect(this             , SIGNAL(sendCADAndScanDatas(const QString, const QString, const QString, const QString)),
          alignment_widget_, SLOT(setCADAndScanParams(const QString, const QString, const QString, const QString)));

  //COMPARISON
  // Will display a status in general status label ( from comparison widget )
  connect(comparison_widget_, SIGNAL(sendStatus(QString)), this, SLOT(displayStatusHandler(QString)));
  connect(comparison_widget_, SIGNAL(sendMsgBox(QString, QString , QString)),
                      this, SLOT(displayMsgBoxHandler(QString, QString, QString)));
  // Call configChanged at each time that comparison_widget_ is modified
  connect(comparison_widget_, SIGNAL(GUIChanged()), this, SLOT(triggerSave()));
  // Enable path_planning_widget when comparison_widget_ will send the SIGNAL
  connect(comparison_widget_,SIGNAL(enablePanelPathPlanning()), this, SLOT(enablePanelPathPlanningHandler()));
  // Enable general panel when comparison_widget_ send the SIGNAL
  connect(comparison_widget_, SIGNAL(enablePanel(bool)), this, SLOT(enablePanelHandler(bool)));
  // Received a signal from comparison widget in order to get CAD and scan params
  connect(comparison_widget_, SIGNAL(getCADAndScanParams()), this, SLOT(sendCADAndScanDatasSlot()));
  // Send a signal to comparison widget in order to give CAD and scan params
  connect(this             , SIGNAL(sendCADAndScanDatas(const QString, const QString, const QString, const QString)),
          comparison_widget_, SLOT(setCADAndScanParams(const QString, const QString, const QString, const QString)));

  //PATH PLANNING
  // Will display a status in general status label ( from path_planning widget )
  connect(path_planning_widget_, SIGNAL(sendStatus(QString)), this, SLOT(displayStatusHandler(QString)));
  connect(path_planning_widget_, SIGNAL(sendMsgBox(QString, QString , QString)),
                           this, SLOT(displayMsgBoxHandler(QString, QString, QString)));
  // Call configChanged at each time that path_planning_widget is modified
  connect(path_planning_widget_, SIGNAL(GUIChanged()), this, SLOT(triggerSave()));
  // Enable path_planning_widget when comparison_widget_ will send the SIGNAL
  connect(path_planning_widget_, SIGNAL(enablePanelPostProcessor()), this, SLOT(enablePanelPostProcessorHandler()));
  // Enable general panel when path_planning send the SIGNAL
  connect(path_planning_widget_, SIGNAL(enablePanel(bool)), this, SLOT(enablePanelHandler(bool)));
  // Received a signal from comparison widget in order to get CAD and scan params
  connect(path_planning_widget_, SIGNAL(getCADAndScanParams()), this, SLOT(sendCADAndScanDatasSlot()));
  // Send a signal to comparison widget in order to give CAD and scan params
  connect(this             , SIGNAL(sendCADAndScanDatas(const QString, const QString, const QString, const QString)),
          path_planning_widget_, SLOT(setCADAndScanParams(const QString, const QString, const QString, const QString)));

  //POST_PROCESSOR
  // Will display a status in general status label ( from post_processor widget )
  connect(post_processor_widget_, SIGNAL(sendStatus(QString)), this, SLOT(displayStatusHandler(QString)));
  connect(post_processor_widget_, SIGNAL(sendMsgBox(QString, QString, QString)),
                      this, SLOT(displayMsgBoxHandler(QString, QString, QString)));
  // Call configChanged at each time that post_processor_widget is modified
  connect(post_processor_widget_, SIGNAL(GUIChanged()), this, SLOT(triggerSave()));
  // Enable general panel when post_processor send the SIGNAL
  connect(post_processor_widget_, SIGNAL(enablePanel(bool)), this, SLOT(enablePanelHandler(bool)));
  // Receive a signal from post processor widget in order to send robot poses data
  connect(post_processor_widget_, SIGNAL(getRobotTrajectoryData()), this, SLOT(setRobotTrajectoryData()));

  connect(this, SIGNAL(displayStatus(const QString)), this, SLOT(displayStatusHandler(const QString)));
}

FanucGrindingRvizPlugin::~FanucGrindingRvizPlugin()
{}

void FanucGrindingRvizPlugin::enablePanelAlignmentHandler()
{
  tab_widget_->setTabEnabled(1, true);
}

void FanucGrindingRvizPlugin::enablePanelComparisonHandler()
{
  tab_widget_->setTabEnabled(2, true);
}

void FanucGrindingRvizPlugin::enablePanelPathPlanningHandler()
{
  tab_widget_->setTabEnabled(3, true);
}

void FanucGrindingRvizPlugin::enablePanelPostProcessorHandler()
{
  tab_widget_->setTabEnabled(4, true);
}

void FanucGrindingRvizPlugin::enablePanelHandler(bool status)
{
  setEnabled(status);
}

void FanucGrindingRvizPlugin::displayStatusHandler(const QString message)
{
  status_label_->setText(message);
}

void FanucGrindingRvizPlugin::displayMsgBoxHandler(const QString title, const QString msg, const QString info_msg)
{
  enablePanelHandler(false);
  QMessageBox msg_box;
  msg_box.setWindowTitle(title);
  msg_box.setText(msg);
  msg_box.setInformativeText(info_msg);
  msg_box.setIcon(QMessageBox::Critical);
  msg_box.setStandardButtons(QMessageBox::Ok);
  msg_box.exec();
  enablePanelHandler(true);
}

void FanucGrindingRvizPlugin::triggerSave()
{
  Q_EMIT configChanged();
}

void FanucGrindingRvizPlugin::setCADDatas(const QString cad_filename, const QString cad_marker_name)
{
  cad_filename_ = cad_filename;
  cad_marker_name_ = cad_marker_name;
}

void FanucGrindingRvizPlugin::setScanDatas(const QString scan_filename, const QString scan_marker_name)
{
  scan_filename_ = scan_filename;
  scan_marker_name_ = scan_marker_name;
}

void FanucGrindingRvizPlugin::setRobotTrajectoryData()
{
  post_processor_widget_->setRobotPoses(path_planning_widget_->getRobotPoses());
  post_processor_widget_->setPointColorViz(path_planning_widget_->getPointColorViz());
  post_processor_widget_->setIndexVector(path_planning_widget_->getIndexVector());
}

void FanucGrindingRvizPlugin::sendCADAndScanDatasSlot()
{
  Q_EMIT sendCADAndScanDatas(cad_filename_, cad_marker_name_, scan_filename_, scan_marker_name_);
}

// Save all configuration data from this panel to the given Config object
void FanucGrindingRvizPlugin::save(rviz::Config config) const
{
  rviz::Panel::save(config);

  scanning_widget_->save(config);
  alignment_widget_->save(config);
  comparison_widget_->save(config);
  path_planning_widget_->save(config);
  post_processor_widget_->save(config);
}

// Load all configuration data for this panel from the given Config object.
void FanucGrindingRvizPlugin::load(const rviz::Config& config)
{
  rviz::Panel::load(config);

  scanning_widget_->load(config);
  alignment_widget_->load(config);
  comparison_widget_->load(config);
  path_planning_widget_->load(config);
  post_processor_widget_->load(config);
}

}  // end namespace

#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(fanuc_grinding_rviz_plugin::FanucGrindingRvizPlugin, rviz::Panel)
<commit_msg>Eclipse auto format on connect calls<commit_after>#include <QVBoxLayout>
#include <QHBoxLayout>
#include <QLabel>
#include <QDoubleSpinBox>
#include <QSpinBox>
#include <QPushButton>
#include <QMessageBox>
#include <QFuture>
#include <QtConcurrentRun>
#include <QWidget>
#include <QTabWidget>
#include <QDebug>
#include <QLabel>
#include <QRadioButton>
#include <QLineEdit>
#include <QFileDialog>

#include <string>
#include <iostream>

#include "fanuc_grinding_rviz_plugin.h"

namespace fanuc_grinding_rviz_plugin
{
FanucGrindingRvizPlugin::FanucGrindingRvizPlugin(QWidget* parent) :
    rviz::Panel(parent)
{
  // Create Tabs
  tab_widget_ = new QTabWidget();

  scanning_widget_ = new ScanningWidget();
  alignment_widget_ = new AlignmentWidget();
  comparison_widget_ = new ComparisonWidget();
  path_planning_widget_ = new PathPlanningWidget();
  post_processor_widget_ = new PostProcessorWidget();

  tab_widget_->addTab(scanning_widget_, "Scanning");
  tab_widget_->addTab(alignment_widget_, "Alignment");
  tab_widget_->addTab(comparison_widget_, "Comparison");
  tab_widget_->addTab(path_planning_widget_, "Path planning");
  tab_widget_->addTab(post_processor_widget_, "Post processor");
  tab_widget_->setTabEnabled(0, true);
  tab_widget_->setTabEnabled(1, false);
  tab_widget_->setTabEnabled(2, false);
  tab_widget_->setTabEnabled(3, false);
  tab_widget_->setTabEnabled(4, false);

  // Bottom status layout
  QVBoxLayout *status_layout_ = new QVBoxLayout;
  status_layout_->addWidget(new QLabel("Status:"));

  // Global Layout
  QVBoxLayout *global_layout_ = new QVBoxLayout;
  global_layout_->addWidget(tab_widget_);
  global_layout_->addLayout(status_layout_);
  status_label_ = new QLabel;
  global_layout_->addWidget(status_label_);
  setLayout(global_layout_);

  // Connect handlers
  // SCANNING
  // Will display a status in general status label ( from scanning widget )
  connect(scanning_widget_, SIGNAL(sendStatus(QString)), this, SLOT(displayStatusHandler(QString)));
  connect(scanning_widget_, SIGNAL(sendMsgBox(QString, QString , QString)), this,
          SLOT(displayMsgBoxHandler(QString, QString, QString)));

  // Call configChanged at each time that scanning_widget_ is modified
  connect(scanning_widget_, SIGNAL(GUIChanged()), this, SLOT(triggerSave()));
  // Enable AlignementPanel when scanning_widget_ will send the SIGNAL
  connect(scanning_widget_, SIGNAL(enablePanelAlignment()), this, SLOT(enablePanelAlignmentHandler()));
  // Enable general panel when scanning_widget_ send the SIGNAL
  connect(scanning_widget_, SIGNAL(enablePanel(bool)), this, SLOT(enablePanelHandler(bool)));

  // Will send information about cad and scan in the other widgets
  connect(scanning_widget_, SIGNAL(sendCADDatas(QString, QString)), this, SLOT(setCADDatas(QString, QString)));
  connect(scanning_widget_, SIGNAL(sendScanDatas(QString, QString)), this, SLOT(setScanDatas(QString, QString)));
  // For the demonstrator, we will skip alignment and comparison parts for the moment
  connect(scanning_widget_, SIGNAL(enablePanelPathPlanning()), this, SLOT(enablePanelPathPlanningHandler()));

  //ALIGNMENT
  // Will display a status in general status label ( from alignment widget )
  connect(alignment_widget_, SIGNAL(sendStatus(QString)), this, SLOT(displayStatusHandler(QString)));
  connect(alignment_widget_, SIGNAL(sendMsgBox(QString, QString , QString)), this,
          SLOT(displayMsgBoxHandler(QString, QString, QString)));
  // Call configChanged at each time that alignment_widget_ is modified
  connect(alignment_widget_, SIGNAL(GUIChanged()), this, SLOT(triggerSave()));
  // Enable compoarison_panel when alignment_widget_ will send the SIGNAL
  connect(alignment_widget_, SIGNAL(enablePanelComparison()), this, SLOT(enablePanelComparisonHandler()));
  // Enable general panel when alignment_widget_ send the SIGNAL
  connect(alignment_widget_, SIGNAL(enablePanel(bool)), this, SLOT(enablePanelHandler(bool)));
  // Received a signal from alignment widget in order to get CAD and scan params
  connect(alignment_widget_, SIGNAL(getCADAndScanParams()), this, SLOT(sendCADAndScanDatasSlot()));
  // Send a signal to alignment widget in order to give CAD and scan params
  connect(this, SIGNAL(sendCADAndScanDatas(const QString, const QString, const QString, const QString)),
          alignment_widget_, SLOT(setCADAndScanParams(const QString, const QString, const QString, const QString)));

  //COMPARISON
  // Will display a status in general status label ( from comparison widget )
  connect(comparison_widget_, SIGNAL(sendStatus(QString)), this, SLOT(displayStatusHandler(QString)));
  connect(comparison_widget_, SIGNAL(sendMsgBox(QString, QString , QString)), this,
          SLOT(displayMsgBoxHandler(QString, QString, QString)));
  // Call configChanged at each time that comparison_widget_ is modified
  connect(comparison_widget_, SIGNAL(GUIChanged()), this, SLOT(triggerSave()));
  // Enable path_planning_widget when comparison_widget_ will send the SIGNAL
  connect(comparison_widget_, SIGNAL(enablePanelPathPlanning()), this, SLOT(enablePanelPathPlanningHandler()));
  // Enable general panel when comparison_widget_ send the SIGNAL
  connect(comparison_widget_, SIGNAL(enablePanel(bool)), this, SLOT(enablePanelHandler(bool)));
  // Received a signal from comparison widget in order to get CAD and scan params
  connect(comparison_widget_, SIGNAL(getCADAndScanParams()), this, SLOT(sendCADAndScanDatasSlot()));
  // Send a signal to comparison widget in order to give CAD and scan params
  connect(this, SIGNAL(sendCADAndScanDatas(const QString, const QString, const QString, const QString)),
          comparison_widget_, SLOT(setCADAndScanParams(const QString, const QString, const QString, const QString)));

  //PATH PLANNING
  // Will display a status in general status label ( from path_planning widget )
  connect(path_planning_widget_, SIGNAL(sendStatus(QString)), this, SLOT(displayStatusHandler(QString)));
  connect(path_planning_widget_, SIGNAL(sendMsgBox(QString, QString , QString)), this,
          SLOT(displayMsgBoxHandler(QString, QString, QString)));
  // Call configChanged at each time that path_planning_widget is modified
  connect(path_planning_widget_, SIGNAL(GUIChanged()), this, SLOT(triggerSave()));
  // Enable path_planning_widget when comparison_widget_ will send the SIGNAL
  connect(path_planning_widget_, SIGNAL(enablePanelPostProcessor()), this, SLOT(enablePanelPostProcessorHandler()));
  // Enable general panel when path_planning send the SIGNAL
  connect(path_planning_widget_, SIGNAL(enablePanel(bool)), this, SLOT(enablePanelHandler(bool)));
  // Received a signal from comparison widget in order to get CAD and scan params
  connect(path_planning_widget_, SIGNAL(getCADAndScanParams()), this, SLOT(sendCADAndScanDatasSlot()));
  // Send a signal to comparison widget in order to give CAD and scan params
  connect(this, SIGNAL(sendCADAndScanDatas(const QString, const QString, const QString, const QString)),
          path_planning_widget_, SLOT(setCADAndScanParams(const QString, const QString, const QString, const QString)));

  //POST_PROCESSOR
  // Will display a status in general status label ( from post_processor widget )
  connect(post_processor_widget_, SIGNAL(sendStatus(QString)), this, SLOT(displayStatusHandler(QString)));
  connect(post_processor_widget_, SIGNAL(sendMsgBox(QString, QString, QString)), this,
          SLOT(displayMsgBoxHandler(QString, QString, QString)));
  // Call configChanged at each time that post_processor_widget is modified
  connect(post_processor_widget_, SIGNAL(GUIChanged()), this, SLOT(triggerSave()));
  // Enable general panel when post_processor send the SIGNAL
  connect(post_processor_widget_, SIGNAL(enablePanel(bool)), this, SLOT(enablePanelHandler(bool)));
  // Receive a signal from post processor widget in order to send robot poses data
  connect(post_processor_widget_, SIGNAL(getRobotTrajectoryData()), this, SLOT(setRobotTrajectoryData()));

  connect(this, SIGNAL(displayStatus(const QString)), this, SLOT(displayStatusHandler(const QString)));
}

FanucGrindingRvizPlugin::~FanucGrindingRvizPlugin()
{}

void FanucGrindingRvizPlugin::enablePanelAlignmentHandler()
{
  tab_widget_->setTabEnabled(1, true);
}

void FanucGrindingRvizPlugin::enablePanelComparisonHandler()
{
  tab_widget_->setTabEnabled(2, true);
}

void FanucGrindingRvizPlugin::enablePanelPathPlanningHandler()
{
  tab_widget_->setTabEnabled(3, true);
}

void FanucGrindingRvizPlugin::enablePanelPostProcessorHandler()
{
  tab_widget_->setTabEnabled(4, true);
}

void FanucGrindingRvizPlugin::enablePanelHandler(bool status)
{
  setEnabled(status);
}

void FanucGrindingRvizPlugin::displayStatusHandler(const QString message)
{
  status_label_->setText(message);
}

void FanucGrindingRvizPlugin::displayMsgBoxHandler(const QString title, const QString msg, const QString info_msg)
{
  enablePanelHandler(false);
  QMessageBox msg_box;
  msg_box.setWindowTitle(title);
  msg_box.setText(msg);
  msg_box.setInformativeText(info_msg);
  msg_box.setIcon(QMessageBox::Critical);
  msg_box.setStandardButtons(QMessageBox::Ok);
  msg_box.exec();
  enablePanelHandler(true);
}

void FanucGrindingRvizPlugin::triggerSave()
{
  Q_EMIT configChanged();
}

void FanucGrindingRvizPlugin::setCADDatas(const QString cad_filename, const QString cad_marker_name)
{
  cad_filename_ = cad_filename;
  cad_marker_name_ = cad_marker_name;
}

void FanucGrindingRvizPlugin::setScanDatas(const QString scan_filename, const QString scan_marker_name)
{
  scan_filename_ = scan_filename;
  scan_marker_name_ = scan_marker_name;
}

void FanucGrindingRvizPlugin::setRobotTrajectoryData()
{
  post_processor_widget_->setRobotPoses(path_planning_widget_->getRobotPoses());
  post_processor_widget_->setPointColorViz(path_planning_widget_->getPointColorViz());
  post_processor_widget_->setIndexVector(path_planning_widget_->getIndexVector());
}

void FanucGrindingRvizPlugin::sendCADAndScanDatasSlot()
{
  Q_EMIT sendCADAndScanDatas(cad_filename_, cad_marker_name_, scan_filename_, scan_marker_name_);
}

// Save all configuration data from this panel to the given Config object
void FanucGrindingRvizPlugin::save(rviz::Config config) const
{
  rviz::Panel::save(config);

  scanning_widget_->save(config);
  alignment_widget_->save(config);
  comparison_widget_->save(config);
  path_planning_widget_->save(config);
  post_processor_widget_->save(config);
}

// Load all configuration data for this panel from the given Config object.
void FanucGrindingRvizPlugin::load(const rviz::Config& config)
{
  rviz::Panel::load(config);

  scanning_widget_->load(config);
  alignment_widget_->load(config);
  comparison_widget_->load(config);
  path_planning_widget_->load(config);
  post_processor_widget_->load(config);
}

}  // end namespace

#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(fanuc_grinding_rviz_plugin::FanucGrindingRvizPlugin, rviz::Panel)
<|endoftext|>


<commit_before>// NBNBNBNBNBNBNBNBNBNBNBNBNBNBNBNB
// Any subscriptions/construction with Model Client must
// be before typical user subscriptions - if the same LCM object
// is used. Otherwise it won't be received before handling them
// NBNBNBNBNBNBNBNBNBNBNBNBNBNBNBNB
#include <stdio.h>
#include <sys/time.h>
#include <iostream>
#include <lcm/lcm-cpp.hpp>

#include "model-client.hpp"

/**
 * lcm_sleep:
 * @lcm: The lcm_t object.
 * @sleeptime max time to wait in seconds
 *
 *  Waits for up to @sleeptime seconds for an LCM message to arrive.
 *  It handles the first message if one arrives.
 *
 */
static inline void lcm_sleep(lcm_t * lcm, double sleeptime)
{ //
  int lcm_fileno = lcm_get_fileno(lcm);

  fd_set rfds;
  int retval;
  FD_ZERO(&rfds);
  FD_SET(lcm_fileno, &rfds);
  struct timeval tv;
  tv.tv_sec = (int) sleeptime;
  tv.tv_usec = (int) ((sleeptime - tv.tv_sec) * 1.0e6);
  retval = select(lcm_fileno + 1, &rfds, NULL, NULL, &tv);
  if (retval == -1) {
    fprintf(stderr, "bot_lcm_poll: select() failed!\n");
    return;
  }
  if (retval) {
    if (FD_ISSET(lcm_fileno, &rfds)) {
      lcm_handle(lcm);
    }
  }
}

static inline int64_t _timestamp_now()
{
  struct timeval tv;
  gettimeofday(&tv, NULL);
  return (int64_t) tv.tv_sec * 1000000 + tv.tv_usec;
}

void ModelClient::doModelClient(){
  model_pub_robot_urdf_t_subscription_t * sub = model_pub_robot_urdf_t_subscribe(lcm_, model_channel_.c_str(), robot_urdf_handler_aux, this);

  //TODO: is there a way to be sure nothing else is subscribed???
  int64_t utime_start = _timestamp_now();
  int64_t last_print_utime = -1;
  while ((_timestamp_now() - utime_start) < 2.5e6) {
    //bot_param_request_t req;
    //req.utime = _timestamp_now();
    //bot_param_request_t_publish(lcm, request_channel, &req);

    lcm_sleep(lcm_, .25);
    if (urdf_parsed_)
      break;
    int64_t now = _timestamp_now();
    if (now - utime_start > 5e5) {
      if (last_print_utime < 0) {
        fprintf(stderr, "ModelClient waiting to get model from robot_model_publisher...");
        last_print_utime = now;
      }
      else if (now - last_print_utime > 5e5) {
        fprintf(stderr, ".");
        last_print_utime = now;
      }
    }
  }

  if (last_print_utime > 0) {
    fprintf(stderr, "\n");
  }
  if (!urdf_parsed_) {
    fprintf(stderr,
        "WARNING: ModelClient could not get model from the robot_model_publisher!\n Did you forget to start one?\n");
    exit(-1);
    return;
  }

  if (!keep_updated_) {
    //unsubscribe from urdf messages
    model_pub_robot_urdf_t_unsubscribe(lcm_, sub);
  //  param->server_id = -1;
  }
}

ModelClient::ModelClient(lcm_t* lcm_, int keep_updated_):
    urdf_parsed_(false),lcm_(lcm_), keep_updated_(keep_updated_){
  model_channel_ = "ROBOT_MODEL";
  doModelClient(  );
}

// TODO implement update
ModelClient::ModelClient(lcm_t* lcm_, std::string model_channel_, int keep_updated_):
    urdf_parsed_(false),lcm_(lcm_),
    model_channel_(model_channel_), keep_updated_(keep_updated_){

  file_read_success_ = false; // file wasn't read, book keeping
  doModelClient();
}

ModelClient::ModelClient(std::string urdf_filename){
  // Received robot urdf string. Store it internally and get all available joints.
  file_read_success_ = readURDFFromFile(urdf_filename);
  if (file_read_success_){
    std::cout<< "Read urdf_xml_string of robot ["
        << robot_name_ << "] from file, storing it internally as a param" << std::endl;
    parseURDFString();
  }else{
    std::cout<< urdf_filename << " could not be read. exiting" << std::endl;
    exit(-1);
  }
}

void ModelClient::robot_urdf_handler(const char* channel, const model_pub_robot_urdf_t *msg){

  // Received robot urdf string. Store it internally and get all available joints.
  robot_name_      = msg->robot_name;
  urdf_xml_string_ = msg->urdf_xml_string;
  std::cout<< "Received urdf_xml_string of robot ["
      << msg->robot_name << "], storing it internally as a param" << std::endl;
  parseURDFString();
}

void ModelClient::parseURDFString(){
  // Get a urdf Model from the xml string and get all the joint names.
  urdf::Model robot_model;
  if (!robot_model.initString( urdf_xml_string_ ))
  {
    std::cerr << "ERROR: Could not generate robot model" << std::endl;
  }

  typedef std::map<std::string, boost::shared_ptr<urdf::Joint> > joints_mapType;
  for( joints_mapType::const_iterator it = robot_model.joints_.begin(); it!=robot_model.joints_.end(); it++)
  {
    if(it->second->type!=6) // All joints that not of the type FIXED.
      joint_names_.push_back(it->first);
  }
  links_map_ =  robot_model.links_;

  std::cout<< "Number of Joints: " << joint_names_.size() <<std::endl;
  setHandConfiguration();

  urdf_parsed_  = true;

}

bool ModelClient::readURDFFromFile(std::string urdf_file){

  // get the entire file
  std::string xml_string;
  std::fstream xml_file(urdf_file.c_str(), std::fstream::in);
  if (xml_file.is_open())
  {
    while ( xml_file.good() )
    {
      std::string line;
      std::getline( xml_file, line);
      xml_string += (line + "\n");
    }
    xml_file.close();
    std::cout << "File ["<< urdf_file << "]  parsed successfully.\n";
  }
  else
  {
    std::cout << "ERROR: Could not open file ["<< urdf_file << "] for parsing.\n";
    return false;
  }

  // Get a urdf Model from the xml string and get all the joint names.
  urdf::Model robot_model;
  if (!robot_model.initString( xml_string ))
  {
    std::cerr << "ERROR: Model Parsing the xml failed" << std::endl;
  }

  // Set the member variable:
  urdf_xml_string_ = xml_string;
  robot_name_ = robot_model.getName();
}

void ModelClient::setHandConfiguration(){

  if(find(joint_names_.begin(), joint_names_.end(), "left_f0_j0" ) != joint_names_.end()){
    std::cout << "Robot fitted with left Sandia hand\n";
    left_hand_ = MODEL_PUB_ROBOT_URDF_T_LEFT_SANDIA; //drc::robot_urdf_t::LEFT_SANDIA;
  }else if(find(joint_names_.begin(), joint_names_.end(), "left_finger[0]/joint_base" ) != joint_names_.end()){
    std::cout << "Robot fitted with left iRobot hand\n";
    left_hand_ = MODEL_PUB_ROBOT_URDF_T_LEFT_IROBOT; //drc::robot_urdf_t::LEFT_IROBOT;
  }else if(find(joint_names_.begin(), joint_names_.end(), "left_finger_1_joint_1" ) != joint_names_.end()){
    std::cout << "Robot fitted with left Robotiq hand\n";
    left_hand_ = MODEL_PUB_ROBOT_URDF_T_LEFT_ROBOTIQ; //drc::robot_urdf_t::LEFT_ROBOTIQ;
  }else{
    std::cout << "Robot has no left hand\n";
    left_hand_ = MODEL_PUB_ROBOT_URDF_T_LEFT_NONE; //drc::robot_urdf_t::LEFT_NONE;
  }

  if(find(joint_names_.begin(), joint_names_.end(), "right_f0_j0" ) != joint_names_.end()){
    std::cout << "Robot fitted with right Sandia hand\n";
    right_hand_ = MODEL_PUB_ROBOT_URDF_T_RIGHT_SANDIA;//drc::robot_urdf_t::RIGHT_SANDIA;
  }else if(find(joint_names_.begin(), joint_names_.end(), "right_finger[0]/joint_base" ) != joint_names_.end()){
    std::cout << "Robot fitted with right iRobot hand\n";
    right_hand_ = MODEL_PUB_ROBOT_URDF_T_RIGHT_IROBOT;//drc::robot_urdf_t::RIGHT_IROBOT;
  }else if(find(joint_names_.begin(), joint_names_.end(), "right_finger_1_joint_1" ) != joint_names_.end()){
    std::cout << "Robot fitted with right Robotiq hand\n";
    right_hand_ = MODEL_PUB_ROBOT_URDF_T_RIGHT_ROBOTIQ;//drc::robot_urdf_t::RIGHT_ROBOTIQ;
  }else{
    std::cout << "Robot has no right hand\n";
    right_hand_ = MODEL_PUB_ROBOT_URDF_T_RIGHT_NONE;//drc::robot_urdf_t::RIGHT_NONE;
  }
}
<commit_msg>Support l_finger in addition to left_finger to identify robotiq grippers<commit_after>// NBNBNBNBNBNBNBNBNBNBNBNBNBNBNBNB
// Any subscriptions/construction with Model Client must
// be before typical user subscriptions - if the same LCM object
// is used. Otherwise it won't be received before handling them
// NBNBNBNBNBNBNBNBNBNBNBNBNBNBNBNB
#include <stdio.h>
#include <sys/time.h>
#include <iostream>
#include <lcm/lcm-cpp.hpp>

#include "model-client.hpp"

/**
 * lcm_sleep:
 * @lcm: The lcm_t object.
 * @sleeptime max time to wait in seconds
 *
 *  Waits for up to @sleeptime seconds for an LCM message to arrive.
 *  It handles the first message if one arrives.
 *
 */
static inline void lcm_sleep(lcm_t * lcm, double sleeptime)
{ //
  int lcm_fileno = lcm_get_fileno(lcm);

  fd_set rfds;
  int retval;
  FD_ZERO(&rfds);
  FD_SET(lcm_fileno, &rfds);
  struct timeval tv;
  tv.tv_sec = (int) sleeptime;
  tv.tv_usec = (int) ((sleeptime - tv.tv_sec) * 1.0e6);
  retval = select(lcm_fileno + 1, &rfds, NULL, NULL, &tv);
  if (retval == -1) {
    fprintf(stderr, "bot_lcm_poll: select() failed!\n");
    return;
  }
  if (retval) {
    if (FD_ISSET(lcm_fileno, &rfds)) {
      lcm_handle(lcm);
    }
  }
}

static inline int64_t _timestamp_now()
{
  struct timeval tv;
  gettimeofday(&tv, NULL);
  return (int64_t) tv.tv_sec * 1000000 + tv.tv_usec;
}

void ModelClient::doModelClient(){
  model_pub_robot_urdf_t_subscription_t * sub = model_pub_robot_urdf_t_subscribe(lcm_, model_channel_.c_str(), robot_urdf_handler_aux, this);

  //TODO: is there a way to be sure nothing else is subscribed???
  int64_t utime_start = _timestamp_now();
  int64_t last_print_utime = -1;
  while ((_timestamp_now() - utime_start) < 2.5e6) {
    //bot_param_request_t req;
    //req.utime = _timestamp_now();
    //bot_param_request_t_publish(lcm, request_channel, &req);

    lcm_sleep(lcm_, .25);
    if (urdf_parsed_)
      break;
    int64_t now = _timestamp_now();
    if (now - utime_start > 5e5) {
      if (last_print_utime < 0) {
        fprintf(stderr, "ModelClient waiting to get model from robot_model_publisher...");
        last_print_utime = now;
      }
      else if (now - last_print_utime > 5e5) {
        fprintf(stderr, ".");
        last_print_utime = now;
      }
    }
  }

  if (last_print_utime > 0) {
    fprintf(stderr, "\n");
  }
  if (!urdf_parsed_) {
    fprintf(stderr,
        "WARNING: ModelClient could not get model from the robot_model_publisher!\n Did you forget to start one?\n");
    exit(-1);
    return;
  }

  if (!keep_updated_) {
    //unsubscribe from urdf messages
    model_pub_robot_urdf_t_unsubscribe(lcm_, sub);
  //  param->server_id = -1;
  }
}

ModelClient::ModelClient(lcm_t* lcm_, int keep_updated_):
    urdf_parsed_(false),lcm_(lcm_), keep_updated_(keep_updated_){
  model_channel_ = "ROBOT_MODEL";
  doModelClient(  );
}

// TODO implement update
ModelClient::ModelClient(lcm_t* lcm_, std::string model_channel_, int keep_updated_):
    urdf_parsed_(false),lcm_(lcm_),
    model_channel_(model_channel_), keep_updated_(keep_updated_){

  file_read_success_ = false; // file wasn't read, book keeping
  doModelClient();
}

ModelClient::ModelClient(std::string urdf_filename){
  // Received robot urdf string. Store it internally and get all available joints.
  file_read_success_ = readURDFFromFile(urdf_filename);
  if (file_read_success_){
    std::cout<< "Read urdf_xml_string of robot ["
        << robot_name_ << "] from file, storing it internally as a param" << std::endl;
    parseURDFString();
  }else{
    std::cout<< urdf_filename << " could not be read. exiting" << std::endl;
    exit(-1);
  }
}

void ModelClient::robot_urdf_handler(const char* channel, const model_pub_robot_urdf_t *msg){

  // Received robot urdf string. Store it internally and get all available joints.
  robot_name_      = msg->robot_name;
  urdf_xml_string_ = msg->urdf_xml_string;
  std::cout<< "Received urdf_xml_string of robot ["
      << msg->robot_name << "], storing it internally as a param" << std::endl;
  parseURDFString();
}

void ModelClient::parseURDFString(){
  // Get a urdf Model from the xml string and get all the joint names.
  urdf::Model robot_model;
  if (!robot_model.initString( urdf_xml_string_ ))
  {
    std::cerr << "ERROR: Could not generate robot model" << std::endl;
  }

  typedef std::map<std::string, boost::shared_ptr<urdf::Joint> > joints_mapType;
  for( joints_mapType::const_iterator it = robot_model.joints_.begin(); it!=robot_model.joints_.end(); it++)
  {
    if(it->second->type!=6) // All joints that not of the type FIXED.
      joint_names_.push_back(it->first);
  }
  links_map_ =  robot_model.links_;

  std::cout<< "Number of Joints: " << joint_names_.size() <<std::endl;
  setHandConfiguration();

  urdf_parsed_  = true;

}

bool ModelClient::readURDFFromFile(std::string urdf_file){

  // get the entire file
  std::string xml_string;
  std::fstream xml_file(urdf_file.c_str(), std::fstream::in);
  if (xml_file.is_open())
  {
    while ( xml_file.good() )
    {
      std::string line;
      std::getline( xml_file, line);
      xml_string += (line + "\n");
    }
    xml_file.close();
    std::cout << "File ["<< urdf_file << "]  parsed successfully.\n";
  }
  else
  {
    std::cout << "ERROR: Could not open file ["<< urdf_file << "] for parsing.\n";
    return false;
  }

  // Get a urdf Model from the xml string and get all the joint names.
  urdf::Model robot_model;
  if (!robot_model.initString( xml_string ))
  {
    std::cerr << "ERROR: Model Parsing the xml failed" << std::endl;
  }

  // Set the member variable:
  urdf_xml_string_ = xml_string;
  robot_name_ = robot_model.getName();
}

void ModelClient::setHandConfiguration() {
  if (find(joint_names_.begin(), joint_names_.end(), "left_f0_j0") !=
      joint_names_.end()) {
    std::cout << "Robot fitted with left Sandia hand\n";
    left_hand_ = MODEL_PUB_ROBOT_URDF_T_LEFT_SANDIA;
  } else if (find(joint_names_.begin(), joint_names_.end(),
                  "left_finger[0]/joint_base") != joint_names_.end()) {
    std::cout << "Robot fitted with left iRobot hand\n";
    left_hand_ = MODEL_PUB_ROBOT_URDF_T_LEFT_IROBOT;
  } else if (find(joint_names_.begin(), joint_names_.end(),
                  "left_finger_1_joint_1") != joint_names_.end() ||
             find(joint_names_.begin(), joint_names_.end(),
                  "l_finger_1_joint_1") != joint_names_.end()) {
    std::cout << "Robot fitted with left Robotiq hand\n";
    left_hand_ = MODEL_PUB_ROBOT_URDF_T_LEFT_ROBOTIQ;
  } else {
    std::cout << "Robot has no left hand\n";
    left_hand_ = MODEL_PUB_ROBOT_URDF_T_LEFT_NONE;
  }

  if (find(joint_names_.begin(), joint_names_.end(), "right_f0_j0") !=
      joint_names_.end()) {
    std::cout << "Robot fitted with right Sandia hand\n";
    right_hand_ = MODEL_PUB_ROBOT_URDF_T_RIGHT_SANDIA;
  } else if (find(joint_names_.begin(), joint_names_.end(),
                  "right_finger[0]/joint_base") != joint_names_.end()) {
    std::cout << "Robot fitted with right iRobot hand\n";
    right_hand_ = MODEL_PUB_ROBOT_URDF_T_RIGHT_IROBOT;
  } else if (find(joint_names_.begin(), joint_names_.end(),
                  "right_finger_1_joint_1") != joint_names_.end() ||
             find(joint_names_.begin(), joint_names_.end(),
                  "r_finger_1_joint_1") != joint_names_.end()) {
    std::cout << "Robot fitted with right Robotiq hand\n";
    right_hand_ = MODEL_PUB_ROBOT_URDF_T_RIGHT_ROBOTIQ;
  } else {
    std::cout << "Robot has no right hand\n";
    right_hand_ = MODEL_PUB_ROBOT_URDF_T_RIGHT_NONE;
  }
}
<|endoftext|>


<commit_before>// Copyright (c) 2009 The Chromium Authors. All rights reserved.
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.

#include <gtk/gtk.h>

#include "build/build_config.h"

#include "app/l10n_util.h"
#include "app/resource_bundle.h"
#include "base/compiler_specific.h"
#include "base/utf_string_conversions.h"
#include "chrome/browser/certificate_viewer.h"
#include "chrome/browser/gtk/gtk_util.h"
#include "chrome/browser/page_info_model.h"
#include "chrome/browser/page_info_window.h"
#include "grit/generated_resources.h"
#include "grit/locale_settings.h"
#include "grit/theme_resources.h"

namespace {

enum {
  RESPONSE_SHOW_CERT_INFO = 0,
};

class PageInfoWindowGtk : public PageInfoModel::PageInfoModelObserver {
 public:
  PageInfoWindowGtk(gfx::NativeWindow parent,
                    Profile* profile,
                    const GURL& url,
                    const NavigationEntry::SSLStatus& ssl,
                    bool show_history);
  ~PageInfoWindowGtk();

  // PageInfoModelObserver implementation:
  virtual void ModelChanged();

  // Shows the page info window.
  void Show();

  // Shows the certificate info window.
  void ShowCertDialog();

  GtkWidget* widget() { return dialog_; }

 private:
  // Layouts the different sections retrieved from the model.
  void InitContents();

  // Returns a widget that contains the UI for the passed |section|.
  GtkWidget* CreateSection(const PageInfoModel::SectionInfo& section);

  // The model containing the different sections to display.
  PageInfoModel model_;

  // The page info dialog.
  GtkWidget* dialog_;

  // The url for this dialog. Should be unique among active dialogs.
  GURL url_;

  // The virtual box containing the sections.
  GtkWidget* contents_;

  // The id of the certificate for this page.
  int cert_id_;

  DISALLOW_COPY_AND_ASSIGN(PageInfoWindowGtk);
};

// We only show one page info per URL (keyed on url.spec()).
typedef std::map<std::string, PageInfoWindowGtk*> PageInfoWindowMap;
PageInfoWindowMap g_page_info_window_map;

// Button callbacks.
void OnDialogResponse(GtkDialog* dialog, gint response_id,
                      PageInfoWindowGtk* page_info) {
  if (response_id == RESPONSE_SHOW_CERT_INFO) {
    page_info->ShowCertDialog();
  } else {
    // "Close" was clicked.
    gtk_widget_destroy(GTK_WIDGET(dialog));
  }
}

void OnDestroy(GtkDialog* dialog, PageInfoWindowGtk* page_info) {
  delete page_info;
}

////////////////////////////////////////////////////////////////////////////////
// PageInfoWindowGtk, public:
PageInfoWindowGtk::PageInfoWindowGtk(gfx::NativeWindow parent,
                                     Profile* profile,
                                     const GURL& url,
                                     const NavigationEntry::SSLStatus& ssl,
                                     bool show_history)
    : ALLOW_THIS_IN_INITIALIZER_LIST(model_(profile, url, ssl,
                                            show_history, this)),
      url_(url),
      contents_(NULL),
      cert_id_(ssl.cert_id()) {
  dialog_ = gtk_dialog_new_with_buttons(
      l10n_util::GetStringUTF8(IDS_PAGEINFO_WINDOW_TITLE).c_str(),
      parent,
      // Non-modal.
      GTK_DIALOG_NO_SEPARATOR,
      NULL);
  if (cert_id_) {
    gtk_dialog_add_button(
        GTK_DIALOG(dialog_),
        l10n_util::GetStringUTF8(IDS_PAGEINFO_CERT_INFO_BUTTON).c_str(),
        RESPONSE_SHOW_CERT_INFO);
  }
  gtk_dialog_add_button(GTK_DIALOG(dialog_), GTK_STOCK_CLOSE,
                        GTK_RESPONSE_CLOSE);
  gtk_dialog_set_default_response(GTK_DIALOG(dialog_), GTK_RESPONSE_CLOSE);

  gtk_box_set_spacing(GTK_BOX(GTK_DIALOG(dialog_)->vbox),
                      gtk_util::kContentAreaSpacing);
  g_signal_connect(dialog_, "response", G_CALLBACK(OnDialogResponse), this);
  g_signal_connect(dialog_, "destroy", G_CALLBACK(OnDestroy), this);

  InitContents();

  g_page_info_window_map[url.spec()] = this;
}

PageInfoWindowGtk::~PageInfoWindowGtk() {
  g_page_info_window_map.erase(url_.spec());
}

void PageInfoWindowGtk::ModelChanged() {
  InitContents();
}

GtkWidget* PageInfoWindowGtk::CreateSection(
    const PageInfoModel::SectionInfo& section) {
  GtkWidget* vbox = gtk_vbox_new(FALSE, gtk_util::kControlSpacing);
  GtkWidget* label = gtk_label_new(UTF16ToUTF8(section.title).c_str());

  PangoAttrList* attributes = pango_attr_list_new();
  pango_attr_list_insert(attributes,
                         pango_attr_weight_new(PANGO_WEIGHT_BOLD));
  gtk_label_set_attributes(GTK_LABEL(label), attributes);
  pango_attr_list_unref(attributes);
  gtk_misc_set_alignment(GTK_MISC(label), 0, 0);
  gtk_box_pack_start(GTK_BOX(vbox), label, FALSE, FALSE, 0);

  GtkWidget* section_box = gtk_hbox_new(FALSE, 0);
  ResourceBundle& rb = ResourceBundle::GetSharedInstance();
  GtkWidget* image = gtk_image_new_from_pixbuf(
      section.state == PageInfoModel::SECTION_STATE_OK ?
      rb.GetPixbufNamed(IDR_PAGEINFO_GOOD) :
      rb.GetPixbufNamed(IDR_PAGEINFO_BAD));
  gtk_box_pack_start(GTK_BOX(section_box), image, FALSE, FALSE,
                     gtk_util::kControlSpacing);
  gtk_misc_set_alignment(GTK_MISC(image), 0, 0);

  GtkWidget* text_box = gtk_vbox_new(FALSE, gtk_util::kControlSpacing);
  if (!section.headline.empty()) {
    label = gtk_label_new(UTF16ToUTF8(section.headline).c_str());
    gtk_misc_set_alignment(GTK_MISC(label), 0, 0);
    gtk_box_pack_start(GTK_BOX(text_box), label, FALSE, FALSE, 0);
  }
  label = gtk_label_new(UTF16ToUTF8(section.description).c_str());
  gtk_misc_set_alignment(GTK_MISC(label), 0, 0);
  gtk_label_set_line_wrap(GTK_LABEL(label), TRUE);
  // Allow linebreaking in the middle of words if necessary, so that extremely
  // long hostnames (longer than one line) will still be completely shown.
  gtk_label_set_line_wrap_mode(GTK_LABEL(label), PANGO_WRAP_WORD_CHAR);
  gtk_box_pack_start(GTK_BOX(text_box), label, FALSE, FALSE, 0);
  gtk_widget_set_size_request(label, 400, -1);

  gtk_box_pack_start(GTK_BOX(section_box), text_box, TRUE, TRUE, 0);
  gtk_box_pack_start(GTK_BOX(vbox), section_box, TRUE, TRUE, 0);

  return vbox;
}

void PageInfoWindowGtk::InitContents() {
  if (contents_)
    gtk_widget_destroy(contents_);
  contents_ = gtk_vbox_new(FALSE, gtk_util::kContentAreaSpacing);
  for (int i = 0; i < model_.GetSectionCount(); i++) {
    gtk_box_pack_start(GTK_BOX(contents_),
                       CreateSection(model_.GetSectionInfo(i)),
                       FALSE, FALSE, 0);
  }
  gtk_widget_show_all(contents_);
  gtk_container_add(GTK_CONTAINER(GTK_DIALOG(dialog_)->vbox), contents_);
}

void PageInfoWindowGtk::Show() {
  gtk_widget_show(dialog_);
}

void PageInfoWindowGtk::ShowCertDialog() {
  ShowCertificateViewerByID(GTK_WINDOW(dialog_), cert_id_);
}

}  // namespace

namespace browser {

void ShowPageInfo(gfx::NativeWindow parent,
                  Profile* profile,
                  const GURL& url,
                  const NavigationEntry::SSLStatus& ssl,
                  bool show_history) {
  PageInfoWindowMap::iterator iter =
      g_page_info_window_map.find(url.spec());
  if (iter != g_page_info_window_map.end()) {
    gtk_window_present(GTK_WINDOW(iter->second->widget()));
    return;
  }

  PageInfoWindowGtk* page_info_window =
      new PageInfoWindowGtk(parent, profile, url, ssl, show_history);
  page_info_window->Show();
}

}  // namespace browser
<commit_msg>[gtk] Update page-info icons to match behavior in page_info_window_view.cc.<commit_after>// Copyright (c) 2009 The Chromium Authors. All rights reserved.
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.

#include <gtk/gtk.h>

#include "build/build_config.h"

#include "app/l10n_util.h"
#include "app/resource_bundle.h"
#include "base/compiler_specific.h"
#include "base/utf_string_conversions.h"
#include "chrome/browser/certificate_viewer.h"
#include "chrome/browser/gtk/gtk_util.h"
#include "chrome/browser/page_info_model.h"
#include "chrome/browser/page_info_window.h"
#include "grit/generated_resources.h"
#include "grit/locale_settings.h"
#include "grit/theme_resources.h"

namespace {

enum {
  RESPONSE_SHOW_CERT_INFO = 0,
};

class PageInfoWindowGtk : public PageInfoModel::PageInfoModelObserver {
 public:
  PageInfoWindowGtk(gfx::NativeWindow parent,
                    Profile* profile,
                    const GURL& url,
                    const NavigationEntry::SSLStatus& ssl,
                    bool show_history);
  ~PageInfoWindowGtk();

  // PageInfoModelObserver implementation:
  virtual void ModelChanged();

  // Shows the page info window.
  void Show();

  // Shows the certificate info window.
  void ShowCertDialog();

  GtkWidget* widget() { return dialog_; }

 private:
  // Layouts the different sections retrieved from the model.
  void InitContents();

  // Returns a widget that contains the UI for the passed |section|.
  GtkWidget* CreateSection(const PageInfoModel::SectionInfo& section);

  // The model containing the different sections to display.
  PageInfoModel model_;

  // The page info dialog.
  GtkWidget* dialog_;

  // The url for this dialog. Should be unique among active dialogs.
  GURL url_;

  // The virtual box containing the sections.
  GtkWidget* contents_;

  // The id of the certificate for this page.
  int cert_id_;

  DISALLOW_COPY_AND_ASSIGN(PageInfoWindowGtk);
};

// We only show one page info per URL (keyed on url.spec()).
typedef std::map<std::string, PageInfoWindowGtk*> PageInfoWindowMap;
PageInfoWindowMap g_page_info_window_map;

// Button callbacks.
void OnDialogResponse(GtkDialog* dialog, gint response_id,
                      PageInfoWindowGtk* page_info) {
  if (response_id == RESPONSE_SHOW_CERT_INFO) {
    page_info->ShowCertDialog();
  } else {
    // "Close" was clicked.
    gtk_widget_destroy(GTK_WIDGET(dialog));
  }
}

void OnDestroy(GtkDialog* dialog, PageInfoWindowGtk* page_info) {
  delete page_info;
}

////////////////////////////////////////////////////////////////////////////////
// PageInfoWindowGtk, public:
PageInfoWindowGtk::PageInfoWindowGtk(gfx::NativeWindow parent,
                                     Profile* profile,
                                     const GURL& url,
                                     const NavigationEntry::SSLStatus& ssl,
                                     bool show_history)
    : ALLOW_THIS_IN_INITIALIZER_LIST(model_(profile, url, ssl,
                                            show_history, this)),
      url_(url),
      contents_(NULL),
      cert_id_(ssl.cert_id()) {
  dialog_ = gtk_dialog_new_with_buttons(
      l10n_util::GetStringUTF8(IDS_PAGEINFO_WINDOW_TITLE).c_str(),
      parent,
      // Non-modal.
      GTK_DIALOG_NO_SEPARATOR,
      NULL);
  if (cert_id_) {
    gtk_dialog_add_button(
        GTK_DIALOG(dialog_),
        l10n_util::GetStringUTF8(IDS_PAGEINFO_CERT_INFO_BUTTON).c_str(),
        RESPONSE_SHOW_CERT_INFO);
  }
  gtk_dialog_add_button(GTK_DIALOG(dialog_), GTK_STOCK_CLOSE,
                        GTK_RESPONSE_CLOSE);
  gtk_dialog_set_default_response(GTK_DIALOG(dialog_), GTK_RESPONSE_CLOSE);

  gtk_box_set_spacing(GTK_BOX(GTK_DIALOG(dialog_)->vbox),
                      gtk_util::kContentAreaSpacing);
  g_signal_connect(dialog_, "response", G_CALLBACK(OnDialogResponse), this);
  g_signal_connect(dialog_, "destroy", G_CALLBACK(OnDestroy), this);

  InitContents();

  g_page_info_window_map[url.spec()] = this;
}

PageInfoWindowGtk::~PageInfoWindowGtk() {
  g_page_info_window_map.erase(url_.spec());
}

void PageInfoWindowGtk::ModelChanged() {
  InitContents();
}

GtkWidget* PageInfoWindowGtk::CreateSection(
    const PageInfoModel::SectionInfo& section) {
  GtkWidget* vbox = gtk_vbox_new(FALSE, gtk_util::kControlSpacing);
  GtkWidget* label = gtk_label_new(UTF16ToUTF8(section.title).c_str());

  PangoAttrList* attributes = pango_attr_list_new();
  pango_attr_list_insert(attributes,
                         pango_attr_weight_new(PANGO_WEIGHT_BOLD));
  gtk_label_set_attributes(GTK_LABEL(label), attributes);
  pango_attr_list_unref(attributes);
  gtk_misc_set_alignment(GTK_MISC(label), 0, 0);
  gtk_box_pack_start(GTK_BOX(vbox), label, FALSE, FALSE, 0);

  GtkWidget* section_box = gtk_hbox_new(FALSE, 0);
  ResourceBundle& rb = ResourceBundle::GetSharedInstance();
  GtkWidget* image = gtk_image_new_from_pixbuf(
      section.state == PageInfoModel::SECTION_STATE_OK ?
      rb.GetPixbufNamed(IDR_PAGEINFO_GOOD) :
      rb.GetPixbufNamed(IDR_PAGEINFO_WARNING_MAJOR));
  gtk_box_pack_start(GTK_BOX(section_box), image, FALSE, FALSE,
                     gtk_util::kControlSpacing);
  gtk_misc_set_alignment(GTK_MISC(image), 0, 0);

  GtkWidget* text_box = gtk_vbox_new(FALSE, gtk_util::kControlSpacing);
  if (!section.headline.empty()) {
    label = gtk_label_new(UTF16ToUTF8(section.headline).c_str());
    gtk_misc_set_alignment(GTK_MISC(label), 0, 0);
    gtk_box_pack_start(GTK_BOX(text_box), label, FALSE, FALSE, 0);
  }
  label = gtk_label_new(UTF16ToUTF8(section.description).c_str());
  gtk_misc_set_alignment(GTK_MISC(label), 0, 0);
  gtk_label_set_line_wrap(GTK_LABEL(label), TRUE);
  // Allow linebreaking in the middle of words if necessary, so that extremely
  // long hostnames (longer than one line) will still be completely shown.
  gtk_label_set_line_wrap_mode(GTK_LABEL(label), PANGO_WRAP_WORD_CHAR);
  gtk_box_pack_start(GTK_BOX(text_box), label, FALSE, FALSE, 0);
  gtk_widget_set_size_request(label, 400, -1);

  gtk_box_pack_start(GTK_BOX(section_box), text_box, TRUE, TRUE, 0);
  gtk_box_pack_start(GTK_BOX(vbox), section_box, TRUE, TRUE, 0);

  return vbox;
}

void PageInfoWindowGtk::InitContents() {
  if (contents_)
    gtk_widget_destroy(contents_);
  contents_ = gtk_vbox_new(FALSE, gtk_util::kContentAreaSpacing);
  for (int i = 0; i < model_.GetSectionCount(); i++) {
    gtk_box_pack_start(GTK_BOX(contents_),
                       CreateSection(model_.GetSectionInfo(i)),
                       FALSE, FALSE, 0);
  }
  gtk_widget_show_all(contents_);
  gtk_container_add(GTK_CONTAINER(GTK_DIALOG(dialog_)->vbox), contents_);
}

void PageInfoWindowGtk::Show() {
  gtk_widget_show(dialog_);
}

void PageInfoWindowGtk::ShowCertDialog() {
  ShowCertificateViewerByID(GTK_WINDOW(dialog_), cert_id_);
}

}  // namespace

namespace browser {

void ShowPageInfo(gfx::NativeWindow parent,
                  Profile* profile,
                  const GURL& url,
                  const NavigationEntry::SSLStatus& ssl,
                  bool show_history) {
  PageInfoWindowMap::iterator iter =
      g_page_info_window_map.find(url.spec());
  if (iter != g_page_info_window_map.end()) {
    gtk_window_present(GTK_WINDOW(iter->second->widget()));
    return;
  }

  PageInfoWindowGtk* page_info_window =
      new PageInfoWindowGtk(parent, profile, url, ssl, show_history);
  page_info_window->Show();
}

}  // namespace browser
<|endoftext|>


<commit_before>#include "rigidBodyManipulatorMexFunctions.h"
#include "RigidBodyManipulator.h"
#include "standardMexConversions.h"
#include <typeinfo>

using namespace std;
using namespace Eigen;

/**
 * fromMex specializations
 */
RigidBodyManipulator &fromMex(const mxArray *source, RigidBodyManipulator *) {
  return *static_cast<RigidBodyManipulator *>(getDrakeMexPointer(source));
}

std::set<int> fromMex(const mxArray *source, std::set<int> *) {
  // for robotnum. this is kind of a weird one, but OK for now
  std::set<int> robotnum_set;
  int num_robot = static_cast<int>(mxGetNumberOfElements(source));
  double *robotnum = mxGetPrSafe(source);
  for (int i = 0; i < num_robot; i++) {
    robotnum_set.insert((int) robotnum[i] - 1);
  }
  return robotnum_set;
}

template<typename Scalar>
KinematicsCache<Scalar> &fromMex(const mxArray *mex, KinematicsCache<Scalar> *) {
  if (!mxIsClass(mex, "DrakeMexPointer")) {
    throw MexToCppConversionError("Expected DrakeMexPointer containing KinematicsCache");
  }
  auto name = mxGetStdString(mxGetPropertySafe(mex, "name"));
  if (name != typeid(KinematicsCache<Scalar>).name()) {
    ostringstream buf;
    buf << "Expected KinematicsCache of type " << typeid(KinematicsCache<Scalar>).name() << ", but got " << name;
    throw MexToCppConversionError(buf.str());
  }
  return *static_cast<KinematicsCache<Scalar> *>(getDrakeMexPointer(mex));
}

/**
 * toMex specializations
 */
void toMex(const KinematicPath &path, mxArray *dest[], int nlhs) {
  if (nlhs > 0) {
    dest[0] = stdVectorToMatlab(path.body_path);
  }
  if (nlhs > 1) {
    dest[1] = stdVectorToMatlab(path.joint_path);
  }
  if (nlhs > 2) {
    dest[2] = stdVectorToMatlab(path.joint_direction_signs);
  }
}

typedef AutoDiffScalar<VectorXd> AutoDiffDynamicSize;
//typedef AutoDiffScalar<Matrix<double, Dynamic, 1, 0, 72, 1> AutoDiffFixedMaxSize;

/**
 * Mex function implementations
 */
void centerOfMassJacobianDotTimesVmex(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
  function<GradientVar<double, SPACE_DIMENSION, 1>(const RigidBodyManipulator &, KinematicsCache<double> &, int, const std::set<int> &)> func_double = mem_fn(
      &RigidBodyManipulator::centerOfMassJacobianDotTimesV<double>);
  function<GradientVar<AutoDiffDynamicSize, SPACE_DIMENSION, 1>(const RigidBodyManipulator &, KinematicsCache<AutoDiffDynamicSize> &, int, const std::set<int> &)> func_autodiff_dynamic = mem_fn(
      &RigidBodyManipulator::centerOfMassJacobianDotTimesV<AutoDiffDynamicSize>);
  mexTryToCallFunctions(nlhs, plhs, nrhs, prhs, func_double, func_autodiff_dynamic);
}

void centerOfMassmex(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
  function<Matrix<double, SPACE_DIMENSION, 1>(const RigidBodyManipulator &, KinematicsCache<double> &, const std::set<int> &)> func_double = mem_fn(&RigidBodyManipulator::centerOfMass<double>);
  function<Matrix<AutoDiffDynamicSize, SPACE_DIMENSION, 1>(const RigidBodyManipulator &, KinematicsCache<AutoDiffDynamicSize> &, const std::set<int> &)> func_autodiff_dynamic = mem_fn(&RigidBodyManipulator::centerOfMass<AutoDiffDynamicSize>);
  mexTryToCallFunctions(nlhs, plhs, nrhs, prhs, func_double, func_autodiff_dynamic);
}

void centerOfMassJacobianmex(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
  function<GradientVar<double, SPACE_DIMENSION, Dynamic>(const RigidBodyManipulator &, KinematicsCache<double> &, int, const std::set<int> &, bool)> func_double = mem_fn(
      &RigidBodyManipulator::centerOfMassJacobian<double>);
  function<GradientVar<AutoDiffDynamicSize, SPACE_DIMENSION, Dynamic>(const RigidBodyManipulator &, KinematicsCache<AutoDiffDynamicSize> &, int, const std::set<int> &, bool)> func_autodiff_dynamic = mem_fn(
      &RigidBodyManipulator::centerOfMassJacobian<AutoDiffDynamicSize>);
  mexTryToCallFunctions(nlhs, plhs, nrhs, prhs, func_double, func_autodiff_dynamic);
}

void centroidalMomentumMatrixDotTimesvmex(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
  function<GradientVar<double, TWIST_SIZE, 1>(const RigidBodyManipulator &, KinematicsCache<double> &, int, const std::set<int> &)> func_double = mem_fn(
      &RigidBodyManipulator::centroidalMomentumMatrixDotTimesV<double>);
  function<GradientVar<AutoDiffDynamicSize, TWIST_SIZE, 1>
      (const RigidBodyManipulator &, KinematicsCache<AutoDiffDynamicSize> &, int, const std::set<int> &)> func_autodiff_dynamic = mem_fn(
      &RigidBodyManipulator::centroidalMomentumMatrixDotTimesV<AutoDiffDynamicSize>);
  mexTryToCallFunctions(nlhs, plhs, nrhs, prhs, func_double, func_autodiff_dynamic);
}

void centroidalMomentumMatrixmex(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
  function<GradientVar<double, TWIST_SIZE, Dynamic> const(RigidBodyManipulator &, KinematicsCache<double> &, int, const std::set<int> &, bool)> func_double = mem_fn(
      &RigidBodyManipulator::centroidalMomentumMatrix<double>);
  function<GradientVar<AutoDiffDynamicSize, TWIST_SIZE, Dynamic> const(RigidBodyManipulator &, KinematicsCache<AutoDiffDynamicSize> &, int, const std::set<int> &, bool)> func_autodiff_dynamic = mem_fn(
      &RigidBodyManipulator::centroidalMomentumMatrix<AutoDiffDynamicSize>);
  mexTryToCallFunctions(nlhs, plhs, nrhs, prhs, func_double, func_autodiff_dynamic);
}

template <typename DerivedQ, typename DerivedV>
void doKinematicsTemp(const RigidBodyManipulator &model, KinematicsCache<typename DerivedQ::Scalar> &cache, const MatrixBase<DerivedQ> &q, const MatrixBase<DerivedV> &v, bool compute_JdotV) {
  // temporary solution. Explicit doKinematics calls will not be necessary in the near future.
  if (v.size() == 0 && model.num_velocities != 0)
    cache.initialize(q);
  else
    cache.initialize(q, v);
  model.doKinematics(cache, compute_JdotV);
}

void doKinematicsmex(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
  typedef Matrix<AutoDiffScalar<VectorXd>, Dynamic, 1> VectorXAutoDiff;
  function<void(const RigidBodyManipulator &, KinematicsCache<double> &, const MatrixBase<Map<const VectorXd>> &, const MatrixBase<Map<const VectorXd>> &, bool)> func_double = &doKinematicsTemp<Map<const VectorXd>, Map<const VectorXd>>;
  function<void(const RigidBodyManipulator &, KinematicsCache<typename VectorXAutoDiff::Scalar> &, const MatrixBase<VectorXAutoDiff> &, const MatrixBase<VectorXAutoDiff> &, bool)> func_autodiff_dynamic = &doKinematicsTemp<VectorXAutoDiff, VectorXAutoDiff>;
  mexTryToCallFunctions(nlhs, plhs, nrhs, prhs, func_double, func_autodiff_dynamic);
}

void findKinematicPathmex(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
  function<KinematicPath(const RigidBodyManipulator &, int, int)> func = mem_fn(&RigidBodyManipulator::findKinematicPath);
  mexCallFunction(func, nlhs, plhs, nrhs, prhs);
}

void forwardJacDotTimesVmex(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
  typedef Map<const Matrix3Xd> DerivedPointsDouble;
  function<GradientVar<typename DerivedPointsDouble::Scalar, Dynamic, 1>(const RigidBodyManipulator &, const KinematicsCache<typename DerivedPointsDouble::Scalar> &, const MatrixBase<DerivedPointsDouble> &, int, int, int, int)> func_double = mem_fn(
      &RigidBodyManipulator::forwardJacDotTimesV<DerivedPointsDouble>);

  typedef Matrix<AutoDiffDynamicSize, 3, Dynamic> DerivedPointsAutoDiff;
  function<GradientVar<typename DerivedPointsAutoDiff::Scalar, Dynamic, 1>(const RigidBodyManipulator &, const KinematicsCache<typename DerivedPointsAutoDiff::Scalar> &, const MatrixBase<DerivedPointsAutoDiff> &, int, int, int, int)> func_autodiff_dynamic = mem_fn(
      &RigidBodyManipulator::forwardJacDotTimesV<DerivedPointsAutoDiff>);

  mexTryToCallFunctions(nlhs, plhs, nrhs, prhs, func_double, func_autodiff_dynamic);
}

void forwardKinmex(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
  typedef Map<const Matrix3Xd> DerivedPointsDouble;
  function<Matrix<typename DerivedPointsDouble::Scalar, Dynamic, DerivedPointsDouble::ColsAtCompileTime>(RigidBodyManipulator &, const KinematicsCache<typename DerivedPointsDouble::Scalar> &, const MatrixBase<DerivedPointsDouble> &, int, int, int)> func_double = mem_fn(
      &RigidBodyManipulator::forwardKin<DerivedPointsDouble>);

  typedef Matrix<AutoDiffDynamicSize, 3, Dynamic> DerivedPointsAutoDiff;
  function<Matrix<typename DerivedPointsAutoDiff::Scalar, Dynamic, DerivedPointsAutoDiff::ColsAtCompileTime>(RigidBodyManipulator &, const KinematicsCache<typename DerivedPointsAutoDiff::Scalar> &, const MatrixBase<DerivedPointsAutoDiff> &, int, int, int)> func_autodiff_dynamic = mem_fn(
      &RigidBodyManipulator::forwardKin<DerivedPointsAutoDiff>);

  mexTryToCallFunctions(nlhs, plhs, nrhs, prhs, func_double, func_autodiff_dynamic);
}

void forwardKinJacobianmex(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
  typedef Map<const Matrix3Xd> DerivedPointsDouble;
  function<GradientVar<typename DerivedPointsDouble::Scalar, Dynamic, Dynamic>(const RigidBodyManipulator &, const KinematicsCache<typename DerivedPointsDouble::Scalar> &, const MatrixBase<DerivedPointsDouble> &, int, int, int, bool, int)> func_double = mem_fn(
      &RigidBodyManipulator::forwardKinJacobian<DerivedPointsDouble>);

  typedef Matrix<AutoDiffDynamicSize, 3, Dynamic> DerivedPointsAutoDiff;
  function<GradientVar<typename DerivedPointsAutoDiff::Scalar, Dynamic, Dynamic>(const RigidBodyManipulator &, const KinematicsCache<typename DerivedPointsAutoDiff::Scalar> &, const MatrixBase<DerivedPointsAutoDiff> &, int, int, int, bool, int)> func_autodiff_dynamic = mem_fn(
      &RigidBodyManipulator::forwardKinJacobian<DerivedPointsAutoDiff>);

  mexTryToCallFunctions(nlhs, plhs, nrhs, prhs, func_double, func_autodiff_dynamic);
}

void forwardKinPositionGradientmex(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
  function<GradientVar<double, Dynamic, Dynamic>(const RigidBodyManipulator &, const KinematicsCache<double> &, int, int, int, int)> func_double = mem_fn(
      &RigidBodyManipulator::forwardKinPositionGradient<double>);

  function<GradientVar<AutoDiffDynamicSize, Dynamic, Dynamic>(const RigidBodyManipulator &, const KinematicsCache<AutoDiffDynamicSize> &, int, int, int, int)> func_autodiff_dynamic = mem_fn(
      &RigidBodyManipulator::forwardKinPositionGradient<AutoDiffDynamicSize>);

  mexTryToCallFunctions(nlhs, plhs, nrhs, prhs, func_double, func_autodiff_dynamic);
}

void geometricJacobianDotTimesVmex(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
  std::function<GradientVar<double, TWIST_SIZE, 1>(const RigidBodyManipulator &, const KinematicsCache<double> &, int, int, int, int)> func_double = mem_fn(
      &RigidBodyManipulator::geometricJacobianDotTimesV<double>);

  std::function<GradientVar<AutoDiffDynamicSize, TWIST_SIZE, 1>(const RigidBodyManipulator &, const KinematicsCache<AutoDiffDynamicSize> &, int, int, int, int)> func_autodiff_dynamic = mem_fn(
      &RigidBodyManipulator::geometricJacobianDotTimesV<AutoDiffDynamicSize>);

  mexTryToCallFunctions(nlhs, plhs, nrhs, prhs, func_double, func_autodiff_dynamic);
}

template <typename Scalar>
GradientVar<Scalar, TWIST_SIZE, Eigen::Dynamic> geometricJacobianTemp(const RigidBodyManipulator &model, const KinematicsCache<Scalar> &cache, int base_body_or_frame_ind, int end_effector_body_or_frame_ind, int expressed_in_body_or_frame_ind, int gradient_order, bool in_terms_of_qdot) {
  // temporary solution. Gross v_or_qdot_indices pointer will be gone soon.
  return model.geometricJacobian(cache, base_body_or_frame_ind, end_effector_body_or_frame_ind, expressed_in_body_or_frame_ind, gradient_order, in_terms_of_qdot, nullptr);
};

void geometricJacobianmex(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
  function<GradientVar<double, TWIST_SIZE, Dynamic>(const RigidBodyManipulator &, const KinematicsCache<double> &, int, int, int, int, bool)> func_double = &geometricJacobianTemp<double>;
  function<GradientVar<AutoDiffDynamicSize, TWIST_SIZE, Dynamic>(const RigidBodyManipulator &, const KinematicsCache<AutoDiffDynamicSize> &, int, int, int, int, bool)> func_autodiff_dynamic = &geometricJacobianTemp<AutoDiffDynamicSize>;
  mexTryToCallFunctions(nlhs, plhs, nrhs, prhs, func_double, func_autodiff_dynamic);
}

void massMatrixmex(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
  function<GradientVar<double, Dynamic, Dynamic>(const RigidBodyManipulator &, KinematicsCache<double> &, int)> func_double = mem_fn(&RigidBodyManipulator::massMatrix<double>);
  function<GradientVar<AutoDiffScalar<VectorXd>, Dynamic, Dynamic>(const RigidBodyManipulator &, KinematicsCache<AutoDiffScalar<VectorXd>> &, int)> func_autodiff_dynamic = mem_fn(&RigidBodyManipulator::massMatrix<AutoDiffScalar<VectorXd>>);
  mexTryToCallFunctions(nlhs, plhs, nrhs, prhs, func_double, func_autodiff_dynamic);
}

template <typename Scalar, typename DerivedF, typename DerivedDF>
GradientVar<Scalar, Eigen::Dynamic, 1>  dynamicsBiasTermTemp(const RigidBodyManipulator &model, KinematicsCache<Scalar> &cache, const MatrixBase<DerivedF> &f_ext_value, const MatrixBase<DerivedDF> &f_ext_gradient, int gradient_order) {
  // temporary solution. GradientVar will disappear, obviating the need for the extra argument. integer body indices will be handled differently.

  unordered_map<const RigidBody *, GradientVar<Scalar, 6, 1> > f_ext;

  if (f_ext_value.size() > 0) {
    assert(f_ext_value.cols() == model.bodies.size());
    if (gradient_order > 0) {
      assert(f_ext_gradient.rows() == f_ext_value.size());
      assert(f_ext_gradient.cols() == model.num_positions + model.num_velocities);
    }

    for (DenseIndex i = 0; i < f_ext_value.cols(); i++) {
      GradientVar<Scalar, TWIST_SIZE, 1> f_ext_gradientvar(TWIST_SIZE, 1, model.num_positions + model.num_velocities, gradient_order);
      f_ext_gradientvar.value() = f_ext_value.col(i);

      if (gradient_order > 0) {
        f_ext_gradientvar.gradient().value() = f_ext_gradient.template middleRows<TWIST_SIZE>(TWIST_SIZE * i);
      }

      f_ext.insert({model.bodies[i].get(), f_ext_gradientvar});
    }
  }

  return model.dynamicsBiasTerm(cache, f_ext, gradient_order);
};

void dynamicsBiasTermmex(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
  function<GradientVar<double, Eigen::Dynamic, 1>(const RigidBodyManipulator &, KinematicsCache<double> &, const MatrixBase<Map<const Matrix<double, 6, Dynamic> > > &, const MatrixBase<Map<const Matrix<double, Dynamic, Dynamic> > >&, int)> func_double = &dynamicsBiasTermTemp<double, Map<const Matrix<double, 6, Dynamic> >, Map<const Matrix<double, Dynamic, Dynamic> > >;
  function<GradientVar<AutoDiffDynamicSize, Eigen::Dynamic, 1>(const RigidBodyManipulator &, KinematicsCache<AutoDiffDynamicSize> &, const MatrixBase<Matrix<AutoDiffDynamicSize, 6, Dynamic>> &, const MatrixBase<Matrix<AutoDiffDynamicSize, Dynamic, Dynamic>> &, int)> func_autodiff_dynamic = &dynamicsBiasTermTemp<AutoDiffDynamicSize, Matrix<AutoDiffDynamicSize, 6, Dynamic>, Matrix<AutoDiffDynamicSize, Dynamic, Dynamic> >;

  mexTryToCallFunctions(nlhs, plhs, nrhs, prhs, func_double, func_autodiff_dynamic);
}
<commit_msg>Clean things up by adding a make_function implementation. Creating a fully general make_function isn't possible, but this works in our cases.<commit_after>#include "rigidBodyManipulatorMexFunctions.h"
#include "RigidBodyManipulator.h"
#include "standardMexConversions.h"
#include <typeinfo>

using namespace std;
using namespace Eigen;

/**
 * fromMex specializations
 */
RigidBodyManipulator &fromMex(const mxArray *source, RigidBodyManipulator *) {
  return *static_cast<RigidBodyManipulator *>(getDrakeMexPointer(source));
}

std::set<int> fromMex(const mxArray *source, std::set<int> *) {
  // for robotnum. this is kind of a weird one, but OK for now
  std::set<int> robotnum_set;
  int num_robot = static_cast<int>(mxGetNumberOfElements(source));
  double *robotnum = mxGetPrSafe(source);
  for (int i = 0; i < num_robot; i++) {
    robotnum_set.insert((int) robotnum[i] - 1);
  }
  return robotnum_set;
}

template<typename Scalar>
KinematicsCache<Scalar> &fromMex(const mxArray *mex, KinematicsCache<Scalar> *) {
  if (!mxIsClass(mex, "DrakeMexPointer")) {
    throw MexToCppConversionError("Expected DrakeMexPointer containing KinematicsCache");
  }
  auto name = mxGetStdString(mxGetPropertySafe(mex, "name"));
  if (name != typeid(KinematicsCache<Scalar>).name()) {
    ostringstream buf;
    buf << "Expected KinematicsCache of type " << typeid(KinematicsCache<Scalar>).name() << ", but got " << name;
    throw MexToCppConversionError(buf.str());
  }
  return *static_cast<KinematicsCache<Scalar> *>(getDrakeMexPointer(mex));
}

/**
 * toMex specializations
 */
void toMex(const KinematicPath &path, mxArray *dest[], int nlhs) {
  if (nlhs > 0) {
    dest[0] = stdVectorToMatlab(path.body_path);
  }
  if (nlhs > 1) {
    dest[1] = stdVectorToMatlab(path.joint_path);
  }
  if (nlhs > 2) {
    dest[2] = stdVectorToMatlab(path.joint_direction_signs);
  }
}

/**
 * make_function
 * Note that a completely general make_function implementation is not possible due to ambiguities, but this works for all of the cases in this file
 * Inspired by from http://stackoverflow.com/a/21740143/2228557
 */

//plain function pointers
template<typename... Args, typename ReturnType>
auto make_function(ReturnType(*p)(Args...))
-> std::function<ReturnType(Args...)> { return {p}; }

//nonconst member function pointers
template<typename... Args, typename ReturnType, typename ClassType>
auto make_function(ReturnType(ClassType::*p)(Args...))
-> std::function<ReturnType(ClassType&, Args...)> { return {p}; }

//const member function pointers
template<typename... Args, typename ReturnType, typename ClassType>
auto make_function(ReturnType(ClassType::*p)(Args...) const)
-> std::function<ReturnType(const ClassType&, Args...)> { return {p}; }

typedef AutoDiffScalar<VectorXd> AutoDiffDynamicSize;
//typedef AutoDiffScalar<Matrix<double, Dynamic, 1, 0, 72, 1> AutoDiffFixedMaxSize;

/**
 * Mex function implementations
 */
void centerOfMassJacobianDotTimesVmex(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
  auto func_double = make_function(&RigidBodyManipulator::centerOfMassJacobianDotTimesV<double>);
  auto func_autodiff_dynamic = make_function(&RigidBodyManipulator::centerOfMassJacobianDotTimesV<AutoDiffDynamicSize>);
  mexTryToCallFunctions(nlhs, plhs, nrhs, prhs, func_double, func_autodiff_dynamic);
}

void centerOfMassmex(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
  auto func_double = make_function(&RigidBodyManipulator::centerOfMass<double>);
  auto func_autodiff_dynamic = make_function(&RigidBodyManipulator::centerOfMass<AutoDiffDynamicSize>);
  mexTryToCallFunctions(nlhs, plhs, nrhs, prhs, func_double, func_autodiff_dynamic);
}

void centerOfMassJacobianmex(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
  auto func_double = make_function(&RigidBodyManipulator::centerOfMassJacobian<double>);
  auto func_autodiff_dynamic = make_function(&RigidBodyManipulator::centerOfMassJacobian<AutoDiffDynamicSize>);
  mexTryToCallFunctions(nlhs, plhs, nrhs, prhs, func_double, func_autodiff_dynamic);
}

void centroidalMomentumMatrixDotTimesvmex(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
  auto func_double = make_function(&RigidBodyManipulator::centroidalMomentumMatrixDotTimesV<double>);
  auto func_autodiff_dynamic = make_function(&RigidBodyManipulator::centroidalMomentumMatrixDotTimesV<AutoDiffDynamicSize>);
  mexTryToCallFunctions(nlhs, plhs, nrhs, prhs, func_double, func_autodiff_dynamic);
}

void centroidalMomentumMatrixmex(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
  auto func_double = make_function(&RigidBodyManipulator::centroidalMomentumMatrix<double>);
  auto func_autodiff_dynamic = make_function(&RigidBodyManipulator::centroidalMomentumMatrix<AutoDiffDynamicSize>);
  mexTryToCallFunctions(nlhs, plhs, nrhs, prhs, func_double, func_autodiff_dynamic);
}

template <typename DerivedQ, typename DerivedV>
void doKinematicsTemp(const RigidBodyManipulator &model, KinematicsCache<typename DerivedQ::Scalar> &cache, const MatrixBase<DerivedQ> &q, const MatrixBase<DerivedV> &v, bool compute_JdotV) {
  // temporary solution. Explicit doKinematics calls will not be necessary in the near future.
  if (v.size() == 0 && model.num_velocities != 0)
    cache.initialize(q);
  else
    cache.initialize(q, v);
  model.doKinematics(cache, compute_JdotV);
}

void doKinematicsmex(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
  typedef Matrix<AutoDiffScalar<VectorXd>, Dynamic, 1> VectorXAutoDiff;
  auto func_double = make_function(&doKinematicsTemp<Map<const VectorXd>, Map<const VectorXd>>);
  auto func_autodiff_dynamic = make_function(&doKinematicsTemp<VectorXAutoDiff, VectorXAutoDiff>);
  mexTryToCallFunctions(nlhs, plhs, nrhs, prhs, func_double, func_autodiff_dynamic);
}

void findKinematicPathmex(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
  auto func = make_function(&RigidBodyManipulator::findKinematicPath);
  mexCallFunction(func, nlhs, plhs, nrhs, prhs);
}

void forwardJacDotTimesVmex(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
  typedef Map<const Matrix3Xd> DerivedPointsDouble;
  auto func_double = make_function(&RigidBodyManipulator::forwardJacDotTimesV<DerivedPointsDouble>);

  typedef Matrix<AutoDiffDynamicSize, 3, Dynamic> DerivedPointsAutoDiff;
  auto func_autodiff_dynamic = make_function(&RigidBodyManipulator::forwardJacDotTimesV<DerivedPointsAutoDiff>);

  mexTryToCallFunctions(nlhs, plhs, nrhs, prhs, func_double, func_autodiff_dynamic);
}

void forwardKinmex(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
  typedef Map<const Matrix3Xd> DerivedPointsDouble;
  auto func_double = make_function(&RigidBodyManipulator::forwardKin<DerivedPointsDouble>);

  typedef Matrix<AutoDiffDynamicSize, 3, Dynamic> DerivedPointsAutoDiff;
  auto func_autodiff_dynamic = make_function(&RigidBodyManipulator::forwardKin<DerivedPointsAutoDiff>);

  mexTryToCallFunctions(nlhs, plhs, nrhs, prhs, func_double, func_autodiff_dynamic);
}

void forwardKinJacobianmex(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
  typedef Map<const Matrix3Xd> DerivedPointsDouble;
  auto func_double = make_function(&RigidBodyManipulator::forwardKinJacobian<DerivedPointsDouble>);

  typedef Matrix<AutoDiffDynamicSize, 3, Dynamic> DerivedPointsAutoDiff;
  auto func_autodiff_dynamic = make_function(&RigidBodyManipulator::forwardKinJacobian<DerivedPointsAutoDiff>);

  mexTryToCallFunctions(nlhs, plhs, nrhs, prhs, func_double, func_autodiff_dynamic);
}

void forwardKinPositionGradientmex(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
  auto func_double = make_function(&RigidBodyManipulator::forwardKinPositionGradient<double>);
  auto func_autodiff_dynamic = make_function(&RigidBodyManipulator::forwardKinPositionGradient<AutoDiffDynamicSize>);

  mexTryToCallFunctions(nlhs, plhs, nrhs, prhs, func_double, func_autodiff_dynamic);
}

void geometricJacobianDotTimesVmex(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
  auto func_double = make_function(&RigidBodyManipulator::geometricJacobianDotTimesV<double>);
  auto func_autodiff_dynamic = make_function(&RigidBodyManipulator::geometricJacobianDotTimesV<AutoDiffDynamicSize>);

  mexTryToCallFunctions(nlhs, plhs, nrhs, prhs, func_double, func_autodiff_dynamic);
}

template <typename Scalar>
GradientVar<Scalar, TWIST_SIZE, Eigen::Dynamic> geometricJacobianTemp(const RigidBodyManipulator &model, const KinematicsCache<Scalar> &cache, int base_body_or_frame_ind, int end_effector_body_or_frame_ind, int expressed_in_body_or_frame_ind, int gradient_order, bool in_terms_of_qdot) {
  // temporary solution. Gross v_or_qdot_indices pointer will be gone soon.
  return model.geometricJacobian(cache, base_body_or_frame_ind, end_effector_body_or_frame_ind, expressed_in_body_or_frame_ind, gradient_order, in_terms_of_qdot, nullptr);
};

void geometricJacobianmex(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
  auto func_double = make_function(&geometricJacobianTemp<double>);
  auto func_autodiff_dynamic = make_function(&geometricJacobianTemp<AutoDiffDynamicSize>);
  mexTryToCallFunctions(nlhs, plhs, nrhs, prhs, func_double, func_autodiff_dynamic);
}

void massMatrixmex(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
  auto func_double = make_function(&RigidBodyManipulator::massMatrix<double>);
  auto func_autodiff_dynamic = make_function(&RigidBodyManipulator::massMatrix<AutoDiffScalar<VectorXd>>);
  mexTryToCallFunctions(nlhs, plhs, nrhs, prhs, func_double, func_autodiff_dynamic);
}

template <typename Scalar, typename DerivedF, typename DerivedDF>
GradientVar<Scalar, Eigen::Dynamic, 1>  dynamicsBiasTermTemp(const RigidBodyManipulator &model, KinematicsCache<Scalar> &cache, const MatrixBase<DerivedF> &f_ext_value, const MatrixBase<DerivedDF> &f_ext_gradient, int gradient_order) {
  // temporary solution. GradientVar will disappear, obviating the need for the extra argument. integer body indices will be handled differently.

  unordered_map<const RigidBody *, GradientVar<Scalar, 6, 1> > f_ext;

  if (f_ext_value.size() > 0) {
    assert(f_ext_value.cols() == model.bodies.size());
    if (gradient_order > 0) {
      assert(f_ext_gradient.rows() == f_ext_value.size());
      assert(f_ext_gradient.cols() == model.num_positions + model.num_velocities);
    }

    for (DenseIndex i = 0; i < f_ext_value.cols(); i++) {
      GradientVar<Scalar, TWIST_SIZE, 1> f_ext_gradientvar(TWIST_SIZE, 1, model.num_positions + model.num_velocities, gradient_order);
      f_ext_gradientvar.value() = f_ext_value.col(i);

      if (gradient_order > 0) {
        f_ext_gradientvar.gradient().value() = f_ext_gradient.template middleRows<TWIST_SIZE>(TWIST_SIZE * i);
      }

      f_ext.insert({model.bodies[i].get(), f_ext_gradientvar});
    }
  }

  return model.dynamicsBiasTerm(cache, f_ext, gradient_order);
};

void dynamicsBiasTermmex(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
  auto func_double = make_function(&dynamicsBiasTermTemp<double, Map<const Matrix<double, 6, Dynamic> >, Map<const Matrix<double, Dynamic, Dynamic> > >);
  auto func_autodiff_dynamic = make_function(&dynamicsBiasTermTemp<AutoDiffDynamicSize, Matrix<AutoDiffDynamicSize, 6, Dynamic>, Matrix<AutoDiffDynamicSize, Dynamic, Dynamic> >);

  mexTryToCallFunctions(nlhs, plhs, nrhs, prhs, func_double, func_autodiff_dynamic);
}
<|endoftext|>


<commit_before>/*!
 * \page LFSTestSuite_cpp Command-Line Test to Demonstrate How To Test the SimLFS Project
 * \code
 */
// //////////////////////////////////////////////////////////////////////
// Import section
// //////////////////////////////////////////////////////////////////////
// STL
#include <sstream>
#include <fstream>
#include <string>
// Boost Unit Test Framework (UTF)
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN
#define BOOST_TEST_MODULE LFSTestSuite
#include <boost/test/unit_test.hpp>
// StdAir
#include <stdair/basic/BasLogParams.hpp>
#include <stdair/basic/BasDBParams.hpp>
#include <stdair/basic/BasFileMgr.hpp>
#include <stdair/bom/TravelSolutionStruct.hpp>
#include <stdair/bom/BookingRequestStruct.hpp>
#include <stdair/service/Logger.hpp>
// SimLFS
#include <simlfs/SIMLFS_Service.hpp>
#include <simlfs/config/simlfs-paths.hpp>

namespace boost_utf = boost::unit_test;

// (Boost) Unit Test XML Report
std::ofstream utfReportStream ("LFSTestSuite_utfresults.xml");

/**
 * Configuration for the Boost Unit Test Framework (UTF)
 */
struct UnitTestConfig {
  /** Constructor. */
  UnitTestConfig() {
    boost_utf::unit_test_log.set_stream (utfReportStream);
    boost_utf::unit_test_log.set_format (boost_utf::XML);
    boost_utf::unit_test_log.set_threshold_level (boost_utf::log_test_units);
    //boost_utf::unit_test_log.set_threshold_level (boost_utf::log_successful_tests);
  }

  /** Destructor. */
  ~UnitTestConfig() {
  }
};

// /////////////// Main: Unit Test Suite //////////////

// Set the UTF configuration (re-direct the output to a specific file)
BOOST_GLOBAL_FIXTURE (UnitTestConfig);

// Start the test suite
BOOST_AUTO_TEST_SUITE (master_test_suite)

/**
 * Test a simple price quotation
 */
BOOST_AUTO_TEST_CASE (simlfs_simple_pricing_test) {

  // Schedule input filename
  const stdair::Filename_T lScheduleInputFilename (STDAIR_SAMPLE_DIR
                                                   "/rds01/schedule.csv");

  // O&D input filename
  const stdair::Filename_T lOnDInputFilename (STDAIR_SAMPLE_DIR "/ond01.csv");

  // Fare input file name
  const stdair::Filename_T lFareInputFilename (STDAIR_SAMPLE_DIR "/rds01/fare.csv");
  SIMFQT::FareFilePath lFareFilePath (lFareInputFilename);

  // Yield input file name
  const stdair::Filename_T lYieldInputFilename (STDAIR_SAMPLE_DIR "/rds01/yield.csv");
  const AIRRAC::YieldFilePath lYieldFilePath (lYieldInputFilename);

  // Check that the file path given as input corresponds to an actual file
  bool doesExistAndIsReadable =
    stdair::BasFileMgr::doesExistAndIsReadable (lScheduleInputFilename);
  BOOST_CHECK_MESSAGE (doesExistAndIsReadable == true,
                       "The '" << lScheduleInputFilename
                       << "' input file can not be open and read");

  // Check that the file path given as input corresponds to an actual file
  doesExistAndIsReadable =
    stdair::BasFileMgr::doesExistAndIsReadable (lOnDInputFilename);
  BOOST_CHECK_MESSAGE (doesExistAndIsReadable == true,
                       "The '" << lOnDInputFilename
                       << "' input file can not be open and read");

  // Check that the file path given as input corresponds to an actual file
  doesExistAndIsReadable =
    stdair::BasFileMgr::doesExistAndIsReadable (lFareInputFilename);
  BOOST_CHECK_MESSAGE (doesExistAndIsReadable == true,
                       "The '" << lFareInputFilename
                       << "' input file can not be open and read");

  // Check that the file path given as input corresponds to an actual file
  doesExistAndIsReadable =
    stdair::BasFileMgr::doesExistAndIsReadable (lYieldInputFilename);
  BOOST_CHECK_MESSAGE (doesExistAndIsReadable == true,
                       "The '" << lYieldInputFilename
                       << "' input file can not be open and read");

  // Output log File
  const stdair::Filename_T lLogFilename ("LFSTestSuite.log");

  // Set the log parameters
  std::ofstream logOutputFile;
  // Open and clean the log outputfile
  logOutputFile.open (lLogFilename.c_str());
  logOutputFile.clear();

  // Initialise the list of classes/buckets
  const stdair::BasLogParams lLogParams (stdair::LOG::DEBUG, logOutputFile);
  SIMLFS::SIMLFS_Service simlfsService (lLogParams, lScheduleInputFilename,
                                        lOnDInputFilename, lFareFilePath,
                                        lYieldFilePath);

  // Create an empty booking request structure
  // TODO: fill the booking request structure from the input parameters
  const stdair::AirportCode_T lOrigin ("SIN");
  const stdair::AirportCode_T lDestination ("BKK");
  const stdair::AirportCode_T lPOS ("SIN");
  const stdair::Date_T lPreferredDepartureDate(2010, boost::gregorian::Jan, 30);
  const stdair::Date_T lRequestDate (2010, boost::gregorian::Jan, 22);
  const stdair::Duration_T lRequestTime (boost::posix_time::hours(10));
  const stdair::DateTime_T lRequestDateTime (lRequestDate, lRequestTime);
  const stdair::CabinCode_T lPreferredCabin ("Eco");
  const stdair::PartySize_T lPartySize (3);
  const stdair::ChannelLabel_T lChannel ("IN");
  const stdair::TripType_T lTripType ("RI");
  const stdair::DayDuration_T lStayDuration (7);
  const stdair::FrequentFlyer_T lFrequentFlyerType ("M");
  const stdair::Duration_T lPreferredDepartureTime (boost::posix_time::hours(10));
  const stdair::WTP_T lWTP (1000.0);
  const stdair::PriceValue_T lValueOfTime (100.0);
  const stdair::BookingRequestStruct lBookingRequest (lOrigin, lDestination,
                                                      lPOS,
                                                      lPreferredDepartureDate,
                                                      lRequestDateTime,
                                                      lPreferredCabin,
                                                      lPartySize, lChannel,
                                                      lTripType, lStayDuration,
                                                      lFrequentFlyerType,
                                                      lPreferredDepartureTime,
                                                      lWTP, lValueOfTime);

  // TODO: build a hand-made segment-path (as AirSched service is not
  //       available from here
  /*
  const stdair::SegmentPathList_T lSegmentPath =
    simlfsService.calculateSegmentPathList (lBookingRequest);

  // Price the travel solution
  stdair::TravelSolutionList_T lTravelSolutionList =
    simlfsService.fareQuote (lBookingRequest, lSegmentPath);

  //
  const unsigned int lNbOfTravelSolutions = lTravelSolutionList.size();

  // TODO: change the expected number of travel solutions to the actual number
  const unsigned int lExpectedNbOfTravelSolutions = 2;

  // DEBUG
  STDAIR_LOG_DEBUG ("Number of travel solutions for the booking request '"
                    << lBookingRequest.describe() << "': "
                    << lNbOfTravelSolutions << ". It is expected to be "
                    << lExpectedNbOfTravelSolutions);

  BOOST_CHECK_EQUAL (lNbOfTravelSolutions, lExpectedNbOfTravelSolutions);

  BOOST_CHECK_MESSAGE(lNbOfTravelSolutions == lExpectedNbOfTravelSolutions,
                      "The number of travel solutions for the booking request '"
                      << lBookingRequest.describe() << "' is equal to "
                      << lNbOfTravelSolutions << ", but it should be equal to "
                      << lExpectedNbOfTravelSolutions);

  //
  const stdair::TravelSolutionStruct& lTravelSolution =
    lTravelSolutionList.front();

  //
  const unsigned int lExpectedPrice = 2000;

  // DEBUG
  STDAIR_LOG_DEBUG ("The price given by the fare quoter for '"
                    << lTravelSolution.describe() << "' is: "
                    << lTravelSolution.getFare() << " Euros, and should be "
                    << lExpectedPrice);

  BOOST_CHECK_EQUAL (std::floor (lTravelSolution.getFare() + 0.5),
                     lExpectedPrice);

  BOOST_CHECK_MESSAGE (std::floor (lTravelSolution.getFare() + 0.5)
                       == lExpectedPrice,
                       "The price given by the fare quoter for '"
                       << lTravelSolution.describe() << "' is: "
                       << lTravelSolution.getFare() << " Euros, and should be "
                       << lExpectedPrice);
  */

  // Close the log file
  logOutputFile.close();
}

// End the test suite
BOOST_AUTO_TEST_SUITE_END()

/*!
 * \endcode
 */

<commit_msg>[Test] Altered the CSV input files so that the test can pass.<commit_after>/*!
 * \page LFSTestSuite_cpp Command-Line Test to Demonstrate How To Test the SimLFS Project
 * \code
 */
// //////////////////////////////////////////////////////////////////////
// Import section
// //////////////////////////////////////////////////////////////////////
// STL
#include <sstream>
#include <fstream>
#include <string>
// Boost Unit Test Framework (UTF)
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN
#define BOOST_TEST_MODULE LFSTestSuite
#include <boost/test/unit_test.hpp>
// StdAir
#include <stdair/basic/BasLogParams.hpp>
#include <stdair/basic/BasDBParams.hpp>
#include <stdair/basic/BasFileMgr.hpp>
#include <stdair/bom/TravelSolutionStruct.hpp>
#include <stdair/bom/BookingRequestStruct.hpp>
#include <stdair/service/Logger.hpp>
// SimLFS
#include <simlfs/SIMLFS_Service.hpp>
#include <simlfs/config/simlfs-paths.hpp>

namespace boost_utf = boost::unit_test;

// (Boost) Unit Test XML Report
std::ofstream utfReportStream ("LFSTestSuite_utfresults.xml");

/**
 * Configuration for the Boost Unit Test Framework (UTF)
 */
struct UnitTestConfig {
  /** Constructor. */
  UnitTestConfig() {
    boost_utf::unit_test_log.set_stream (utfReportStream);
    boost_utf::unit_test_log.set_format (boost_utf::XML);
    boost_utf::unit_test_log.set_threshold_level (boost_utf::log_test_units);
    //boost_utf::unit_test_log.set_threshold_level (boost_utf::log_successful_tests);
  }

  /** Destructor. */
  ~UnitTestConfig() {
  }
};

// /////////////// Main: Unit Test Suite //////////////

// Set the UTF configuration (re-direct the output to a specific file)
BOOST_GLOBAL_FIXTURE (UnitTestConfig);

// Start the test suite
BOOST_AUTO_TEST_SUITE (master_test_suite)

/**
 * Test a simple price quotation
 */
BOOST_AUTO_TEST_CASE (simlfs_simple_pricing_test) {

  // Schedule input filename
  const stdair::Filename_T lScheduleInputFilename (STDAIR_SAMPLE_DIR
                                                   "/schedule01.csv");

  // O&D input filename
  const stdair::Filename_T lOnDInputFilename (STDAIR_SAMPLE_DIR "/ond01.csv");

  // Fare input file name
  const stdair::Filename_T lFareInputFilename (STDAIR_SAMPLE_DIR "/rds01/fare.csv");
  SIMFQT::FareFilePath lFareFilePath (lFareInputFilename);

  // Yield input file name
  const stdair::Filename_T lYieldInputFilename (STDAIR_SAMPLE_DIR "/yieldstore01.csv");
  const AIRRAC::YieldFilePath lYieldFilePath (lYieldInputFilename);

  // Check that the file path given as input corresponds to an actual file
  bool doesExistAndIsReadable =
    stdair::BasFileMgr::doesExistAndIsReadable (lScheduleInputFilename);
  BOOST_CHECK_MESSAGE (doesExistAndIsReadable == true,
                       "The '" << lScheduleInputFilename
                       << "' input file can not be open and read");

  // Check that the file path given as input corresponds to an actual file
  doesExistAndIsReadable =
    stdair::BasFileMgr::doesExistAndIsReadable (lOnDInputFilename);
  BOOST_CHECK_MESSAGE (doesExistAndIsReadable == true,
                       "The '" << lOnDInputFilename
                       << "' input file can not be open and read");

  // Check that the file path given as input corresponds to an actual file
  doesExistAndIsReadable =
    stdair::BasFileMgr::doesExistAndIsReadable (lFareInputFilename);
  BOOST_CHECK_MESSAGE (doesExistAndIsReadable == true,
                       "The '" << lFareInputFilename
                       << "' input file can not be open and read");

  // Check that the file path given as input corresponds to an actual file
  doesExistAndIsReadable =
    stdair::BasFileMgr::doesExistAndIsReadable (lYieldInputFilename);
  BOOST_CHECK_MESSAGE (doesExistAndIsReadable == true,
                       "The '" << lYieldInputFilename
                       << "' input file can not be open and read");

  // Output log File
  const stdair::Filename_T lLogFilename ("LFSTestSuite.log");

  // Set the log parameters
  std::ofstream logOutputFile;
  // Open and clean the log outputfile
  logOutputFile.open (lLogFilename.c_str());
  logOutputFile.clear();

  // Initialise the list of classes/buckets
  const stdair::BasLogParams lLogParams (stdair::LOG::DEBUG, logOutputFile);
  SIMLFS::SIMLFS_Service simlfsService (lLogParams, lScheduleInputFilename,
                                        lOnDInputFilename, lFareFilePath,
                                        lYieldFilePath);

  // Create an empty booking request structure
  // TODO: fill the booking request structure from the input parameters
  const stdair::AirportCode_T lOrigin ("SIN");
  const stdair::AirportCode_T lDestination ("BKK");
  const stdair::AirportCode_T lPOS ("SIN");
  const stdair::Date_T lPreferredDepartureDate(2010, boost::gregorian::Jan, 30);
  const stdair::Date_T lRequestDate (2010, boost::gregorian::Jan, 22);
  const stdair::Duration_T lRequestTime (boost::posix_time::hours(10));
  const stdair::DateTime_T lRequestDateTime (lRequestDate, lRequestTime);
  const stdair::CabinCode_T lPreferredCabin ("Eco");
  const stdair::PartySize_T lPartySize (3);
  const stdair::ChannelLabel_T lChannel ("IN");
  const stdair::TripType_T lTripType ("RI");
  const stdair::DayDuration_T lStayDuration (7);
  const stdair::FrequentFlyer_T lFrequentFlyerType ("M");
  const stdair::Duration_T lPreferredDepartureTime (boost::posix_time::hours(10));
  const stdair::WTP_T lWTP (1000.0);
  const stdair::PriceValue_T lValueOfTime (100.0);
  const stdair::BookingRequestStruct lBookingRequest (lOrigin, lDestination,
                                                      lPOS,
                                                      lPreferredDepartureDate,
                                                      lRequestDateTime,
                                                      lPreferredCabin,
                                                      lPartySize, lChannel,
                                                      lTripType, lStayDuration,
                                                      lFrequentFlyerType,
                                                      lPreferredDepartureTime,
                                                      lWTP, lValueOfTime);

  // TODO: build a hand-made segment-path (as AirSched service is not
  //       available from here
  /*
  const stdair::SegmentPathList_T lSegmentPath =
    simlfsService.calculateSegmentPathList (lBookingRequest);

  // Price the travel solution
  stdair::TravelSolutionList_T lTravelSolutionList =
    simlfsService.fareQuote (lBookingRequest, lSegmentPath);

  //
  const unsigned int lNbOfTravelSolutions = lTravelSolutionList.size();

  // TODO: change the expected number of travel solutions to the actual number
  const unsigned int lExpectedNbOfTravelSolutions = 2;

  // DEBUG
  STDAIR_LOG_DEBUG ("Number of travel solutions for the booking request '"
                    << lBookingRequest.describe() << "': "
                    << lNbOfTravelSolutions << ". It is expected to be "
                    << lExpectedNbOfTravelSolutions);

  BOOST_CHECK_EQUAL (lNbOfTravelSolutions, lExpectedNbOfTravelSolutions);

  BOOST_CHECK_MESSAGE(lNbOfTravelSolutions == lExpectedNbOfTravelSolutions,
                      "The number of travel solutions for the booking request '"
                      << lBookingRequest.describe() << "' is equal to "
                      << lNbOfTravelSolutions << ", but it should be equal to "
                      << lExpectedNbOfTravelSolutions);

  //
  const stdair::TravelSolutionStruct& lTravelSolution =
    lTravelSolutionList.front();

  //
  const unsigned int lExpectedPrice = 2000;

  // DEBUG
  STDAIR_LOG_DEBUG ("The price given by the fare quoter for '"
                    << lTravelSolution.describe() << "' is: "
                    << lTravelSolution.getFare() << " Euros, and should be "
                    << lExpectedPrice);

  BOOST_CHECK_EQUAL (std::floor (lTravelSolution.getFare() + 0.5),
                     lExpectedPrice);

  BOOST_CHECK_MESSAGE (std::floor (lTravelSolution.getFare() + 0.5)
                       == lExpectedPrice,
                       "The price given by the fare quoter for '"
                       << lTravelSolution.describe() << "' is: "
                       << lTravelSolution.getFare() << " Euros, and should be "
                       << lExpectedPrice);
  */

  // Close the log file
  logOutputFile.close();
}

// End the test suite
BOOST_AUTO_TEST_SUITE_END()

/*!
 * \endcode
 */

<|endoftext|>