support for ubuntu 16 on OH master

cmake/modules/FindMapsLcm.cmake

@@ -0,0 +1,35 @@
+###############################################################################
+# Find package: maps_lcm
+#
+# This sets the following variables:
+
+# <package>_FOUND
+# <package>_INCLUDE_DIRS
+# <package>_LIBRARIES
+
+
+macro(pkg_config_find_module varname pkgname header library pathsuffix)
+
+    find_package(PkgConfig)
+    pkg_check_modules(${varname}_pkgconfig ${pkgname})
+
+    find_path(${varname}_INCLUDE_DIR ${header}
+        HINTS ${${varname}_pkgconfig_INCLUDEDIR}
+        PATH_SUFFIXES ${pathsuffix}
+        DOC "Path to ${pkgname} include directory")
+
+    find_library(${varname}_LIBRARY ${library} HINTS ${${varname}_pkgconfig_LIBDIR} DOC "Path to ${pkgname} library")
+
+    set(${varname}_INCLUDE_DIRS ${${varname}_INCLUDE_DIR})
+    set(${varname}_LIBRARIES ${${varname}_LIBRARY})
+
+    include(FindPackageHandleStandardArgs)
+    find_package_handle_standard_args(${varname} DEFAULT_MSG ${varname}_INCLUDE_DIR ${varname}_LIBRARY)
+
+    mark_as_advanced(${varname}_INCLUDE_DIR)
+    mark_as_advanced(${varmame}_LIBRARY)
+
+endmacro()
+
+
+pkg_config_find_module(MAPSLCM maps-lcm LcmTranslator.hpp maps-lcm maps-lcm)

distro/pods/drc/Makefile

@@ -43,13 +43,13 @@ configure:
 		   -DUSE_PORTMIDI:BOOL=ON \
 		   -DUSE_LCM:BOOL=ON \
 		   -DUSE_LCMGL:BOOL=ON \
-		   -DUSE_OCTOMAP:BOOL=ON \
+		   -DUSE_OCTOMAP:BOOL=OFF \
 		   -DUSE_COLLECTIONS:BOOL=ON \
 		   -DUSE_LIBBOT:BOOL=ON \
 		   -DUSE_DRAKE:BOOL=ON \
 		   -DUSE_DRC:BOOL=ON \
-		   -DUSE_DRC_MAPS:BOOL=ON \
-		   -DUSE_DRC_PLANE_SEG:BOOL=ON \
+		   -DUSE_DRC_MAPS:BOOL=OFF \
+		   -DUSE_DRC_PLANE_SEG:BOOL=OFF \
 		   ../../../../
 
 clean:

src/app/ddBotImageQueue.cpp

@@ -516,13 +516,11 @@ vtkSmartPointer<vtkImageData> ddBotImageQueue::toVtkImage(CameraData* cameraData
 
   vtkSmartPointer<vtkImageData> image = vtkSmartPointer<vtkImageData>::New();
 
-  image->SetWholeExtent(0, w-1, 0, h-1, 0, 0);
+  image->SetExtent(0, w-1, 0, h-1, 0, 0);
   image->SetSpacing(1.0, 1.0, 1.0);
   image->SetOrigin(0.0, 0.0, 0.0);
-  image->SetExtent(image->GetWholeExtent());
-  image->SetNumberOfScalarComponents(nComponents);
-  image->SetScalarType(componentType);
-  image->AllocateScalars();
+  image->SetExtent(image->GetExtent());
+  image->AllocateScalars(componentType, nComponents);
 
   unsigned char* outPtr = static_cast<unsigned char*>(image->GetScalarPointer(0, 0, 0));
 
@@ -576,7 +574,7 @@ vtkSmartPointer<vtkPolyData> PolyDataFromPointCloud(pcl::PointCloud<pcl::PointXY
     float point[3] = {cloud->points[i].x, cloud->points[i].y, cloud->points[i].z};
     unsigned char color[3] = {cloud->points[i].r, cloud->points[i].g, cloud->points[i].b};
     points->SetPoint(j, point);
-    rgbArray->SetTupleValue(j, color);
+    rgbArray->SetTypedTuple(j, color);
     j++;
   }
   nr_points = j;

src/app/ddDrakeModel.h

@@ -54,9 +54,6 @@ class DD_APP_EXPORT ddDrakeModel : public QObject
   QList<QString> getLinkNames();
   QList<QString> getJointNames();
   int findLinkID(const QString& linkName) const;
-  int findJointID(const QString& jointName) const;
-  int findFrameID(const QString& frameName) const;
-  QString findNameOfChildBodyOfJoint(const QString& jointName) const;
   void getModelMesh(vtkPolyData* polyData);
   void getModelMeshWithLinkInfoAndNormals(vtkPolyData* polyData);
   void getLinkModelMesh(const QString& linkName, vtkPolyData* polyData);

src/app/ddDrakeModelOH.cpp

@@ -318,7 +318,7 @@ vtkSmartPointer<vtkTexture> getTextureForMesh(vtkSmartPointer<vtkPolyData> polyD
   }
 
   vtkSmartPointer<vtkTexture> texture = vtkSmartPointer<vtkTexture>::New();
-  texture->SetInput(image);
+  texture->SetInputData(image);
   texture->EdgeClampOn();
   texture->RepeatOn();
   TextureMap[textureFileName] = texture;
@@ -1137,25 +1137,6 @@ int ddDrakeModel::findLinkID(const QString& linkName) const
   return this->Internal->Model->findLinkId(linkName.toLocal8Bit().data(), -1);
 }
 
-//-----------------------------------------------------------------------------
-int ddDrakeModel::findFrameID(const QString& frameName) const
-{
-  return this->Internal->Model->findFrame(frameName.toAscii().data())->frame_index;
-}
-
-//-----------------------------------------------------------------------------
-int ddDrakeModel::findJointID(const QString& jointName) const
-{
-  return this->Internal->Model->findJointId(jointName.toAscii().data(), -1);
-}
-
-//-----------------------------------------------------------------------------
-QString ddDrakeModel::findNameOfChildBodyOfJoint(const QString &jointName) const
-{
-  std::string body_name = this->Internal->Model->findJoint(jointName.toAscii().data())->linkname;
-  return body_name.c_str();
-}
-
 //-----------------------------------------------------------------------------
 QList<QString> ddDrakeModel::getJointNames()
 {

src/app/ddKinectLCM.cpp

@@ -299,7 +299,7 @@ vtkSmartPointer<vtkPolyData> PolyDataFromPointCloud(pcl::PointCloud<pcl::PointXY
     float point[3] = {cloud->points[i].x, cloud->points[i].y, cloud->points[i].z};
     unsigned char color[3] = {cloud->points[i].r, cloud->points[i].g, cloud->points[i].b};
     points->SetPoint(j, point);
-    rgbArray->SetTupleValue(j, color);
+    rgbArray->SetTypedTuple(j, color);
     j++;
   }
   nr_points = j;

src/app/ddPointCloudLCM.cpp

@@ -180,7 +180,7 @@ vtkSmartPointer<vtkPolyData> PolyDataFromPointCloudMessage(bot_core::pointcloud_
        {
          unsigned char color[3];
          unpackColor(msg.channels[j][i], color);
-         rgbArray->SetTupleValue(i, color);
+         rgbArray->SetTypedTuple(i, color);
        }
        rgbArray->SetNumberOfTuples(nr_points);
        polyData->GetPointData()->AddArray(rgbArray.GetPointer());
@@ -345,4 +345,4 @@ QList<int> ddPointCloudLCM::getLidarIntensity(const QString& lidarName) {
   }
 
   return qIntensities;
-}
+}

src/app/ddSensorDataRequest.ui

@@ -203,6 +203,78 @@
        </property>
       </widget>
      </item>
+     <item row="12" column="0">
+      <widget class="QPushButton" name="mapHeightCoarseButton">
+       <property name="text">
+        <string/>
+       </property>
+      </widget>
+     </item>
+     <item row="12" column="1">
+      <widget class="QCheckBox" name="mapHeightCoarseCheck">
+       <property name="text">
+        <string>Coarse Height</string>
+       </property>
+      </widget>
+     </item>
+     <item row="12" column="2">
+      <widget class="QSpinBox" name="mapHeightCoarseSpinner">
+       <property name="minimum">
+        <number>1</number>
+       </property>
+       <property name="maximum">
+        <number>30</number>
+       </property>
+      </widget>
+     </item>
+     <item row="13" column="0">
+      <widget class="QPushButton" name="mapHeightDenseButton">
+       <property name="text">
+        <string/>
+       </property>
+      </widget>
+     </item>
+     <item row="13" column="1">
+      <widget class="QCheckBox" name="mapHeightDenseCheck">
+       <property name="text">
+        <string>Dense Height</string>
+       </property>
+      </widget>
+     </item>
+     <item row="13" column="2">
+      <widget class="QSpinBox" name="mapHeightDenseSpinner">
+       <property name="minimum">
+        <number>1</number>
+       </property>
+       <property name="maximum">
+        <number>30</number>
+       </property>
+      </widget>
+     </item>
+     <item row="14" column="0">
+      <widget class="QPushButton" name="mapHeightSceneLongButton">
+       <property name="text">
+        <string/>
+       </property>
+      </widget>
+     </item>
+     <item row="14" column="1">
+      <widget class="QCheckBox" name="mapHeightSceneLongCheck">
+       <property name="text">
+        <string>Scene Height Long</string>
+       </property>
+      </widget>
+     </item>
+     <item row="14" column="2">
+      <widget class="QSpinBox" name="mapHeightSceneLongSpinner">
+       <property name="minimum">
+        <number>1</number>
+       </property>
+       <property name="maximum">
+        <number>30</number>
+       </property>
+      </widget>
+     </item>
      <item row="5" column="0" colspan="3">
       <widget class="Line" name="line_2">
        <property name="lineWidth">

src/app/wrapped_methods_drake.txt

@@ -21,9 +21,6 @@ QStringList ddDrakeModel::getLinkNames();
 QStringList ddDrakeModel::getJointNames();
 QString ddDrakeModel::getBodyOrFrameName(int);
 int ddDrakeModel::findLinkID(const QString&) const;
-int ddDrakeModel::findJointID(const QString&) const;
-int ddDrakeModel::findFrameID(const QString&) const;
-QString ddDrakeModel::findNameOfChildBodyOfJoint(const QString &) const;
 void ddDrakeModel::setAlpha(double);
 void ddDrakeModel::setVisible(bool);
 void ddDrakeModel::setTexturesEnabled(bool);

src/matlab/send_bot_pose

@@ -0,0 +1,14 @@
++function send_bot_pose(pos,quat, varargin)
+ +if nargin>2
+ +    channel=varargin{1};
+ +else
+ +    channel='POSE_BODY_ALT';
+ +end
+ +
+ +status = bot_core.pose_t();
+ +status.utime  = etime(clock,[1970 1 1 0 0 0])*1000000;
+ +status.pos = pos;
+ +status.orientation = quat;
+ +lc = lcm.lcm.LCM.getSingleton();
+ +lc.publish(channel, status);
+ +%pause(0.25)

src/matlab/send_bot_pose.m

@@ -0,0 +1,14 @@
+function send_bot_pose(pos,quat, varargin)
+if nargin>2
+    channel=varargin{1};
+else
+    channel='POSE_BODY_ALT';
+end
+
+status = bot_core.pose_t();
+status.utime  = etime(clock,[1970 1 1 0 0 0])*1000000;
+status.pos = pos;
+status.orientation = quat;
+lc = lcm.lcm.LCM.getSingleton();
+lc.publish(channel, status);
+%pause(0.25)

src/matlab/send_est_pose.m

@@ -0,0 +1,43 @@
+function send_est_pose(pos, quat, joint_name, joint_position)
+% pos = [0,0,0.627];
+% quat =[1,0,0,0]
+% joint_name = {'lf_haa_joint', 'lf_hfe_joint', 'lf_kfe_joint',...
+%               'rf_haa_joint', 'rf_hfe_joint', 'rf_kfe_joint',...
+%               'lh_haa_joint', 'lh_hfe_joint', 'lh_kfe_joint',...
+%               'rh_haa_joint', 'rh_hfe_joint', 'rh_kfe_joint'};
+% joint_position = [-0.139, 0.665, -1.336, -0.158,...
+%         0.68, -1.35, -0.17, -0.68,  1.35,...
+%        -0.14, -0.69,  1.34];
+
+status = bot_core.robot_state_t();
+status.utime  = etime(clock,[1970 1 1 0 0 0])*1000000;
+status.joint_name = joint_name;%{'aa','bb'};
+status.joint_velocity = zeros(size(joint_position));% [0,0];
+status.joint_position  = joint_position;
+status.joint_effort = zeros(size(joint_position));
+status.num_joints =size(joint_position,2);
+
+pose = bot_core.position_3d_t();
+pose.translation = bot_core.vector_3d_t();
+pose.translation.x = pos(1);
+pose.translation.y = pos(2);
+pose.translation.z = pos(3);
+pose.rotation = bot_core.quaternion_t();
+pose.rotation.w = quat(1);
+pose.rotation.x = quat(2);
+pose.rotation.y = quat(3);
+pose.rotation.z = quat(4);
+status.pose = pose;
+
+
+twist = bot_core.twist_t();
+twist.linear_velocity = bot_core.vector_3d_t();
+twist.angular_velocity = bot_core.vector_3d_t();
+status.twist = twist;
+
+ft = bot_core.force_torque_t();
+status.force_torque = ft;
+
+lc = lcm.lcm.LCM.getSingleton();
+lc.publish('EST_ROBOT_STATE', status);
+%pause(0.25)

src/matlab/send_est_pose_hyq.m

@@ -0,0 +1,14 @@
+function send_est_pose_hyq(q_end)
+joint_name = {'lf_haa_joint', 'lf_hfe_joint', 'lf_kfe_joint',...
+               'rf_haa_joint', 'rf_hfe_joint', 'rf_kfe_joint',...
+               'lh_haa_joint', 'lh_hfe_joint', 'lh_kfe_joint',...
+               'rh_haa_joint', 'rh_hfe_joint', 'rh_kfe_joint'};
+joint_pos = q_end(7:end)';
+pos = q_end(1:3);
+quat = rpy2quat(q_end(4:6));
+
+%if (info ==1)
+send_est_pose(pos,quat,joint_name, joint_pos)
+%else
+%  disp('fail')
+%end 

src/matlab/send_est_pose_hyq_reset.m

@@ -0,0 +1,11 @@
+pos = [0,0,0.627];
+quat =[1,0,0,0]
+joint_name = {'lf_haa_joint', 'lf_hfe_joint', 'lf_kfe_joint',...
+               'rf_haa_joint', 'rf_hfe_joint', 'rf_kfe_joint',...
+               'lh_haa_joint', 'lh_hfe_joint', 'lh_kfe_joint',...
+               'rh_haa_joint', 'rh_hfe_joint', 'rh_kfe_joint'};
+joint_position = [-0.139, 0.665, -1.336, -0.158,...
+         0.68, -1.35, -0.17, -0.68,  1.35,...
+        -0.14, -0.69,  1.34];
+
+send_est_pose(pos, quat, joint_name, joint_position);