diff --git a/robowflex_dart/include/robowflex_dart/robot.h b/robowflex_dart/include/robowflex_dart/robot.h index cca13ac4e..1acac4240 100644 --- a/robowflex_dart/include/robowflex_dart/robot.h +++ b/robowflex_dart/include/robowflex_dart/robot.h @@ -3,8 +3,10 @@ #ifndef ROBOWFLEX_DART_ROBOT_ #define ROBOWFLEX_DART_ROBOT_ +#ifndef ROBOWFLEX_DART_ONLY #include #include +#endif #include #include @@ -95,6 +97,7 @@ namespace robowflex /** \name State Operations \{ */ +#ifndef ROBOWFLEX_DART_ONLY /** \brief Set the current state of this robot from a MoveIt message. * \param[in] msg Message to set state to. */ @@ -139,6 +142,7 @@ namespace robowflex */ void setMoveItJMGFromState(const std::string &jmg, Eigen::Ref vec) const; +#endif /** \} */ /** \name Group Operations diff --git a/robowflex_dart/src/robot.cpp b/robowflex_dart/src/robot.cpp index c55cefb1d..663726062 100644 --- a/robowflex_dart/src/robot.cpp +++ b/robowflex_dart/src/robot.cpp @@ -461,6 +461,7 @@ void Robot::setNamedGroupState(const std::string &group, const std::string &name it->second = q; } +#ifndef ROBOWFLEX_DART_ONLY void Robot::setStateFromMoveItMsg(const moveit_msgs::RobotState &msg) { for (std::size_t i = 0; i < msg.joint_state.name.size(); ++i) @@ -564,6 +565,7 @@ void Robot::setMoveItJMGFromState(const std::string &jmg, Eigen::RefgetScratchState()); // copy current state robot_->getScratchState()->copyJointGroupPositions(jmg, vec.data()); // copy JMG state } +#endif void Robot::processGroup(const std::string &group) {