void publishMarker(const tf::Stamped<tf::Point>& target) {
//create marker object
visualization_msgs::Marker marker;
uint32_t shape = visualization_msgs::Marker::SPHERE;
// Set the frame ID and timestamp.
marker.header.frame_id = target.frame_id_;
marker.header.stamp = ros::Time::now();
marker.ns = "head_target";
marker.id = 0;
// Set the marker type.
marker.type = shape;
// Set the marker action.
marker.action = visualization_msgs::Marker::ADD;
// Set the pose of the marker.
marker.pose.position.x = target.getX();
marker.pose.position.y = target.getY();
marker.pose.position.z = target.getZ();
marker.pose.orientation.x = 0.0;
marker.pose.orientation.y = 0.0;
marker.pose.orientation.z = 0.0;
marker.pose.orientation.w = 1.0;
// Set the scale of the marker -- 1x1x1 here means 1m on a side
marker.scale.x = 0.08;
marker.scale.y = 0.08;
marker.scale.z = 0.08;
// Set the color -- be sure to set alpha to something non-zero!
marker.color.r = 1.0f;
marker.color.g = 0.0f;
marker.color.b = 1.0f;
marker.color.a = 0.6;
marker.lifetime = ros::Duration();
// Publish the marker
marker_pub_.publish(marker);
}
void SlamGMapping::writeMinMax(tf::Stamped<tf::Quaternion> & minmax){
tf::Quaternion quat (minmax.getX(),minmax.getY(),minmax.getZ(),minmax.getW());
fwrite(&quat,sizeof(tf::Quaternion),1,minmaxfile);
}