skip fixed joint, see start-jsk#68 (comment)

k-okada · Oct 22, 2017 · 415e794 · 415e794
1 parent 86a3b26
commit 415e794
Show file tree

Hide file tree

Showing 2 changed files with 6 additions and 0 deletions.
diff --git a/denso_ros_control/CMakeLists.txt b/denso_ros_control/CMakeLists.txt
@@ -107,6 +107,7 @@ catkin_package(
 ## Build ##
 ###########
 
+add_compile_options(-std=c++11)
 ## Specify additional locations of header files
 ## Your package locations should be listed before other locations
 include_directories(include  ${catkin_INCLUDE_DIRS})

diff --git a/denso_ros_control/include/denso_ros_control/denso_hardware_interface.h b/denso_ros_control/include/denso_ros_control/denso_hardware_interface.h
@@ -60,6 +60,11 @@ class DensoRobotHW : public hardware_interface::RobotHW
     KDL::SegmentMap::const_iterator root_segment = tree.getRootSegment();
     std::vector<const KDL::Joint*> joints;
     appendJoint(root_segment->second.children[0], joints);
+    // skip fixed joint https://stackoverflow.com/questions/4478636/stdremove-if-lambda-not-removing-anything-from-the-collection
+    auto new_end = std::remove_if(joints.begin(), joints.end(),
+                                  [](const KDL::Joint*& joint)->bool
+                                  { return joint->getType() == KDL::Joint::None; });
+    joints.erase(new_end, joints.end());
 
     int num_joints = joints.size();
     cmd_.resize(num_joints);