WmObjectCapture::WmObjectCapture(ros::NodeHandle private_nh_)
: nh_(),
objectFrameId_("/object"),
cameraFrameId_("/camera_link"),
cameraTopicId_("/camera/depth_registered/points"),
object_(new pcl::PointCloud<pcl::PointXYZRGB>),
object_octree_(new pcl::octree::OctreePointCloudSearch<pcl::PointXYZRGB>(128.0f)),
max_z_(0.5),
making_object_(true),
unique_shot_(false)
{
ros::NodeHandle private_nh(private_nh_);
private_nh.param("objectFrameId", objectFrameId_, objectFrameId_);
private_nh.param("cameraFrameId", cameraFrameId_, cameraFrameId_);
private_nh.param("cameraTopicId", cameraTopicId_, cameraTopicId_);
private_nh.param("pointcloud_max_z", max_z_,max_z_);
private_nh.param("make_object", making_object_,making_object_);
private_nh.param("unique_shot", unique_shot_,unique_shot_);
pointCloudSub_ = new message_filters::Subscriber<sensor_msgs::PointCloud2> (nh_, cameraTopicId_, 5);
tfPointCloudSub_ = new tf::MessageFilter<sensor_msgs::PointCloud2> (*pointCloudSub_, tfListener_, cameraFrameId_, 5);
tfPointCloudSub_->registerCallback(boost::bind(&WmObjectCapture::insertCloudCallback, this, _1));
objectService_ = nh_.advertiseService("object", &WmObjectCapture::objectSrv, this);
objectPub_ = nh_.advertise<sensor_msgs::PointCloud2>("/object", 1, true);
object_octree_->setInputCloud(object_);
initMarkers();
}
Marker MarkerHandler::processMarker(string markerText, const int& where) {
StringUtil::trim(markerText);
if(markerText.find("#pragma ")!=0)
{
throw string("Invalid Marker, all markers should start with the #pragma pre-processor directive");
}
StringUtil::replaceFirst(markerText, "#pragma ", "");
vector<string> fragments = StringUtil::splitAndReturn<vector<string> >(markerText, " ");
string name = fragments.at(0);
if(name=="")
{
throw string("NOT_MARKER");
}
if(validMarkers.size()==0)
{
initMarkers();
}
Marker targetMarker = getMarker(name, where);
if(targetMarker.name=="")
{
string err = "Could not find the "+Marker::getTypeName(where)+" type marker with name " + name;
throw err;
}
targetMarker = getMarker(name);
if(targetMarker.name=="")
{
string err = "Could not find a marker with name " + name;
throw err;
}
if(targetMarker.reqAttrSize>0 && (int)fragments.size()==1)
{
string err = "No attributes specified for the marker " + targetMarker.name;
throw err;
}
else
{
map<string, string> tvalues;
for (int i = 1; i < (int)fragments.size(); ++i) {
if(fragments.at(i)!="")
{
vector<string> attr = StringUtil::splitAndReturn<vector<string> >(fragments.at(i), "=");
string attname = attr.at(0);
string value = attr.at(1);
StringUtil::trim(attname);
StringUtil::trim(value);
if(value.at(0)!='"' && value.at(value.length()-1)!='"')
{
string err = "Attribute value for "+attname+" should be within double quotes (\"\") for the marker " + targetMarker.name;
throw err;
}
if(value!="\"\"")
{
value = value.substr(1, value.length()-2);
}
else
{
value = "";
}
tvalues[attname] = value;
}
}
map<string, bool>::iterator it = targetMarker.attributes.begin();
for(;it!=targetMarker.attributes.end();++it) {
if(tvalues.find(it->first)==tvalues.end())
{
if(it->second) {
string err = "No value specified for mandatory attribute "+it->first+" for the marker " + targetMarker.name;
throw err;
}
cout << "Ignoring attribute " + it->first + " for marker " + targetMarker.name << endl;
tvalues.erase(it->first);
}
else
{
vector<string> vss = targetMarker.valueSet[it->first];
string value = tvalues[it->first];
if(value=="")
{
string err = "Attribute value for "+it->first+" cannot be blank for the marker " + targetMarker.name;
throw err;
}
if(vss.size()>0)
{
bool valid = false;
for (int var = 0; var < (int)vss.size(); ++var) {
if(vss.at(var)==value)
{
valid = true;
break;
}
}
if(!valid)
{
string err = "Attribute "+it->first+" cannot have a value of "+value+" for the marker " + targetMarker.name;
throw err;
}
}
}
}
targetMarker.attributeValues = tvalues;
}
return targetMarker;
}
MarkerHandler::MarkerHandler() {
initMarkers();
}
