Merge pull request #17 from martimbrandao/master

pcl version compatibility

src/cloudproc/cloudproc.cpp

@@ -17,7 +17,7 @@
 #include "pcl/impl/instantiate.hpp"
 #include <boost/filesystem.hpp>
 #include <pcl/features/integral_image_normal.h>
-#if PCL_MINOR_VERSION > 6
+#if PCL_VERSION_COMPARE(>=, 1, 7, 0)
 #include <pcl/filters/median_filter.h>
 #include <pcl/filters/fast_bilateral.h>
 #endif
@@ -43,10 +43,18 @@ void setWidthToSize(const CloudT& cloud) {
 
 template <class T>
 typename pcl::PointCloud<T>::Ptr readPCD(const std::string& pcdfile) {
+#if PCL_VERSION_COMPARE(>=, 1, 7, 0)
+  pcl::PCLPointCloud2 cloud_blob;
+#else
   sensor_msgs::PointCloud2 cloud_blob;
+#endif
   typename pcl::PointCloud<T>::Ptr cloud (new typename pcl::PointCloud<T>);
   if (pcl::io::loadPCDFile (pcdfile, cloud_blob) != 0) FILE_OPEN_ERROR(pcdfile);
+#if PCL_VERSION_COMPARE(>=, 1, 7, 0)
+  pcl::fromPCLPointCloud2 (cloud_blob, *cloud);
+#else
   pcl::fromROSMsg (cloud_blob, *cloud);
+#endif
   return cloud;
 }
 
@@ -194,7 +202,11 @@ pcl::PolygonMesh::Ptr meshOFM(PointCloud<pcl::PointXYZ>::ConstPtr cloud, int edg
   ofm.setTriangulationType (pcl::OrganizedFastMesh<PointXYZ>::TRIANGLE_ADAPTIVE_CUT);
   pcl::PolygonMeshPtr mesh(new pcl::PolygonMesh());
   ofm.reconstruct(mesh->polygons);
+#if PCL_VERSION_COMPARE(>=, 1, 7, 0)
+  pcl::toPCLPointCloud2(*cloud, mesh->cloud);
+#else
   pcl::toROSMsg(*cloud, mesh->cloud);
+#endif
   mesh->header = cloud->header;
   return mesh;
 }
@@ -287,7 +299,7 @@ typename pcl::PointCloud<T>::Ptr maskFilter(typename pcl::PointCloud<T>::ConstPt
 
 template <class T>
 typename pcl::PointCloud<T>::Ptr medianFilter(typename pcl::PointCloud<T>::ConstPtr in, int windowSize, float maxAllowedMovement) {
-#if PCL_MINOR_VERSION > 6
+#if PCL_VERSION_COMPARE(>=, 1, 7, 0)
   pcl::MedianFilter<T> mf;
   mf.setWindowSize(windowSize);
   mf.setMaxAllowedMovement(maxAllowedMovement);
@@ -302,7 +314,7 @@ typename pcl::PointCloud<T>::Ptr medianFilter(typename pcl::PointCloud<T>::Const
 
 template <class T>
 typename pcl::PointCloud<T>::Ptr fastBilateralFilter(typename pcl::PointCloud<T>::ConstPtr in, float sigmaS, float sigmaR) {
-#if PCL_MINOR_VERSION > 6
+#if PCL_VERSION_COMPARE(>=, 1, 7, 0)
   pcl::FastBilateralFilter<T> mf;
   mf.setSigmaS(sigmaS);
   mf.setSigmaR(sigmaR);
@@ -315,8 +327,13 @@ typename pcl::PointCloud<T>::Ptr fastBilateralFilter(typename pcl::PointCloud<T>
 #endif
 }
 
+#if PCL_VERSION_COMPARE(>=, 1, 7, 0)
+void removenans(pcl::PCLPointCloud2& cloud, float fillval=0);
+void removenans(pcl::PCLPointCloud2& cloud, float fillval) {
+#else
 void removenans(sensor_msgs::PointCloud2& cloud, float fillval=0);
 void removenans(sensor_msgs::PointCloud2& cloud, float fillval) {
+#endif
   int npts = cloud.width * cloud.height;
   for (int i=0; i < npts; ++i) {
     float* ptdata = (float*)(cloud.data.data() + cloud.point_step * i);

src/cloudproc/cloudprocpy.cpp

@@ -7,10 +7,14 @@
 #include "utils/stl_to_string.hpp"
  #include "cloudgrabber.hpp"
 #include "cloudproc/mesh_simplification.hpp"
-#include <pcl/ros/conversions.h>
 #include <pcl/point_types.h>
 #include <boost/format.hpp>
 #include "hacd_interface.hpp"
+#if PCL_VERSION_COMPARE(>=, 1, 7, 0)
+#include <pcl/conversions.h>
+#else
+#include <pcl/ros/conversions.h>
+#endif
 
 using namespace Eigen;
 using namespace pcl;
@@ -100,7 +104,11 @@ struct PyCloud {
 
 CloudXYZ::Ptr PolygonMesh_getCloud(const PolygonMesh* mesh) {
   CloudXYZ::Ptr cloud(new CloudXYZ());
+#if PCL_VERSION_COMPARE(>=, 1, 7, 0)
+  pcl::fromPCLPointCloud2(mesh->cloud, *cloud);
+#else
   pcl::fromROSMsg(mesh->cloud, *cloud);
+#endif
   return cloud;
 }
 py::object PolygonMesh_getVertices(const PolygonMesh* mesh) {

src/cloudproc/hacd_interface.cpp

@@ -2,8 +2,12 @@
 #include <pcl/PolygonMesh.h>
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
-#include <pcl/ros/conversions.h>
 #include <hacdHACD.h>
+#if PCL_VERSION_COMPARE(>=, 1, 7, 0)
+#include <pcl/conversions.h>
+#else
+#include <pcl/ros/conversions.h>
+#endif
 using HACD::Vec3;
 using HACD::Real;
 using namespace pcl;
@@ -16,7 +20,11 @@ void polygonMeshFromPointsTriangles(pcl::PolygonMesh& mesh, const vector< Vec3<R
     cloud.points[i].y = points[i].Y();
     cloud.points[i].z = points[i].Z();
   }
+#if PCL_VERSION_COMPARE(>=, 1, 7, 0)
+  pcl::toPCLPointCloud2(cloud, mesh.cloud);
+#else
   pcl::toROSMsg(cloud, mesh.cloud);
+#endif
   mesh.polygons.resize(triangles.size());
   for (int i=0; i < triangles.size(); ++i) {
     vector<uint32_t>& vertices = mesh.polygons[i].vertices;
@@ -28,7 +36,11 @@ void polygonMeshFromPointsTriangles(pcl::PolygonMesh& mesh, const vector< Vec3<R
 }
 void pointsTrianglesFromPolygonMesh(const pcl::PolygonMesh& mesh, vector< Vec3<Real> >& points, vector< Vec3<long> >& triangles) {
   PointCloud<PointXYZ> cloud;
+#if PCL_VERSION_COMPARE(>=, 1, 7, 0)
+  pcl::fromPCLPointCloud2 (mesh.cloud, cloud);
+#else
   pcl::fromROSMsg (mesh.cloud, cloud);
+#endif
   points.resize(cloud.size());
   for (int i=0; i < cloud.size(); ++i) {
     points[i].X() = cloud.points[i].x;

src/cloudproc/mesh_simplification.cpp

@@ -9,8 +9,12 @@
 #include <pcl/PolygonMesh.h>
 #include <pcl/point_types.h>
 #include <pcl/point_cloud.h>
-#include <pcl/ros/conversions.h>
 #include "macros.h"
+#if PCL_VERSION_COMPARE(>=, 1, 7, 0)
+#include <pcl/conversions.h>
+#else
+#include <pcl/ros/conversions.h>
+#endif
 
 using namespace pcl;
 namespace {
@@ -43,12 +47,20 @@ void toPCL(vtkPolyData& in, pcl::PolygonMesh& out) {
     vertices[1] = idxdata[1];
     vertices[2] = idxdata[2];
   }
+#if PCL_VERSION_COMPARE(>=, 1, 7, 0)
+  pcl::toPCLPointCloud2(cloud, out.cloud);
+#else
   pcl::toROSMsg(cloud, out.cloud);
+#endif
 
 }
 void toVTK(pcl::PolygonMesh& in, vtkPolyData& out) {
   pcl::PointCloud<pcl::PointXYZ> cloud;
+#if PCL_VERSION_COMPARE(>=, 1, 7, 0)
+  pcl::fromPCLPointCloud2(in.cloud, cloud);
+#else
   pcl::fromROSMsg(in.cloud, cloud);
+#endif
 
   vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();
   BOOST_FOREACH(const PointXYZ& pt, cloud.points) {

src/osgviewer/osgviewer.cpp

@@ -14,6 +14,8 @@
 #include <osg/BlendFunc>
 #include <osg/io_utils>
 #include <iostream>
+#include <osg/CameraNode>
+#include <osgDB/WriteFile>
 #include <osgDB/ReadFile>
 #include "utils/logging.hpp"
 #include "openrave_userdata_utils.hpp"
@@ -232,6 +234,46 @@ void AddLights(osg::Group* group) {
 }
 
 
+// http://forum.openscenegraph.org/viewtopic.php?t=7214
+class SnapImageDrawCallback : public osg::CameraNode::DrawCallback {
+public:
+  SnapImageDrawCallback() { _snapImageOnNextFrame = false; }
+  void setFileName(const std::string& filename) { _filename = filename; }
+  const std::string& getFileName() const { return _filename; }
+  void setSnapImageOnNextFrame(bool flag) { _snapImageOnNextFrame = flag; }
+  bool getSnapImageOnNextFrame() const { return _snapImageOnNextFrame; }
+  virtual void operator () (const osg::CameraNode& camera) const {
+    if (!_snapImageOnNextFrame) return;
+    int x,y,width,height;
+    x = camera.getViewport()->x();
+    y = camera.getViewport()->y();
+    width = camera.getViewport()->width();
+    height = camera.getViewport()->height();
+    osg::ref_ptr<osg::Image> image = new osg::Image;
+    image->readPixels(x,y,width,height,GL_RGB,GL_UNSIGNED_BYTE);
+    // make a local copy
+    unsigned char * p = image->data();
+    unsigned int numBytes = image->computeNumComponents(image->getPixelFormat());
+    _image = std::vector<unsigned char>(p, p + height*width*numBytes / sizeof(unsigned char));
+    _height = height;
+    _width = width;
+    // save file
+    if (_filename != "" && osgDB::writeImageFile(*image,_filename))
+      std::cout << "Saved screenshot to `"<<_filename<<"`"<< std::endl;
+    _snapImageOnNextFrame = false;
+  }
+  std::vector<unsigned char> getImage() const { return _image; }
+  unsigned int getHeight() const { return _height; }
+  unsigned int getWidth() const { return _width; }
+protected:
+  std::string _filename;
+  mutable bool _snapImageOnNextFrame;
+  mutable std::vector<unsigned char> _image;
+  mutable unsigned int _height;
+  mutable unsigned int _width;
+};
+
+
 // http://forum.openscenegraph.org/viewtopic.php?t=7806
 void   AddCylinderBetweenPoints(const osg::Vec3& StartPoint, osg::Vec3 EndPoint, float radius, const osg::Vec4& CylinderColor, osg::Group *pAddToThisGroup, bool use_cone)
 {
@@ -461,8 +503,12 @@ OSGViewer::OSGViewer(EnvironmentBasePtr env) : ViewerBase(env), m_idling(false)
   AddLights(m_root);
   m_cam->setClearColor(osg::Vec4(1,1,1,1));
 
+  osg::ref_ptr<SnapImageDrawCallback> snapImageDrawCallback = new SnapImageDrawCallback();
+  m_cam->setPostDrawCallback (snapImageDrawCallback.get());
+
   AddKeyCallback('h', boost::bind(&OSGViewer::PrintHelp, this), "Display help");
   AddKeyCallback('p', boost::bind(&OSGViewer::Idle, this), "Toggle idle");
+  AddKeyCallback('s', boost::bind(&OSGViewer::TakeScreenshot, this), "Take screenshot");
   AddKeyCallback(osgGA::GUIEventAdapter::KEY_Escape, &throw_runtime_error, "Quit (raise exception)");
   PrintHelp();
   m_viewer.setRunFrameScheme(osgViewer::ViewerBase::ON_DEMAND);
@@ -528,6 +574,42 @@ void OSGViewer::UpdateSceneData() {
   m_viewer.requestRedraw();
 }
 
+void OSGViewer::SetBkgndColor(const RaveVectorf& color) {
+  m_cam->setClearColor(toOsgVec4(color));
+}
+
+void OSGViewer::TakeScreenshot(const std::string& filename) {
+  osg::ref_ptr<SnapImageDrawCallback> snapImageDrawCallback = dynamic_cast<SnapImageDrawCallback*> (m_cam->getPostDrawCallback());
+  if(snapImageDrawCallback.get()) {
+    snapImageDrawCallback->setFileName(filename);
+    snapImageDrawCallback->setSnapImageOnNextFrame(true);
+  } else {
+    std::cout << "Warning: could not take screenshot" << std::endl;
+  }
+}
+
+void OSGViewer::TakeScreenshot() {
+  time_t rawtime;
+  struct tm * timeinfo;
+  char buffer[80];
+  time (&rawtime);
+  timeinfo = localtime(&rawtime);
+  strftime(buffer,80,"%Y%m%d-%I%M%S.png",timeinfo);
+  std::string filename(buffer);
+  TakeScreenshot(filename);
+}
+
+bool OSGViewer::GetLastScreenshot(std::vector<unsigned char>& image, unsigned int& height, unsigned int& width)
+{
+  osg::ref_ptr<SnapImageDrawCallback> snapImageDrawCallback = dynamic_cast<SnapImageDrawCallback*> (m_cam->getPostDrawCallback());
+  if(!snapImageDrawCallback.get())
+    return false;
+  image = snapImageDrawCallback->getImage();
+  height = snapImageDrawCallback->getHeight();
+  width = snapImageDrawCallback->getWidth();
+  return true;
+}
+
 void OSGViewer::AddMouseCallback(const MouseCallback& cb) {
   m_handler->event_cbs.push_back(cb);
   // todo: print help

src/osgviewer/osgviewer.hpp

@@ -23,8 +23,10 @@ class TRAJOPT_API OSGViewer : public OpenRAVE::ViewerBase {
   void Idle(); // should be called ToggleIdle
   void UpdateSceneData();
   const std::string& GetName() const {return m_name;}
-  void SetBkgndColor(const RaveVectorf &) {printf("warning: SetBkgndColor not implemented\n");}
-
+  void SetBkgndColor(const RaveVectorf& color);
+  void TakeScreenshot(const std::string& filename);
+  void TakeScreenshot();
+  bool GetLastScreenshot(std::vector<unsigned char>& image, unsigned int& height, unsigned int& width);
 
   // return false if you want to disable the default TrackballManipulator handling
   typedef boost::function<bool(const osgGA::GUIEventAdapter &)> MouseCallback;

src/sco/gurobi_interface.cpp

@@ -188,7 +188,10 @@ CvxOptStatus GurobiModel::optimize(){
     LOG_DEBUG("solver objective value: %.3e", objval);
     return CVX_SOLVED;
   }
-  else if (status == GRB_INFEASIBLE) return CVX_INFEASIBLE;
+  else if (status == GRB_INFEASIBLE) {
+    GRBcomputeIIS(m_model);
+    return CVX_INFEASIBLE;
+  }
   else return CVX_FAILED;
 }
 CvxOptStatus GurobiModel::optimizeFeasRelax(){

src/sco/modeling.hpp

@@ -97,6 +97,8 @@ class Cost {
   virtual double value(const vector<double>&) = 0;
   /** Convexify at solution vector x*/
   virtual ConvexObjectivePtr convex(const vector<double>& x, Model* model) = 0;
+  /** Get problem variables associated with this cost */
+  virtual VarVector getVars() {return VarVector();}
 
   string name() {return name_;}
   void setName(const string& name) {name_=name;}
@@ -123,6 +125,8 @@ class Constraint {
   vector<double> violations(const vector<double>& x);
   /** Sum of violations */
   double violation(const vector<double>& x);
+  /** Get problem variables associated with this constraint */
+  virtual VarVector getVars() {return VarVector();}
 
   string name() {return name_;}
   void setName(const string& name) {name_=name;}

src/sco/modeling_utils.hpp

@@ -42,6 +42,7 @@ class CostFromFunc : public Cost {
   CostFromFunc(ScalarOfVectorPtr f, const VarVector& vars, const string& name, bool full_hessian=false);
   double value(const vector<double>& x);
   ConvexObjectivePtr convex(const vector<double>& x, Model* model);
+  VarVector getVars() {return vars_;}
 protected:
   ScalarOfVectorPtr f_;
   VarVector vars_;
@@ -57,6 +58,7 @@ class CostFromErrFunc : public Cost {
   CostFromErrFunc(VectorOfVectorPtr f, MatrixOfVectorPtr dfdx, const VarVector& vars, const VectorXd& coeffs, PenaltyType pen_type, const string& name);
   double value(const vector<double>& x);
   ConvexObjectivePtr convex(const vector<double>& x, Model* model);
+  VarVector getVars() {return vars_;}
 protected:
   VectorOfVectorPtr f_;
   MatrixOfVectorPtr dfdx_;
@@ -75,6 +77,7 @@ class ConstraintFromFunc : public Constraint {
   vector<double> value(const vector<double>& x);
   ConvexConstraintsPtr convex(const vector<double>& x, Model* model);
   ConstraintType type() {return type_;}
+  VarVector getVars() {return vars_;}
 protected:
   VectorOfVectorPtr f_;
   MatrixOfVectorPtr dfdx_;

src/sco/optimizers.cpp

@@ -289,8 +289,9 @@ OptStatus BasicTrustRegionSQP::optimize() {
         CvxOptStatus status = model_->optimize();
         ++results_.n_qp_solves;
         if (status != CVX_SOLVED) {
-          LOG_ERROR("convex solver failed! set TRAJOPT_LOG_THRESH=DEBUG to see solver output. saving model to /tmp/fail.lp");
+          LOG_ERROR("convex solver failed! set TRAJOPT_LOG_THRESH=DEBUG to see solver output. saving model to /tmp/fail.lp and IIS to /tmp/fail.ilp");
           model_->writeToFile("/tmp/fail.lp");
+          model_->writeToFile("/tmp/fail.ilp");
           retval = OPT_FAILED;
           goto cleanup;
         }

src/trajopt/bullet_collision_checker.cpp

@@ -202,6 +202,7 @@ btCollisionShape* createShapePrimitive(OR::KinBody::Link::GeometryPtr geom, bool
       }
 
     }     
+    break;
   }
   default:
     assert(0 && "unrecognized collision shape type");
@@ -348,6 +349,15 @@ class BulletCollisionChecker : public CollisionChecker {
     COW *cow0 = GetCow(&link0), *cow1 = GetCow(&link1);
     if (cow0 && cow1) m_allowedCollisionMatrix(cow0->m_index, cow1->m_index) = 1;
   }
+  virtual bool RayCastCollision(const OpenRAVE::Vector& point1, const OpenRAVE::Vector& point2) {
+    btVector3 btFrom = toBt(point1);
+    btVector3 btTo = toBt(point2);
+    btCollisionWorld::ClosestRayResultCallback res(btFrom, btTo);
+    UpdateBulletFromRave();
+    m_world->updateAabbs();
+    m_world->rayTest(btFrom, btTo, res);
+    return res.hasHit();
+  }
   // collision checking
   virtual void AllVsAll(vector<Collision>& collisions);
   virtual void LinksVsAll(const vector<KinBody::LinkPtr>& links, vector<Collision>& collisions, short filterMask);

src/trajopt/collision_checker.hpp

@@ -69,6 +69,9 @@ class TRAJOPT_API CollisionChecker : public OR::UserData {
   virtual void ExcludeCollisionPair(const KinBody::Link& link0, const KinBody::Link& link1) = 0;
   virtual void IncludeCollisionPair(const KinBody::Link& link0, const KinBody::Link& link1) = 0;
 
+  /** Check whether a raycast hits the environment */
+  virtual bool RayCastCollision(const OpenRAVE::Vector& point1, const OpenRAVE::Vector& point2) = 0;
+
 
   OpenRAVE::EnvironmentBaseConstPtr GetEnv() {return m_env;}