bool process_ros(duplo_v0::Process_PCD::Request &req,
duplo_v0::Process_PCD::Response &res)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudz(new pcl::PointCloud<pcl::PointXYZRGB>); //Filtered cloud
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_inliers(new pcl::PointCloud<pcl::PointXYZRGB>); //Inliers after removing outliers
pcl::PointCloud<pcl::PointXYZRGB>::Ptr planar_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr object_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cluster1(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::fromROSMsg(req.pcd_in,*cloud);
/****************** Filter out the non-table points ******************/
if (pass_through(cloud,cloudz) != 0)
{
std::cerr << "Pass-through filter error" << std::endl;
return false;
}
/* ********* Segmentation of the cloud into table and object clouds **********/
planar_seg(cloudz,planar_cloud,object_cloud,"table_try.pcd","object_try.pcd");
/********************** Cluster objects based on Euclidean distance **********/
//vector<double> volumes = cluster(object_cloud);
int num_clusters_found = cluster(object_cloud);
if (num_clusters_found == 0)
the_chosen_one.data.clear();
res.n_clusters = num_clusters_found;
processed_pointcloud = true;
return true;
}
int main() {
test4();
return 0;
std::cout << "unique ownership semantics demo\n";
{
auto p = std::make_unique<D>(); // p is a unique_ptr that owns a D
auto q = pass_through(std::move(p));
assert(!p); // now p owns nothing and holds a null pointer
q->bar(); // and q owns the D object
} // ~D called here
std::cout << "Runtime polymorphism demo\n";
{
std::unique_ptr<B> p = std::make_unique<D>(); // p is a unique_ptr that owns a D
// as a pointer to base
p->bar(); // virtual dispatch
std::vector<std::unique_ptr<B>> v; // unique_ptr can be stored in a container
v.push_back(std::make_unique<D>());
v.push_back(std::move(p));
v.emplace_back(new D);
for (auto &p : v) p->bar(); // virtual dispatch
} // ~D called 3 times
std::cout << "Custom deleter demo\n";
std::ofstream("demo.txt") << 'x'; // prepare the file to read
{
std::unique_ptr<std::FILE, decltype(&close_file)> fp(std::fopen("demo.txt", "r"),
&close_file);
if (fp) // fopen could have failed; in which case fp holds a null pointer
std::cout << (char)std::fgetc(fp.get()) << '\n';
} // fclose() called here, but only if FILE* is not a null pointer
// (that is, if fopen succeeded)
std::cout << "Custom lambda-expression deleter demo\n";
{
std::unique_ptr<D, std::function<void(D *)>> p(new D, [](D * ptr) {
std::cout << "destroying from a custom deleter...\n";
delete ptr;
}); // p owns D
p->bar();
} // the lambda above is called and D is destroyed
std::cout << "Array form of unique_ptr demo\n";
{
std::unique_ptr<D[]> p{ new D[3] };
} // calls ~D 3 times
}
static void emit_passthrough( i830ContextPtr i830 )
{
GLuint tmp[I830_TEXBLEND_SIZE], tmp_sz;
GLuint unit = 0;
tmp_sz = pass_through( tmp, unit );
tmp[0] |= TEXOP_LAST_STAGE;
if (tmp_sz != i830->state.TexBlendWordsUsed[unit] ||
memcmp( tmp, i830->state.TexBlend[unit], tmp_sz * sizeof(GLuint))) {
I830_STATECHANGE( i830, I830_UPLOAD_TEXBLEND(unit) );
memcpy( i830->state.TexBlend[unit], tmp, tmp_sz * sizeof(GLuint));
i830->state.TexBlendWordsUsed[unit] = tmp_sz;
}
I830_ACTIVESTATE(i830, I830_UPLOAD_TEXBLEND(unit), GL_TRUE);
}
/* Fork off spamc */
static void run_spamc(void) {
if (pipe(spamc_in)) _exit(FAIL_MEM);
if (pipe(spamc_out)) _exit(FAIL_MEM);
spamc_pid = fork();
if (spamc_pid == -1) pass_through("failed to fork spamc");
if (spamc_pid == 0) {
char *options[16];
build_spamc_argv(options);
if (dup2(spamc_in [0], 0) == -1) _exit(FAIL_BUG);
if (dup2(spamc_out[1], 1) == -1) _exit(FAIL_BUG);
close(spamc_in [0]);
close(spamc_in [1]);
close(spamc_out[0]);
close(spamc_out[1]);
execv(options[0], options);
_exit(FAIL_BUG);
}
close(spamc_in [0]);
close(spamc_out[1]);
}
/**
* Calculate the hardware instuctions to setup the current texture enviromnemt
* settings. Since \c gl_texture_unit::_CurrentCombine is used, both
* "classic" texture enviroments and GL_ARB_texture_env_combine type texture
* environments are treated identically.
*
* \todo
* This function should return \c bool. When \c false is returned,
* it means that an environment is selected that the hardware cannot do. This
* is the way the Radeon and R200 drivers work.
*
* \todo
* Looking at i830_3d_regs.h, it seems the i830 can do part of
* GL_ATI_texture_env_combine3. It can handle using \c GL_ONE and
* \c GL_ZERO as combine inputs (which the code already supports). It can
* also handle the \c GL_MODULATE_ADD_ATI mode. Is it worth investigating
* partial support for the extension?
*/
GLuint
i830SetTexEnvCombine(struct i830_context * i830,
const struct gl_tex_env_combine_state * combine,
GLint blendUnit,
GLuint texel_op, GLuint * state, const GLfloat * factor)
{
const GLuint numColorArgs = combine->_NumArgsRGB;
GLuint numAlphaArgs = combine->_NumArgsA;
GLuint blendop;
GLuint ablendop;
GLuint args_RGB[3];
GLuint args_A[3];
GLuint rgb_shift;
GLuint alpha_shift;
bool need_factor = 0;
int i;
unsigned used;
static const GLuint tex_blend_rgb[3] = {
TEXPIPE_COLOR | TEXBLEND_ARG1 | TEXBLENDARG_MODIFY_PARMS,
TEXPIPE_COLOR | TEXBLEND_ARG2 | TEXBLENDARG_MODIFY_PARMS,
TEXPIPE_COLOR | TEXBLEND_ARG0 | TEXBLENDARG_MODIFY_PARMS,
};
static const GLuint tex_blend_a[3] = {
TEXPIPE_ALPHA | TEXBLEND_ARG1 | TEXBLENDARG_MODIFY_PARMS,
TEXPIPE_ALPHA | TEXBLEND_ARG2 | TEXBLENDARG_MODIFY_PARMS,
TEXPIPE_ALPHA | TEXBLEND_ARG0 | TEXBLENDARG_MODIFY_PARMS,
};
if (INTEL_DEBUG & DEBUG_TEXTURE)
fprintf(stderr, "%s\n", __func__);
/* The EXT version of the DOT3 extension does not support the
* scale factor, but the ARB version (and the version in OpenGL
* 1.3) does.
*/
switch (combine->ModeRGB) {
case GL_DOT3_RGB_EXT:
alpha_shift = combine->ScaleShiftA;
rgb_shift = 0;
break;
case GL_DOT3_RGBA_EXT:
alpha_shift = 0;
rgb_shift = 0;
break;
default:
rgb_shift = combine->ScaleShiftRGB;
alpha_shift = combine->ScaleShiftA;
break;
}
switch (combine->ModeRGB) {
case GL_REPLACE:
blendop = TEXBLENDOP_ARG1;
break;
case GL_MODULATE:
blendop = TEXBLENDOP_MODULATE;
break;
case GL_ADD:
blendop = TEXBLENDOP_ADD;
break;
case GL_ADD_SIGNED:
blendop = TEXBLENDOP_ADDSIGNED;
break;
case GL_INTERPOLATE:
blendop = TEXBLENDOP_BLEND;
break;
case GL_SUBTRACT:
blendop = TEXBLENDOP_SUBTRACT;
break;
case GL_DOT3_RGB_EXT:
case GL_DOT3_RGB:
blendop = TEXBLENDOP_DOT3;
break;
case GL_DOT3_RGBA_EXT:
case GL_DOT3_RGBA:
blendop = TEXBLENDOP_DOT4;
break;
default:
return pass_through(state, blendUnit);
}
blendop |= (rgb_shift << TEXOP_SCALE_SHIFT);
/* Handle RGB args */
for (i = 0; i < 3; i++) {
switch (combine->SourceRGB[i]) {
case GL_TEXTURE:
args_RGB[i] = texel_op;
break;
case GL_TEXTURE0:
case GL_TEXTURE1:
case GL_TEXTURE2:
case GL_TEXTURE3:
args_RGB[i] = GetTexelOp(combine->SourceRGB[i] - GL_TEXTURE0);
break;
case GL_CONSTANT:
args_RGB[i] = TEXBLENDARG_FACTOR_N;
need_factor = 1;
break;
case GL_PRIMARY_COLOR:
args_RGB[i] = TEXBLENDARG_DIFFUSE;
break;
case GL_PREVIOUS:
args_RGB[i] = TEXBLENDARG_CURRENT;
break;
default:
return pass_through(state, blendUnit);
}
switch (combine->OperandRGB[i]) {
case GL_SRC_COLOR:
args_RGB[i] |= 0;
break;
case GL_ONE_MINUS_SRC_COLOR:
args_RGB[i] |= TEXBLENDARG_INV_ARG;
break;
case GL_SRC_ALPHA:
args_RGB[i] |= TEXBLENDARG_REPLICATE_ALPHA;
break;
case GL_ONE_MINUS_SRC_ALPHA:
args_RGB[i] |= (TEXBLENDARG_REPLICATE_ALPHA | TEXBLENDARG_INV_ARG);
break;
default:
return pass_through(state, blendUnit);
}
}
/* Need to knobble the alpha calculations of TEXBLENDOP_DOT4 to
* match the spec. Can't use DOT3 as it won't propogate values
* into alpha as required:
*
* Note - the global factor is set up with alpha == .5, so
* the alpha part of the DOT4 calculation should be zero.
*/
if (combine->ModeRGB == GL_DOT3_RGBA_EXT ||
combine->ModeRGB == GL_DOT3_RGBA) {
ablendop = TEXBLENDOP_DOT4;
numAlphaArgs = 2;
args_A[0] = TEXBLENDARG_FACTOR; /* the global factor */
args_A[1] = TEXBLENDARG_FACTOR;
args_A[2] = TEXBLENDARG_FACTOR;
}
else {
switch (combine->ModeA) {
case GL_REPLACE:
ablendop = TEXBLENDOP_ARG1;
break;
case GL_MODULATE:
ablendop = TEXBLENDOP_MODULATE;
break;
case GL_ADD:
ablendop = TEXBLENDOP_ADD;
break;
case GL_ADD_SIGNED:
ablendop = TEXBLENDOP_ADDSIGNED;
break;
case GL_INTERPOLATE:
ablendop = TEXBLENDOP_BLEND;
break;
case GL_SUBTRACT:
ablendop = TEXBLENDOP_SUBTRACT;
break;
default:
return pass_through(state, blendUnit);
}
ablendop |= (alpha_shift << TEXOP_SCALE_SHIFT);
/* Handle A args */
for (i = 0; i < 3; i++) {
switch (combine->SourceA[i]) {
case GL_TEXTURE:
args_A[i] = texel_op;
break;
case GL_TEXTURE0:
case GL_TEXTURE1:
case GL_TEXTURE2:
case GL_TEXTURE3:
args_A[i] = GetTexelOp(combine->SourceA[i] - GL_TEXTURE0);
break;
case GL_CONSTANT:
args_A[i] = TEXBLENDARG_FACTOR_N;
need_factor = 1;
break;
case GL_PRIMARY_COLOR:
args_A[i] = TEXBLENDARG_DIFFUSE;
break;
case GL_PREVIOUS:
args_A[i] = TEXBLENDARG_CURRENT;
break;
default:
return pass_through(state, blendUnit);
}
switch (combine->OperandA[i]) {
case GL_SRC_ALPHA:
args_A[i] |= 0;
break;
case GL_ONE_MINUS_SRC_ALPHA:
args_A[i] |= TEXBLENDARG_INV_ARG;
break;
default:
return pass_through(state, blendUnit);
}
}
}
/* Native Arg1 == Arg0 in GL_EXT_texture_env_combine spec */
/* Native Arg2 == Arg1 in GL_EXT_texture_env_combine spec */
/* Native Arg0 == Arg2 in GL_EXT_texture_env_combine spec */
/* When we render we need to figure out which is the last really enabled
* tex unit, and put last stage on it
*/
/* Build color & alpha pipelines */
used = 0;
state[used++] = (_3DSTATE_MAP_BLEND_OP_CMD(blendUnit) |
TEXPIPE_COLOR |
ENABLE_TEXOUTPUT_WRT_SEL |
TEXOP_OUTPUT_CURRENT |
DISABLE_TEX_CNTRL_STAGE | TEXOP_MODIFY_PARMS | blendop);
state[used++] = (_3DSTATE_MAP_BLEND_OP_CMD(blendUnit) |
TEXPIPE_ALPHA |
ENABLE_TEXOUTPUT_WRT_SEL |
TEXOP_OUTPUT_CURRENT | TEXOP_MODIFY_PARMS | ablendop);
for (i = 0; i < numColorArgs; i++) {
state[used++] = (_3DSTATE_MAP_BLEND_ARG_CMD(blendUnit) |
tex_blend_rgb[i] | args_RGB[i]);
}
for (i = 0; i < numAlphaArgs; i++) {
state[used++] = (_3DSTATE_MAP_BLEND_ARG_CMD(blendUnit) |
tex_blend_a[i] | args_A[i]);
}
if (need_factor)
return emit_factor(blendUnit, state, used, factor);
else
return used;
}
// this function gets called every time new pcl data comes in
void cloudcb(const sensor_msgs::PointCloud2ConstPtr &scan)
{
ROS_INFO("Kinect point cloud receieved");
ros::Time start_time = ros::Time::now();
ros::Time tcur = ros::Time::now();
Eigen::Vector4f centroid;
geometry_msgs::Point point;
pcl::PassThrough<pcl::PointXYZ> pass;
Eigen::VectorXf lims(6);
sensor_msgs::PointCloud2::Ptr
ros_cloud (new sensor_msgs::PointCloud2 ()),
ros_cloud_filtered (new sensor_msgs::PointCloud2 ());
pcl::PointCloud<pcl::PointXYZ>::Ptr
cloud (new pcl::PointCloud<pcl::PointXYZ> ()),
cloud_downsampled (new pcl::PointCloud<pcl::PointXYZ> ()),
cloud_filtered (new pcl::PointCloud<pcl::PointXYZ> ());
// set a parameter telling the world that I am tracking the robot
ros::param::set("/tracking_robot", true);
ROS_DEBUG("finished declaring vars : %f", (ros::Time::now()-tcur).toSec());
tcur = ros::Time::now();
ROS_DEBUG("About to transform cloud");
// New sensor message for holding the transformed data
sensor_msgs::PointCloud2::Ptr scan_transformed
(new sensor_msgs::PointCloud2());
try{
pcl_ros::transformPointCloud("/oriented_optimization_frame",
tf, *scan, *scan_transformed);
}
catch(tf::TransformException ex) {
ROS_ERROR("%s", ex.what());
}
scan_transformed->header.frame_id = "/oriented_optimization_frame";
// Convert to pcl
ROS_DEBUG("Convert cloud to pcd from rosmsg");
pcl::fromROSMsg(*scan_transformed, *cloud);
ROS_DEBUG("cloud transformed and converted to pcl : %f",
(ros::Time::now()-tcur).toSec());
tcur = ros::Time::now();
// set time stamp and frame id
ros::Time tstamp = ros::Time::now();
// run through pass-through filter to eliminate tarp and below robots.
ROS_DEBUG("Pass-through filter");
pass_through(cloud, cloud_filtered, robot_limits);
ROS_DEBUG("done with pass-through : %f", (ros::Time::now()-tcur).toSec());
tcur = ros::Time::now();
// now let's publish that filtered cloud
ROS_DEBUG("Converting and publishing cloud");
pcl::toROSMsg(*cloud_filtered, *ros_cloud_filtered);
ros_cloud_filtered->header.frame_id =
"/oriented_optimization_frame";
cloud_pub[0].publish(ros_cloud_filtered);
ROS_DEBUG("published filtered cloud : %f", (ros::Time::now()-tcur).toSec());
tcur = ros::Time::now();
// Let's do a downsampling before doing cluster extraction
pcl::VoxelGrid<pcl::PointXYZ> vg;
vg.setInputCloud (cloud_filtered);
vg.setLeafSize (0.01f, 0.01f, 0.01f);
vg.filter (*cloud_downsampled);
ROS_DEBUG("finished downsampling : %f", (ros::Time::now()-tcur).toSec());
tcur = ros::Time::now();
ROS_DEBUG("Begin extraction filtering");
// build a KdTree object for the search method of the extraction
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree
(new pcl::search::KdTree<pcl::PointXYZ> ());
tree->setInputCloud (cloud_downsampled);
ROS_DEBUG("done with KdTree initialization : %f",
(ros::Time::now()-tcur).toSec());
tcur = ros::Time::now();
// create a vector for storing the indices of the clusters
std::vector<pcl::PointIndices> cluster_indices;
// setup extraction:
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance (0.02); // cm
ec.setMinClusterSize (50);
ec.setMaxClusterSize (3000);
ec.setSearchMethod (tree);
ec.setInputCloud (cloud_downsampled);
// now we can perform cluster extraction
ec.extract (cluster_indices);
ROS_DEBUG("finished extraction : %f", (ros::Time::now()-tcur).toSec());
tcur = ros::Time::now();
// run through the indices, create new clouds, and then publish them
int j=1;
int number_clusters=0;
geometry_msgs::PointStamped pt;
puppeteer_msgs::Robots robots;
std::vector<int> num;
for (std::vector<pcl::PointIndices>::const_iterator
it=cluster_indices.begin();
it!=cluster_indices.end (); ++it)
{
number_clusters = (int) cluster_indices.size();
ROS_DEBUG("Number of clusters found: %d",number_clusters);
pcl::PointCloud<pcl::PointXYZ>::Ptr
cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
for (std::vector<int>::const_iterator
pit = it->indices.begin ();
pit != it->indices.end (); pit++)
{
cloud_cluster->points.push_back(cloud_downsampled->points[*pit]);
}
cloud_cluster->width = cloud_cluster->points.size ();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
// convert to rosmsg and publish:
ROS_DEBUG("Publishing extracted cloud");
pcl::toROSMsg(*cloud_cluster, *ros_cloud_filtered);
ros_cloud_filtered->header.frame_id =
"/oriented_optimization_frame";
if(j < MAX_CLUSTERS+1)
cloud_pub[j].publish(ros_cloud_filtered);
j++;
// compute centroid and add to Robots:
pcl::compute3DCentroid(*cloud_cluster, centroid);
pt.point.x = centroid(0);
pt.point.y = centroid(1);
pt.point.z = centroid(2);
pt.header.stamp = tstamp;
pt.header.frame_id = "/oriented_optimization_frame";
robots.robots.push_back(pt);
// add number of points in cluster to num:
num.push_back(cloud_cluster->points.size());
}
ROS_DEBUG("finished creating and publishing clusters : %f",
(ros::Time::now()-tcur).toSec());
tcur = ros::Time::now();
if (number_clusters > number_robots)
{
ROS_WARN("Number of clusters found is greater "
"than the number of robots");
// pop minimum cluster count
remove_least_likely(&robots, &num);
}
robots.header.stamp = tstamp;
robots.header.frame_id = "/oriented_optimization_frame";
robots.number = number_clusters;
robots_pub.publish(robots);
ROS_DEBUG("removed extra clusters, and published : %f",
(ros::Time::now()-tcur).toSec());
tcur = ros::Time::now();
ros::Duration d = ros::Time::now() - start_time;
ROS_DEBUG("End of cloudcb; time elapsed = %f (%f Hz)",
d.toSec(), 1/d.toSec());
}
// this function gets called every time new pcl data comes in
void cloudcb(const sensor_msgs::PointCloud2ConstPtr &scan)
{
Eigen::Vector4f centroid;
float xpos = 0.0;
float ypos = 0.0;
float zpos = 0.0;
float D_sphere = 0.05; //meters
float R_search = 2.0*D_sphere;
puppeteer_msgs::PointPlus pointplus;
geometry_msgs::Point point;
pcl::PassThrough<pcl::PointXYZ> pass;
Eigen::VectorXf lims(6);
sensor_msgs::PointCloud2::Ptr
robot_cloud (new sensor_msgs::PointCloud2 ()),
robot_cloud_filtered (new sensor_msgs::PointCloud2 ());
pcl::PointCloud<pcl::PointXYZ>::Ptr
cloud (new pcl::PointCloud<pcl::PointXYZ> ()),
cloud_filtered (new pcl::PointCloud<pcl::PointXYZ> ());
// set a parameter telling the world that I am tracking the robot
ros::param::set("/tracking_robot", true);
// New sensor message for holding the transformed data
sensor_msgs::PointCloud2::Ptr scan_transformed
(new sensor_msgs::PointCloud2());
try{
pcl_ros::transformPointCloud("/oriented_optimization_frame",
tf, *scan, *scan_transformed);
}
catch(tf::TransformException ex)
{
ROS_ERROR("%s", ex.what());
}
scan_transformed->header.frame_id = "/oriented_optimization_frame";
// Convert to pcl
pcl::fromROSMsg(*scan_transformed, *cloud);
// set time stamp and frame id
pointplus.header.stamp = scan->header.stamp;
pointplus.header.frame_id = "/oriented_optimization_frame";
// do we need to find the object?
if (locate == true)
{
// lims << XMIN, XMAX, YMIN, YMAX, ZMIN, ZMAX;
lims << frame_limits;
pass_through(cloud, cloud_filtered, lims);
pcl::compute3DCentroid(*cloud_filtered, centroid);
xpos = centroid(0); ypos = centroid(1); zpos = centroid(2);
// Publish cloud:
pcl::toROSMsg(*cloud_filtered, *robot_cloud_filtered);
robot_cloud_filtered->header.frame_id =
"/oriented_optimization_frame";
cloud_pub[1].publish(robot_cloud_filtered);
// are there enough points in the point cloud?
if(cloud_filtered->points.size() > POINT_THRESHOLD)
{
locate = false; // We have re-found the object!
// set values to publish
pointplus.x = xpos; pointplus.y = ypos; pointplus.z = zpos;
pointplus.error = false;
pointplus_pub.publish(pointplus);
}
// otherwise we should publish a blank centroid
// position with an error flag
else
{
// set values to publish
pointplus.x = 0.0;
pointplus.y = 0.0;
pointplus.z = 0.0;
pointplus.error = true;
pointplus_pub.publish(pointplus);
}
}
// if "else", we are just going to calculate the centroid
// of the input cloud
else
{
lims <<
xpos_last-R_search, xpos_last+R_search,
ypos_last-R_search, ypos_last+R_search,
zpos_last-R_search, zpos_last+R_search;
pass_through(cloud, cloud_filtered, lims);
// are there enough points in the point cloud?
if(cloud_filtered->points.size() < POINT_THRESHOLD)
{
locate = true;
ROS_WARN("Lost Object at: x = %f y = %f z = %f\n",
xpos_last,ypos_last,zpos_last);
pointplus.x = 0.0;
pointplus.y = 0.0;
pointplus.z = 0.0;
pointplus.error = true;
pointplus_pub.publish(pointplus);
return;
}
pcl::compute3DCentroid(*cloud_filtered, centroid);
xpos = centroid(0); ypos = centroid(1); zpos = centroid(2);
pcl::toROSMsg(*cloud_filtered, *robot_cloud);
robot_cloud_filtered->header.frame_id =
"/oriented_optimization_frame";
cloud_pub[0].publish(robot_cloud);
tf::Transform transform;
transform.setOrigin(tf::Vector3(xpos, ypos, zpos));
transform.setRotation(tf::Quaternion(0, 0, 0, 1));
static tf::TransformBroadcaster br;
br.sendTransform(tf::StampedTransform
(transform,ros::Time::now(),
"/oriented_optimization_frame",
"/robot_kinect_frame"));
// set pointplus message values and publish
pointplus.x = xpos;
pointplus.y = ypos;
pointplus.z = zpos;
pointplus.error = false;
pointplus_pub.publish(pointplus);
}
xpos_last = xpos;
ypos_last = ypos;
zpos_last = zpos;
}
int main(void) {
int spamc_out_len = 0;
char buf[4096];
ssize_t len;
int status;
char *val;
if ((val = getenv("SPAMDLIMIT"))) {
max_mail_size = atoi(val);
if (max_mail_size <= 0) max_mail_size = DEFAULT_LIMIT;
}
if ((val = getenv("QSPAMEXIT"))) {
spamexit = atoi(val);
}
mail = malloc((size_t)max_mail_size);
/* Pass it through if we're low on memory */
if (!mail) pass_through("out of mem");
while ((len = read(0, mail + mail_size, (size_t)(max_mail_size - mail_size))) > 0) {
mail_size += len;
/* Pass it through if it's too big */
if (mail_size == max_mail_size) pass_through("mail too big");
}
/* Fail if there was a problem reading the mail */
if (len < 0) _exit(FAIL_BUG);
if (!mail_size) _exit(FAIL_BUG);
read_envelope();
run_spamc();
run_qqueue();
/* Feed the mail to spamc */
len = write(spamc_in[1], mail, (size_t)mail_size);
/* Pass the mail through if this fails */
if (len != mail_size) pass_through("spamc write failure");
close(spamc_in[1]);
write_header("X-Spam-Queue: Filtered for <%s>\n", user ? user : "******", 0);
/* Copy the mail from spamc to qmail-queue */
while ((len = read(spamc_out[0], buf, sizeof(buf))) > 0) {
spamc_out_len += len;
if (write(qq_mail[1], buf, (size_t)len) != len) {
/* Problem writing to spamc -> pass original through */
pass_through("spamc read failure");
}
}
/* Pass the original message through if we had a read error from spamc. */
if (len < 0) pass_through("spamc returned incomplete mail");
/* Or if we didn't get at least as much back from spamc as we put in */
if (spamc_out_len < mail_size) pass_through("spamc returned short mail");
if (waitpid(spamc_pid, &status, 0) != spamc_pid) _exit(FAIL_BUG);
if (!WIFEXITED(status)) _exit(FAIL_BUG);
switch (WEXITSTATUS(status)) {
case 1:
if (spamexit != -1) _exit(spamexit);
break;
case 0:
break;
default:
_exit(FAIL_BUG);
break;
}
/* Ok, this is not spam, and we have copied the message to qmail-queue without problems */
/* Now we transfer the envelope */
qq_write_envelope();
/* And we're done. */
qq_done();
}
