Exemplo n.º 1
0
int main( int argc, char** argv )
{
  //ros::init(argc, argv, "basic_shapes");
  //ros::NodeHandle n;
  //ros::Rate r(1);

  void* rd = initRviz(argc, argv, "test");

  //while (ros::ok())
  while (1) 
  {
    sendMarker(rd, "fiducial_frame", 0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.1, 0.1);

    //r.sleep();
  }
}
void AddWayPoint::onInitialize()
{
  /*!  Creating main layout object, object for the Base Placement Planning Class and the RQT Widget.
       Here we also create the Interactive Marker Menu handler for the Way-Points.
       Make all the necessary connections for the QObject communications.
       Inter Object connections for communications between the classes.
   */

  place_base = new PlaceBase();

  widget_ = new widgets::BasePlacementWidget("~");
  this->parentWidget()->resize(widget_->width(), widget_->height());
  QHBoxLayout* main_layout = new QHBoxLayout(this);
  main_layout->addWidget(widget_);

  //! Inform the user that the RViz is initializing
  ROS_INFO("initializing..");

  menu_handler.insert("Delete", boost::bind(&AddWayPoint::processFeedback, this, _1));
  menu_handler.setCheckState(
      menu_handler.insert("Fine adjustment", boost::bind(&AddWayPoint::processFeedback, this, _1)),
      interactive_markers::MenuHandler::UNCHECKED);

  // Place base to this

  connect(place_base, SIGNAL(getinitialmarkerFrame_signal(const tf::Transform)), this,
          SLOT(getRobotModelFrame_slot(const tf::Transform)));

  // Place base to widget

  connect(place_base, SIGNAL(getinitialmarkerFrame_signal(const tf::Transform)), widget_,
          SLOT(setAddPointUIStartPos(const tf::Transform)));
  connect(place_base, SIGNAL(basePlacementProcessStarted()), widget_, SLOT(PlaceBaseStartedHandler()));
  connect(place_base, SIGNAL(basePlacementProcessFinished()), widget_, SLOT(PlaceBaseFinishedHandler()));
  connect(place_base, SIGNAL(basePlacementProcessCompleted(double)), widget_, SLOT(PlaceBaseCompleted_slot(double)));
  connect(place_base, SIGNAL(sendBasePlaceMethods_signal(std::vector< std::string >)), widget_,
          SLOT(getBasePlacePlanMethod(std::vector< std::string >)));
  connect(place_base, SIGNAL(sendOuputType_signal(std::vector< std::string >)), widget_,
          SLOT(getOutputType(std::vector< std::string >)));
  connect(place_base, SIGNAL(sendGroupType_signal(std::vector< std::string >)), widget_,
          SLOT(getRobotGroups(std::vector< std::string >)));
  connect(place_base, SIGNAL(sendSelectedGroup_signal(std::string)), widget_,
          SLOT(getSelectedGroup(std::string)));



  // Widget to place base
  connect(widget_, SIGNAL(basePlacementParamsFromUI_signal(int, int)), place_base, SLOT(setBasePlaceParams(int, int)));
  connect(widget_, SIGNAL(reachabilityData_signal(std::multimap< std::vector< double >, std::vector< double > >,
                                                  std::multimap< std::vector< double >, double >, float)),
          place_base, SLOT(setReachabilityData(std::multimap< std::vector< double >, std::vector< double > >,
                                               std::multimap< std::vector< double >, double >, float)));
  connect(widget_, SIGNAL(showUnionMap_signal(bool)), place_base, SLOT(ShowUnionMap(bool)));
  connect(widget_, SIGNAL(clearUnionMap_signal(bool)), place_base, SLOT(clearUnionMap(bool)));
  connect(widget_, SIGNAL(SendSelectedMethod(int)), place_base, SLOT(getSelectedMethod(int)));
  connect(widget_, SIGNAL(SendSelectedOpType(int)), place_base, SLOT(getSelectedOpType(int)));
  connect(widget_, SIGNAL(SendSelectedRobotGroup(int)), place_base, SLOT(getSelectedRobotGroup(int)));
  connect(widget_, SIGNAL(SendShowUmodel(bool)), place_base, SLOT(getShowUreachModels(bool)));
  connect(widget_, SIGNAL(SendBasePoses(std::vector<geometry_msgs::Pose>)), place_base, SLOT(getBasePoses(std::vector<geometry_msgs::Pose>)));

  // Widget to this

  connect(widget_, SIGNAL(addPoint(tf::Transform)), this, SLOT(addPointFromUI(tf::Transform)));
  connect(widget_, SIGNAL(pointDelUI_signal(std::string)), this, SLOT(pointDeleted(std::string)));
  connect(widget_, SIGNAL(parseWayPointBtn_signal()), this, SLOT(parseWayPoints()));
  connect(widget_, SIGNAL(pointPosUpdated_signal(const tf::Transform&, const char*)), this,
          SLOT(pointPoseUpdated(const tf::Transform&, const char*)));
  connect(widget_, SIGNAL(saveToFileBtn_press()), this, SLOT(saveWayPointsToFile()));
  connect(widget_, SIGNAL(clearAllPoints_signal()), this, SLOT(clearAllPointsRViz()));



  // This to widget
  connect(this, SIGNAL(addPointRViz(const tf::Transform&, const int)), widget_,
          SLOT(insertRow(const tf::Transform&, const int)));
  connect(this, SIGNAL(pointPoseUpdatedRViz(const tf::Transform&, const char*)), widget_,
          SLOT(pointPosUpdated_slot(const tf::Transform&, const char*)));
  connect(this, SIGNAL(pointDeleteRviz(int)), widget_, SLOT(removeRow(int)));

  // This to place base

  connect(this, SIGNAL(wayPoints_signal(std::vector< geometry_msgs::Pose >)), place_base,
          SLOT(BasePlacementHandler(std::vector< geometry_msgs::Pose >)));
  connect(this, SIGNAL(initRviz()), place_base, SLOT(initRvizDone()));

  /*!  With the signal initRviz() we call a function PlaceBase::initRvizDone() which sets the initial parameters of the
     MoveIt enviroment.

  */
  Q_EMIT initRviz();
  ROS_INFO("ready.");
}