void CWintec::parseAL(QString line) {
// expect the responses from the set to command mode state machine.
// wait for LoginOK. If received, switchng to command mode was successful
// and we can continue reading out data.
QString expectstr("@AL,02,01");
//qDebug() << QString("expecting line %1: %2").arg(m_command_response_step).arg(expectstr);
if (expectstr.compare(line) == 0) {
m_timer->stop();
qDebug() << QString("got %1 proceeding... ").arg(expectstr);
m_command_mode_step = 2;
m_step_complete = true;
return;
}
expectstr = QString("@AL,LoginOK");
//qDebug() << QString("expecting line %1: %2").arg(m_command_response_step).arg(expectstr);
if (expectstr.compare(line) == 0) {
m_timer->stop();
qDebug("got LoginOK, proceeding... ");
m_command_mode_step = 3;
m_step_complete = true;
return;
}
// get device name (usually WBT201)
expectstr = QString("@AL,07,01,");
if (line.startsWith(expectstr)) {
m_timer->stop();
QString model = line.section(',', 3, 3);
qDebug() << QString("model = %1").arg(model);
m_dev_data->setDeviceName(model);
m_command_mode_step = 4;
m_step_complete = true;
return;
}
// get device info
expectstr = QString("@AL,07,02,");
if (line.startsWith(expectstr)) {
m_timer->stop();
QString info = line.section(',', 3, 3);
qDebug() << QString("device info = %1.").arg(info);
m_dev_data->setDeviceInfo(info);
m_command_mode_step = 5;
m_step_complete = true;
return;
}
// get device serial No
expectstr = QString("@AL,07,03,");
if (line.startsWith(expectstr)) {
m_timer->stop();
QString serial = line.section(',', 3, 3);
qDebug() << QString("device serial No = %1.").arg(serial);
m_dev_data->setDeviceSerial(serial);
m_command_mode_step = 6;
m_step_complete = true;
return;
}
// get log start address
expectstr = QString("@AL,05,01,");
if (line.startsWith(expectstr)) {
m_timer->stop();
QString log_start_str = line.section(',', 3, 3);
long logstart = log_start_str.toLong();
qDebug() << QString("Log start = %1. As int %2").arg(log_start_str).arg(logstart);
m_dev_data->setLogStart(logstart);
m_command_mode_step = 7;
m_step_complete = true;
if(m_dev_data->complete()) {
readLogMode();
}
return;
}
// get log end address
expectstr = QString("@AL,05,02,");
if (line.startsWith(expectstr)) {
m_timer->stop();
QString log_end_str = line.section(',', 3, 3);
long logend = log_end_str.toLong();
qDebug() << QString("Log end = %1. As int %2").arg(log_end_str).arg(logend);
m_dev_data->setLogEnd(logend);
m_command_mode_step = 8;
m_step_complete = true;
if(m_dev_data->complete()) {
readLogMode();
}
return;
}
// get log area start address
expectstr = QString("@AL,05,09,");
if (line.startsWith(expectstr)) {
m_timer->stop();
QString log_area_start_str = line.section(',', 3, 3);
long logareastart = log_area_start_str.toLong();
qDebug() << QString("Log area start = %1. As int %2").arg(log_area_start_str).arg(logareastart);
m_dev_data->setLogAreaStart(logareastart);
m_command_mode_step = 9;
m_step_complete = true;
if(m_dev_data->complete()) {
readLogMode();
}
return;
}
// get log area end address
expectstr = QString("@AL,05,10,");
//qDebug() << QString("expecting line %1: %2").arg(m_command_response_step).arg(expectstr);
if (line.startsWith(expectstr)) {
m_timer->stop();
QString log_area_end_str = line.section(',', 3, 3);
long logareaend = log_area_end_str.toLong();
qDebug() << QString("Log area end = %1. As int %2").arg(log_area_end_str).arg(logareaend);
m_dev_data->setLogAreaEnd(logareaend);
m_command_mode_step = 10;
m_step_complete = true;
if(m_dev_data->complete()) {
readLogMode();
}
return;
}
// get checksum over received binary data
expectstr = QString("@AL,CS,");
//qDebug() << QString("expecting line %1: %2").arg(m_command_response_step).arg(expectstr);
if (line.startsWith(expectstr)) {
QString checksum_str = line.section(',', 2, 2);
qDebug() << QString("checksum string = %1.").arg(checksum_str);
m_timer->stop();
if(!checksumCorrect(m_tmp_buf, checksum_str)) {
qDebug() << QString("Checksum error! Retrying %1").arg(m_retry_count);
m_retry_count--;
readLogData();
return;
}
m_log_buf.append(m_tmp_buf);
m_retry_count = 5;
parseWayPoints(m_tmp_buf);
// make TrackPoints from m_tmp_buf here.
//
if(m_lastsection == 1 || m_tmp_buf.size() == 0) {
//createTrackpoints();
if(m_tmp_track != 0) {
m_tmp_track->commit();
emit newTrack(m_tmp_track);
}
emit readLogFinished();
leaveCommandMode();
return;
}
int percent_done = m_read_start * 100
/ (m_dev_data->getLogEnd() - m_dev_data->getLogStart());
emit progress(percent_done);
m_read_start += m_tmp_buf.size();
readLogData();
return;
}
}
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.");
}
