OctomapInterface.cc
Go to the documentation of this file.
00001 /*
00002  * Octomapinterface.cc
00003  *
00004  *  Created on: Dec 26, 2012
00005  *      Author: slynen
00006  */
00007 
00008 #include <ptam/OctomapInterface.h>
00009 #include <ptam_com/OctoMapScan.h>
00010 #include <ptam_com/OctoMapPointArray.h>
00011 #include <tf/tf.h>
00012 #include <std_msgs/String.h>
00013 
00014 OctoMapInterface::OctoMapInterface( ros::NodeHandle& nh):nh_(nh)
00015 {
00016   pub_scan_= nh_.advertise<ptam_com::OctoMapScan> ("vslam/octomapscan", 10);
00017   pub_points_= nh_.advertise<ptam_com::OctoMapPointArray> ("vslam/octomappoints", 10);
00018 
00019   kfseq_ = 0;
00020   pointseq_ = 0;
00021 }
00022 
00023 OctoMapInterface::~OctoMapInterface()
00024 {
00025 
00026 }
00027 
00028 void OctoMapInterface::addKeyFrame(KeyFrame::Ptr k)
00029 {
00030 
00031   ptam_com::OctoMapScanPtr msg(new ptam_com::OctoMapScan);
00032 
00033   //assemble the keyframe pose from the SE3 stored with the PTAM KeyFrame strcut
00034   TooN::SE3<double> pose;
00035   TooN::Matrix<3, 3, double> rot;
00036   TooN::Vector<3, double> trans;
00037   tf::Quaternion q;
00038   tf::Vector3 t;
00039   geometry_msgs::PoseWithCovarianceStamped& buffpose = msg->keyFramePose;
00040 
00041   double scale = 1.0;
00042 
00043   pose = k->se3CfromW;
00044   rot = pose.get_rotation().get_matrix();
00045   trans = pose.get_translation();
00046   tf::Transform transform(tf::Matrix3x3(rot(0, 0), rot(0, 1), rot(0, 2),
00047                                         rot(1, 0), rot(1, 1), rot(1, 2),
00048                                         rot(2, 0), rot(2, 1), rot(2, 2)),
00049                           tf::Vector3(trans[0] / scale, trans[1]/ scale, trans[2] / scale));
00050   q = transform.getRotation();
00051   t = transform.getOrigin();
00052   buffpose.header.seq=kfseq_++;
00053   buffpose.header.stamp=ros::Time::now();
00054   buffpose.pose.pose.position.x=t[0];
00055   buffpose.pose.pose.position.y=t[1];
00056   buffpose.pose.pose.position.z=t[2];
00057   buffpose.pose.pose.orientation.w=q.w();
00058   buffpose.pose.pose.orientation.x=q.x();
00059   buffpose.pose.pose.orientation.y=q.y();
00060   buffpose.pose.pose.orientation.z=q.z();
00061   memset(&(buffpose.pose.covariance[0]),0,sizeof(double)*6*6);
00062 
00063   //add all points that this KF measures
00064   msg->mapPoints.mapPoints.reserve(k->mMeasurements.size());
00065   for(std::map<boost::shared_ptr<MapPoint>, Measurement>::const_iterator it = k->mMeasurements.begin() ; it != k->mMeasurements.end() ; ++it){
00066     ptam_com::OctoMapPointStamped pt;
00067     pt.header.seq = pointseq_++;
00068     pt.header.stamp = ros::Time::now();
00069     pt.action = ptam_com::OctoMapPointStamped::INSERT;
00070     pt.position.x = it->first->v3WorldPos[0];
00071     pt.position.y = it->first->v3WorldPos[1];
00072     pt.position.z = it->first->v3WorldPos[2];
00073     msg->mapPoints.mapPoints.push_back(pt);
00074   }
00075 
00076   pub_scan_.publish(msg);
00077 }
00078 
00079 void OctoMapInterface::updatePoint(MapPoint::Ptr p){
00080 
00081   localUpdateQueue_.insert(p); //slow down the update rate a little, this function is called at approx 1KHz
00082 
00083   publishPointUpdateFromQueue(); //request publishing of local queue
00084 }
00085 
00086 void OctoMapInterface::updatePoints(std::set<MapPoint::Ptr>& updateSet){
00087 
00088   localUpdateQueue_.insert(updateSet.begin(), updateSet.end());
00089 
00090   publishPointUpdateFromQueue(); //request publishing of local queue
00091 
00092 }
00093 
00094 void OctoMapInterface::publishPointUpdateFromQueue(){
00095 
00096   static double lastTime = 0;
00097   if(pub_points_.getNumSubscribers()){
00098     if(ros::Time::now().toSec() - lastTime > 1.0){ //only perform this once a second
00099       ptam_com::OctoMapPointArrayPtr msg(new ptam_com::OctoMapPointArray);
00100       for(std::set<MapPoint::Ptr>::const_iterator it = localUpdateQueue_.begin(); it != localUpdateQueue_.end() ; ++it){
00101         ptam_com::OctoMapPointStamped pt;
00102         pt.header.seq = pointseq_++;
00103         pt.header.stamp = ros::Time::now();
00104         pt.action = ptam_com::OctoMapPointStamped::UPDATE;
00105         pt.position.x = (*it)->v3WorldPos[0];
00106         pt.position.y = (*it)->v3WorldPos[1];
00107         pt.position.z = (*it)->v3WorldPos[2];
00108         msg->mapPoints.push_back(pt);
00109       }
00110       pub_points_.publish(msg);
00111 
00112       lastTime = ros::Time::now().toSec();
00113       localUpdateQueue_.clear();
00114 
00115       //    std::cout<<"publishing now "<<lastTime - ((int)lastTime)%1000<<std::endl;
00116 
00117     }
00118   }
00119 }
00120 
00121 void OctoMapInterface::deletePoint(MapPoint::Ptr p){
00122   ptam_com::OctoMapPointArrayPtr msg(new ptam_com::OctoMapPointArray);
00123   ptam_com::OctoMapPointStamped pt;
00124   pt.header.seq = pointseq_++;
00125   pt.header.stamp = ros::Time::now();
00126   pt.action = ptam_com::OctoMapPointStamped::DELETE;
00127   pt.position.x = p->v3WorldPos[0];
00128   pt.position.y = p->v3WorldPos[1];
00129   pt.position.z = p->v3WorldPos[2];
00130   msg->mapPoints.push_back(pt);
00131   pub_points_.publish(msg);
00132 }


ptam
Author(s): Markus Achtelik , Stephan Weiss , Simon Lynen
autogenerated on Sun Oct 5 2014 23:52:33