From 1b3fc385edc6ad85189651934615d030760f4850 Mon Sep 17 00:00:00 2001 From: gilwoolee Date: Fri, 14 Apr 2017 14:29:58 -0400 Subject: [PATCH 1/7] ros_control/RosTrajectoryExecutor (#149) * ros/Conversions * Cleaning up CMakeLists. * CMakeLists cleanup * Style change to match AIKIDO style guide. * Checking joint-name matches * Initial commit for RosTrajectoryExecutor * Temporarily removing tests in CMakeLists. * Addressing some of Mike's comments * Addressing Miks and JS's comments. * Minor bug fix, adding one test for joint mapping. * Adding conversion from trajectory to ros trajectory. * Addressing Mike's comments. * Line wrapping * Removing a line. * Addressing Mike's comments * Changing method names * Addressing Mike and JS's comments. * Changing constructor to templated, removing reorderMap. * Suppress warning * Fix Conversions * Add missing dependencies to package.xml and use find modules to find actionlib and control_msgs * Suppress warnings --- cmake/Findactionlib.cmake | 40 +++ cmake/Findcontrol_msgs.cmake | 33 +++ include/aikido/control/TrajectoryExecutor.hpp | 2 +- include/aikido/control/ros/Conversions.hpp | 17 +- .../ros/RosTrajectoryExecutionException.hpp | 33 +++ .../control/ros/RosTrajectoryExecutor.hpp | 90 +++++++ include/aikido/perception/eigen_yaml.hpp | 7 +- package.xml | 4 + src/control/ros/CMakeLists.txt | 22 +- src/control/ros/Conversions.cpp | 181 ++++++++++--- .../ros/RosTrajectoryExecutionException.cpp | 32 +++ src/control/ros/RosTrajectoryExecutor.cpp | 252 ++++++++++++++++++ tests/control/ros/CMakeLists.txt | 11 +- tests/control/ros/test_Conversions.cpp | 28 +- .../ros/test_RosTrajectoryConversions.cpp | 158 +++++++++++ 15 files changed, 846 insertions(+), 64 deletions(-) create mode 100644 cmake/Findactionlib.cmake create mode 100644 cmake/Findcontrol_msgs.cmake create mode 100644 include/aikido/control/ros/RosTrajectoryExecutionException.hpp create mode 100644 include/aikido/control/ros/RosTrajectoryExecutor.hpp create mode 100644 src/control/ros/RosTrajectoryExecutionException.cpp create mode 100644 src/control/ros/RosTrajectoryExecutor.cpp create mode 100644 tests/control/ros/test_RosTrajectoryConversions.cpp diff --git a/cmake/Findactionlib.cmake b/cmake/Findactionlib.cmake new file mode 100644 index 0000000000..79aafc22c5 --- /dev/null +++ b/cmake/Findactionlib.cmake @@ -0,0 +1,40 @@ +# Find actionlib +# +# This sets the following variables: +# actionlib_FOUND +# actionlib_INCLUDE_DIRS +# actionlib_LIBRARIES +# actionlib_VERSION + +# Note: This find module is necessary because the config file imports "gtest", +# "tests", and "run_tests" that conflict with the targets defined by Aikido. + +find_package(PkgConfig QUIET REQUIRED) + +# Check to see if pkgconfig is installed. +set(PKG_CONFIG_USE_CMAKE_PREFIX_PATH TRUE) +pkg_check_modules(PC_actionlib actionlib QUIET REQUIRED) + +# Include directories +find_path(actionlib_INCLUDE_DIRS + NAMES actionlib/action_definition.h + HINTS ${PC_actionlib_LIBRARY_DIRS} +) + +# Libraries +find_library(actionlib_LIBRARIES + actionlib + HINTS ${PC_actionlib_LIBRARY_DIRS} +) + +# Version +set(actionlib_VERSION ${PC_actionlib_VERSION}) + +# Set (NAME)_FOUND if all the variables and the version are satisfied. +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(actionlib + FOUND_VAR actionlib_FOUND + FAIL_MESSAGE DEFAULT_MSG + REQUIRED_VARS actionlib_INCLUDE_DIRS actionlib_LIBRARIES + VERSION_VAR actionlib_VERSION +) diff --git a/cmake/Findcontrol_msgs.cmake b/cmake/Findcontrol_msgs.cmake new file mode 100644 index 0000000000..ad9fab691f --- /dev/null +++ b/cmake/Findcontrol_msgs.cmake @@ -0,0 +1,33 @@ +# Find control_msgs +# +# This sets the following variables: +# control_msgs_FOUND +# control_msgs_INCLUDE_DIRS +# control_msgs_VERSION + +# Note: This find module is necessary because the config file imports "gtest", +# "tests", and "run_tests" that conflict with the targets defined by Aikido. + +find_package(PkgConfig QUIET REQUIRED) + +# Check to see if pkgconfig is installed. +set(PKG_CONFIG_USE_CMAKE_PREFIX_PATH TRUE) +pkg_check_modules(PC_control_msgs control_msgs QUIET REQUIRED) + +# Include directories +find_path(control_msgs_INCLUDE_DIRS + NAMES control_msgs/JointTolerance.h + HINTS ${PC_control_msgs_LIBRARY_DIRS} +) + +# Version +set(control_msgs_VERSION ${PC_control_msgs_VERSION}) + +# Set (NAME)_FOUND if all the variables and the version are satisfied. +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(control_msgs + FOUND_VAR control_msgs_FOUND + FAIL_MESSAGE DEFAULT_MSG + REQUIRED_VARS control_msgs_INCLUDE_DIRS + VERSION_VAR control_msgs_VERSION +) diff --git a/include/aikido/control/TrajectoryExecutor.hpp b/include/aikido/control/TrajectoryExecutor.hpp index 88af71c8be..ff567339c7 100644 --- a/include/aikido/control/TrajectoryExecutor.hpp +++ b/include/aikido/control/TrajectoryExecutor.hpp @@ -1,7 +1,7 @@ #ifndef AIKIDO_CONTROL_TRAJECTORYEXECUTOR_HPP_ #define AIKIDO_CONTROL_TRAJECTORYEXECUTOR_HPP_ #include "TrajectoryResult.hpp" -#include "../trajectory/Trajectory.hpp" +#include #include namespace aikido { diff --git a/include/aikido/control/ros/Conversions.hpp b/include/aikido/control/ros/Conversions.hpp index 49a8514c0f..ab6efbc6e2 100644 --- a/include/aikido/control/ros/Conversions.hpp +++ b/include/aikido/control/ros/Conversions.hpp @@ -1,5 +1,6 @@ #ifndef AIKIDO_CONTROL_ROS_CONVERSIONS_HPP_ #define AIKIDO_CONTROL_ROS_CONVERSIONS_HPP_ + #include #include #include @@ -11,18 +12,24 @@ namespace ros { /// Converts a ROS JointTrajectory into an aikido's Spline trajectory. /// This method only handles single-DOF joints. -/// \param[in] _space MetaSkeletonStateSpace for Spline trajectory. -// Subspaces must be either 1D RnJoint or SO2Joint. -/// \param[in] _jointTrajectory ROS JointTrajectory to be converted. +/// \param[in] space MetaSkeletonStateSpace for Spline trajectory. +// Subspaces must be either R1Joint or SO2Joint. +/// \param[in] jointTrajectory ROS JointTrajectory to be converted. /// \return Spline trajectory. -std::unique_ptr convertJointTrajectory( +std::unique_ptr toSplineJointTrajectory( const std::shared_ptr< aikido::statespace::dart::MetaSkeletonStateSpace>& space, const trajectory_msgs::JointTrajectory& jointTrajectory); +/// Converts Aikido Trajectory to ROS JointTrajectory. +/// Supports only 1D RnJoints and SO2Joints. +/// \param[in] trajectory Aikido trajectory to be converted. +/// \param[in] timestep Timestep between two consecutive waypoints. +trajectory_msgs::JointTrajectory toRosJointTrajectory( + const aikido::trajectory::TrajectoryPtr& trajectory, double timestep); + } // namespace ros } // namespace control } // namespace aikido #endif // ifndef AIKIDO_CONTROL_ROS_CONVERSIONS_HPP_ - diff --git a/include/aikido/control/ros/RosTrajectoryExecutionException.hpp b/include/aikido/control/ros/RosTrajectoryExecutionException.hpp new file mode 100644 index 0000000000..9f5428764d --- /dev/null +++ b/include/aikido/control/ros/RosTrajectoryExecutionException.hpp @@ -0,0 +1,33 @@ +#ifndef AIKIDO_CONTROL_ROS_ROSTRAJECTORYEXECUTIONEXCEPTION_HPP_ +#define AIKIDO_CONTROL_ROS_ROSTRAJECTORYEXECUTIONEXCEPTION_HPP_ +#include +#include +#include + +namespace aikido { +namespace control { +namespace ros { + +/// This class wraps various exceptions that may arise during trajectory +/// execution over ROS. +class RosTrajectoryExecutionException: public std::runtime_error +{ +public: + + RosTrajectoryExecutionException( + const std::string& what, + actionlib::TerminalState terminalState); + + RosTrajectoryExecutionException( + const std::string& what, + int result); + + virtual ~RosTrajectoryExecutionException() = default; + +}; + +} // ros +} // control +} // aikido + +#endif diff --git a/include/aikido/control/ros/RosTrajectoryExecutor.hpp b/include/aikido/control/ros/RosTrajectoryExecutor.hpp new file mode 100644 index 0000000000..6ad01cf2ff --- /dev/null +++ b/include/aikido/control/ros/RosTrajectoryExecutor.hpp @@ -0,0 +1,90 @@ +#ifndef AIKIDO_CONTROL_ROS_ROSTRAJECTORYEXECUTOR_HPP_ +#define AIKIDO_CONTROL_ROS_ROSTRAJECTORYEXECUTOR_HPP_ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace aikido { +namespace control { +namespace ros { + +/// This class sends trajectory commands to ROS server. +class RosTrajectoryExecutor : public aikido::control::TrajectoryExecutor +{ +public: + /// Constructor. + /// \param[in] space Space in which trajectries operate. + /// \param[in] node ROS node handle for action client. + /// \param[in] serverName Name of the server to send traejctory to. + /// \param[in] timestep Step size for interpolating trajectories. + /// \param[in] goalTimeTolerance + /// \param[in] connectionTimeout Timeout for server connection. + /// \param[in] connectionPollingPeriod Polling period for server connection. + template + RosTrajectoryExecutor( + statespace::dart::MetaSkeletonStateSpacePtr space, + ::ros::NodeHandle node, + const std::string& serverName, + double timestep, + double goalTimeTolerance, + const DurationA& connectionTimeout = std::chrono::milliseconds{1000}, + const DurationB& connectionPollingPeriod = std::chrono::milliseconds{20} + ); + + virtual ~RosTrajectoryExecutor(); + + /// Sends trajectory to ROS server for execution. + /// \param[in] traj Trajecotry to be executed. + std::future execute(trajectory::TrajectoryPtr traj) override; + + /// Sends trajectory to ROS server for execution. + /// \param[in] traj Trajectrory to be executed. + /// \param[in] startTime Start time for the trajectory. + std::future execute( + trajectory::TrajectoryPtr traj, const ::ros::Time& startTime); + + /// To be executed on a separate thread. + /// Regularly checks for the completion of a sent trajectory. + void spin(); + +private: + using TrajectoryActionClient + = actionlib::ActionClient; + using GoalHandle = TrajectoryActionClient::GoalHandle; + + bool waitForServer(); + + void transitionCallback(GoalHandle handle); + + statespace::dart::MetaSkeletonStateSpacePtr mSpace; + ::ros::NodeHandle mNode; + ::ros::CallbackQueue mCallbackQueue; + TrajectoryActionClient mClient; + TrajectoryActionClient::GoalHandle mGoalHandle; + + double mTimestep; + double mGoalTimeTolerance; + + std::chrono::milliseconds mConnectionTimeout; + std::chrono::milliseconds mConnectionPollingPeriod; + + bool mInProgress; + std::promise mPromise; + trajectory::TrajectoryPtr mTrajectory; + + std::mutex mMutex; +}; + +} // namespace ros +} // namespace control +} // namespace aikido + +#endif // ifndef AIKIDO_CONTROL_ROS_ROSTRAJECTORYEXECUTOR_HPP_ + diff --git a/include/aikido/perception/eigen_yaml.hpp b/include/aikido/perception/eigen_yaml.hpp index 3155dc4069..3d5b818718 100644 --- a/include/aikido/perception/eigen_yaml.hpp +++ b/include/aikido/perception/eigen_yaml.hpp @@ -89,12 +89,7 @@ namespace YAML { namespace detail { template -struct encode_impl { - static Node encode(MatrixType const &matrix) - { - assert(false && "Unknown MatrixType."); - } -}; +struct encode_impl {}; template struct encode_impl { diff --git a/package.xml b/package.xml index a0a306ad16..c49102e4f4 100644 --- a/package.xml +++ b/package.xml @@ -15,6 +15,10 @@ python tinyxml2 yaml-cpp + actionlib + control_msgs + libmicrohttpd + interactive_markers doxygen nlopt diff --git a/src/control/ros/CMakeLists.txt b/src/control/ros/CMakeLists.txt index a6fedd31d9..b2155c2ee3 100644 --- a/src/control/ros/CMakeLists.txt +++ b/src/control/ros/CMakeLists.txt @@ -4,10 +4,25 @@ find_package(trajectory_msgs QUIET) aikido_check_package(trajectory_msgs "aikido::control::ros" "trajectory_msgs") +# Note: Intentionally use "Module" mode because the config file imports "gtest", +# "tests", and "run_tests" that conflict with the targets defined by Aikido. +find_package(actionlib QUIET MODULE) +aikido_check_package(actionlib "aikido::control::ros" "actionlib") + +# Note: Intentionally use "Module" mode because the config file imports "gtest", +# "tests", and "run_tests" that conflict with the targets defined by Aikido. +find_package(control_msgs QUIET MODULE) +aikido_check_package(control_msgs "aikido::control::ros" "control_msgs") + +find_package(roscpp QUIET) +aikido_check_package(roscpp "aikido::control::ros" "roscpp") + #============================================================================== # Libraries # set(sources + RosTrajectoryExecutor.cpp + RosTrajectoryExecutionException.cpp Conversions.cpp ) @@ -15,6 +30,9 @@ add_library("${PROJECT_NAME}_control_ros" SHARED ${sources}) target_include_directories("${PROJECT_NAME}_control_ros" SYSTEM PUBLIC ${trajectory_msgs_INCLUDE_DIRS} + ${actionlib_INCLUDE_DIRS} + ${control_msgs_INCLUDE_DIRS} + ${roscpp_INCLUDE_DIRS} ) target_link_libraries("${PROJECT_NAME}_control_ros" @@ -23,6 +41,9 @@ target_link_libraries("${PROJECT_NAME}_control_ros" "${PROJECT_NAME}_trajectory" ${DART_LIBRARIES} ${trajectory_msgs_LIBRARIES} + ${actionlib_LIBRARIES} + ${control_msgs_LIBRARIES} + ${roscpp_LIBRARIES} ) add_component(${PROJECT_NAME} control_ros) @@ -30,4 +51,3 @@ add_component_targets(${PROJECT_NAME} control_ros "${PROJECT_NAME}_control_ros") add_component_dependencies(${PROJECT_NAME} control_ros control statespace trajectory) coveralls_add_sources(${sources}) - diff --git a/src/control/ros/Conversions.cpp b/src/control/ros/Conversions.cpp index 223eeea648..d2769d54fd 100644 --- a/src/control/ros/Conversions.cpp +++ b/src/control/ros/Conversions.cpp @@ -1,7 +1,8 @@ #include -#include #include #include +#include +#include #include #include #include @@ -126,16 +127,51 @@ void extractJointTrajectoryPoint( } } +//============================================================================= +void extractTrajectoryPoint( + const std::shared_ptr& space, + const aikido::trajectory::TrajectoryPtr& trajectory, + double timeFromStart, trajectory_msgs::JointTrajectoryPoint& waypoint) +{ + const auto numDerivatives = std::min(trajectory->getNumDerivatives(), 1); + const auto timeAbsolute = trajectory->getStartTime() + timeFromStart; + const int numDof = space->getDimension(); + DART_UNUSED(numDof); + + Eigen::VectorXd tangentVector; + auto state = space->createState(); + trajectory->evaluate(timeAbsolute, state); + space->logMap(state, tangentVector); + + assert(tangentVector.size() == numDof); + + waypoint.time_from_start = ::ros::Duration(timeFromStart); + waypoint.positions.assign(tangentVector.data(), + tangentVector.data() + tangentVector.size()); + + assert(0 <= numDerivatives && numDerivatives <= 2); + const std::array*, 2> derivatives{ + &waypoint.velocities, &waypoint.accelerations}; + for (int iDerivative = 1; iDerivative <= numDerivatives; ++iDerivative) + { + trajectory->evaluateDerivative(timeAbsolute, iDerivative, tangentVector); + assert(tangentVector.size() == numDof); + + derivatives[iDerivative - 1]->assign(tangentVector.data(), + tangentVector.data() + tangentVector.size()); + } +} + //============================================================================= // The rows of inVector is reordered in outVector. void reorder(std::map indexMap, const Eigen::VectorXd& inVector, Eigen::VectorXd* outVector) { - *outVector = Eigen::VectorXd::Zero(inVector.size()); - for (auto it = indexMap.begin(); it != indexMap.end(); ++it) - { - (*outVector)(it->second) = inVector(it->first); - } + assert(outVector != nullptr); + assert(indexMap.size() == static_cast(inVector.size())); + outVector->resize(inVector.size()); + for (auto index : indexMap) + (*outVector)[index.second] = inVector[index.first]; } //============================================================================= @@ -147,11 +183,8 @@ std::vector findJointByName( for (size_t i = 0; i < metaSkeleton.getNumJoints(); ++i) { - auto joint = metaSkeleton.getJoint(i); - if (joint->getName() == jointName) - { + if (metaSkeleton.getJoint(i)->getName() == jointName) joints.emplace_back(metaSkeleton.getJoint(i)); - } } return joints; @@ -160,7 +193,7 @@ std::vector findJointByName( } // namespace //============================================================================= -std::unique_ptr convertJointTrajectory( +std::unique_ptr toSplineJointTrajectory( const std::shared_ptr& space, const trajectory_msgs::JointTrajectory& jointTrajectory) { @@ -178,15 +211,20 @@ std::unique_ptr convertJointTrajectory( } // Check that the names in jointTrajectory are unique. - auto joint_names = jointTrajectory.joint_names; + std::vector joint_names; + joint_names.reserve(numControlledJoints); + for (size_t i = 0; i < numControlledJoints; ++i) + joint_names.emplace_back(jointTrajectory.joint_names[i]); std::sort(joint_names.begin(), joint_names.end()); - auto duplicate_it = std::adjacent_find(std::begin(joint_names), std::end(joint_names)); - if (duplicate_it != std::end(joint_names)) + for (size_t i = 0; i < numControlledJoints - 1; ++i) { - std::stringstream message; - message << "JointTrajectory has multiple joints with same name [" - << *duplicate_it << "]."; - throw std::invalid_argument{message.str()}; + if (joint_names[i] == joint_names[i+1]) + { + std::stringstream message; + message << "JointTrajectory has multiple joints with same name [" + << joint_names[i] << "]."; + throw std::invalid_argument{message.str()}; + } } // Check that all joints are R1Joint or SO2JOint state spaces. @@ -220,24 +258,23 @@ std::unique_ptr convertJointTrajectory( auto metaSkeleton = space->getMetaSkeleton(); auto nJoints = jointTrajectory.joint_names.size(); - for (size_t trajJointIndex = 0; trajJointIndex < nJoints; ++trajJointIndex) + for (size_t trajIndex = 0; trajIndex < nJoints; ++trajIndex) { - const auto& jointName = jointTrajectory.joint_names[trajJointIndex]; + const auto& jointName = jointTrajectory.joint_names[trajIndex]; auto joints = findJointByName(*metaSkeleton, jointName); - if (joints.empty()) + if (joints.size() == 0) { std::stringstream message; - message << "Joint " << jointName << " (trajectory index: " - << trajJointIndex << ")" << " does not exist in MetaSkeleton."; + message << "Joint " << jointName << " (index: " << trajIndex << ")" + << " does not exist in metaSkeleton."; throw std::invalid_argument{message.str()}; } else if (joints.size() > 1) { std::stringstream message; message << "Multiple (" << joints.size() - << ") joints have the same name [" << jointName << "] " - << "in the JointTrajectory."; + << ") joints have the same name [" << jointName << "]."; throw std::invalid_argument{message.str()}; } @@ -246,7 +283,7 @@ std::unique_ptr convertJointTrajectory( assert(metaSkeletonIndex != dart::dynamics::INVALID_INDEX); rosJointToMetaSkeletonJoint.emplace( - std::make_pair(trajJointIndex, metaSkeletonIndex)); + std::make_pair(trajIndex, metaSkeletonIndex)); } // Extract the first waypoint to infer the dimensionality of the trajectory. @@ -280,17 +317,19 @@ std::unique_ptr convertJointTrajectory( auto currState = space->createState(); const auto& waypoints = jointTrajectory.points; - for (size_t iwaypoint = 1; iwaypoint < waypoints.size(); ++iwaypoint) + for (size_t iWaypoint = 1; iWaypoint < waypoints.size(); ++iWaypoint) { - Eigen::VectorXd nextPosition, nextVelocity, nextAcceleration; - extractJointTrajectoryPoint(jointTrajectory, iwaypoint, numControlledJoints, + Eigen::VectorXd nextPosition; + Eigen::VectorXd nextVelocity; + Eigen::VectorXd nextAcceleration; + extractJointTrajectoryPoint(jointTrajectory, iWaypoint, numControlledJoints, &nextPosition, isPositionRequired, &nextVelocity, isVelocityRequired, &nextAcceleration, isAccelerationRequired, rosJointToMetaSkeletonJoint); // Compute spline coefficients for this polynomial segment. - const auto nextTimeFromStart = waypoints[iwaypoint].time_from_start.toSec(); + const auto nextTimeFromStart = waypoints[iWaypoint].time_from_start.toSec(); const auto segmentDuration = nextTimeFromStart - currTimeFromStart; const auto segmentCoefficients = fitPolynomial( 0., Eigen::VectorXd::Zero(numControlledJoints), currVelocity, currAcceleration, @@ -308,10 +347,88 @@ std::unique_ptr convertJointTrajectory( currTimeFromStart = nextTimeFromStart; } - return std::move(trajectory); + return trajectory; +} + +//============================================================================= +trajectory_msgs::JointTrajectory toRosJointTrajectory( + const aikido::trajectory::TrajectoryPtr& trajectory, double timestep) +{ + using statespace::dart::MetaSkeletonStateSpace; + using statespace::dart::SO2Joint; + using statespace::dart::R1Joint; + + if (!trajectory) + throw std::invalid_argument("Trajectory is null."); + + if (timestep <= 0) + throw std::invalid_argument("Timestep must be positive."); + + const auto space = std::dynamic_pointer_cast< + MetaSkeletonStateSpace>(trajectory->getStateSpace()); + if (!space) + { + throw std::invalid_argument( + "Trajectory is not in a MetaSkeletonStateSpace."); + } + + for (size_t i = 0; i < space->getNumSubspaces(); ++i) + { + // Supports only R1Joints and SO2Joints. + auto rnJoint = std::dynamic_pointer_cast(space->getSubspace(i)); + auto so2Joint = std::dynamic_pointer_cast(space->getSubspace(i)); + if (!rnJoint && !so2Joint) + { + throw std::invalid_argument( + "MetaSkeletonStateSpace must contain only RnJonts and SO2Joints."); + } + + // For RnJoint, supports only 1D. + if (rnJoint && rnJoint->getDimension() != 1) + { + std::stringstream message; + message << "RnJoint must be 1D. Joint " + << i << " has " << rnJoint->getDimension() << " dimensions."; + throw std::invalid_argument{message.str()}; + } + } + + util::StepSequence timeSequence{timestep, true, 0., trajectory->getDuration()}; + const auto numJoints = space->getNumSubspaces(); + const auto numWaypoints = timeSequence.getMaxSteps(); + const auto metaSkeleton = space->getMetaSkeleton(); + trajectory_msgs::JointTrajectory jointTrajectory; + jointTrajectory.joint_names.reserve(numJoints); + + for (size_t i = 0; i < numJoints; ++i) + { + const auto joint = space->getJointSpace(i)->getJoint(); + const auto jointName = joint->getName(); + auto joints = findJointByName(*metaSkeleton, jointName); + if (joints.size() > 1) + { + std::stringstream message; + message << "Metaskeleton has multiple joints with same name [" + << jointName << "]."; + throw std::invalid_argument{message.str()}; + } + jointTrajectory.joint_names.emplace_back(jointName); + } + + // Evaluate trajectory at each timestep and insert it into jointTrajectory + jointTrajectory.points.reserve(numWaypoints); + for (const auto timeFromStart : timeSequence) + { + trajectory_msgs::JointTrajectoryPoint waypoint; + + extractTrajectoryPoint(space, trajectory, timeFromStart, waypoint); + + jointTrajectory.points.emplace_back(waypoint); + } + + return jointTrajectory; } } // namespace ros } // namespace control } // namespace aikido - diff --git a/src/control/ros/RosTrajectoryExecutionException.cpp b/src/control/ros/RosTrajectoryExecutionException.cpp new file mode 100644 index 0000000000..854f3c202b --- /dev/null +++ b/src/control/ros/RosTrajectoryExecutionException.cpp @@ -0,0 +1,32 @@ +#include +#include +#include +#include +#include +#include + +namespace aikido { +namespace control { +namespace ros { + +//============================================================================= +RosTrajectoryExecutionException::RosTrajectoryExecutionException( + const std::string& what, + actionlib::TerminalState /*terminalState*/) +: std::runtime_error(what) +{ + // Do nothing +} + +//============================================================================= +RosTrajectoryExecutionException::RosTrajectoryExecutionException( + const std::string& what, + int /*result*/) +: std::runtime_error(what) +{ + // Do nothing +} + +} // namespace ros +} // namespace control +} // namespace aikido diff --git a/src/control/ros/RosTrajectoryExecutor.cpp b/src/control/ros/RosTrajectoryExecutor.cpp new file mode 100644 index 0000000000..3cfe573751 --- /dev/null +++ b/src/control/ros/RosTrajectoryExecutor.cpp @@ -0,0 +1,252 @@ +#include +#include +#include +#include +#include +#include +#include + +namespace aikido { +namespace control { +namespace ros { +namespace { + +using std::chrono::milliseconds; + +//============================================================================= +std::string getFollowJointTrajectoryErrorMessage(int32_t errorCode) +{ + using Result = control_msgs::FollowJointTrajectoryResult; + + switch (errorCode) + { + case Result::SUCCESSFUL: + return "successful"; + + case Result::INVALID_GOAL: + return "invalid goal"; + + case Result::INVALID_JOINTS: + return "invalid joints"; + + case Result::OLD_HEADER_TIMESTAMP: + return "old header timestamp"; + + case Result::PATH_TOLERANCE_VIOLATED: + return "path tolerance violated"; + + case Result::GOAL_TOLERANCE_VIOLATED: + return "goal tolerance violated"; + + default: + std::stringstream errorString; + errorString << "unknown (" << errorCode << ")"; + return errorString.str(); + } +} + +} // namespace + +//============================================================================= +template +RosTrajectoryExecutor::RosTrajectoryExecutor( + statespace::dart::MetaSkeletonStateSpacePtr space, + ::ros::NodeHandle node, + const std::string& serverName, + double timestep, + double goalTimeTolerance, + const DurationA& connectionTimeout, + const DurationB& connectionPollingPeriod) + : mSpace{std::move(space)} + , mNode{std::move(node)} + , mCallbackQueue{} + , mClient{mNode, serverName, &mCallbackQueue} + , mTimestep{timestep} + , mGoalTimeTolerance{goalTimeTolerance} + , mConnectionTimeout{ + std::chrono::duration_cast(connectionTimeout)} + , mConnectionPollingPeriod{ + std::chrono::duration_cast(connectionPollingPeriod)} + , mInProgress{false} +{ + if (!mSpace) + throw std::invalid_argument("Space is null."); + + if (mTimestep <= 0) + throw std::invalid_argument("Timestep must be positive."); + + if (mGoalTimeTolerance <= 0) + throw std::invalid_argument("Goal time tolerance must be positive."); +} + +//============================================================================= +RosTrajectoryExecutor::~RosTrajectoryExecutor() +{ + // Do nothing. + // TODO: Should we wait for the current trajectory to finish executing? +} + +//============================================================================= +std::future RosTrajectoryExecutor::execute( + trajectory::TrajectoryPtr traj) +{ + static const ::ros::Time invalidTime; + return execute(traj, invalidTime); +} + +//============================================================================= +std::future RosTrajectoryExecutor::execute( + trajectory::TrajectoryPtr traj, const ::ros::Time& startTime) +{ + using aikido::control::ros::toRosJointTrajectory; + using aikido::statespace::dart::MetaSkeletonStateSpace; + + if (!traj) + throw std::invalid_argument("Trajectory is null."); + + const auto space = std::dynamic_pointer_cast< + MetaSkeletonStateSpace>(traj->getStateSpace()); + if (!space) + { + throw std::invalid_argument( + "Trajectory is not in a MetaSkeletonStateSpace."); + } + + if (space != mSpace) + { + throw std::invalid_argument( + "Trajectory is not in the same StateSpace as this RosTrajectoryExecutor."); + } + + // Setup the goal properties. + // TODO: Also set goal_tolerance, path_tolerance. + control_msgs::FollowJointTrajectoryGoal goal; + goal.trajectory.header.stamp = startTime; + goal.goal_time_tolerance = ::ros::Duration(mGoalTimeTolerance); + + // Convert the Aikido trajectory into a ROS JointTrajectory. + goal.trajectory = toRosJointTrajectory( + traj, mTimestep); + + if (!waitForServer()) + throw std::runtime_error("Unable to connect to action server."); + + { + std::lock_guard lock(mMutex); + DART_UNUSED(lock); // Suppress unused variable warning + + if (mInProgress) + throw std::runtime_error("Another trajectory is in progress."); + + mPromise = std::promise(); + mInProgress = true; + mGoalHandle = mClient.sendGoal(goal, + boost::bind(&RosTrajectoryExecutor::transitionCallback, this, _1)); + + return mPromise.get_future(); + } +} + +//============================================================================= +bool RosTrajectoryExecutor::waitForServer() +{ + using Clock = std::chrono::steady_clock; + + const auto startTime = Clock::now(); + const auto endTime = startTime + mConnectionTimeout; + auto currentTime = startTime + mConnectionPollingPeriod; + + while (currentTime < endTime) + { + mCallbackQueue.callAvailable(); + + // TODO: Is this thread safe? + if (mClient.isServerConnected()) + return true; + + currentTime += mConnectionPollingPeriod; + std::this_thread::sleep_until(currentTime); + } + + return false; +} + +//============================================================================= +void RosTrajectoryExecutor::transitionCallback(GoalHandle handle) +{ + // This function assumes that mMutex is locked. + + using actionlib::TerminalState; + using Result = control_msgs::FollowJointTrajectoryResult; + + if (handle.getCommState() == actionlib::CommState::DONE) + { + std::stringstream message; + bool isSuccessful = true; + + // Check the status of the actionlib call. Note that the actionlib call can + // succeed, even if execution failed. + const auto terminalState = handle.getTerminalState(); + if (terminalState != TerminalState::SUCCEEDED) + { + message << "Action " << terminalState.toString(); + + const auto terminalMessage = terminalState.getText(); + if (!terminalMessage.empty()) + message << " (" << terminalMessage << ")"; + + mPromise.set_exception( + std::make_exception_ptr( + RosTrajectoryExecutionException(message.str(), terminalState))); + + isSuccessful = false; + } + else + { + message << "Execution failed."; + } + + // Check the status of execution. This is only possible if the actionlib + // call succeeded. + const auto result = handle.getResult(); + if (result && result->error_code != Result::SUCCESSFUL) + { + message << ": " + << getFollowJointTrajectoryErrorMessage(result->error_code); + + if (!result->error_string.empty()) + message << " (" << result->error_string << ")"; + + mPromise.set_exception(std::make_exception_ptr( + RosTrajectoryExecutionException(message.str(), result->error_code))); + + isSuccessful = false; + } + + if (isSuccessful) + mPromise.set_value(); + + mInProgress = false; + } +} + +//============================================================================= +void RosTrajectoryExecutor::spin() +{ + std::lock_guard lock(mMutex); + DART_UNUSED(lock); // Suppress unused variable warning. + + mCallbackQueue.callAvailable(); + + if (!::ros::ok() && mInProgress) + { + mPromise.set_exception( + std::make_exception_ptr(std::runtime_error("Detected ROS shutdown."))); + mInProgress = false; + } +} + +} // namespace ros +} // namespace control +} // namespace aikido + diff --git a/tests/control/ros/CMakeLists.txt b/tests/control/ros/CMakeLists.txt index 25eeaa8da5..52640691f7 100644 --- a/tests/control/ros/CMakeLists.txt +++ b/tests/control/ros/CMakeLists.txt @@ -1,7 +1,8 @@ if(TARGET "${PROJECT_NAME}_control_ros") - aikido_add_test(test_Conversions - test_Conversions.cpp) - target_link_libraries(test_Conversions - "${PROJECT_NAME}_control_ros") -endif() + aikido_add_test(test_RosTrajectoryConversions + test_RosTrajectoryConversions.cpp) + target_link_libraries(test_RosTrajectoryConversions "${PROJECT_NAME}_control_ros") + aikido_add_test(test_Conversions test_Conversions.cpp) + target_link_libraries(test_Conversions "${PROJECT_NAME}_control_ros") +endif() diff --git a/tests/control/ros/test_Conversions.cpp b/tests/control/ros/test_Conversions.cpp index 49adb8dff1..f26db48575 100644 --- a/tests/control/ros/test_Conversions.cpp +++ b/tests/control/ros/test_Conversions.cpp @@ -4,7 +4,7 @@ #include "eigen_tests.hpp" using dart::dynamics::SkeletonPtr; -using aikido::control::ros::convertJointTrajectory; +using aikido::control::ros::toSplineJointTrajectory; using aikido::statespace::dart::MetaSkeletonStateSpace; using aikido::tests::make_vector; @@ -94,7 +94,7 @@ class ConvertJointTrajectoryTests : public testing::Test TEST_F(ConvertJointTrajectoryTests, StateSpaceIsNull_Throws) { EXPECT_THROW({ - convertJointTrajectory(nullptr, mTwoWaypointMessage); + toSplineJointTrajectory(nullptr, mTwoWaypointMessage); }, std::invalid_argument); } @@ -104,7 +104,7 @@ TEST_F(ConvertJointTrajectoryTests, NoWaypoints_Throws) zeroWaypointMessage.points.clear(); EXPECT_THROW({ - convertJointTrajectory(mStateSpace, zeroWaypointMessage); + toSplineJointTrajectory(mStateSpace, zeroWaypointMessage); }, std::invalid_argument); } @@ -114,7 +114,7 @@ TEST_F(ConvertJointTrajectoryTests, LessThanTwoWaypoints_Throws) oneWaypointMessage.points.resize(1); EXPECT_THROW({ - convertJointTrajectory(mStateSpace, oneWaypointMessage); + toSplineJointTrajectory(mStateSpace, oneWaypointMessage); }, std::invalid_argument); } @@ -123,7 +123,7 @@ TEST_F(ConvertJointTrajectoryTests, IncorrectNumberOfJoints_Throws) mTwoWaypointMessage.joint_names.emplace_back("Joint1"); EXPECT_THROW({ - convertJointTrajectory(mStateSpace, mTwoWaypointMessage); + toSplineJointTrajectory(mStateSpace, mTwoWaypointMessage); }, std::invalid_argument); } @@ -132,7 +132,7 @@ TEST_F(ConvertJointTrajectoryTests, TrajectoryHasUnknownJoint_Throws) mTwoWaypointMessage.joint_names[0] = "MissingJoint"; EXPECT_THROW({ - convertJointTrajectory(mStateSpace, mTwoWaypointMessage); + toSplineJointTrajectory(mStateSpace, mTwoWaypointMessage); }, std::invalid_argument); } @@ -141,7 +141,7 @@ TEST_F(ConvertJointTrajectoryTests, StateSpaceHasUnknownJoint_Throws) mSkeleton->getJoint(0)->setName("MissingJoint"); EXPECT_THROW({ - convertJointTrajectory(mStateSpace, mTwoWaypointMessage); + toSplineJointTrajectory(mStateSpace, mTwoWaypointMessage); }, std::invalid_argument); } @@ -149,7 +149,7 @@ TEST_F(ConvertJointTrajectoryTests, StateSpaceHasUnknownJoint_Throws) TEST_F(ConvertJointTrajectoryTests, LinearTrajectory) { const auto linearTwoWaypointMessage = mTwoWaypointMessage; - const auto trajectory = convertJointTrajectory( + const auto trajectory = toSplineJointTrajectory( mStateSpace, linearTwoWaypointMessage); ASSERT_TRUE(!!trajectory); @@ -183,7 +183,7 @@ TEST_F(ConvertJointTrajectoryTests, CubicTrajectory) cubicTwoWaypointMessage.points[0].velocities.assign({3.}); cubicTwoWaypointMessage.points[1].velocities.assign({4.}); - const auto trajectory = convertJointTrajectory( + const auto trajectory = toSplineJointTrajectory( mStateSpace, cubicTwoWaypointMessage); ASSERT_TRUE(!!trajectory); @@ -219,7 +219,7 @@ TEST_F(ConvertJointTrajectoryTests, QuinticTrajectory) quinticTwoWaypointMessage.points[1].velocities.assign({4.}); quinticTwoWaypointMessage.points[1].accelerations.assign({6.}); - const auto trajectory = convertJointTrajectory( + const auto trajectory = toSplineJointTrajectory( mStateSpace, quinticTwoWaypointMessage); ASSERT_TRUE(!!trajectory); @@ -255,7 +255,7 @@ TEST_F(ConvertJointTrajectoryTests, QuinticTrajectory) TEST_F(ConvertJointTrajectoryTests, DifferentOrderingOfJoints) { - const auto trajectory = convertJointTrajectory( + const auto trajectory = toSplineJointTrajectory( mStateSpace2Joints, mTwoWaypointMessage2Joints); ASSERT_TRUE(!!trajectory); @@ -286,7 +286,7 @@ TEST_F(ConvertJointTrajectoryTests, DifferentOrderingOfJoints) TEST_F(ConvertJointTrajectoryTests, StateSpaceMissingJoint_Throws) { EXPECT_THROW({ - convertJointTrajectory(mStateSpace, mTwoWaypointMessage2Joints); + toSplineJointTrajectory(mStateSpace, mTwoWaypointMessage2Joints); }, std::invalid_argument); } @@ -307,7 +307,7 @@ TEST_F(ConvertJointTrajectoryTests, waypoint2.positions.assign({2., 3.}); EXPECT_THROW({ - convertJointTrajectory(mStateSpace2Joints, trajectory); + toSplineJointTrajectory(mStateSpace2Joints, trajectory); }, std::invalid_argument); } @@ -336,6 +336,6 @@ TEST_F(ConvertJointTrajectoryTests, auto space = std::make_shared(groupSkeleton); EXPECT_THROW({ - convertJointTrajectory(space, mTwoWaypointMessage2Joints); + toSplineJointTrajectory(space, mTwoWaypointMessage2Joints); }, std::invalid_argument); } diff --git a/tests/control/ros/test_RosTrajectoryConversions.cpp b/tests/control/ros/test_RosTrajectoryConversions.cpp new file mode 100644 index 0000000000..428693dd4a --- /dev/null +++ b/tests/control/ros/test_RosTrajectoryConversions.cpp @@ -0,0 +1,158 @@ +#include +#include +#include +#include +#include +#include "eigen_tests.hpp" + +using dart::dynamics::SkeletonPtr; +using dart::dynamics::Skeleton; +using dart::dynamics::BodyNode; +using dart::dynamics::RevoluteJoint; +using dart::dynamics::BallJoint; +using aikido::trajectory::Spline; +using aikido::control::ros::toRosJointTrajectory; +using aikido::statespace::dart::MetaSkeletonStateSpacePtr; +using aikido::statespace::dart::MetaSkeletonStateSpace; +using aikido::tests::make_vector; + +static const double kTolerance{1e-6}; + +// TODO: We might want to merge this with test_Conversions.cpp +class ToRosJointTrajectoryTests : public testing::Test +{ +protected: + void SetUp() override + { + // Create a single-DOF skeleton. + auto skeleton = Skeleton::create(); + skeleton->createJointAndBodyNodePair(); + skeleton->getJoint(0)->setName("Joint1"); + + mStateSpace = std::make_shared(skeleton); + auto startState = mStateSpace->createState(); + mStateSpace->getState(startState); + + // Spline trajectory + mTrajectory = std::make_shared(mStateSpace, 0.); + Eigen::Matrix coeffs; + coeffs << 0., 1.; + mTrajectory->addSegment(coeffs, 0.1, startState); + + // Timestep + mTimestep = 0.1; + + // Create a 2-DOF skeleton. + auto skeleton2 = Skeleton::create(); + RevoluteJoint::Properties jointProperties; + jointProperties.mName = "Joint1"; + BodyNode::Properties bnProperties; + bnProperties.mName = "BodyNode1"; + + auto bn = skeleton2->createJointAndBodyNodePair< + RevoluteJoint, BodyNode>( + nullptr, jointProperties, bnProperties).second; + skeleton2->createJointAndBodyNodePair(bn); + skeleton2->getJoint(0)->setName("Joint1"); + skeleton2->getJoint(1)->setName("Joint2"); + skeleton2->getJoint(1)->setPosition(0, 1); + + mStateSpace2DOF = std::make_shared(skeleton2); + auto startState2DOF = mStateSpace2DOF->createState(); + mStateSpace2DOF->getState(startState2DOF); + + // Spline trajectory + mTrajectory2DOF = std::make_shared(mStateSpace2DOF, 0.); + Eigen::Matrix2d coeffs2; + coeffs2 << 3., 1., + 1., 1.; + mTrajectory2DOF->addSegment(coeffs2, 0.1, startState2DOF); + } + + std::shared_ptr mTrajectory; + MetaSkeletonStateSpacePtr mStateSpace; + + std::shared_ptr mTrajectory2DOF; + MetaSkeletonStateSpacePtr mStateSpace2DOF; + + double mTimestep; +}; + +TEST_F(ToRosJointTrajectoryTests, TrajectoryIsNull_Throws) +{ + EXPECT_THROW({ + toRosJointTrajectory(nullptr, mTimestep); + }, std::invalid_argument); +} + +TEST_F(ToRosJointTrajectoryTests, TimestepIsZero_Throws) +{ + EXPECT_THROW({ + toRosJointTrajectory(mTrajectory, 0.0); + }, std::invalid_argument); +} + +TEST_F(ToRosJointTrajectoryTests, TimestepIsNegative_Throws) +{ + EXPECT_THROW({ + toRosJointTrajectory(mTrajectory, -0.1); + }, std::invalid_argument); +} + +TEST_F(ToRosJointTrajectoryTests, SkeletonHasUnsupportedJoint_Throws) +{ + auto skeleton = Skeleton::create(); + skeleton->createJointAndBodyNodePair(); + auto space = std::make_shared(skeleton); + + auto trajectory = std::make_shared(space, 0.0); + EXPECT_THROW({ + toRosJointTrajectory(trajectory, mTimestep); + }, std::invalid_argument); +} + +TEST_F(ToRosJointTrajectoryTests, TrajectoryHasCorrectWaypoints) +{ + auto rosTrajectory = toRosJointTrajectory( + mTrajectory, mTimestep); + + EXPECT_EQ(2, rosTrajectory.points.size()); + for (int i = 0; i < 2; ++i) + { + ASSERT_DOUBLE_EQ(mTimestep*i, rosTrajectory.points[i].time_from_start.toSec()); + + auto state = mStateSpace->createState(); + Eigen::VectorXd values; + + mTrajectory->evaluate(mTimestep*i, state); + mStateSpace->convertStateToPositions(state, values); + + EXPECT_EIGEN_EQUAL(values, + make_vector(rosTrajectory.points[i].positions[0]), kTolerance); + } + ASSERT_DOUBLE_EQ(mTimestep*(rosTrajectory.points.size()-1), + mTrajectory->getEndTime()); + + // Finer timesteps + double timestep = 0.01; + auto rosTrajectory2 = toRosJointTrajectory( + mTrajectory, timestep); + + EXPECT_EQ(11, rosTrajectory2.points.size()); + for (int i = 0; i < 11; ++i) + { + ASSERT_DOUBLE_EQ(timestep*i, + rosTrajectory2.points[i].time_from_start.toSec()); + + auto state = mStateSpace->createState(); + Eigen::VectorXd values; + + mTrajectory->evaluate(timestep*i, state); + mStateSpace->convertStateToPositions(state, values); + EXPECT_EIGEN_EQUAL(values, + make_vector(rosTrajectory2.points[i].positions[0]), kTolerance); + } + ASSERT_DOUBLE_EQ(timestep*(rosTrajectory2.points.size()-1), + mTrajectory->getEndTime()); + +} From 65d3c23eacb8eaee487fe28b853d61f7d762c487 Mon Sep 17 00:00:00 2001 From: gilwoolee Date: Fri, 14 Apr 2017 15:03:09 -0400 Subject: [PATCH 2/7] ros_control/RosJointStateClient (#150) * Initial commit for RosJointStateClient * Add some documentation. * Address Mike's comments. --- .../control/ros/RosJointStateClient.hpp | 72 +++++++++++ src/control/ros/CMakeLists.txt | 16 +++ src/control/ros/RosJointStateClient.cpp | 116 ++++++++++++++++++ 3 files changed, 204 insertions(+) create mode 100644 include/aikido/control/ros/RosJointStateClient.hpp create mode 100644 src/control/ros/RosJointStateClient.cpp diff --git a/include/aikido/control/ros/RosJointStateClient.hpp b/include/aikido/control/ros/RosJointStateClient.hpp new file mode 100644 index 0000000000..ece02108a8 --- /dev/null +++ b/include/aikido/control/ros/RosJointStateClient.hpp @@ -0,0 +1,72 @@ +#ifndef AIKIDO_CONTROL_ROS_ROSJOINTSTATECLIENT_HPP_ +#define AIKIDO_CONTROL_ROS_ROSJOINTSTATECLIENT_HPP_ +#include +#include +#include +#include +#include +#include + +namespace aikido { +namespace control { +namespace ros { + +/// Client that listens for JointState messages for each skeleton joint and +/// provides a method for extracting the most recent position of each joint. +class RosJointStateClient +{ +public: + /// Constructor. + /// \param _skeleton Skeleton to read JointState updates for. + /// \param _nodeHandle ROS node. + /// \param _topicName Name of topic to subscribe to for JointState updates. + /// \param _capacity Number of JointStateRecords that are saved per joint. + RosJointStateClient( + dart::dynamics::SkeletonPtr _skeleton, + ::ros::NodeHandle _nodeHandle, + const std::string& _topicName, + size_t capacity); + + /// Update mBuffer with any JointState messages that have been received. + void spin(); + + /// Returns the most recent position of each joint in _metaSkeleton. + /// \param _metaSkeleton Skeleton to read DOFs from. + /// \return vector of positions for each DOF + Eigen::VectorXd getLatestPosition( + const dart::dynamics::MetaSkeleton& _metaSkeleton) const; + + // TODO: implement + // getPositionAtTime(const MetaSkeleton&, const ros::Time&, bool) + // that interpolates position at the specified time, optionally blocking for + // new data. +private: + struct JointStateRecord + { + inline bool isValid() const { return mStamp.isValid(); } + + ::ros::Time mStamp; + double mPosition; + }; + + /// Callback to add a new JointState to mBuffer + /// \param _jointState New JointState to add to mBuffer + void jointStateCallback(const sensor_msgs::JointState& _jointState); + + mutable std::mutex mMutex; + + dart::dynamics::SkeletonPtr mSkeleton; + std::unordered_map> mBuffer; + size_t mCapacity; + + ::ros::CallbackQueue mCallbackQueue; + ::ros::NodeHandle mNodeHandle; + ::ros::Subscriber mSubscriber; +}; + +} // namespace ros +} // namespace control +} // namespace aikido + +#endif // ifndef AIKIDO_CONTROL_ROS_ROSJOINTSTATECLIENT_HPP_ diff --git a/src/control/ros/CMakeLists.txt b/src/control/ros/CMakeLists.txt index b2155c2ee3..b1fec362f5 100644 --- a/src/control/ros/CMakeLists.txt +++ b/src/control/ros/CMakeLists.txt @@ -1,6 +1,15 @@ #============================================================================== # Dependencies # +find_package(actionlib QUIET) +aikido_check_package(actionlib "aikido::control::ros" "actionlib") + +find_package(control_msgs QUIET) +aikido_check_package(control_msgs "aikido::control::ros" "control_msgs") + +find_package(roscpp QUIET) +aikido_check_package(roscpp "aikido::control::ros" "roscpp") + find_package(trajectory_msgs QUIET) aikido_check_package(trajectory_msgs "aikido::control::ros" "trajectory_msgs") @@ -24,11 +33,15 @@ set(sources RosTrajectoryExecutor.cpp RosTrajectoryExecutionException.cpp Conversions.cpp + RosJointStateClient.cpp ) add_library("${PROJECT_NAME}_control_ros" SHARED ${sources}) target_include_directories("${PROJECT_NAME}_control_ros" SYSTEM PUBLIC + ${actionlib_INCLUDE_DIRS} + ${control_msgs_INCLUDE_DIRS} + ${roscpp_INCLUDE_DIRS} ${trajectory_msgs_INCLUDE_DIRS} ${actionlib_INCLUDE_DIRS} ${control_msgs_INCLUDE_DIRS} @@ -40,6 +53,9 @@ target_link_libraries("${PROJECT_NAME}_control_ros" "${PROJECT_NAME}_statespace" "${PROJECT_NAME}_trajectory" ${DART_LIBRARIES} + ${actionlib_LIBRARIES} + ${control_msgs_LIBRARIES} + ${roscpp_LIBRARIES} ${trajectory_msgs_LIBRARIES} ${actionlib_LIBRARIES} ${control_msgs_LIBRARIES} diff --git a/src/control/ros/RosJointStateClient.cpp b/src/control/ros/RosJointStateClient.cpp new file mode 100644 index 0000000000..ae6ce97d7a --- /dev/null +++ b/src/control/ros/RosJointStateClient.cpp @@ -0,0 +1,116 @@ +#include + +namespace aikido { +namespace control { +namespace ros { + +//============================================================================= +RosJointStateClient::RosJointStateClient( + dart::dynamics::SkeletonPtr _skeleton, + ::ros::NodeHandle _nodeHandle, + const std::string& _topicName, + size_t capacity) + : mSkeleton{std::move(_skeleton)} + , mBuffer{} + , mCapacity{capacity} + , mCallbackQueue{} // Must be after mNodeHandle for order of destruction. + , mNodeHandle{std::move(_nodeHandle)} +{ + if (!mSkeleton) + throw std::invalid_argument("Skeleton is null."); + + if (capacity < 1) + throw std::invalid_argument("Capacity must be positive."); + + mNodeHandle.setCallbackQueue(&mCallbackQueue); + mSubscriber = mNodeHandle.subscribe(_topicName, 1, + &RosJointStateClient::jointStateCallback, this); +} + +//============================================================================= +void RosJointStateClient::spin() +{ + std::lock_guard skeletonLock{mSkeleton->getMutex()}; + std::lock_guard bufferLock{mMutex}; + + mCallbackQueue.callAvailable(); +} + +//============================================================================= +Eigen::VectorXd RosJointStateClient::getLatestPosition( + const dart::dynamics::MetaSkeleton& _metaSkeleton) const +{ + std::lock_guard bufferLock{mMutex}; + Eigen::VectorXd position(_metaSkeleton.getNumDofs()); + + for (size_t idof = 0; idof < _metaSkeleton.getNumDofs(); ++idof) + { + const auto dof = _metaSkeleton.getDof(idof); + const auto it = mBuffer.find(dof->getName()); + if (it == std::end(mBuffer)) + { + std::stringstream msg; + msg << "No data is available for '" << dof->getName() << "'."; + throw std::runtime_error(msg.str()); + } + + const auto& buffer = it->second; + if (buffer.empty()) + { + std::stringstream msg; + msg << "Data for '" << dof->getName() << "' is invalid."; + throw std::runtime_error(msg.str()); + } + + const auto& record = buffer.back(); + position[idof] = record.mPosition; + } + + return position; +} + +//============================================================================= +void RosJointStateClient::jointStateCallback( + const sensor_msgs::JointState& _jointState) +{ + // This method assumes that mSkeleton->getMutex() and mMutex are locked. + + if (_jointState.position.size() != _jointState.name.size()) + { + ROS_WARN_STREAM("Incorrect number of positions: expected " + << _jointState.name.size() << ", got " + << _jointState.position.size() << "."); + return; + } + // TODO: Also check for velocities. + + for (size_t i = 0; i < _jointState.name.size(); ++i) + { + const auto result = mBuffer.emplace(_jointState.name[i], + boost::circular_buffer{mCapacity}); + auto& buffer = result.first->second; + + if (_jointState.header.stamp < buffer.back().mStamp) + { + // Ignore out of order JointState message. + ROS_WARN_STREAM("Ignoring out of order message: received timestamp of " + << _jointState.header.stamp << " is before previously " + << "received timestamp of " << buffer.back().mStamp); + + continue; + } + + JointStateRecord record; + record.mStamp = _jointState.header.stamp; + record.mPosition = _jointState.position[i]; + + // TODO: check that the data in buffer is sequential, i.e. that we did not + // receive JointState messages out of order. It's probably safe to ignore + // the message (and print a warning!) if this occurs. + buffer.push_back(record); + } +} + +} // namespace ros +} // namespace control +} // namespace aikido From 929dcb693469f145c40afc80b2bb69f59d74cada Mon Sep 17 00:00:00 2001 From: Pengju Jin Date: Fri, 14 Apr 2017 15:28:12 -0400 Subject: [PATCH 3/7] adding simulation world env to ApriltagsModule (#156) * added dart world * simluation world env added to apriltag module * apriltags module env header file * changed perceptionModule * changed dart simulation world * Address Mike's comments --- include/aikido/perception/AprilTagsModule.hpp | 5 +- .../aikido/perception/PerceptionModule.hpp | 4 +- src/perception/AprilTagsModule.cpp | 76 ++++++++++--------- src/planner/ompl/CRRTConnect.cpp | 8 +- 4 files changed, 49 insertions(+), 44 deletions(-) diff --git a/include/aikido/perception/AprilTagsModule.hpp b/include/aikido/perception/AprilTagsModule.hpp index 26947f29fc..d7ce0f303a 100644 --- a/include/aikido/perception/AprilTagsModule.hpp +++ b/include/aikido/perception/AprilTagsModule.hpp @@ -43,8 +43,9 @@ class AprilTagsModule : public PerceptionModule virtual ~AprilTagsModule() = default; // Documentation inherited - bool detectObjects(std::vector& skeleton_list, ros::Duration timeout = ros::Duration(0.0), ros::Time timestamp=ros::Time(0.0)) override; - + bool detectObjects(const dart::simulation::WorldPtr& env, + ros::Duration timeout = ros::Duration(0.0), + ros::Time timestamp=ros::Time(0.0)) override; private: ///The name of the ROS topic to read marker info from diff --git a/include/aikido/perception/PerceptionModule.hpp b/include/aikido/perception/PerceptionModule.hpp index 6fae0175b6..11525a5baa 100644 --- a/include/aikido/perception/PerceptionModule.hpp +++ b/include/aikido/perception/PerceptionModule.hpp @@ -20,7 +20,7 @@ class PerceptionModule{ /// \param[in] timeout The duration up to which to wait for the transform. Returns false if none of the markers get correctly transformed /// \param[in] timestamp Only detections more recent than this timestamp will be accepted. A timestamp of 0 greedily takes the first available message, and is the default behaviour. /// \return bool Returns \c false if no detection observed, or if none of the detections has a more recent timestamp than the parameter. Returns \c true otherwise. - virtual bool detectObjects(std::vector& skeleton_list, ros::Duration timeout, ros::Time timestamp) = 0; + virtual bool detectObjects(const dart::simulation::WorldPtr& env, ros::Duration timeout, ros::Time timestamp) = 0; }; @@ -28,4 +28,4 @@ class PerceptionModule{ } //namespace aikido -#endif //AIKIDO_PERCEPTION_PERCEPTIONMODULE_H \ No newline at end of file +#endif //AIKIDO_PERCEPTION_PERCEPTIONMODULE_H diff --git a/src/perception/AprilTagsModule.cpp b/src/perception/AprilTagsModule.cpp index eb85b74bc8..873756f4b9 100644 --- a/src/perception/AprilTagsModule.cpp +++ b/src/perception/AprilTagsModule.cpp @@ -33,15 +33,21 @@ AprilTagsModule::AprilTagsModule( ros::NodeHandle node, std::string markerTopic, } //================================================================================================================================ -bool AprilTagsModule::detectObjects(std::vector& skeleton_list,ros::Duration timeout, ros::Time timestamp) +bool AprilTagsModule::detectObjects(const dart::simulation::WorldPtr& env, ros::Duration timeout, ros::Time timestamp) { bool any_valid = false; tf::TransformListener listener(mNode); //Looks at all detected tags, looks up config file - //Appends new skeletons to skeleton list + //Adding new skeletons to the world env visualization_msgs::MarkerArrayConstPtr marker_message = ros::topic::waitForMessage(mMarkerTopic,mNode,timeout); + // Making sure the Apriltag Message is non empty + if(marker_message == nullptr){ + dtwarn<<"[AprilTagsModule::detectObjects] nullptr Marker Message "<markers.size()==0){ dtwarn<<"[AprilTagsModule::detectObjects] No markers on topic "<& sk const auto& marker_stamp = marker_transform.header.stamp; const auto& tag_name = marker_transform.ns; const auto& detection_frame = marker_transform.header.frame_id; - if(!timestamp.isValid() || marker_stamp < timestamp){ continue; } @@ -62,7 +67,7 @@ bool AprilTagsModule::detectObjects(std::vector& sk std::string skel_name; Eigen::Isometry3d skel_offset; dart::common::Uri skel_resource; - + ros::Time t = ros::Time(0); //check if tag name is in database if (mConfigData->getTagNameOffset(tag_name,skel_name,skel_resource,skel_offset)) { @@ -70,16 +75,16 @@ bool AprilTagsModule::detectObjects(std::vector& sk //Get orientation of marker Eigen::Isometry3d marker_pose = aikido::perception::convertROSPoseToEigen(marker_transform.pose); - - //For the frame-frame transform + //For the frame-frame transform from TF + // TODO: what happens if the TF transform is unavailable? tf::StampedTransform transform; try{ mListener.waitForTransform(mReferenceFrameId,detection_frame, - marker_stamp,timeout); + t,timeout); mListener.lookupTransform(mReferenceFrameId,detection_frame, - marker_stamp, transform); + t, transform); } catch(const tf::ExtrapolationException& ex){ dtwarn<< "[AprilTagsModule::detectObjects] TF timestamp is out-of-date compared to marker timestamp " << ex.what(); @@ -87,62 +92,59 @@ bool AprilTagsModule::detectObjects(std::vector& sk } //Get the transform as a pose matrix - Eigen::Isometry3d frame_pose = aikido::perception::convertStampedTransformToEigen(transform); + Eigen::Isometry3d frame_pose = + aikido::perception::convertStampedTransformToEigen(transform); //Compose to get actual skeleton pose Eigen::Isometry3d temp_pose = frame_pose * marker_pose; Eigen::Isometry3d skel_pose = temp_pose * skel_offset; - Eigen::Isometry3d link_offset = mReferenceLink->getWorldTransform(); skel_pose = link_offset * skel_pose; //Check if skel in skel_list //If there then update pose //If not then append skeleton + //Resolving naming conflicts by adding the marker names. skel_name.append(std::to_string(marker_transform.id)); bool is_new_skel; - //Search skeleton_list for skeleton - const auto it = std::find_if(std::begin(skeleton_list), std::end(skeleton_list), - [&](const dart::dynamics::SkeletonPtr& skeleton) - { - return skeleton->getName() == skel_name; - } - ); - + //Search the enviornment for skeleton dart::dynamics::SkeletonPtr skel_to_update; - if (it == std::end(skeleton_list)){ - //New skeleton + dart::dynamics::SkeletonPtr env_skeleton = + env -> getSkeleton(skel_name); + + if(env_skeleton == nullptr){ + // Getting the model for the new object + std::cout << "getting new thing" << std::endl; is_new_skel = true; dart::utils::DartLoader urdfLoader; skel_to_update = - urdfLoader.parseSkeleton(skel_resource,mResourceRetriever); - - if(!skel_to_update) - dtwarn<<"[AprilTagsModule::detectObjects] Failed to load skeleton for URI "<setName(skel_name); + urdfLoader.parseSkeleton(skel_resource, mResourceRetriever); - } - else{ - //Get existing skeletonPtr + if(!skel_to_update){ + dtwarn<<"[AprilTagsModule::detectObjects] Failed to load skeleton for URI "<< skel_resource.toString()<setName(skel_name); + } else { + // is_new_skel = false; - skel_to_update = *it; + skel_to_update = env_skeleton; } - dart::dynamics::Joint* jtptr; //Get root joint of skeleton if(skel_to_update->getNumDofs() > 0){ jtptr = skel_to_update->getJoint(0); - } - else{ + } else { dtwarn<< "[AprilTagsModule::detectObjects] Skeleton "<(jtptr); + dart::dynamics::FreeJoint* freejtptr = + dynamic_cast(jtptr); //Check if successful down-cast if(freejtptr == nullptr){ @@ -153,15 +155,17 @@ bool AprilTagsModule::detectObjects(std::vector& sk freejtptr->setTransform(skel_pose); if(is_new_skel){ - //Append to list - skeleton_list.push_back(skel_to_update); + //Adding new skeleton to the world env + env->addSkeleton(skel_to_update); + std::cout << "added new thing" << std::endl; } } } + if(!any_valid){ - dtwarn<< "[AprilTagsModule::detectObjects] No marker up-to-date with timestamp parameter\n"; + dtwarn << "[AprilTagsModule::detectObjects] No marker up-to-date with timestamp parameter\n"; return false; } diff --git a/src/planner/ompl/CRRTConnect.cpp b/src/planner/ompl/CRRTConnect.cpp index 807d244014..0317a6a589 100644 --- a/src/planner/ompl/CRRTConnect.cpp +++ b/src/planner/ompl/CRRTConnect.cpp @@ -178,14 +178,14 @@ CRRTConnect::solve(const ::ompl::base::PlannerTerminationCondition &ptc) { /* construct the solution path */ Motion *solution = startMotion; std::vector mpath1; - while (solution != NULL) { + while (solution != nullptr) { mpath1.push_back(solution); solution = solution->parent; } solution = goalMotion; std::vector mpath2; - while (solution != NULL) { + while (solution != nullptr) { mpath2.push_back(solution); solution = solution->parent; } @@ -228,7 +228,7 @@ void CRRTConnect::getPlannerData(::ompl::base::PlannerData &data) const { mStartTree->list(motions); for (unsigned int i = 0; i < motions.size(); ++i) { - if (motions[i]->parent == NULL) + if (motions[i]->parent == nullptr) data.addStartVertex( ::ompl::base::PlannerDataVertex(motions[i]->state, 1)); else { @@ -243,7 +243,7 @@ void CRRTConnect::getPlannerData(::ompl::base::PlannerData &data) const { mGoalTree->list(motions); for (unsigned int i = 0; i < motions.size(); ++i) { - if (motions[i]->parent == NULL) + if (motions[i]->parent == nullptr) data.addGoalVertex(::ompl::base::PlannerDataVertex(motions[i]->state, 2)); else { // The edges in the goal tree are reversed to be consistent with start From 6740dbc4612727930c44001ff581c02c017adbcd Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Fri, 14 Apr 2017 18:57:06 -0400 Subject: [PATCH 4/7] Code format: planner (#167) --- CMakeLists.txt | 2 + include/aikido/planner/PlanningResult.hpp | 16 +- include/aikido/planner/SnapPlanner.hpp | 18 +- .../planner/ompl/BackwardCompatibility.hpp | 12 +- include/aikido/planner/ompl/CRRT.hpp | 73 +++-- include/aikido/planner/ompl/CRRTConnect.hpp | 36 +-- .../planner/ompl/GeometricStateSpace.hpp | 69 +++-- include/aikido/planner/ompl/GoalRegion.hpp | 18 +- .../aikido/planner/ompl/MotionValidator.hpp | 24 +- include/aikido/planner/ompl/Planner.hpp | 125 ++++---- include/aikido/planner/ompl/StateSampler.hpp | 26 +- .../planner/ompl/StateValidityChecker.hpp | 21 +- include/aikido/planner/ompl/dart.hpp | 17 +- .../aikido/planner/ompl/detail/CRRT-impl.hpp | 7 +- .../planner/ompl/detail/CRRTConnect-impl.hpp | 12 +- .../planner/ompl/detail/Planner-impl.hpp | 81 +++-- .../planner/parabolic/ParabolicTimer.hpp | 8 +- include/aikido/trajectory/Interpolated.hpp | 2 +- include/aikido/util/StepSequence.hpp | 4 +- include/aikido/util/detail/Spline-impl.hpp | 2 +- src/planner/CMakeLists.txt | 1 + src/planner/SnapPlanner.cpp | 29 +- src/planner/ompl/CMakeLists.txt | 1 + src/planner/ompl/CRRT.cpp | 281 ++++++++++------- src/planner/ompl/CRRTConnect.cpp | 288 +++++++++++------- src/planner/ompl/GeometricStateSpace.cpp | 131 ++++---- src/planner/ompl/GoalRegion.cpp | 41 ++- src/planner/ompl/MotionValidator.cpp | 47 +-- src/planner/ompl/Planner.cpp | 247 +++++++++------ src/planner/ompl/StateSampler.cpp | 41 +-- src/planner/ompl/StateValidityChecker.cpp | 27 +- src/planner/ompl/dart.cpp | 38 ++- src/planner/parabolic/CMakeLists.txt | 1 + src/planner/parabolic/ParabolicTimer.cpp | 33 +- src/trajectory/Interpolated.cpp | 2 +- src/util/StepSequence.cpp | 4 +- src/util/VanDerCorput.cpp | 8 +- tests/gtest/CMakeLists.txt | 2 +- 38 files changed, 1061 insertions(+), 734 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index bfa868a0f0..dc5277beed 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -87,7 +87,9 @@ function(format_add_sources) endfunction() # TODO: Temporarily split header list per subdirectories. +file(GLOB_RECURSE headers_planner "${CMAKE_SOURCE_DIR}/include/aikido/planner/*.hpp") file(GLOB_RECURSE headers_util "${CMAKE_SOURCE_DIR}/include/aikido/util/*.hpp") +format_add_sources(${headers_planner}) format_add_sources(${headers_util}) #============================================================================== diff --git a/include/aikido/planner/PlanningResult.hpp b/include/aikido/planner/PlanningResult.hpp index 7798911c4e..3b7322ee9c 100644 --- a/include/aikido/planner/PlanningResult.hpp +++ b/include/aikido/planner/PlanningResult.hpp @@ -3,14 +3,16 @@ #include -namespace aikido -{ -namespace planner +namespace aikido { +namespace planner { + +struct PlanningResult { -struct PlanningResult { // TODO fill out std::string message; }; -} -} -#endif // AIKIDO_PLANNER_PLANNINGRESULT_HPP_ + +} // namespace planner +} // namespace aikido + +#endif // AIKIDO_PLANNER_PLANNINGRESULT_HPP_ diff --git a/include/aikido/planner/SnapPlanner.hpp b/include/aikido/planner/SnapPlanner.hpp index 918e21ca4f..d4ad30a252 100644 --- a/include/aikido/planner/SnapPlanner.hpp +++ b/include/aikido/planner/SnapPlanner.hpp @@ -1,8 +1,8 @@ #ifndef AIKIDO_PLANNER_SNAP_PLANNER_HPP_ #define AIKIDO_PLANNER_SNAP_PLANNER_HPP_ -#include "../statespace/StateSpace.hpp" -#include "../statespace/Interpolator.hpp" #include "../constraint/Testable.hpp" +#include "../statespace/Interpolator.hpp" +#include "../statespace/StateSpace.hpp" #include "../trajectory/Interpolated.hpp" #include "PlanningResult.hpp" @@ -23,14 +23,14 @@ namespace planner { /// \param[out] planningResult information about success or failure /// \return trajectory or \c nullptr if planning failed trajectory::InterpolatedPtr planSnap( - const std::shared_ptr& stateSpace, - const statespace::StateSpace::State *startState, - const statespace::StateSpace::State *goalState, - const std::shared_ptr& interpolator, - const std::shared_ptr& constraint, - planner::PlanningResult& planningResult); + const std::shared_ptr& stateSpace, + const statespace::StateSpace::State* startState, + const statespace::StateSpace::State* goalState, + const std::shared_ptr& interpolator, + const std::shared_ptr& constraint, + planner::PlanningResult& planningResult); } // namespace planner } // namespace aikido -#endif // AIKIDO_PLANNER_SNAP_PLANNER_HPP_ +#endif // AIKIDO_PLANNER_SNAP_PLANNER_HPP_ diff --git a/include/aikido/planner/ompl/BackwardCompatibility.hpp b/include/aikido/planner/ompl/BackwardCompatibility.hpp index ec9758cc88..03a2c1dc25 100644 --- a/include/aikido/planner/ompl/BackwardCompatibility.hpp +++ b/include/aikido/planner/ompl/BackwardCompatibility.hpp @@ -7,6 +7,8 @@ #include +// clang-format off + #define OMPL_VERSION_AT_LEAST(x,y,z) \ (OMPL_MAJOR_VERSION > x || (OMPL_MAJOR_VERSION >= x && \ (OMPL_MINOR_VERSION > y || (OMPL_MINOR_VERSION >= y && \ @@ -25,7 +27,9 @@ (OMPL_MAJOR_VERSION < x || (OMPL_MAJOR_VERSION <= x && \ (OMPL_MINOR_VERSION < y || (OMPL_MINOR_VERSION <= y)))) -#if OMPL_VERSION_AT_LEAST(1,2,0) +// clang-format on + +#if OMPL_VERSION_AT_LEAST(1, 2, 0) #include #else #include @@ -36,7 +40,7 @@ namespace aikido { namespace planner { namespace ompl { -#if OMPL_VERSION_AT_LEAST(1,2,0) +#if OMPL_VERSION_AT_LEAST(1, 2, 0) #define OMPL_PLACEHOLDER(ph) std::placeholders::ph @@ -66,7 +70,7 @@ ompl_shared_ptr ompl_static_pointer_cast(const ompl_shared_ptr& r) template auto ompl_bind(F&& f, Args&&... args) --> decltype(std::bind(std::forward(f), std::forward(args)...)) + -> decltype(std::bind(std::forward(f), std::forward(args)...)) { return std::bind(std::forward(f), std::forward(args)...); } @@ -101,7 +105,7 @@ ompl_shared_ptr ompl_static_pointer_cast(const ompl_shared_ptr& r) template auto ompl_bind(F&& f, Args&&... args) --> decltype(boost::bind(std::forward(f), std::forward(args)...)) + -> decltype(boost::bind(std::forward(f), std::forward(args)...)) { return boost::bind(std::forward(f), std::forward(args)...); } diff --git a/include/aikido/planner/ompl/CRRT.hpp b/include/aikido/planner/ompl/CRRT.hpp index b39a6a3df5..6f2d634d51 100644 --- a/include/aikido/planner/ompl/CRRT.hpp +++ b/include/aikido/planner/ompl/CRRT.hpp @@ -2,37 +2,38 @@ #define AIKIDO_PLANNER_OMPL_CRRT_HPP_ #include -#include #include +#include -#include "../../planner/ompl/BackwardCompatibility.hpp" #include "../../constraint/Projectable.hpp" +#include "../../planner/ompl/BackwardCompatibility.hpp" namespace aikido { namespace planner { namespace ompl { + /// Implements a constrained RRT planner class CRRT : public ::ompl::base::Planner { public: /// Constructor /// \param _si Information about the planning space - CRRT(const ::ompl::base::SpaceInformationPtr &_si); + CRRT(const ::ompl::base::SpaceInformationPtr& _si); /// Constructor /// \param _si Information about the planning space /// \param _name A name for this planner - CRRT(const ::ompl::base::SpaceInformationPtr &_si, const std::string &name); + CRRT(const ::ompl::base::SpaceInformationPtr& _si, const std::string& name); /// Destructor - virtual ~CRRT(void); + virtual ~CRRT(); /// Get information about the current run of the motion planner. Repeated /// calls to this function will update data (only additions are made). This is /// useful to see what changed in the exploration datastructure, between calls /// to solve(), for example (without calling clear() in between). /// \param[out] data Data about the current run of the motion planner - void getPlannerData(::ompl::base::PlannerData &_data) const override; + void getPlannerData(::ompl::base::PlannerData& _data) const override; /// Function that can solve the motion planning problem. This function can be /// called multiple times on the same problem, without calling clear() in @@ -45,7 +46,7 @@ class CRRT : public ::ompl::base::Planner /// true. /// \param ptc Conditions for terminating planning before a solution is found ::ompl::base::PlannerStatus solve( - const ::ompl::base::PlannerTerminationCondition &_ptc) override; + const ::ompl::base::PlannerTerminationCondition& _ptc) override; /// Solve the motion planning problem in the given time /// \param solveTime The maximum allowable time to solve the planning problem @@ -53,7 +54,7 @@ class CRRT : public ::ompl::base::Planner /// Clear all internal datastructures. Planner settings are not affected. /// Subsequent calls to solve() will ignore all previous work. - void clear(void) override; + void clear() override; /// Set the goal bias. In the process of randomly selecting states in the /// state space to attempt to go towards, the algorithm may in fact choose the @@ -64,7 +65,7 @@ class CRRT : public ::ompl::base::Planner void setGoalBias(double _goalBias); /// Get the goal bias the planner is using - double getGoalBias(void) const; + double getGoalBias() const; /// Set the range the planner is supposed to use. This parameter greatly /// influences the runtime of the algorithm. It represents the maximum length @@ -74,14 +75,13 @@ class CRRT : public ::ompl::base::Planner void setRange(double _distance); /// Get the range the planner is using - double getRange(void) const; + double getRange() const; /// Set a projectable constraint to be applied throughout the trajectory. /// The projection is applied at the resolution set via /// setProjectionResolution /// \param _projectable The constraint - void setPathConstraint( - constraint::ProjectablePtr _projectable); + void setPathConstraint(constraint::ProjectablePtr _projectable); /// Set the resolution for the projection. During tree extension, a projection /// back to the constraint will be performed after any step larger than this @@ -93,7 +93,7 @@ class CRRT : public ::ompl::base::Planner /// Get the resolution for the projection. During tree extension, a /// projection back to the constraint will be performed after any step /// larger than this distance. - double getProjectionResolution(void) const; + double getProjectionResolution() const; /// Set the minimum distance between two states for them to be considered /// "equivalent". This is used during extension to determine if a projection @@ -105,16 +105,16 @@ class CRRT : public ::ompl::base::Planner /// "equivalent". This is used during extension to determine if a projection /// is near enough the previous projection to say progress is no longer being /// made and quit extending. - double getMinStateDifference(void) const; + double getMinStateDifference() const; /// Set a nearest neighbors data structure template