INT geometry::scan(istream& f)
{
INT nr;
BYTE buf[MYBUFSIZE];
BYTE str1[MYBUFSIZE];
BYTE *p_str;
//cout << "in geometry::scan()" << endl;
while (TRUE) {
if (f.eof()) {
return FALSE;
}
f.getline(buf, sizeof(buf));
//cout << "read line:" << buf << endl;
if (strncmp(buf, "GEOMETRY", 8) == 0)
break;
}
p_str = &buf[9];
//cout << "GEOMETRY " << p_str << endl;
s_scan_int(&p_str, &nr);
//cout << "number = " << nr << endl;
str1[0] = 0;
s_scan_token_arbitrary(&p_str, str1);
//cout << "label = " << str1 << endl;
scan_body(f, nr, str1);
return TRUE;
}
void Network::sendScantoMap(gtsam::Pose3 pose, sensor_msgs::PointCloud2 scan) {
mapPub.publish(scan);
sensor_msgs::PointCloud::Ptr scan_body(new sensor_msgs::PointCloud);
sensor_msgs::convertPointCloud2ToPointCloud(scan, *scan_body);
//mapPub.publish(scan_body);
gtsam::Vector3 rpy = pose.rotation().rpy();
tf::Quaternion q;
q.setRPY(rpy(0), rpy(1), rpy(2));
geometry_msgs::Quaternion qmsg;
tf::quaternionTFToMsg(q,qmsg);
geometry_msgs::Pose pose_msg;
pose_msg.orientation = qmsg;
pose_msg.position.x = pose.x();
pose_msg.position.y = pose.y();
pose_msg.position.z = pose.z();
geometry_msgs::PoseStamped stamped_pose;
stamped_pose.pose = pose_msg;
stamped_pose.header.frame_id = "world";
stamped_pose.header.stamp = ros::Time::now();
posePub.publish(stamped_pose);
dvm_.addRegisteredPointCloud(pose_msg,scan_body,range_max_);
}
scanner::token_type scanner::scan_head()
{
wchar c = skip_whitespace();
if(c == '>') { c_scan = &scanner::scan_body; return scan_body(); }
if(c == '/')
{
wchar t = get_char();
if(t == '>') { c_scan = &scanner::scan_body; return TT_TAG_END; }
else { push_back(t); return TT_ERROR; } // erroneous situtation - standalone '/'
}
attr_name_length = 0;
value_length = 0;
// attribute name...
while(c != '=')
{
if( c == 0) return TT_EOF;
if( c == '>' ) { push_back(c); return TT_ATTR; } // attribute without value (HTML style)
if( is_whitespace(c) )
{
c = skip_whitespace();
if(c != '=') { push_back(c); return TT_ATTR; } // attribute without value (HTML style)
else break;
}
if( c == '<') return TT_ERROR;
append_attr_name(c);
c = get_char();
}
c = skip_whitespace();
// attribute value...
if(c == '\"')
while(c = get_char())
{
if(c == '\"') return TT_ATTR;
if(c == '&') c = scan_entity();
append_value(c);
}
else if(c == '\'') // allowed in html
while(c = get_char())
{
if(c == '\'') return TT_ATTR;
if(c == '&') c = scan_entity();
append_value(c);
}
else // scan token, allowed in html: e.g. align=center
do
{
if( is_whitespace(c) ) return TT_ATTR;
/* these two removed in favour of better html support:
if( c == '/' || c == '>' ) { push_back(c); return TT_ATTR; }
if( c == '&' ) c = scan_entity();*/
if( c == '>' ) { push_back(c); return TT_ATTR; }
append_value(c);
} while(c = get_char());
return TT_ERROR;
}
