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.");
}