Initial commit
This commit is contained in:
@@ -0,0 +1,210 @@
|
||||
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
Changelog for package canopen_motor_node
|
||||
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
|
||||
0.8.5 (2020-09-22)
|
||||
------------------
|
||||
|
||||
0.8.4 (2020-08-22)
|
||||
------------------
|
||||
* moved XmlRpcSettings to socketcan_interface
|
||||
* Contributors: Mathias Lüdtke
|
||||
|
||||
0.8.3 (2020-05-07)
|
||||
------------------
|
||||
* Bump CMake version to avoid CMP0048 warning
|
||||
Signed-off-by: ahcorde <ahcorde@gmail.com>
|
||||
* Contributors: ahcorde
|
||||
|
||||
0.8.2 (2019-11-04)
|
||||
------------------
|
||||
|
||||
0.8.1 (2019-07-14)
|
||||
------------------
|
||||
* Set C++ standard to c++14
|
||||
* inherit LimitsHandle from LimitsHandleBase
|
||||
* Contributors: Harsh Deshpande, Mathias Lüdtke
|
||||
|
||||
0.8.0 (2018-07-11)
|
||||
------------------
|
||||
* migrated to std::function and std::bind
|
||||
* use std::isnan
|
||||
* migrated to std::atomic
|
||||
* migrated to std::unordered_map and std::unordered_set
|
||||
* migrated to std pointers
|
||||
* removed deprecated types
|
||||
* introduced HandleLayerSharedPtr
|
||||
* Contributors: Mathias Lüdtke
|
||||
|
||||
0.7.8 (2018-05-04)
|
||||
------------------
|
||||
|
||||
0.7.7 (2018-05-04)
|
||||
------------------
|
||||
* added types for all function objects
|
||||
* added types for all shared_ptrs
|
||||
* error if muparser is not available
|
||||
* address catkin_lint errors/warnings
|
||||
* Contributors: Mathias Lüdtke
|
||||
|
||||
0.7.6 (2017-08-30)
|
||||
------------------
|
||||
|
||||
0.7.5 (2017-05-29)
|
||||
------------------
|
||||
|
||||
0.7.4 (2017-04-25)
|
||||
------------------
|
||||
* use portable boost::math::isnan
|
||||
* Contributors: Mathias Lüdtke
|
||||
|
||||
0.7.3 (2017-04-25)
|
||||
------------------
|
||||
* use urdf::JointConstSharedPtr
|
||||
* Contributors: Mathias Lüdtke
|
||||
|
||||
0.7.2 (2017-03-28)
|
||||
------------------
|
||||
|
||||
0.7.1 (2017-03-20)
|
||||
------------------
|
||||
* Decouble RobotLayer by introducing HandleLayerBase
|
||||
* Split layers into different headers and compile units
|
||||
* do not call handleReadread in HandleLayer::handleRecover
|
||||
this prevents a race condition, it is not needed anyway.
|
||||
* protect ObjectVariables with mutex
|
||||
* added test for norm function
|
||||
* fix for joint limit handling
|
||||
* introduced per-controller enforce_limits parameter
|
||||
* implemented per-joint limits handling
|
||||
* check if hardware interface matches mode
|
||||
* implemented mixed-mode switching (`#197 <https://github.com/ipa-mdl/ros_canopen/issues/197>`_)
|
||||
* introduced joint reference for *res_it
|
||||
* Contributors: Mathias Lüdtke, Michael Stoll
|
||||
|
||||
0.7.0 (2016-12-13)
|
||||
------------------
|
||||
* multi-mode controllers are not supported (`#197 <https://github.com/ros-industrial/ros_canopen/issues/197>`_)
|
||||
* Adaption to https://github.com/ros-controls/ros_control/commit/afaf9403d1daf6e7f0a93e4a06aa9695e2883632
|
||||
* Contributors: Mathias Lüdtke, Michael Stoll
|
||||
|
||||
0.6.5 (2016-12-10)
|
||||
------------------
|
||||
* protect MotorChain setup with RosChain lock
|
||||
* Merge pull request `#153 <https://github.com/ipa-mdl/ros_canopen/issues/153>`_ from ipa-mdl/deprecated-canswitch
|
||||
deprecated canSwitch
|
||||
* fix for issue `#171 <https://github.com/ipa-mdl/ros_canopen/issues/171>`_
|
||||
* Merge pull request `#168 <https://github.com/ipa-mdl/ros_canopen/issues/168>`_ from ipa-mdl/state-filters
|
||||
added filter chain for state values
|
||||
* do not start driver if filter config fails
|
||||
* added filter chain for state values
|
||||
* log control period settings
|
||||
* use update_period\_ for controll unless use_realtime_period is set true
|
||||
* better initialize last_time\_
|
||||
* removed canSwitch implementation, added compile-time check for prepareSwitch
|
||||
* exit code for generic error should be 1, not -1
|
||||
* styled and sorted CMakeLists.txt
|
||||
* removed boilerplate comments
|
||||
* indention
|
||||
* reviewed exported dependencies
|
||||
* styled and sorted package.xml
|
||||
* update package URLs
|
||||
* foward commands ony if enabled in doSwitch
|
||||
* moved switch implemenation to non-RT prepareSwitch
|
||||
* migrated to non-const prepareSwitch
|
||||
* Splitted control_node.cpp into control_node.cpp, robot_layer.cpp and robot_layer.h
|
||||
* renamed chain_ros.h to ros_chain.h, fixes `#126 <https://github.com/ipa-mdl/ros_canopen/issues/126>`_
|
||||
* added strictness to service call, extend error message for doSwitch fails
|
||||
* stop controllers that failed switching via service call
|
||||
* stop all cotnroller joints if one failed to switch
|
||||
* check for ready state before controller/mode switching
|
||||
* improved init bevaviour:
|
||||
* URDF is not read again (was not needed anyway=
|
||||
* register interfaces only of first init
|
||||
* remove unnecessary atomic reads
|
||||
* halt motor if switch failed
|
||||
* Fix for switching controllers with same mode
|
||||
* More expressive comments for compile-time check
|
||||
* Contributors: Mathias Lüdtke, Michael Stoll
|
||||
|
||||
0.6.4 (2015-07-03)
|
||||
------------------
|
||||
|
||||
0.6.3 (2015-06-30)
|
||||
------------------
|
||||
* added motor prefix to allocator entry
|
||||
* only register limit interfaces with actual limits
|
||||
* added motor_layer settings
|
||||
* Migrated to ClassAllocator helper
|
||||
* do not run controller manager on shutdown
|
||||
* migrated to motor plug-in
|
||||
* working compile-time check
|
||||
* reset commands without controllers to current value
|
||||
* got rid of getModeMask
|
||||
* added check for old unit factors
|
||||
* added closing braces in default conversion strings
|
||||
* forgot var_func assignment in constructor
|
||||
* ensured UnitConverter access to factory is valid during lifetime
|
||||
* add unit conversion based on muparser
|
||||
* dependency on muparser
|
||||
* Refer to ipa320/ros_control overlay
|
||||
* migrated to new hwi switch interface
|
||||
* atomic joint handle pointer
|
||||
* test if mode is support, add No_Mode
|
||||
* enabled limit enforcing again
|
||||
* removed debug output
|
||||
* Fixes https://github.com/ipa320/ros_canopen/issues/81
|
||||
* Enforce limits and current_state necessary for writing
|
||||
* Merge remote-tracking branch 'mdl/indigo_dev' into refactor_sm
|
||||
Conflicts:
|
||||
canopen_402/include/canopen_402/canopen_402.h
|
||||
canopen_402/src/canopen_402/canopen_402.cpp
|
||||
canopen_motor_node/src/control_node.cpp
|
||||
* refactored Layer mechanisms
|
||||
* Fixes crash for unitialized boost pointer for ``target_vel_`` and ``target_pos_``
|
||||
* MotorChain is now a template
|
||||
* early check if joint is listed in URDF
|
||||
* introduced 'joint' parameter (defaults to 'name')
|
||||
* 'modules' was renamed to 'nodes'
|
||||
* Merge branch 'indigo_dev' of https://github.com/ipa320/ros_canopen into indigo_dev
|
||||
* Merge pull request `#70 <https://github.com/ros-industrial/ros_canopen/issues/70>`_ from ipa-mdl/pluginlib
|
||||
added plugin feature to socketcan_interface
|
||||
* compile-time check for ros_control notifyHardwareInterface supportcompü
|
||||
* added driver_plugin parameter for pluginlib look-up
|
||||
* implemented threading in CANLayer
|
||||
* removed SimpleLayer, migrated to Layer
|
||||
* Layer::pending and Layer::halt are now virtual pure as well
|
||||
* * Eliminates Internal State conflict
|
||||
* Treats exceptions inside the state machine
|
||||
* keep loop running
|
||||
* proper locking for hardware interface switch (might fix `#61 <https://github.com/ros-industrial/ros_canopen/issues/61>`_)
|
||||
* Merge branch 'auto_scale' into indigo_dev
|
||||
Conflicts:
|
||||
canopen_chain_node/include/canopen_chain_node/chain_ros.h
|
||||
* Merge remote-tracking branch 'ipa320/indigo_dev' into indigo_dev
|
||||
Conflicts:
|
||||
canopen_chain_node/include/canopen_chain_node/chain_ros.h
|
||||
canopen_motor_node/src/control_node.cpp
|
||||
* removed MasterType form template
|
||||
* Merge branch 'indigo_dev' into merge
|
||||
Conflicts:
|
||||
canopen_chain_node/include/canopen_chain_node/chain_ros.h
|
||||
canopen_master/include/canopen_master/canopen.h
|
||||
canopen_master/include/canopen_master/layer.h
|
||||
canopen_master/src/node.cpp
|
||||
canopen_motor_node/CMakeLists.txt
|
||||
canopen_motor_node/src/control_node.cpp
|
||||
* added unit factor parameter parsing
|
||||
* Scale factor acquired from yaml file
|
||||
* Contributors: Mathias Lüdtke, thiagodefreitas
|
||||
|
||||
0.6.2 (2014-12-18)
|
||||
------------------
|
||||
|
||||
0.6.1 (2014-12-15)
|
||||
------------------
|
||||
* remove ipa_* and IPA_* prefixes
|
||||
* fixed catkin_lint errors
|
||||
* added descriptions and authors
|
||||
* renamed ipa_canopen_motor_control to canopen_motor_node
|
||||
* Contributors: Florian Weisshardt, Mathias Lüdtke
|
||||
@@ -0,0 +1,123 @@
|
||||
cmake_minimum_required(VERSION 3.0.2)
|
||||
project(canopen_motor_node)
|
||||
|
||||
find_package(catkin REQUIRED
|
||||
COMPONENTS
|
||||
canopen_402
|
||||
canopen_chain_node
|
||||
canopen_master
|
||||
controller_manager
|
||||
controller_manager_msgs
|
||||
filters
|
||||
hardware_interface
|
||||
joint_limits_interface
|
||||
roscpp
|
||||
urdf
|
||||
)
|
||||
|
||||
find_package(Boost REQUIRED
|
||||
COMPONENTS
|
||||
thread
|
||||
)
|
||||
|
||||
find_package(PkgConfig)
|
||||
pkg_check_modules(PC_MUPARSER QUIET muparser)
|
||||
set(MUPARSER_DEFINITIONS ${PC_MUPARSER_CFLAGS_OTHER})
|
||||
|
||||
find_path(MUPARSER_INCLUDE_DIR muParser.h
|
||||
HINTS ${PC_MUPARSER_INCLUDEDIR} ${PC_MUPARSER_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
find_library(MUPARSER_LIBRARY NAMES muparser libmurser
|
||||
HINTS ${PC_MUPARSER_LIBDIR} ${PC_MUPARSER_LIBRARY_DIRS} )
|
||||
|
||||
include(FindPackageHandleStandardArgs)
|
||||
# handle the QUIETLY and REQUIRED arguments and set MUPARSER_FOUND to TRUE
|
||||
# if all listed variables are TRUE
|
||||
find_package_handle_standard_args(MUPARSER DEFAULT_MSG
|
||||
MUPARSER_LIBRARY MUPARSER_INCLUDE_DIR)
|
||||
mark_as_advanced(MUPARSER_INCLUDE_DIR MUPARSER_LIBRARY )
|
||||
|
||||
if(NOT ${MUPARSER_FOUND})
|
||||
message(FATAL_ERROR "muparser library not found")
|
||||
endif()
|
||||
|
||||
set(MUPARSER_LIBRARIES ${MUPARSER_LIBRARY} )
|
||||
set(MUPARSER_INCLUDE_DIRS ${MUPARSER_INCLUDE_DIR} )
|
||||
|
||||
catkin_package(
|
||||
INCLUDE_DIRS
|
||||
include
|
||||
LIBRARIES
|
||||
canopen_motor
|
||||
CATKIN_DEPENDS
|
||||
canopen_402
|
||||
canopen_chain_node
|
||||
canopen_master
|
||||
controller_manager
|
||||
hardware_interface
|
||||
joint_limits_interface
|
||||
roscpp
|
||||
urdf
|
||||
DEPENDS
|
||||
Boost
|
||||
MUPARSER
|
||||
)
|
||||
|
||||
include_directories(
|
||||
include
|
||||
${catkin_INCLUDE_DIRS}
|
||||
${MUPARSER_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
# canopen_motor
|
||||
add_library(canopen_motor
|
||||
src/controller_manager_layer.cpp
|
||||
src/handle_layer.cpp
|
||||
src/motor_chain.cpp
|
||||
src/robot_layer.cpp
|
||||
)
|
||||
target_link_libraries(canopen_motor
|
||||
${catkin_LIBRARIES}
|
||||
${MUPARSER_LIBRARIES}
|
||||
)
|
||||
add_dependencies(canopen_motor
|
||||
${catkin_EXPORTED_TARGETS}
|
||||
)
|
||||
|
||||
# canopen_motor_node
|
||||
add_executable(${PROJECT_NAME}
|
||||
src/canopen_motor_chain_node.cpp
|
||||
)
|
||||
target_link_libraries(${PROJECT_NAME}
|
||||
canopen_motor
|
||||
${catkin_LIBRARIES}
|
||||
)
|
||||
add_dependencies(${PROJECT_NAME}
|
||||
${catkin_EXPORTED_TARGETS}
|
||||
)
|
||||
|
||||
install(
|
||||
TARGETS
|
||||
canopen_motor
|
||||
${PROJECT_NAME}
|
||||
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
)
|
||||
|
||||
install(DIRECTORY include/${PROJECT_NAME}/
|
||||
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
|
||||
FILES_MATCHING PATTERN "*.h"
|
||||
)
|
||||
|
||||
if(CATKIN_ENABLE_TESTING)
|
||||
catkin_add_gtest(${PROJECT_NAME}-test_muparser
|
||||
test/test_muparser.cpp
|
||||
)
|
||||
target_link_libraries(${PROJECT_NAME}-test_muparser
|
||||
canopen_motor
|
||||
${catkin_LIBRARIES}
|
||||
${MUPARSER_LIBRARIES}
|
||||
)
|
||||
endif()
|
||||
+44
@@ -0,0 +1,44 @@
|
||||
|
||||
#ifndef CANOPEN_MOTOR_NODE_CONTROLLER_MANAGER_LAYER_H_
|
||||
#define CANOPEN_MOTOR_NODE_CONTROLLER_MANAGER_LAYER_H_
|
||||
|
||||
#include <memory>
|
||||
|
||||
#include <ros/node_handle.h>
|
||||
#include <atomic>
|
||||
#include <canopen_master/canopen.h>
|
||||
#include <canopen_motor_node/robot_layer.h>
|
||||
|
||||
// forward declarations
|
||||
namespace controller_manager {
|
||||
class ControllerManager;
|
||||
}
|
||||
|
||||
namespace canopen {
|
||||
|
||||
class ControllerManagerLayer : public canopen::Layer {
|
||||
std::shared_ptr<controller_manager::ControllerManager> cm_;
|
||||
canopen::RobotLayerSharedPtr robot_;
|
||||
ros::NodeHandle nh_;
|
||||
|
||||
canopen::time_point last_time_;
|
||||
std::atomic<bool> recover_;
|
||||
const ros::Duration fixed_period_;
|
||||
|
||||
public:
|
||||
ControllerManagerLayer(const canopen::RobotLayerSharedPtr robot, const ros::NodeHandle &nh, const ros::Duration &fixed_period)
|
||||
:Layer("ControllerManager"), robot_(robot), nh_(nh), fixed_period_(fixed_period) {
|
||||
}
|
||||
|
||||
virtual void handleRead(canopen::LayerStatus &status, const LayerState ¤t_state);
|
||||
virtual void handleWrite(canopen::LayerStatus &status, const LayerState ¤t_state);
|
||||
virtual void handleDiag(canopen::LayerReport &report) { /* nothing to do */ }
|
||||
virtual void handleHalt(canopen::LayerStatus &status) { /* nothing to do (?) */ }
|
||||
virtual void handleInit(canopen::LayerStatus &status);
|
||||
virtual void handleRecover(canopen::LayerStatus &status);
|
||||
virtual void handleShutdown(canopen::LayerStatus &status);
|
||||
};
|
||||
|
||||
} // namespace canopen
|
||||
|
||||
#endif /* CANOPEN_MOTOR_NODE_CONTROLLER_MANAGER_LAYER_H_ */
|
||||
+176
@@ -0,0 +1,176 @@
|
||||
#ifndef CANOPEN_MOTOR_NODE_HANDLE_LAYER_H_
|
||||
#define CANOPEN_MOTOR_NODE_HANDLE_LAYER_H_
|
||||
|
||||
#include <memory>
|
||||
#include <unordered_map>
|
||||
|
||||
#include <atomic>
|
||||
#include <functional>
|
||||
#include <boost/thread/mutex.hpp>
|
||||
#include <ros/common.h> // for ROS_VERSION_MINIMUM
|
||||
#if ROS_VERSION_MINIMUM(1, 15, 0)
|
||||
#include <filters/filter_chain.hpp>
|
||||
#else
|
||||
#include <filters/filter_chain.h>
|
||||
#endif
|
||||
#include <hardware_interface/joint_command_interface.h>
|
||||
#include <hardware_interface/joint_state_interface.h>
|
||||
#include <joint_limits_interface/joint_limits_interface.h>
|
||||
#include <canopen_master/objdict.h>
|
||||
#include <canopen_master/layer.h>
|
||||
#include <canopen_402/base.h>
|
||||
#include <canopen_motor_node/unit_converter.h>
|
||||
#include <canopen_motor_node/handle_layer_base.h>
|
||||
|
||||
namespace canopen {
|
||||
|
||||
class LimitsHandleBase {
|
||||
public:
|
||||
virtual void enforce(const ros::Duration &period) = 0;
|
||||
virtual void reset() = 0;
|
||||
virtual ~LimitsHandleBase() = default;
|
||||
};
|
||||
typedef std::shared_ptr<LimitsHandleBase> LimitsHandleBaseSharedPtr;
|
||||
|
||||
class ObjectVariables {
|
||||
const ObjectStorageSharedPtr storage_;
|
||||
struct Getter {
|
||||
std::shared_ptr<double> val_ptr;
|
||||
std::function<bool(double&)> func;
|
||||
bool operator ()() { return func(*val_ptr); }
|
||||
template<typename T> Getter(const ObjectStorage::Entry<T> &entry): func(std::bind(&Getter::readObject<T>, entry, std::placeholders::_1)), val_ptr(new double) { }
|
||||
template<typename T> static bool readObject(ObjectStorage::Entry<T> &entry, double &res){
|
||||
T val;
|
||||
if(!entry.get(val)) return false;
|
||||
res = val;
|
||||
return true;
|
||||
}
|
||||
operator double*() const { return val_ptr.get(); }
|
||||
};
|
||||
typedef std::unordered_map<ObjectDict::Key, Getter, ObjectDict::KeyHash> GetterMap;
|
||||
GetterMap getters_;
|
||||
boost::mutex mutex_;
|
||||
public:
|
||||
template<const uint16_t dt> static double* func(ObjectVariables &list, const canopen::ObjectDict::Key &key){
|
||||
typedef typename ObjectStorage::DataType<dt>::type type;
|
||||
return list.getters_.insert(std::make_pair(key, Getter(list.storage_->entry<type>(key)))).first->second;
|
||||
}
|
||||
ObjectVariables(const ObjectStorageSharedPtr storage) : storage_(storage) {}
|
||||
bool sync(){
|
||||
boost::mutex::scoped_lock lock(mutex_);
|
||||
bool ok = true;
|
||||
for(GetterMap::iterator it = getters_.begin(); it != getters_.end(); ++it){
|
||||
ok = it->second() && ok;
|
||||
}
|
||||
return ok;
|
||||
}
|
||||
double * getVariable(const std::string &n) {
|
||||
boost::mutex::scoped_lock lock(mutex_);
|
||||
try{
|
||||
if(n.find("obj") == 0){
|
||||
canopen::ObjectDict::Key key(n.substr(3));
|
||||
GetterMap::const_iterator it = getters_.find(key);
|
||||
if(it != getters_.end()) return it->second;
|
||||
return canopen::branch_type<ObjectVariables, double * (ObjectVariables &list, const canopen::ObjectDict::Key &k)>(storage_->dict_->get(key)->data_type)(*this, key);
|
||||
}
|
||||
}
|
||||
catch( const std::exception &e){
|
||||
ROS_ERROR_STREAM("Could not find variable '" << n << "', reason: " << boost::diagnostic_information(e));
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
};
|
||||
|
||||
template<> inline double* ObjectVariables::func<canopen::ObjectDict::DEFTYPE_VISIBLE_STRING >(ObjectVariables &, const canopen::ObjectDict::Key &){ return 0; }
|
||||
template<> inline double* ObjectVariables::func<canopen::ObjectDict::DEFTYPE_OCTET_STRING >(ObjectVariables &, const canopen::ObjectDict::Key &){ return 0; }
|
||||
template<> inline double* ObjectVariables::func<canopen::ObjectDict::DEFTYPE_UNICODE_STRING >(ObjectVariables &, const canopen::ObjectDict::Key &){ return 0; }
|
||||
template<> inline double* ObjectVariables::func<canopen::ObjectDict::DEFTYPE_DOMAIN >(ObjectVariables &, const canopen::ObjectDict::Key &){ return 0; }
|
||||
|
||||
|
||||
class HandleLayer: public canopen::HandleLayerBase {
|
||||
canopen::MotorBaseSharedPtr motor_;
|
||||
double pos_, vel_, eff_;
|
||||
|
||||
double cmd_pos_, cmd_vel_, cmd_eff_;
|
||||
|
||||
ObjectVariables variables_;
|
||||
std::unique_ptr<UnitConverter> conv_target_pos_, conv_target_vel_, conv_target_eff_;
|
||||
std::unique_ptr<UnitConverter> conv_pos_, conv_vel_, conv_eff_;
|
||||
|
||||
filters::FilterChain<double> filter_pos_, filter_vel_, filter_eff_;
|
||||
XmlRpc::XmlRpcValue options_;
|
||||
|
||||
hardware_interface::JointStateHandle jsh_;
|
||||
hardware_interface::JointHandle jph_, jvh_, jeh_;
|
||||
std::atomic<hardware_interface::JointHandle*> jh_;
|
||||
std::atomic<bool> forward_command_;
|
||||
|
||||
typedef std::unordered_map< MotorBase::OperationMode,hardware_interface::JointHandle* > CommandMap;
|
||||
CommandMap commands_;
|
||||
|
||||
template <typename T> hardware_interface::JointHandle* addHandle( T &iface, hardware_interface::JointHandle *jh, const std::vector<MotorBase::OperationMode> & modes){
|
||||
|
||||
bool supported = false;
|
||||
for(size_t i=0; i < modes.size(); ++i){
|
||||
if(motor_->isModeSupported(modes[i])){
|
||||
supported = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if(!supported) return 0;
|
||||
|
||||
iface.registerHandle(*jh);
|
||||
|
||||
for(size_t i=0; i < modes.size(); ++i){
|
||||
commands_[modes[i]] = jh;
|
||||
}
|
||||
return jh;
|
||||
}
|
||||
|
||||
bool select(const canopen::MotorBase::OperationMode &m);
|
||||
std::vector<LimitsHandleBaseSharedPtr> limits_;
|
||||
bool enable_limits_;
|
||||
public:
|
||||
HandleLayer(const std::string &name, const canopen::MotorBaseSharedPtr & motor, const canopen::ObjectStorageSharedPtr storage, XmlRpc::XmlRpcValue & options);
|
||||
static double * assignVariable(const std::string &name, double * ptr, const std::string &req) { return name == req ? ptr : 0; }
|
||||
|
||||
CanSwitchResult canSwitch(const canopen::MotorBase::OperationMode &m);
|
||||
bool switchMode(const canopen::MotorBase::OperationMode &m);
|
||||
bool forwardForMode(const canopen::MotorBase::OperationMode &m);
|
||||
|
||||
void registerHandle(hardware_interface::JointStateInterface &iface){
|
||||
iface.registerHandle(jsh_);
|
||||
}
|
||||
hardware_interface::JointHandle* registerHandle(hardware_interface::PositionJointInterface &iface,
|
||||
const joint_limits_interface::JointLimits &limits,
|
||||
const joint_limits_interface::SoftJointLimits *soft_limits = 0);
|
||||
hardware_interface::JointHandle* registerHandle(hardware_interface::VelocityJointInterface &iface,
|
||||
const joint_limits_interface::JointLimits &limits,
|
||||
const joint_limits_interface::SoftJointLimits *soft_limits = 0);
|
||||
hardware_interface::JointHandle* registerHandle(hardware_interface::EffortJointInterface &iface,
|
||||
const joint_limits_interface::JointLimits &limits,
|
||||
const joint_limits_interface::SoftJointLimits *soft_limits = 0);
|
||||
|
||||
void enforceLimits(const ros::Duration &period, bool reset);
|
||||
void enableLimits(bool enable);
|
||||
|
||||
bool prepareFilters(canopen::LayerStatus &status);
|
||||
|
||||
private:
|
||||
virtual void handleRead(canopen::LayerStatus &status, const LayerState ¤t_state);
|
||||
virtual void handleWrite(canopen::LayerStatus &status, const LayerState ¤t_state);
|
||||
virtual void handleInit(canopen::LayerStatus &status);
|
||||
virtual void handleDiag(canopen::LayerReport &report) { /* nothing to do */ }
|
||||
virtual void handleShutdown(canopen::LayerStatus &status) { /* nothing to do */ }
|
||||
virtual void handleHalt(canopen::LayerStatus &status) { /* TODO */ }
|
||||
virtual void handleRecover(canopen::LayerStatus &status) { /* nothing to do */ }
|
||||
|
||||
};
|
||||
|
||||
typedef std::shared_ptr<HandleLayer> HandleLayerSharedPtr;
|
||||
|
||||
} // namespace canopen
|
||||
|
||||
|
||||
|
||||
#endif /* INCLUDE_CANOPEN_MOTOR_NODE_HANDLE_LAYER_H_ */
|
||||
+45
@@ -0,0 +1,45 @@
|
||||
|
||||
#ifndef CANOPEN_MOTOR_NODE_HANDLE_LAYER_BASE_H_
|
||||
#define CANOPEN_MOTOR_NODE_HANDLE_LAYER_BASE_H_
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <canopen_master/layer.h>
|
||||
|
||||
namespace canopen {
|
||||
|
||||
class HandleLayerBase: public canopen::Layer{
|
||||
public:
|
||||
HandleLayerBase(const std::string &name) : Layer(name) {}
|
||||
|
||||
enum CanSwitchResult{
|
||||
NotSupported,
|
||||
NotReadyToSwitch,
|
||||
ReadyToSwitch,
|
||||
NoNeedToSwitch
|
||||
};
|
||||
|
||||
virtual CanSwitchResult canSwitch(const canopen::MotorBase::OperationMode &m) = 0;
|
||||
virtual bool switchMode(const canopen::MotorBase::OperationMode &m) = 0;
|
||||
|
||||
virtual bool forwardForMode(const canopen::MotorBase::OperationMode &m) = 0;
|
||||
|
||||
virtual void registerHandle(hardware_interface::JointStateInterface &iface) = 0;
|
||||
virtual hardware_interface::JointHandle* registerHandle(hardware_interface::PositionJointInterface &iface,
|
||||
const joint_limits_interface::JointLimits &limits,
|
||||
const joint_limits_interface::SoftJointLimits *soft_limits = 0) = 0;
|
||||
virtual hardware_interface::JointHandle* registerHandle(hardware_interface::VelocityJointInterface &iface,
|
||||
const joint_limits_interface::JointLimits &limits,
|
||||
const joint_limits_interface::SoftJointLimits *soft_limits = 0) = 0;
|
||||
virtual hardware_interface::JointHandle* registerHandle(hardware_interface::EffortJointInterface &iface,
|
||||
const joint_limits_interface::JointLimits &limits,
|
||||
const joint_limits_interface::SoftJointLimits *soft_limits = 0) = 0;
|
||||
|
||||
virtual void enforceLimits(const ros::Duration &period, bool reset) = 0;
|
||||
virtual void enableLimits(bool enable) = 0;
|
||||
};
|
||||
typedef std::shared_ptr<HandleLayerBase> HandleLayerBaseSharedPtr;
|
||||
|
||||
} // namespace canopen
|
||||
|
||||
#endif /* CANOPEN_MOTOR_NODE_HANDLE_LAYER_BASE_H_ */
|
||||
+33
@@ -0,0 +1,33 @@
|
||||
|
||||
#ifndef CANOPEN_MOTOR_NODE_MOTOR_CHAIN_H_
|
||||
#define CANOPEN_MOTOR_NODE_MOTOR_CHAIN_H_
|
||||
|
||||
#include <memory>
|
||||
|
||||
#include <ros/node_handle.h>
|
||||
#include <canopen_chain_node/ros_chain.h>
|
||||
|
||||
#include <canopen_motor_node/robot_layer.h>
|
||||
#include <canopen_motor_node/controller_manager_layer.h>
|
||||
|
||||
|
||||
namespace canopen {
|
||||
|
||||
class MotorChain : public canopen::RosChain {
|
||||
ClassAllocator<canopen::MotorBase> motor_allocator_;
|
||||
std::shared_ptr< canopen::LayerGroupNoDiag<canopen::MotorBase> > motors_;
|
||||
RobotLayerSharedPtr robot_layer_;
|
||||
|
||||
std::shared_ptr<ControllerManagerLayer> cm_;
|
||||
|
||||
virtual bool nodeAdded(XmlRpc::XmlRpcValue ¶ms, const canopen::NodeSharedPtr &node, const LoggerSharedPtr &logger);
|
||||
|
||||
public:
|
||||
MotorChain(const ros::NodeHandle &nh, const ros::NodeHandle &nh_priv);
|
||||
|
||||
virtual bool setup_chain();
|
||||
};
|
||||
|
||||
} // namespace canopen
|
||||
|
||||
#endif /* INCLUDE_CANOPEN_MOTOR_NODE_MOTOR_CHAIN_H_ */
|
||||
+64
@@ -0,0 +1,64 @@
|
||||
|
||||
#ifndef CANOPEN_MOTOR_NODE_ROBOT_LAYER_H_
|
||||
#define CANOPEN_MOTOR_NODE_ROBOT_LAYER_H_
|
||||
|
||||
#include <unordered_map>
|
||||
|
||||
#include <hardware_interface/joint_command_interface.h>
|
||||
#include <hardware_interface/joint_state_interface.h>
|
||||
#include <joint_limits_interface/joint_limits_interface.h>
|
||||
#include <hardware_interface/robot_hw.h>
|
||||
#include <urdf/urdfdom_compatibility.h>
|
||||
#include <urdf/model.h>
|
||||
#include <canopen_402/base.h>
|
||||
#include <canopen_motor_node/handle_layer_base.h>
|
||||
|
||||
|
||||
namespace canopen {
|
||||
|
||||
class RobotLayer : public LayerGroupNoDiag<HandleLayerBase>, public hardware_interface::RobotHW{
|
||||
hardware_interface::JointStateInterface state_interface_;
|
||||
hardware_interface::PositionJointInterface pos_interface_;
|
||||
hardware_interface::VelocityJointInterface vel_interface_;
|
||||
hardware_interface::EffortJointInterface eff_interface_;
|
||||
|
||||
joint_limits_interface::PositionJointSoftLimitsInterface pos_soft_limits_interface_;
|
||||
joint_limits_interface::PositionJointSaturationInterface pos_saturation_interface_;
|
||||
joint_limits_interface::VelocityJointSoftLimitsInterface vel_soft_limits_interface_;
|
||||
joint_limits_interface::VelocityJointSaturationInterface vel_saturation_interface_;
|
||||
joint_limits_interface::EffortJointSoftLimitsInterface eff_soft_limits_interface_;
|
||||
joint_limits_interface::EffortJointSaturationInterface eff_saturation_interface_;
|
||||
|
||||
ros::NodeHandle nh_;
|
||||
urdf::Model urdf_;
|
||||
|
||||
typedef std::unordered_map< std::string, HandleLayerBaseSharedPtr > HandleMap;
|
||||
HandleMap handles_;
|
||||
struct SwitchData {
|
||||
HandleLayerBaseSharedPtr handle;
|
||||
canopen::MotorBase::OperationMode mode;
|
||||
bool enforce_limits;
|
||||
};
|
||||
typedef std::vector<SwitchData> SwitchContainer;
|
||||
typedef std::unordered_map<std::string, SwitchContainer> SwitchMap;
|
||||
SwitchMap switch_map_;
|
||||
|
||||
std::atomic<bool> first_init_;
|
||||
|
||||
void stopControllers(const std::vector<std::string> controllers);
|
||||
public:
|
||||
void add(const std::string &name, HandleLayerBaseSharedPtr handle);
|
||||
RobotLayer(ros::NodeHandle nh);
|
||||
urdf::JointConstSharedPtr getJoint(const std::string &n) const { return urdf_.getJoint(n); }
|
||||
|
||||
virtual void handleInit(canopen::LayerStatus &status);
|
||||
void enforce(const ros::Duration &period, bool reset);
|
||||
virtual bool prepareSwitch(const std::list<hardware_interface::ControllerInfo> &start_list, const std::list<hardware_interface::ControllerInfo> &stop_list);
|
||||
virtual void doSwitch(const std::list<hardware_interface::ControllerInfo> &start_list, const std::list<hardware_interface::ControllerInfo> &stop_list);
|
||||
};
|
||||
|
||||
typedef std::shared_ptr<RobotLayer> RobotLayerSharedPtr;
|
||||
|
||||
} // namespace canopen
|
||||
|
||||
#endif
|
||||
+89
@@ -0,0 +1,89 @@
|
||||
|
||||
#ifndef CANOPEN_MOTOR_NODE_UNIT_CONVERTER_H_
|
||||
#define CANOPEN_MOTOR_NODE_UNIT_CONVERTER_H_
|
||||
|
||||
#include <string>
|
||||
#include <list>
|
||||
#include <memory>
|
||||
|
||||
#include <functional>
|
||||
#include "muParser.h"
|
||||
|
||||
namespace canopen {
|
||||
class UnitConverter{
|
||||
public:
|
||||
typedef std::function<double * (const std::string &) > GetVarFuncType;
|
||||
|
||||
UnitConverter(const std::string &expression, GetVarFuncType var_func)
|
||||
: var_func_(var_func)
|
||||
{
|
||||
parser_.SetVarFactory(UnitConverter::createVariable, this);
|
||||
|
||||
parser_.DefineConst("pi", M_PI);
|
||||
parser_.DefineConst("nan", std::numeric_limits<double>::quiet_NaN());
|
||||
|
||||
parser_.DefineFun("rad2deg", UnitConverter::rad2deg);
|
||||
parser_.DefineFun("deg2rad", UnitConverter::deg2rad);
|
||||
parser_.DefineFun("norm", UnitConverter::norm);
|
||||
parser_.DefineFun("smooth", UnitConverter::smooth);
|
||||
parser_.DefineFun("avg", UnitConverter::avg);
|
||||
|
||||
parser_.SetExpr(expression);
|
||||
}
|
||||
|
||||
void reset(){
|
||||
for(variable_ptr_list::iterator it = var_list_.begin(); it != var_list_.end(); ++it){
|
||||
**it = std::numeric_limits<double>::quiet_NaN();
|
||||
}
|
||||
}
|
||||
double evaluate() { int num; return parser_.Eval(num)[0]; }
|
||||
private:
|
||||
typedef std::shared_ptr<double> variable_ptr;
|
||||
typedef std::list<variable_ptr> variable_ptr_list;
|
||||
|
||||
static double* createVariable(const char *name, void * userdata) {
|
||||
UnitConverter * uc = static_cast<UnitConverter*>(userdata);
|
||||
double *p = uc->var_func_ ? uc->var_func_(name) : 0;
|
||||
if(!p){
|
||||
p = new double(std::numeric_limits<double>::quiet_NaN());
|
||||
uc->var_list_.push_back(variable_ptr(p));
|
||||
}
|
||||
return p;
|
||||
}
|
||||
variable_ptr_list var_list_;
|
||||
GetVarFuncType var_func_;
|
||||
|
||||
mu::Parser parser_;
|
||||
|
||||
static double rad2deg(double r){
|
||||
return r*180.0/M_PI;
|
||||
}
|
||||
static double deg2rad(double d){
|
||||
return d*M_PI/180.0;
|
||||
}
|
||||
static double norm(double val, double min, double max){
|
||||
while(val >= max) val -= (max-min);
|
||||
while(val < min) val += (max-min);
|
||||
return val;
|
||||
}
|
||||
static double smooth(double val, double old_val, double alpha){
|
||||
if(std::isnan(val)) return 0;
|
||||
if(std::isnan(old_val)) return val;
|
||||
return alpha*val + (1.0-alpha)*old_val;
|
||||
}
|
||||
static double avg(const double *vals, int num)
|
||||
{
|
||||
double s = 0.0;
|
||||
int i=0;
|
||||
for (; i<num; ++i){
|
||||
const double &val = vals[i];
|
||||
if(std::isnan(val)) break;
|
||||
s += val;
|
||||
}
|
||||
return s / double(i+1);
|
||||
}
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif /* CANOPEN_MOTOR_NODE_UNIT_CONVERTER_H_ */
|
||||
@@ -0,0 +1,36 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>canopen_motor_node</name>
|
||||
<version>0.8.5</version>
|
||||
<description>canopen_chain_node specialization for handling of canopen_402 motor devices. It facilitates interface abstraction with ros_control.</description>
|
||||
|
||||
<maintainer email="mathias.luedtke@ipa.fraunhofer.de">Mathias Lüdtke</maintainer>
|
||||
<author email="mathias.luedtke@ipa.fraunhofer.de">Mathias Lüdtke</author>
|
||||
|
||||
<license>LGPLv3</license>
|
||||
|
||||
<url type="website">http://wiki.ros.org/canopen_motor_node</url>
|
||||
<url type="repository">https://github.com/ros-industrial/ros_canopen</url>
|
||||
<url type="bugtracker">https://github.com/ros-industrial/ros_canopen/issues</url>
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
|
||||
<depend>libboost-dev</depend>
|
||||
<depend>libboost-thread-dev</depend>
|
||||
<depend>canopen_402</depend>
|
||||
<depend>canopen_chain_node</depend>
|
||||
<depend>canopen_master</depend>
|
||||
<depend>controller_manager</depend>
|
||||
<depend>filters</depend>
|
||||
<depend>hardware_interface</depend>
|
||||
<depend>joint_limits_interface</depend>
|
||||
<depend>muparser</depend>
|
||||
<depend>roscpp</depend>
|
||||
<depend>urdf</depend>
|
||||
|
||||
<build_depend>controller_manager_msgs</build_depend>
|
||||
<exec_depend>controller_manager_msgs</exec_depend>
|
||||
|
||||
<test_depend>rosunit</test_depend>
|
||||
|
||||
</package>
|
||||
@@ -0,0 +1,22 @@
|
||||
#include <canopen_motor_node/motor_chain.h>
|
||||
|
||||
using namespace canopen;
|
||||
|
||||
|
||||
int main(int argc, char** argv){
|
||||
ros::init(argc, argv, "canopen_motor_chain_node");
|
||||
ros::AsyncSpinner spinner(0);
|
||||
spinner.start();
|
||||
|
||||
ros::NodeHandle nh;
|
||||
ros::NodeHandle nh_priv("~");
|
||||
|
||||
MotorChain chain(nh, nh_priv);
|
||||
|
||||
if(!chain.setup()){
|
||||
return 1;
|
||||
}
|
||||
|
||||
ros::waitForShutdown();
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,54 @@
|
||||
|
||||
#include <canopen_motor_node/controller_manager_layer.h>
|
||||
#include <controller_manager/controller_manager.h>
|
||||
|
||||
using namespace canopen;
|
||||
|
||||
void ControllerManagerLayer::handleRead(canopen::LayerStatus &status, const LayerState ¤t_state) {
|
||||
if(current_state > Shutdown){
|
||||
if(!cm_) status.error("controller_manager is not intialized");
|
||||
}
|
||||
}
|
||||
|
||||
void ControllerManagerLayer::handleWrite(canopen::LayerStatus &status, const LayerState ¤t_state) {
|
||||
if(current_state > Shutdown){
|
||||
if(!cm_){
|
||||
status.error("controller_manager is not intialized");
|
||||
}else{
|
||||
time_point abs_now = canopen::get_abs_time();
|
||||
ros::Time now = ros::Time::now();
|
||||
|
||||
ros::Duration period = fixed_period_;
|
||||
|
||||
if(period.isZero()) {
|
||||
period.fromSec(boost::chrono::duration<double>(abs_now -last_time_).count());
|
||||
}
|
||||
|
||||
last_time_ = abs_now;
|
||||
|
||||
bool recover = recover_.exchange(false);
|
||||
cm_->update(now, period, recover);
|
||||
robot_->enforce(period, recover);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void ControllerManagerLayer::handleInit(canopen::LayerStatus &status) {
|
||||
if(cm_){
|
||||
status.warn("controller_manager is already intialized");
|
||||
}else{
|
||||
recover_ = true;
|
||||
last_time_ = canopen::get_abs_time();
|
||||
cm_.reset(new controller_manager::ControllerManager(robot_.get(), nh_));
|
||||
}
|
||||
}
|
||||
|
||||
void ControllerManagerLayer::handleRecover(canopen::LayerStatus &status) {
|
||||
if(!cm_) status.error("controller_manager is not intialized");
|
||||
else recover_ = true;
|
||||
}
|
||||
|
||||
void ControllerManagerLayer::handleShutdown(canopen::LayerStatus &status) {
|
||||
cm_.reset();
|
||||
}
|
||||
|
||||
@@ -0,0 +1,215 @@
|
||||
|
||||
#include <canopen_motor_node/handle_layer.h>
|
||||
#include "interface_mapping.h"
|
||||
|
||||
using namespace canopen;
|
||||
|
||||
|
||||
template<typename T > class LimitsHandle : public LimitsHandleBase {
|
||||
T limits_handle_;
|
||||
public:
|
||||
LimitsHandle(const T &handle) : limits_handle_(handle) {}
|
||||
virtual void enforce(const ros::Duration &period) override { limits_handle_.enforceLimits(period); }
|
||||
virtual void reset() override {}
|
||||
};
|
||||
|
||||
template<> void LimitsHandle<joint_limits_interface::PositionJointSaturationHandle>::reset() { limits_handle_.reset(); }
|
||||
template<>void LimitsHandle<joint_limits_interface::PositionJointSoftLimitsHandle>::reset() { limits_handle_.reset(); }
|
||||
|
||||
bool HandleLayer::select(const MotorBase::OperationMode &m){
|
||||
CommandMap::iterator it = commands_.find(m);
|
||||
if(it == commands_.end()) return false;
|
||||
jh_ = it->second;
|
||||
return true;
|
||||
}
|
||||
|
||||
HandleLayer::HandleLayer(const std::string &name, const MotorBaseSharedPtr & motor, const ObjectStorageSharedPtr storage, XmlRpc::XmlRpcValue & options)
|
||||
: HandleLayerBase(name + " Handle"), motor_(motor), variables_(storage), jsh_(name, &pos_, &vel_, &eff_), jph_(jsh_, &cmd_pos_), jvh_(jsh_, &cmd_vel_), jeh_(jsh_, &cmd_eff_), jh_(0), forward_command_(false),
|
||||
filter_pos_("double"), filter_vel_("double"), filter_eff_("double"), options_(options), enable_limits_(true)
|
||||
{
|
||||
commands_[MotorBase::No_Mode] = 0;
|
||||
|
||||
std::string p2d("rint(rad2deg(pos)*1000)"), v2d("rint(rad2deg(vel)*1000)"), e2d("rint(eff)");
|
||||
std::string p2r("deg2rad(obj6064)/1000"), v2r("deg2rad(obj606C)/1000"), e2r("0");
|
||||
|
||||
if(options.hasMember("pos_unit_factor") || options.hasMember("vel_unit_factor") || options.hasMember("eff_unit_factor")){
|
||||
const std::string reason("*_unit_factor parameters are not supported anymore, please migrate to conversion functions.");
|
||||
ROS_FATAL_STREAM(reason);
|
||||
throw std::invalid_argument(reason);
|
||||
}
|
||||
|
||||
if(options.hasMember("pos_to_device")) p2d = (const std::string&) options["pos_to_device"];
|
||||
if(options.hasMember("pos_from_device")) p2r = (const std::string&) options["pos_from_device"];
|
||||
|
||||
if(options.hasMember("vel_to_device")) v2d = (const std::string&) options["vel_to_device"];
|
||||
if(options.hasMember("vel_from_device")) v2r = (const std::string&) options["vel_from_device"];
|
||||
|
||||
if(options.hasMember("eff_to_device")) e2d = (const std::string&) options["eff_to_device"];
|
||||
if(options.hasMember("eff_from_device")) e2r = (const std::string&) options["eff_from_device"];
|
||||
|
||||
conv_target_pos_.reset(new UnitConverter(p2d, std::bind(assignVariable, "pos", &cmd_pos_, std::placeholders::_1)));
|
||||
conv_target_vel_.reset(new UnitConverter(v2d, std::bind(assignVariable, "vel", &cmd_vel_, std::placeholders::_1)));
|
||||
conv_target_eff_.reset(new UnitConverter(e2d, std::bind(assignVariable, "eff", &cmd_eff_, std::placeholders::_1)));
|
||||
|
||||
conv_pos_.reset(new UnitConverter(p2r, std::bind(&ObjectVariables::getVariable, &variables_, std::placeholders::_1)));
|
||||
conv_vel_.reset(new UnitConverter(v2r, std::bind(&ObjectVariables::getVariable, &variables_, std::placeholders::_1)));
|
||||
conv_eff_.reset(new UnitConverter(e2r, std::bind(&ObjectVariables::getVariable, &variables_, std::placeholders::_1)));
|
||||
}
|
||||
|
||||
HandleLayer::CanSwitchResult HandleLayer::canSwitch(const MotorBase::OperationMode &m){
|
||||
if(!motor_->isModeSupported(m) || commands_.find(m) == commands_.end()){
|
||||
return NotSupported;
|
||||
}else if(motor_->getMode() == m){
|
||||
return NoNeedToSwitch;
|
||||
}else if(motor_->getLayerState() == Ready){
|
||||
return ReadyToSwitch;
|
||||
}else{
|
||||
return NotReadyToSwitch;
|
||||
}
|
||||
}
|
||||
|
||||
bool HandleLayer::switchMode(const MotorBase::OperationMode &m){
|
||||
if(motor_->getMode() != m){
|
||||
forward_command_ = false;
|
||||
jh_ = 0; // disconnect handle
|
||||
if(!motor_->enterModeAndWait(m)){
|
||||
ROS_ERROR_STREAM(jsh_.getName() << "could not enter mode " << (int)m);
|
||||
LayerStatus s;
|
||||
motor_->halt(s);
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return select(m);
|
||||
}
|
||||
|
||||
bool HandleLayer::forwardForMode(const MotorBase::OperationMode &m){
|
||||
if(motor_->getMode() == m){
|
||||
forward_command_ = true;
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
template<typename T> void addLimitsHandle(std::vector<LimitsHandleBaseSharedPtr> &limits, const T &t) {
|
||||
LimitsHandleBaseSharedPtr p = std::make_shared<LimitsHandle<T>>(t);
|
||||
limits.push_back(p);
|
||||
}
|
||||
|
||||
hardware_interface::JointHandle* HandleLayer::registerHandle(hardware_interface::PositionJointInterface &iface,
|
||||
const joint_limits_interface::JointLimits &limits,
|
||||
const joint_limits_interface::SoftJointLimits *soft_limits){
|
||||
hardware_interface::JointHandle* h = addHandle(iface, &jph_, g_interface_mapping.getInterfaceModes("hardware_interface::PositionJointInterface"));
|
||||
if(h && limits.has_position_limits){
|
||||
addLimitsHandle(limits_, joint_limits_interface::PositionJointSaturationHandle(*h, limits));
|
||||
if(soft_limits){
|
||||
addLimitsHandle(limits_, joint_limits_interface::PositionJointSoftLimitsHandle(*h, limits, *soft_limits));
|
||||
}
|
||||
}
|
||||
return h;
|
||||
}
|
||||
|
||||
hardware_interface::JointHandle* HandleLayer::registerHandle(hardware_interface::VelocityJointInterface &iface,
|
||||
const joint_limits_interface::JointLimits&limits,
|
||||
const joint_limits_interface::SoftJointLimits *soft_limits){
|
||||
hardware_interface::JointHandle* h = addHandle(iface, &jvh_, g_interface_mapping.getInterfaceModes("hardware_interface::VelocityJointInterface"));
|
||||
if(h && limits.has_velocity_limits){
|
||||
addLimitsHandle(limits_, joint_limits_interface::VelocityJointSaturationHandle(*h, limits));
|
||||
if(soft_limits){
|
||||
addLimitsHandle(limits_, joint_limits_interface::VelocityJointSoftLimitsHandle(*h, limits, *soft_limits));
|
||||
}
|
||||
}
|
||||
return h;
|
||||
}
|
||||
|
||||
hardware_interface::JointHandle* HandleLayer::registerHandle(hardware_interface::EffortJointInterface &iface,
|
||||
const joint_limits_interface::JointLimits&limits,
|
||||
const joint_limits_interface::SoftJointLimits *soft_limits){
|
||||
hardware_interface::JointHandle* h = addHandle(iface, &jeh_, g_interface_mapping.getInterfaceModes("hardware_interface::EffortJointInterface"));
|
||||
if(h && limits.has_effort_limits){
|
||||
addLimitsHandle(limits_, joint_limits_interface::EffortJointSaturationHandle(*h, limits));
|
||||
if(soft_limits){
|
||||
addLimitsHandle(limits_, joint_limits_interface::EffortJointSoftLimitsHandle(*h, limits, *soft_limits));
|
||||
}
|
||||
}
|
||||
return h;
|
||||
}
|
||||
|
||||
void HandleLayer::handleRead(LayerStatus &status, const LayerState ¤t_state) {
|
||||
if(current_state > Shutdown){
|
||||
variables_.sync();
|
||||
filter_pos_.update(conv_pos_->evaluate(), pos_);
|
||||
filter_vel_.update(conv_vel_->evaluate(), vel_);
|
||||
filter_eff_.update(conv_eff_->evaluate(), eff_);
|
||||
}
|
||||
}
|
||||
void HandleLayer::handleWrite(LayerStatus &status, const LayerState ¤t_state) {
|
||||
if(current_state == Ready){
|
||||
hardware_interface::JointHandle* jh = 0;
|
||||
if(forward_command_) jh = jh_;
|
||||
|
||||
if(jh == &jph_){
|
||||
motor_->setTarget(conv_target_pos_->evaluate());
|
||||
cmd_vel_ = vel_;
|
||||
cmd_eff_ = eff_;
|
||||
}else if(jh == &jvh_){
|
||||
motor_->setTarget(conv_target_vel_->evaluate());
|
||||
cmd_pos_ = pos_;
|
||||
cmd_eff_ = eff_;
|
||||
}else if(jh == &jeh_){
|
||||
motor_->setTarget(conv_target_eff_->evaluate());
|
||||
cmd_pos_ = pos_;
|
||||
cmd_vel_ = vel_;
|
||||
}else{
|
||||
cmd_pos_ = pos_;
|
||||
cmd_vel_ = vel_;
|
||||
cmd_eff_ = eff_;
|
||||
if(jh) status.warn("unsupported mode active");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool prepareFilter(const std::string& joint_name, const std::string& filter_name, filters::FilterChain<double> &filter, XmlRpc::XmlRpcValue & options, canopen::LayerStatus &status){
|
||||
filter.clear();
|
||||
if(options.hasMember(filter_name)){
|
||||
if(!filter.configure(options[filter_name],joint_name + "/" + filter_name)){
|
||||
status.error("could not configure " + filter_name+ " for " + joint_name);
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool HandleLayer::prepareFilters(canopen::LayerStatus &status){
|
||||
return prepareFilter(jsh_.getName(), "position_filters", filter_pos_, options_, status) &&
|
||||
prepareFilter(jsh_.getName(), "velocity_filters", filter_vel_, options_, status) &&
|
||||
prepareFilter(jsh_.getName(), "effort_filters", filter_eff_, options_, status);
|
||||
}
|
||||
|
||||
void HandleLayer::handleInit(LayerStatus &status){
|
||||
// TODO: implement proper init
|
||||
conv_pos_->reset();
|
||||
conv_vel_->reset();
|
||||
conv_eff_->reset();
|
||||
conv_target_pos_->reset();
|
||||
conv_target_vel_->reset();
|
||||
conv_target_eff_->reset();
|
||||
|
||||
|
||||
if(prepareFilters(status))
|
||||
{
|
||||
handleRead(status, Layer::Ready);
|
||||
}
|
||||
}
|
||||
|
||||
void HandleLayer::enforceLimits(const ros::Duration &period, bool reset){
|
||||
for(std::vector<LimitsHandleBaseSharedPtr>::iterator it = limits_.begin(); it != limits_.end(); ++it){
|
||||
if(reset) (*it)->reset();
|
||||
if(enable_limits_) (*it)->enforce(period);
|
||||
}
|
||||
}
|
||||
|
||||
void HandleLayer::enableLimits(bool enable){
|
||||
enable_limits_ = enable;
|
||||
}
|
||||
@@ -0,0 +1,48 @@
|
||||
|
||||
#ifndef INTERFACE_MAPPING_H_
|
||||
#define INTERFACE_MAPPING_H_
|
||||
|
||||
#include <string>
|
||||
|
||||
#include <boost/bimap.hpp>
|
||||
#include <boost/bimap/multiset_of.hpp>
|
||||
#include <boost/foreach.hpp>
|
||||
|
||||
#include <canopen_402/base.h>
|
||||
|
||||
class InterfaceMapping {
|
||||
typedef boost::bimap<boost::bimaps::multiset_of<std::string>, boost::bimaps::set_of<canopen::MotorBase::OperationMode> > bimap_type;
|
||||
bimap_type mapping_;
|
||||
public:
|
||||
InterfaceMapping(){
|
||||
mapping_.insert(bimap_type::value_type("hardware_interface::PositionJointInterface" ,canopen::MotorBase::Profiled_Position));
|
||||
mapping_.insert(bimap_type::value_type("hardware_interface::PositionJointInterface" ,canopen::MotorBase::Interpolated_Position));
|
||||
mapping_.insert(bimap_type::value_type("hardware_interface::PositionJointInterface" ,canopen::MotorBase::Cyclic_Synchronous_Position));
|
||||
|
||||
mapping_.insert(bimap_type::value_type("hardware_interface::VelocityJointInterface" ,canopen::MotorBase::Velocity));
|
||||
mapping_.insert(bimap_type::value_type("hardware_interface::VelocityJointInterface" ,canopen::MotorBase::Profiled_Velocity));
|
||||
mapping_.insert(bimap_type::value_type("hardware_interface::VelocityJointInterface" ,canopen::MotorBase::Cyclic_Synchronous_Velocity));
|
||||
|
||||
mapping_.insert(bimap_type::value_type("hardware_interface::EffortJointInterface" ,canopen::MotorBase::Profiled_Torque));
|
||||
mapping_.insert(bimap_type::value_type("hardware_interface::EffortJointInterface" ,canopen::MotorBase::Cyclic_Synchronous_Torque));
|
||||
}
|
||||
std::vector<canopen::MotorBase::OperationMode> getInterfaceModes(const std::string &interface){
|
||||
std::vector<canopen::MotorBase::OperationMode> modes;
|
||||
BOOST_FOREACH(bimap_type::left_reference i, mapping_.left.equal_range(interface)){
|
||||
modes.push_back(i.second);
|
||||
}
|
||||
return modes;
|
||||
}
|
||||
bool hasConflict(const std::string &interface, canopen::MotorBase::OperationMode mode){
|
||||
bimap_type::right_const_iterator it;
|
||||
if((it = mapping_.right.find(mode)) != mapping_.right.end()){
|
||||
return it->second != interface;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
extern InterfaceMapping g_interface_mapping;
|
||||
|
||||
#endif /* INTERFACE_MAPPING_H_ */
|
||||
@@ -0,0 +1,84 @@
|
||||
|
||||
#include <canopen_motor_node/motor_chain.h>
|
||||
#include <canopen_motor_node/handle_layer.h>
|
||||
#include <socketcan_interface/xmlrpc_settings.h>
|
||||
using namespace canopen;
|
||||
|
||||
MotorChain::MotorChain(const ros::NodeHandle &nh, const ros::NodeHandle &nh_priv) :
|
||||
RosChain(nh, nh_priv), motor_allocator_("canopen_402", "canopen::MotorBase::Allocator") {}
|
||||
|
||||
bool MotorChain::nodeAdded(XmlRpc::XmlRpcValue ¶ms, const canopen::NodeSharedPtr &node, const LoggerSharedPtr &logger)
|
||||
{
|
||||
std::string name = params["name"];
|
||||
std::string &joint = name;
|
||||
if(params.hasMember("joint")) joint.assign(params["joint"]);
|
||||
|
||||
if(!robot_layer_->getJoint(joint)){
|
||||
ROS_ERROR_STREAM("joint " + joint + " was not found in URDF");
|
||||
return false;
|
||||
}
|
||||
|
||||
std::string alloc_name = "canopen::Motor402::Allocator";
|
||||
if(params.hasMember("motor_allocator")) alloc_name.assign(params["motor_allocator"]);
|
||||
|
||||
XmlRpcSettings settings;
|
||||
if(params.hasMember("motor_layer")) settings = params["motor_layer"];
|
||||
|
||||
MotorBaseSharedPtr motor;
|
||||
|
||||
try{
|
||||
motor = motor_allocator_.allocateInstance(alloc_name, name + "_motor", node->getStorage(), settings);
|
||||
}
|
||||
catch( const std::exception &e){
|
||||
std::string info = boost::diagnostic_information(e);
|
||||
ROS_ERROR_STREAM(info);
|
||||
return false;
|
||||
}
|
||||
|
||||
if(!motor){
|
||||
ROS_ERROR_STREAM("Could not allocate motor.");
|
||||
return false;
|
||||
}
|
||||
|
||||
motor->registerDefaultModes(node->getStorage());
|
||||
motors_->add(motor);
|
||||
logger->add(motor);
|
||||
|
||||
HandleLayerSharedPtr handle = std::make_shared<HandleLayer>(joint, motor, node->getStorage(), params);
|
||||
|
||||
canopen::LayerStatus s;
|
||||
if(!handle->prepareFilters(s)){
|
||||
ROS_ERROR_STREAM(s.reason());
|
||||
return false;
|
||||
}
|
||||
|
||||
robot_layer_->add(joint, handle);
|
||||
logger->add(handle);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool MotorChain::setup_chain() {
|
||||
motors_.reset(new LayerGroupNoDiag<MotorBase>("402 Layer"));
|
||||
robot_layer_.reset(new RobotLayer(nh_));
|
||||
|
||||
ros::Duration dur(0.0) ;
|
||||
|
||||
if(RosChain::setup_chain()){
|
||||
add(motors_);
|
||||
add(robot_layer_);
|
||||
|
||||
if(!nh_.param("use_realtime_period", false)){
|
||||
dur.fromSec(boost::chrono::duration<double>(update_duration_).count());
|
||||
ROS_INFO_STREAM("Using fixed control period: " << dur);
|
||||
}else{
|
||||
ROS_INFO("Using real-time control period");
|
||||
}
|
||||
cm_.reset(new ControllerManagerLayer(robot_layer_, nh_, dur));
|
||||
add(cm_);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
@@ -0,0 +1,246 @@
|
||||
|
||||
|
||||
#include <joint_limits_interface/joint_limits.h>
|
||||
#include <joint_limits_interface/joint_limits_urdf.h>
|
||||
#include <joint_limits_interface/joint_limits_rosparam.h>
|
||||
|
||||
#include <controller_manager/controller_manager.h>
|
||||
#include <controller_manager_msgs/SwitchController.h>
|
||||
|
||||
#include <canopen_motor_node/robot_layer.h>
|
||||
|
||||
#include "interface_mapping.h"
|
||||
|
||||
using namespace canopen;
|
||||
|
||||
InterfaceMapping g_interface_mapping;
|
||||
|
||||
void RobotLayer::stopControllers(const std::vector<std::string> controllers){
|
||||
controller_manager_msgs::SwitchController srv;
|
||||
srv.request.stop_controllers = controllers;
|
||||
srv.request.strictness = srv.request.BEST_EFFORT;
|
||||
boost::thread call(std::bind(ros::service::call<controller_manager_msgs::SwitchController>, "controller_manager/switch_controller", srv));
|
||||
call.detach();
|
||||
}
|
||||
|
||||
void RobotLayer::add(const std::string &name, HandleLayerBaseSharedPtr handle){
|
||||
LayerGroupNoDiag::add(handle);
|
||||
handles_.insert(std::make_pair(name, handle));
|
||||
}
|
||||
|
||||
RobotLayer::RobotLayer(ros::NodeHandle nh) : LayerGroupNoDiag<HandleLayerBase>("RobotLayer"), nh_(nh), first_init_(true)
|
||||
{
|
||||
registerInterface(&state_interface_);
|
||||
registerInterface(&pos_interface_);
|
||||
registerInterface(&vel_interface_);
|
||||
registerInterface(&eff_interface_);
|
||||
|
||||
urdf_.initParam("robot_description");
|
||||
}
|
||||
|
||||
void RobotLayer::handleInit(LayerStatus &status){
|
||||
if(first_init_){
|
||||
for(HandleMap::iterator it = handles_.begin(); it != handles_.end(); ++it){
|
||||
joint_limits_interface::JointLimits limits;
|
||||
joint_limits_interface::SoftJointLimits soft_limits;
|
||||
|
||||
urdf::JointConstSharedPtr joint = getJoint(it->first);
|
||||
|
||||
if(!joint){
|
||||
status.error("joint " + it->first + " not found");
|
||||
return;
|
||||
}
|
||||
|
||||
bool has_joint_limits = joint_limits_interface::getJointLimits(joint, limits);
|
||||
|
||||
has_joint_limits = joint_limits_interface::getJointLimits(it->first, nh_, limits) || has_joint_limits;
|
||||
|
||||
bool has_soft_limits = has_joint_limits && joint_limits_interface::getSoftJointLimits(joint, soft_limits);
|
||||
|
||||
if(!has_joint_limits){
|
||||
ROS_WARN_STREAM("No limits found for " << it->first);
|
||||
}
|
||||
|
||||
it->second->registerHandle(state_interface_);
|
||||
|
||||
const hardware_interface::JointHandle *h = 0;
|
||||
|
||||
it->second->registerHandle(pos_interface_, limits, has_soft_limits ? &soft_limits : 0);
|
||||
it->second->registerHandle(vel_interface_, limits, has_soft_limits ? &soft_limits : 0);
|
||||
it->second->registerHandle(eff_interface_, limits, has_soft_limits ? &soft_limits : 0);
|
||||
}
|
||||
first_init_ = false;
|
||||
}
|
||||
LayerGroupNoDiag::handleInit(status);
|
||||
}
|
||||
|
||||
void RobotLayer::enforce(const ros::Duration &period, bool reset){
|
||||
for(HandleMap::iterator it = handles_.begin(); it != handles_.end(); ++it){
|
||||
it->second->enforceLimits(period, reset);
|
||||
}
|
||||
}
|
||||
class ModeLookup {
|
||||
int default_mode_;
|
||||
bool has_default_mode_;
|
||||
std::map<std::string, int> lookup_;
|
||||
bool has_lookup_;
|
||||
public:
|
||||
ModeLookup(ros::NodeHandle &nh_c){
|
||||
has_default_mode_ = nh_c.getParam("required_drive_mode", default_mode_);
|
||||
has_lookup_ = nh_c.getParam("required_drive_modes", lookup_);
|
||||
}
|
||||
bool hasModes() { return has_default_mode_ || has_lookup_; }
|
||||
bool hasMixedModes() { return has_lookup_; }
|
||||
bool getMode(MotorBase::OperationMode &om, const std::string &key) {
|
||||
std::map<std::string, int>::iterator f = lookup_.find(key);
|
||||
if(f != lookup_.end()){
|
||||
om = MotorBase::OperationMode(f->second);
|
||||
return true;
|
||||
}else if (has_default_mode_) {
|
||||
om = MotorBase::OperationMode(default_mode_);
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
};
|
||||
bool RobotLayer::prepareSwitch(const std::list<hardware_interface::ControllerInfo> &start_list, const std::list<hardware_interface::ControllerInfo> &stop_list) {
|
||||
// compile-time check for mode switching support in ros_control
|
||||
(void) &hardware_interface::RobotHW::prepareSwitch; // please upgrade to ros_control/contoller_manager 0.9.4 or newer
|
||||
|
||||
// stop handles
|
||||
for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it = stop_list.begin(); controller_it != stop_list.end(); ++controller_it){
|
||||
|
||||
if(switch_map_.find(controller_it->name) == switch_map_.end()){
|
||||
ROS_ERROR_STREAM(controller_it->name << " was not started before");
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
// start handles
|
||||
for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it = start_list.begin(); controller_it != start_list.end(); ++controller_it){
|
||||
SwitchContainer to_switch;
|
||||
ros::NodeHandle nh(nh_, controller_it->name);
|
||||
ModeLookup ml(nh);
|
||||
|
||||
std::set<std::string> claimed_interfaces;
|
||||
|
||||
if(controller_it->claimed_resources.size() > 0){
|
||||
for (std::vector<hardware_interface::InterfaceResources>::const_iterator cres_it = controller_it->claimed_resources.begin(); cres_it != controller_it->claimed_resources.end(); ++cres_it){
|
||||
for (std::set<std::string>::const_iterator res_it = cres_it->resources.begin(); res_it != cres_it->resources.end(); ++res_it){
|
||||
claimed_interfaces.insert(cres_it->hardware_interface);
|
||||
if(!ml.hasModes()){
|
||||
ROS_ERROR_STREAM("Please set required_drive_mode(s) for controller " << controller_it->name);
|
||||
return false;
|
||||
}
|
||||
if(claimed_interfaces.size() > 1 && !ml.hasMixedModes()){
|
||||
ROS_ERROR_STREAM("controller "<< controller_it->name << " has mixed interfaces, please set required_drive_modes.");
|
||||
return false;
|
||||
}
|
||||
|
||||
std::unordered_map< std::string, HandleLayerBaseSharedPtr >::const_iterator h_it = handles_.find(*res_it);
|
||||
|
||||
const std::string & joint = *res_it;
|
||||
|
||||
if(h_it == handles_.end()){
|
||||
ROS_ERROR_STREAM(joint << " not found");
|
||||
return false;
|
||||
}
|
||||
SwitchData sd;
|
||||
sd.enforce_limits = nh.param("enforce_limits", true);
|
||||
|
||||
if(!ml.getMode(sd.mode, joint)){
|
||||
ROS_ERROR_STREAM("could not determine drive mode for " << joint);
|
||||
return false;
|
||||
}
|
||||
|
||||
if(g_interface_mapping.hasConflict(cres_it->hardware_interface, sd.mode)){
|
||||
ROS_ERROR_STREAM(cres_it->hardware_interface << " cannot be provided in mode " << sd.mode);
|
||||
return false;
|
||||
}
|
||||
|
||||
HandleLayerBase::CanSwitchResult res = h_it->second->canSwitch(sd.mode);
|
||||
|
||||
switch(res){
|
||||
case HandleLayerBase::NotSupported:
|
||||
ROS_ERROR_STREAM("Mode " << sd.mode << " is not available for " << joint);
|
||||
return false;
|
||||
case HandleLayerBase::NotReadyToSwitch:
|
||||
ROS_ERROR_STREAM(joint << " is not ready to switch mode");
|
||||
return false;
|
||||
case HandleLayerBase::ReadyToSwitch:
|
||||
case HandleLayerBase::NoNeedToSwitch:
|
||||
sd.handle = h_it->second;
|
||||
to_switch.push_back(sd);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
switch_map_.insert(std::make_pair(controller_it->name, to_switch));
|
||||
}
|
||||
|
||||
// perform mode switches
|
||||
std::unordered_set<HandleLayerBaseSharedPtr > to_stop;
|
||||
std::vector<std::string> failed_controllers;
|
||||
for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it = stop_list.begin(); controller_it != stop_list.end(); ++controller_it){
|
||||
SwitchContainer &to_switch = switch_map_.at(controller_it->name);
|
||||
for(RobotLayer::SwitchContainer::iterator it = to_switch.begin(); it != to_switch.end(); ++it){
|
||||
to_stop.insert(it->handle);
|
||||
}
|
||||
}
|
||||
for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it = start_list.begin(); controller_it != start_list.end(); ++controller_it){
|
||||
SwitchContainer &to_switch = switch_map_.at(controller_it->name);
|
||||
bool okay = true;
|
||||
for(RobotLayer::SwitchContainer::iterator it = to_switch.begin(); it != to_switch.end(); ++it){
|
||||
it->handle->switchMode(MotorBase::No_Mode); // stop all
|
||||
}
|
||||
for(RobotLayer::SwitchContainer::iterator it = to_switch.begin(); it != to_switch.end(); ++it){
|
||||
if(!it->handle->switchMode(it->mode)){
|
||||
failed_controllers.push_back(controller_it->name);
|
||||
ROS_ERROR_STREAM("Could not switch one joint for " << controller_it->name << ", will stop all related joints and the controller.");
|
||||
for(RobotLayer::SwitchContainer::iterator stop_it = to_switch.begin(); stop_it != to_switch.end(); ++stop_it){
|
||||
to_stop.insert(stop_it->handle);
|
||||
}
|
||||
okay = false;
|
||||
break;
|
||||
}else{
|
||||
it->handle->enableLimits(it->enforce_limits);
|
||||
}
|
||||
to_stop.erase(it->handle);
|
||||
}
|
||||
}
|
||||
for(std::unordered_set<HandleLayerBaseSharedPtr >::iterator it = to_stop.begin(); it != to_stop.end(); ++it){
|
||||
(*it)->switchMode(MotorBase::No_Mode);
|
||||
}
|
||||
if(!failed_controllers.empty()){
|
||||
stopControllers(failed_controllers);
|
||||
// will not return false here since this would prevent other controllers to be started and therefore lead to an inconsistent state
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void RobotLayer::doSwitch(const std::list<hardware_interface::ControllerInfo> &start_list, const std::list<hardware_interface::ControllerInfo> &stop_list) {
|
||||
std::vector<std::string> failed_controllers;
|
||||
for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it = start_list.begin(); controller_it != start_list.end(); ++controller_it){
|
||||
try{
|
||||
SwitchContainer &to_switch = switch_map_.at(controller_it->name);
|
||||
for(RobotLayer::SwitchContainer::iterator it = to_switch.begin(); it != to_switch.end(); ++it){
|
||||
if(!it->handle->forwardForMode(it->mode)){
|
||||
failed_controllers.push_back(controller_it->name);
|
||||
ROS_ERROR_STREAM("Could not switch one joint for " << controller_it->name << ", will stop all related joints and the controller.");
|
||||
for(RobotLayer::SwitchContainer::iterator stop_it = to_switch.begin(); stop_it != to_switch.end(); ++stop_it){
|
||||
it->handle->switchMode(MotorBase::No_Mode);
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
}catch(const std::out_of_range&){
|
||||
ROS_ERROR_STREAM("Conttroller " << controller_it->name << "not found, will stop it");
|
||||
failed_controllers.push_back(controller_it->name);
|
||||
}
|
||||
}
|
||||
if(!failed_controllers.empty()){
|
||||
stopControllers(failed_controllers);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,29 @@
|
||||
#include <gtest/gtest.h>
|
||||
#include <canopen_motor_node/unit_converter.h>
|
||||
#include <canopen_motor_node/handle_layer.h>
|
||||
#include <functional>
|
||||
|
||||
using namespace canopen;
|
||||
|
||||
double * mapVariable(const std::string &, double *p) {
|
||||
return p;
|
||||
}
|
||||
|
||||
TEST(TestMuparser, CheckNorm){
|
||||
double input = 0;
|
||||
UnitConverter uc("norm(in,-1000,1000)", std::bind(HandleLayer::assignVariable, std::placeholders::_1, &input, "in"));
|
||||
input = 0; EXPECT_EQ(0, uc.evaluate());
|
||||
input = 10; EXPECT_EQ(10, uc.evaluate());
|
||||
input = -10; EXPECT_EQ(-10, uc.evaluate());
|
||||
input = 1000; EXPECT_EQ(-1000, uc.evaluate());
|
||||
input = 1001; EXPECT_EQ(-999, uc.evaluate());
|
||||
input = 2000; EXPECT_EQ(0, uc.evaluate());
|
||||
input = 2001; EXPECT_EQ(1, uc.evaluate());
|
||||
input = -1000; EXPECT_EQ(-1000, uc.evaluate());
|
||||
input = 999; EXPECT_EQ(999, uc.evaluate());
|
||||
}
|
||||
|
||||
int main(int argc, char **argv){
|
||||
testing::InitGoogleTest(&argc, argv);
|
||||
return RUN_ALL_TESTS();
|
||||
}
|
||||
Reference in New Issue
Block a user