HiepLM/AMR_T800
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

git commit -m "first commit for v2"

Browse Source
This commit is contained in:
HiepLM
2025-12-29 16:21:22 +07:00
commit aa3d832d5c
1807 changed files with 307078 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

233
Devices/Libraries/Ros/ros_canopen/canopen_402/CHANGELOG.rst Executable file
View File

@@ -0,0 +1,233 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package canopen_402
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.8.5 (2020-09-22)
------------------
0.8.4 (2020-08-22)
------------------
* handle illegal states in nextStateForEnabling
* tranistion -> transition
* Contributors: Mathias Lüdtke, Mikael Arguedas
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)
------------------
* enable rosconsole_bridge bindings
* switch to new logging macros
* Contributors: Mathias Lüdtke
0.8.1 (2019-07-14)
------------------
* Set C++ standard to c++14
* Contributors: Harsh Deshpande
0.8.0 (2018-07-11)
------------------
* handle invalid supported drive modes object
* made Mode402::registerMode a variadic template
* use std::isnan
* migrated to std::atomic
* migrated to std::unordered_map and std::unordered_set
* migrated to std pointers
* fix initialization bug in ProfiledPositionMode
* Contributors: Mathias Lüdtke
0.7.8 (2018-05-04)
------------------
* Revert "pull make_shared into namespaces"
This reverts commit 9b2cd05df76d223647ca81917d289ca6330cdee6.
* Contributors: Mathias Lüdtke
0.7.7 (2018-05-04)
------------------
* Added state_switch_timeout parameter to motor.
* added types for all function objects
* pull make_shared into namespaces
* added types for all shared_ptrs
* migrate to new classloader headers
* address catkin_lint errors/warnings
* Contributors: Alexander Gutenkunst, 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)
------------------
0.7.2 (2017-03-28)
------------------
0.7.1 (2017-03-20)
------------------
* do quickstop for halt only if operation is enabled
* Contributors: Mathias Lüdtke
0.7.0 (2016-12-13)
------------------
0.6.5 (2016-12-10)
------------------
* stop on internal limit only if limit was not reached before
* hardened code with the help of cppcheck
* Do not send control if it was not changed
Otherwise invalid state machine transitions might get commanded
* styled and sorted CMakeLists.txt
* removed boilerplate comments
* indention
* reviewed exported dependencies
* styled and sorted package.xml
* update package URLs
* added option to turn off mode monitor
* reset Fault_Reset bit in output only
* enforce rising edge on fault reset bit on init and recover
* Revert "Enforce rising edge on fault reset bit on init and recover"
* enforce rising edge on fault reset bit on init and recover
* Merge pull request `#117 <https://github.com/ipa-mdl/ros_canopen/issues/117>`_ from ipa-mdl/condvars
Reviewed condition variables
* use boost::numeric_cast for limit checks since it handles 64bit values right
* add clamping test
* enforce target type limits
* ModeTargetHelper is now template
* readability fix
* simplified State402::waitForNewState and Motor402::switchState
* Set Halt bit unless current mode releases it
* Contributors: Mathias Lüdtke
0.6.4 (2015-07-03)
------------------
0.6.3 (2015-06-30)
------------------
* improved PP mode
* do not quickstop in fault states
* do not diable selected mode on recover, just restart it
* initialize to No_Mode
* removed some empty lines
* implemented support for mode switching in a certain 402 state, defaults to Operation_Enable
* pass LayerStatus to switchMode
* remove enableMotor, introduced switchState
* added motor_layer settings
* fixed PP mode
* explicit find for class_loader
* fail-safe setting of op_mode to No_Mode
* Improved behaviour of concurrent commands
* added alternative Transitions 7 and 10 via Quickstop
* added alternative success condition to waitForNewState
* make motor init/recover interruptable
* changed maintainer
* removed SM-based 402 implementation
* added Motor402 plugin
* added new 402 implementation
* added MotorBase
* Added validity checks
* Removed overloaded functions and makes the handle functions protected
* Removes test executable
* Removes unnecessary configure_drive and clears the set Targets
* Some position fixes
* Removed timeout
Reduced recover trials
* Removes some logs
* Homing integrated
* handleRead, handleWrite fixes
* 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
* Moved supported_drive_modes to ModeSpecificEntries
* * Init, Recover, Halt for SCHUNK
* Removed sleeps from the state machine
* Now works as reentrant states
* refactored Layer mechanisms
* Recover failure
* 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
* Removing some unnecessary couts
* First version with Recover
* Tested on SCHUNK LWA4D
* Initializing all modules at once
* Moving SCHUNK using the IP mode sub-state machine
* Schunk does not set operation mode via synchronized RPDO
* initialize homing_needed to false
* Working with the guard handling and some scoped_locks to prevent unwanted access
* Passing ``state_`` to motor machine
* Fixes crash for unitialized boost pointer for ``target_vel_`` and ``target_pos_``
* Thread running
* Separates the hw with the SM test
Advance on the Mode Switching Machine
* Organizing IPMode State Machine
* Adds mode switch and a pre-version of the turnOn sequence
* Event argument passed to the Motor state machine
* Adds the internal actions
* Control_word is set from motor state machine
* Motor abstraction on higher level machine
Some pointers organization
* * Begins with the Higher Level Machine
* Separates the status and control from the 402 node
* Ip mode sub-machine
* Organizing the status and control machine
* do not read homing method if homing mode is not supported
* inti ``enter_mode_failure_`` to false
* require message strings for error indicators, added missing strings, added ROS logging in sync loop
* Merge pull request `#75 <https://github.com/ros-industrial/ros_canopen/issues/75>`_ from mistoll/indigo_release_candidate
Move ip_mode_sub_mode to configureModeSpecificEntries
* Fixed tabs/spaces
* bind IP sub mode only if IP is supported
* Move ip_mode_sub_mode to configureModeSpecificEntries
* Update state_machine.h
* Ongoing changes for the state machine
* * Eliminates Internal State conflict
* Treats exceptions inside the state machine
* Cleaning the 402.cpp file
* Test file
* Adds state machine definition
* Adds state machine simple test
* Some cleaning necessary to proceed
* Header files for isolating the 402 state machine
* Effort value
* Merge pull request `#6 <https://github.com/ros-industrial/ros_canopen/issues/6>`_ from ipa-mdl/indigo_dev
Work-around for https://github.com/ipa320/ros_canopen/issues/62
* Merge branch 'indigo_dev' of https://github.com/ipa-mdl/ros_canopen into indigo_dev
* fixed unintialized members
* Mode Error priority
* Order issue
* Merge branch 'indigo_dev' of https://github.com/ipa-mdl/ros_canopen into indigo_dev
Conflicts:
canopen_motor_node/CMakeLists.txt
* Error status
* 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
* Contributors: Florian Weisshardt, Mathias Lüdtke, Michael Stoll, Thiago de Freitas Oliveira Araujo, thiagodefreitas
0.6.2 (2014-12-18)
------------------
0.6.1 (2014-12-15)
------------------
* remove ipa_* and IPA_* prefixes
* added descriptions and authors
* renamed ipa_canopen_402 to canopen_402
* Contributors: Florian Weisshardt, Mathias Lüdtke

78
Devices/Libraries/Ros/ros_canopen/canopen_402/CMakeLists.txt Executable file
View File

@@ -0,0 +1,78 @@
cmake_minimum_required(VERSION 3.0.2)
project(canopen_402)
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CXX_STANDARD 14)
endif()
find_package(catkin REQUIRED
COMPONENTS
canopen_master
class_loader
)
find_package(Boost REQUIRED
)
catkin_package(
INCLUDE_DIRS
include
LIBRARIES
${PROJECT_NAME}
CATKIN_DEPENDS
canopen_master
DEPENDS
Boost
)
include_directories(
include
${Boost_INCLUDE_DIRS}
${catkin_INCLUDE_DIRS}
)
# canopen_402
add_library(${PROJECT_NAME}
src/motor.cpp
)
target_link_libraries(${PROJECT_NAME}
${catkin_LIBRARIES}
)
# canopen_402_plugin
add_library(${PROJECT_NAME}_plugin
src/plugin.cpp
)
target_link_libraries(${PROJECT_NAME}_plugin
${catkin_LIBRARIES}
${PROJECT_NAME}
)
install(
TARGETS
${PROJECT_NAME}
${PROJECT_NAME}_plugin
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"
)
install(
FILES
${PROJECT_NAME}_plugin.xml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
if(CATKIN_ENABLE_TESTING)
catkin_add_gtest(${PROJECT_NAME}-test_clamping
test/clamping.cpp
)
target_link_libraries(${PROJECT_NAME}-test_clamping
${catkin_LIBRARIES}
)
endif()

5
Devices/Libraries/Ros/ros_canopen/canopen_402/canopen_402_plugin.xml Executable file
View File

@@ -0,0 +1,5 @@
<library path="lib/libcanopen_402_plugin">
<class type="canopen::Motor402::Allocator" base_class_type="canopen::MotorBase::Allocator">
<description>Allocator for Motor402 instances</description>
</class>
</library>

45
Devices/Libraries/Ros/ros_canopen/canopen_402/include/canopen_402/base.h Executable file
View File

@@ -0,0 +1,45 @@
#ifndef CANOPEN_402_BASE_H
#define CANOPEN_402_BASE_H
#include <canopen_master/canopen.h>
namespace canopen
{
class MotorBase : public canopen::Layer {
protected:
MotorBase(const std::string &name) : Layer(name) {}
public:
enum OperationMode
{
No_Mode = 0,
Profiled_Position = 1,
Velocity = 2,
Profiled_Velocity = 3,
Profiled_Torque = 4,
Reserved = 5,
Homing = 6,
Interpolated_Position = 7,
Cyclic_Synchronous_Position = 8,
Cyclic_Synchronous_Velocity = 9,
Cyclic_Synchronous_Torque = 10,
};
virtual bool setTarget(double val) = 0;
virtual bool enterModeAndWait(uint16_t mode) = 0;
virtual bool isModeSupported(uint16_t mode) = 0;
virtual uint16_t getMode() = 0;
virtual void registerDefaultModes(ObjectStorageSharedPtr storage) {}
typedef std::shared_ptr<MotorBase> MotorBaseSharedPtr;
class Allocator {
public:
virtual MotorBaseSharedPtr allocate(const std::string &name, ObjectStorageSharedPtr storage, const canopen::Settings &settings) = 0;
virtual ~Allocator() {}
};
};
typedef MotorBase::MotorBaseSharedPtr MotorBaseSharedPtr;
}
#endif

390
Devices/Libraries/Ros/ros_canopen/canopen_402/include/canopen_402/motor.h Executable file
View File

@@ -0,0 +1,390 @@
#ifndef CANOPEN_402_MOTOR_H
#define CANOPEN_402_MOTOR_H
#include <canopen_402/base.h>
#include <canopen_master/canopen.h>
#include <functional>
#include <boost/container/flat_map.hpp>
#include <boost/numeric/conversion/cast.hpp>
#include <limits>
#include <algorithm>
namespace canopen
{
class State402{
public:
enum StatusWord
{
SW_Ready_To_Switch_On=0,
SW_Switched_On=1,
SW_Operation_enabled=2,
SW_Fault=3,
SW_Voltage_enabled=4,
SW_Quick_stop=5,
SW_Switch_on_disabled=6,
SW_Warning=7,
SW_Manufacturer_specific0=8,
SW_Remote=9,
SW_Target_reached=10,
SW_Internal_limit=11,
SW_Operation_mode_specific0=12,
SW_Operation_mode_specific1=13,
SW_Manufacturer_specific1=14,
SW_Manufacturer_specific2=15
};
enum InternalState
{
Unknown = 0,
Start = 0,
Not_Ready_To_Switch_On = 1,
Switch_On_Disabled = 2,
Ready_To_Switch_On = 3,
Switched_On = 4,
Operation_Enable = 5,
Quick_Stop_Active = 6,
Fault_Reaction_Active = 7,
Fault = 8,
};
InternalState getState();
InternalState read(uint16_t sw);
bool waitForNewState(const time_point &abstime, InternalState &state);
State402() : state_(Unknown) {}
private:
boost::condition_variable cond_;
boost::mutex mutex_;
InternalState state_;
};
class Command402 {
struct Op {
uint16_t to_set_;
uint16_t to_reset_;
Op(uint16_t to_set ,uint16_t to_reset) : to_set_(to_set), to_reset_(to_reset) {}
void operator()(uint16_t &val) const {
val = (val & ~to_reset_) | to_set_;
}
};
class TransitionTable {
boost::container::flat_map< std::pair<State402::InternalState, State402::InternalState>, Op > transitions_;
void add(const State402::InternalState &from, const State402::InternalState &to, Op op){
transitions_.insert(std::make_pair(std::make_pair(from, to), op));
}
public:
TransitionTable();
const Op& get(const State402::InternalState &from, const State402::InternalState &to) const {
return transitions_.at(std::make_pair(from, to));
}
};
static const TransitionTable transitions_;
static State402::InternalState nextStateForEnabling(State402::InternalState state);
Command402();
public:
enum ControlWord
{
CW_Switch_On=0,
CW_Enable_Voltage=1,
CW_Quick_Stop=2,
CW_Enable_Operation=3,
CW_Operation_mode_specific0=4,
CW_Operation_mode_specific1=5,
CW_Operation_mode_specific2=6,
CW_Fault_Reset=7,
CW_Halt=8,
CW_Operation_mode_specific3=9,
// CW_Reserved1=10,
CW_Manufacturer_specific0=11,
CW_Manufacturer_specific1=12,
CW_Manufacturer_specific2=13,
CW_Manufacturer_specific3=14,
CW_Manufacturer_specific4=15,
};
static bool setTransition(uint16_t &cw, const State402::InternalState &from, const State402::InternalState &to, State402::InternalState *next);
};
template<uint16_t MASK> class WordAccessor{
uint16_t &word_;
public:
WordAccessor(uint16_t &word) : word_(word) {}
bool set(uint8_t bit){
uint16_t val = MASK & (1<<bit);
word_ |= val;
return val;
}
bool reset(uint8_t bit){
uint16_t val = MASK & (1<<bit);
word_ &= ~val;
return val;
}
bool get(uint8_t bit) const { return word_ & (1<<bit); }
uint16_t get() const { return word_ & MASK; }
WordAccessor & operator=(const uint16_t &val){
uint16_t was = word_;
word_ = (word_ & ~MASK) | (val & MASK);
return *this;
}
};
class Mode {
public:
const uint16_t mode_id_;
Mode(uint16_t id) : mode_id_(id) {}
typedef WordAccessor<(1<<Command402::CW_Operation_mode_specific0)|(1<<Command402::CW_Operation_mode_specific1)|(1<<Command402::CW_Operation_mode_specific2)|(1<<Command402::CW_Operation_mode_specific3)> OpModeAccesser;
virtual bool start() = 0;
virtual bool read(const uint16_t &sw) = 0;
virtual bool write(OpModeAccesser& cw) = 0;
virtual bool setTarget(const double &val) { ROSCANOPEN_ERROR("canopen_402", "Mode::setTarget not implemented"); return false; }
virtual ~Mode() {}
};
typedef std::shared_ptr<Mode> ModeSharedPtr;
template<typename T> class ModeTargetHelper : public Mode {
T target_;
std::atomic<bool> has_target_;
public:
ModeTargetHelper(uint16_t mode) : Mode (mode) {}
bool hasTarget() { return has_target_; }
T getTarget() { return target_; }
virtual bool setTarget(const double &val) {
if(std::isnan(val)){
ROSCANOPEN_ERROR("canopen_402", "target command is not a number");
return false;
}
using boost::numeric_cast;
using boost::numeric::positive_overflow;
using boost::numeric::negative_overflow;
try
{
target_= numeric_cast<T>(val);
}
catch(negative_overflow&) {
ROSCANOPEN_WARN("canopen_402", "Command " << val << " does not fit into target, clamping to min limit");
target_= std::numeric_limits<T>::min();
}
catch(positive_overflow&) {
ROSCANOPEN_WARN("canopen_402", "Command " << val << " does not fit into target, clamping to max limit");
target_= std::numeric_limits<T>::max();
}
catch(...){
ROSCANOPEN_ERROR("canopen_402", "Was not able to cast command " << val);
return false;
}
has_target_ = true;
return true;
}
virtual bool start() { has_target_ = false; return true; }
};
template<uint16_t ID, typename TYPE, uint16_t OBJ, uint8_t SUB, uint16_t CW_MASK> class ModeForwardHelper : public ModeTargetHelper<TYPE> {
canopen::ObjectStorage::Entry<TYPE> target_entry_;
public:
ModeForwardHelper(ObjectStorageSharedPtr storage) : ModeTargetHelper<TYPE>(ID) {
if(SUB) storage->entry(target_entry_, OBJ, SUB);
else storage->entry(target_entry_, OBJ);
}
virtual bool read(const uint16_t &sw) { return true;}
virtual bool write(Mode::OpModeAccesser& cw) {
if(this->hasTarget()){
cw = cw.get() | CW_MASK;
target_entry_.set(this->getTarget());
return true;
}else{
cw = cw.get() & ~CW_MASK;
return false;
}
}
};
typedef ModeForwardHelper<MotorBase::Profiled_Velocity, int32_t, 0x60FF, 0, 0> ProfiledVelocityMode;
typedef ModeForwardHelper<MotorBase::Profiled_Torque, int16_t, 0x6071, 0, 0> ProfiledTorqueMode;
typedef ModeForwardHelper<MotorBase::Cyclic_Synchronous_Position, int32_t, 0x607A, 0, 0> CyclicSynchronousPositionMode;
typedef ModeForwardHelper<MotorBase::Cyclic_Synchronous_Velocity, int32_t, 0x60FF, 0, 0> CyclicSynchronousVelocityMode;
typedef ModeForwardHelper<MotorBase::Cyclic_Synchronous_Torque, int16_t, 0x6071, 0, 0> CyclicSynchronousTorqueMode;
typedef ModeForwardHelper<MotorBase::Velocity, int16_t, 0x6042, 0, (1<<Command402::CW_Operation_mode_specific0)|(1<<Command402::CW_Operation_mode_specific1)|(1<<Command402::CW_Operation_mode_specific2)> VelocityMode;
typedef ModeForwardHelper<MotorBase::Interpolated_Position, int32_t, 0x60C1, 0x01, (1<<Command402::CW_Operation_mode_specific0)> InterpolatedPositionMode;
class ProfiledPositionMode : public ModeTargetHelper<int32_t> {
canopen::ObjectStorage::Entry<int32_t> target_position_;
double last_target_;
uint16_t sw_;
public:
enum SW_masks {
MASK_Reached = (1<<State402::SW_Target_reached),
MASK_Acknowledged = (1<<State402::SW_Operation_mode_specific0),
MASK_Error = (1<<State402::SW_Operation_mode_specific1),
};
enum CW_bits {
CW_NewPoint = Command402::CW_Operation_mode_specific0,
CW_Immediate = Command402::CW_Operation_mode_specific1,
CW_Blending = Command402::CW_Operation_mode_specific3,
};
ProfiledPositionMode(ObjectStorageSharedPtr storage) : ModeTargetHelper(MotorBase::Profiled_Position) {
storage->entry(target_position_, 0x607A);
}
virtual bool start() { sw_ = 0; last_target_= std::numeric_limits<double>::quiet_NaN(); return ModeTargetHelper::start(); }
virtual bool read(const uint16_t &sw) { sw_ = sw; return (sw & MASK_Error) == 0; }
virtual bool write(OpModeAccesser& cw) {
cw.set(CW_Immediate);
if(hasTarget()){
int32_t target = getTarget();
if((sw_ & MASK_Acknowledged) == 0 && target != last_target_){
if(cw.get(CW_NewPoint)){
cw.reset(CW_NewPoint); // reset if needed
}else{
target_position_.set(target);
cw.set(CW_NewPoint);
last_target_ = target;
}
} else if (sw_ & MASK_Acknowledged){
cw.reset(CW_NewPoint);
}
return true;
}
return false;
}
};
class HomingMode: public Mode{
protected:
enum SW_bits {
SW_Attained = State402::SW_Operation_mode_specific0,
SW_Error = State402::SW_Operation_mode_specific1,
};
enum CW_bits {
CW_StartHoming = Command402::CW_Operation_mode_specific0,
};
public:
HomingMode() : Mode(MotorBase::Homing) {}
virtual bool executeHoming(canopen::LayerStatus &status) = 0;
};
class DefaultHomingMode: public HomingMode{
canopen::ObjectStorage::Entry<int8_t> homing_method_;
std::atomic<bool> execute_;
boost::mutex mutex_;
boost::condition_variable cond_;
uint16_t status_;
enum SW_masks {
MASK_Reached = (1<<State402::SW_Target_reached),
MASK_Attained = (1<<SW_Attained),
MASK_Error = (1<<SW_Error),
};
bool error(canopen::LayerStatus &status, const std::string& msg) { execute_= false; status.error(msg); return false; }
public:
DefaultHomingMode(ObjectStorageSharedPtr storage) {
storage->entry(homing_method_, 0x6098);
}
virtual bool start();
virtual bool read(const uint16_t &sw);
virtual bool write(OpModeAccesser& cw);
virtual bool executeHoming(canopen::LayerStatus &status);
};
class Motor402 : public MotorBase
{
public:
Motor402(const std::string &name, ObjectStorageSharedPtr storage, const canopen::Settings &settings)
: MotorBase(name), status_word_(0),control_word_(0),
switching_state_(State402::InternalState(settings.get_optional<unsigned int>("switching_state", static_cast<unsigned int>(State402::Operation_Enable)))),
monitor_mode_(settings.get_optional<bool>("monitor_mode", true)),
state_switch_timeout_(settings.get_optional<unsigned int>("state_switch_timeout", 5))
{
storage->entry(status_word_entry_, 0x6041);
storage->entry(control_word_entry_, 0x6040);
storage->entry(op_mode_display_, 0x6061);
storage->entry(op_mode_, 0x6060);
try{
storage->entry(supported_drive_modes_, 0x6502);
}
catch(...){
}
}
virtual bool setTarget(double val);
virtual bool enterModeAndWait(uint16_t mode);
virtual bool isModeSupported(uint16_t mode);
virtual uint16_t getMode();
template<typename T, typename ...Args>
bool registerMode(uint16_t mode, Args&&... args) {
return mode_allocators_.insert(std::make_pair(mode, [args..., mode, this](){
if(isModeSupportedByDevice(mode)) registerMode(mode, ModeSharedPtr(new T(args...)));
})).second;
}
virtual void registerDefaultModes(ObjectStorageSharedPtr storage){
registerMode<ProfiledPositionMode> (MotorBase::Profiled_Position, storage);
registerMode<VelocityMode> (MotorBase::Velocity, storage);
registerMode<ProfiledVelocityMode> (MotorBase::Profiled_Velocity, storage);
registerMode<ProfiledTorqueMode> (MotorBase::Profiled_Torque, storage);
registerMode<DefaultHomingMode> (MotorBase::Homing, storage);
registerMode<InterpolatedPositionMode> (MotorBase::Interpolated_Position, storage);
registerMode<CyclicSynchronousPositionMode> (MotorBase::Cyclic_Synchronous_Position, storage);
registerMode<CyclicSynchronousVelocityMode> (MotorBase::Cyclic_Synchronous_Velocity, storage);
registerMode<CyclicSynchronousTorqueMode> (MotorBase::Cyclic_Synchronous_Torque, storage);
}
class Allocator : public MotorBase::Allocator{
public:
virtual MotorBaseSharedPtr allocate(const std::string &name, ObjectStorageSharedPtr storage, const canopen::Settings &settings);
};
protected:
virtual void handleRead(LayerStatus &status, const LayerState &current_state);
virtual void handleWrite(LayerStatus &status, const LayerState &current_state);
virtual void handleDiag(LayerReport &report);
virtual void handleInit(LayerStatus &status);
virtual void handleShutdown(LayerStatus &status);
virtual void handleHalt(LayerStatus &status);
virtual void handleRecover(LayerStatus &status);
private:
virtual bool isModeSupportedByDevice(uint16_t mode);
void registerMode(uint16_t id, const ModeSharedPtr &m);
ModeSharedPtr allocMode(uint16_t mode);
bool readState(LayerStatus &status, const LayerState &current_state);
bool switchMode(LayerStatus &status, uint16_t mode);
bool switchState(LayerStatus &status, const State402::InternalState &target);
std::atomic<uint16_t> status_word_;
uint16_t control_word_;
boost::mutex cw_mutex_;
std::atomic<bool> start_fault_reset_;
std::atomic<State402::InternalState> target_state_;
State402 state_handler_;
boost::mutex map_mutex_;
std::unordered_map<uint16_t, ModeSharedPtr > modes_;
typedef std::function<void()> AllocFuncType;
std::unordered_map<uint16_t, AllocFuncType> mode_allocators_;
ModeSharedPtr selected_mode_;
uint16_t mode_id_;
boost::condition_variable mode_cond_;
boost::mutex mode_mutex_;
const State402::InternalState switching_state_;
const bool monitor_mode_;
const boost::chrono::seconds state_switch_timeout_;
canopen::ObjectStorage::Entry<uint16_t> status_word_entry_;
canopen::ObjectStorage::Entry<uint16_t > control_word_entry_;
canopen::ObjectStorage::Entry<int8_t> op_mode_display_;
canopen::ObjectStorage::Entry<int8_t> op_mode_;
canopen::ObjectStorage::Entry<uint32_t> supported_drive_modes_;
};
}
#endif

28
Devices/Libraries/Ros/ros_canopen/canopen_402/package.xml Executable file
View File

@@ -0,0 +1,28 @@
<?xml version="1.0"?>
<package format="2">
<name>canopen_402</name>
<version>0.8.5</version>
<description>This implements the CANopen device profile for drives and motion control. CiA(r) 402</description>
<maintainer email="mathias.luedtke@ipa.fraunhofer.de">Mathias Lüdtke</maintainer>
<author email="tdf@ipa.fhg.de">Thiago de Freitas</author>
<author email="mathias.luedtke@ipa.fraunhofer.de">Mathias Lüdtke</author>
<license>LGPLv3</license>
<url type="website">http://wiki.ros.org/canopen_402</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>canopen_master</depend>
<depend>class_loader</depend>
<test_depend>rosunit</test_depend>
<export>
<canopen_402 plugin="${prefix}/canopen_402_plugin.xml" />
</export>
</package>

549
Devices/Libraries/Ros/ros_canopen/canopen_402/src/motor.cpp Executable file
View File

@@ -0,0 +1,549 @@
#include <canopen_402/motor.h>
#include <boost/thread/reverse_lock.hpp>
namespace canopen
{
State402::InternalState State402::getState(){
boost::mutex::scoped_lock lock(mutex_);
return state_;
}
State402::InternalState State402::read(uint16_t sw) {
static const uint16_t r = (1 << SW_Ready_To_Switch_On);
static const uint16_t s = (1 << SW_Switched_On);
static const uint16_t o = (1 << SW_Operation_enabled);
static const uint16_t f = (1 << SW_Fault);
static const uint16_t q = (1 << SW_Quick_stop);
static const uint16_t d = (1 << SW_Switch_on_disabled);
InternalState new_state = Unknown;
uint16_t state = sw & ( d | q | f | o | s | r );
switch ( state )
{
// ( d | q | f | o | s | r ):
case ( 0 | 0 | 0 | 0 | 0 | 0 ):
case ( 0 | q | 0 | 0 | 0 | 0 ):
new_state = Not_Ready_To_Switch_On;
break;
case ( d | 0 | 0 | 0 | 0 | 0 ):
case ( d | q | 0 | 0 | 0 | 0 ):
new_state = Switch_On_Disabled;
break;
case ( 0 | q | 0 | 0 | 0 | r ):
new_state = Ready_To_Switch_On;
break;
case ( 0 | q | 0 | 0 | s | r ):
new_state = Switched_On;
break;
case ( 0 | q | 0 | o | s | r ):
new_state = Operation_Enable;
break;
case ( 0 | 0 | 0 | o | s | r ):
new_state = Quick_Stop_Active;
break;
case ( 0 | 0 | f | o | s | r ):
case ( 0 | q | f | o | s | r ):
new_state = Fault_Reaction_Active;
break;
case ( 0 | 0 | f | 0 | 0 | 0 ):
case ( 0 | q | f | 0 | 0 | 0 ):
new_state = Fault;
break;
default:
ROSCANOPEN_WARN("canopen_402", "Motor is currently in an unknown state: " << std::hex << state << std::dec);
}
boost::mutex::scoped_lock lock(mutex_);
if(new_state != state_){
state_ = new_state;
cond_.notify_all();
}
return state_;
}
bool State402::waitForNewState(const time_point &abstime, State402::InternalState &state){
boost::mutex::scoped_lock lock(mutex_);
while(state_ == state && cond_.wait_until(lock, abstime) == boost::cv_status::no_timeout) {}
bool res = state != state_;
state = state_;
return res;
}
const Command402::TransitionTable Command402::transitions_;
Command402::TransitionTable::TransitionTable(){
typedef State402 s;
transitions_.reserve(32);
Op disable_voltage(0,(1<<CW_Fault_Reset) | (1<<CW_Enable_Voltage));
/* 7*/ add(s::Ready_To_Switch_On, s::Switch_On_Disabled, disable_voltage);
/* 9*/ add(s::Operation_Enable, s::Switch_On_Disabled, disable_voltage);
/*10*/ add(s::Switched_On, s::Switch_On_Disabled, disable_voltage);
/*12*/ add(s::Quick_Stop_Active, s::Switch_On_Disabled, disable_voltage);
Op automatic(0,0);
/* 0*/ add(s::Start, s::Not_Ready_To_Switch_On, automatic);
/* 1*/ add(s::Not_Ready_To_Switch_On, s::Switch_On_Disabled, automatic);
/*14*/ add(s::Fault_Reaction_Active, s::Fault, automatic);
Op shutdown((1<<CW_Quick_Stop) | (1<<CW_Enable_Voltage), (1<<CW_Fault_Reset) | (1<<CW_Switch_On));
/* 2*/ add(s::Switch_On_Disabled, s::Ready_To_Switch_On, shutdown);
/* 6*/ add(s::Switched_On, s::Ready_To_Switch_On, shutdown);
/* 8*/ add(s::Operation_Enable, s::Ready_To_Switch_On, shutdown);
Op switch_on((1<<CW_Quick_Stop) | (1<<CW_Enable_Voltage) | (1<<CW_Switch_On), (1<<CW_Fault_Reset) | (1<<CW_Enable_Operation));
/* 3*/ add(s::Ready_To_Switch_On, s::Switched_On, switch_on);
/* 5*/ add(s::Operation_Enable, s::Switched_On, switch_on);
Op enable_operation((1<<CW_Quick_Stop) | (1<<CW_Enable_Voltage) | (1<<CW_Switch_On) | (1<<CW_Enable_Operation), (1<<CW_Fault_Reset));
/* 4*/ add(s::Switched_On, s::Operation_Enable, enable_operation);
/*16*/ add(s::Quick_Stop_Active, s::Operation_Enable, enable_operation);
Op quickstop((1<<CW_Enable_Voltage), (1<<CW_Fault_Reset) | (1<<CW_Quick_Stop));
/* 7*/ add(s::Ready_To_Switch_On, s::Quick_Stop_Active, quickstop); // transit to Switch_On_Disabled
/*10*/ add(s::Switched_On, s::Quick_Stop_Active, quickstop); // transit to Switch_On_Disabled
/*11*/ add(s::Operation_Enable, s::Quick_Stop_Active, quickstop);
// fault reset
/*15*/ add(s::Fault, s::Switch_On_Disabled, Op((1<<CW_Fault_Reset), 0));
}
State402::InternalState Command402::nextStateForEnabling(State402::InternalState state){
switch(state){
case State402::Start:
return State402::Not_Ready_To_Switch_On;
case State402::Fault:
case State402::Not_Ready_To_Switch_On:
return State402::Switch_On_Disabled;
case State402::Switch_On_Disabled:
return State402::Ready_To_Switch_On;
case State402::Ready_To_Switch_On:
return State402::Switched_On;
case State402::Switched_On:
case State402::Quick_Stop_Active:
case State402::Operation_Enable:
return State402::Operation_Enable;
case State402::Fault_Reaction_Active:
return State402::Fault;
}
throw std::out_of_range ("state value is illegal");
}
bool Command402::setTransition(uint16_t &cw, const State402::InternalState &from, const State402::InternalState &to, State402::InternalState *next){
try{
if(from != to){
State402::InternalState hop = to;
if(next){
if(to == State402::Operation_Enable) hop = nextStateForEnabling(from);
*next = hop;
}
transitions_.get(from, hop)(cw);
}
return true;
}
catch(...){
ROSCANOPEN_WARN("canopen_402", "illegal transition " << from << " -> " << to);
}
return false;
}
template<uint16_t mask, uint16_t not_equal> struct masked_status_not_equal {
uint16_t &status_;
masked_status_not_equal(uint16_t &status) : status_(status) {}
bool operator()() const { return (status_ & mask) != not_equal; }
};
bool DefaultHomingMode::start() {
execute_ = false;
return read(0);
}
bool DefaultHomingMode::read(const uint16_t &sw) {
boost::mutex::scoped_lock lock(mutex_);
uint16_t old = status_;
status_ = sw & (MASK_Reached | MASK_Attained | MASK_Error);
if(old != status_){
cond_.notify_all();
}
return true;
}
bool DefaultHomingMode::write(Mode::OpModeAccesser& cw) {
cw = 0;
if(execute_){
cw.set(CW_StartHoming);
return true;
}
return false;
}
bool DefaultHomingMode::executeHoming(canopen::LayerStatus &status) {
if(!homing_method_.valid()){
return error(status, "homing method entry is not valid");
}
if(homing_method_.get_cached() == 0){
return true;
}
time_point prepare_time = get_abs_time(boost::chrono::seconds(1));
// ensure homing is not running
boost::mutex::scoped_lock lock(mutex_);
if(!cond_.wait_until(lock, prepare_time, masked_status_not_equal<MASK_Error | MASK_Reached, 0> (status_))){
return error(status, "could not prepare homing");
}
if(status_ & MASK_Error){
return error(status, "homing error before start");
}
execute_ = true;
// ensure start
if(!cond_.wait_until(lock, prepare_time, masked_status_not_equal<MASK_Error | MASK_Attained | MASK_Reached, MASK_Reached> (status_))){
return error(status, "homing did not start");
}
if(status_ & MASK_Error){
return error(status, "homing error at start");
}
time_point finish_time = get_abs_time(boost::chrono::seconds(10)); //
// wait for attained
if(!cond_.wait_until(lock, finish_time, masked_status_not_equal<MASK_Error | MASK_Attained, 0> (status_))){
return error(status, "homing not attained");
}
if(status_ & MASK_Error){
return error(status, "homing error during process");
}
// wait for motion stop
if(!cond_.wait_until(lock, finish_time, masked_status_not_equal<MASK_Error | MASK_Reached, 0> (status_))){
return error(status, "homing did not stop");
}
if(status_ & MASK_Error){
return error(status, "homing error during stop");
}
if((status_ & MASK_Reached) && (status_ & MASK_Attained)){
execute_ = false;
return true;
}
return error(status, "something went wrong while homing");
}
bool Motor402::setTarget(double val){
if(state_handler_.getState() == State402::Operation_Enable){
boost::mutex::scoped_lock lock(mode_mutex_);
return selected_mode_ && selected_mode_->setTarget(val);
}
return false;
}
bool Motor402::isModeSupported(uint16_t mode) { return mode != MotorBase::Homing && allocMode(mode); }
bool Motor402::enterModeAndWait(uint16_t mode) {
LayerStatus s;
bool okay = mode != MotorBase::Homing && switchMode(s, mode);
if(!s.bounded<LayerStatus::Ok>()){
ROSCANOPEN_ERROR("canopen_402", "Could not switch to mode " << mode << ", reason: " << s.reason());
}
return okay;
}
uint16_t Motor402::getMode() {
boost::mutex::scoped_lock lock(mode_mutex_);
return selected_mode_ ? selected_mode_->mode_id_ : MotorBase::No_Mode;
}
bool Motor402::isModeSupportedByDevice(uint16_t mode){
if(!supported_drive_modes_.valid()) {
BOOST_THROW_EXCEPTION( std::runtime_error("Supported drive modes (object 6502) is not valid"));
}
return mode > 0 && mode <= 32 && (supported_drive_modes_.get_cached() & (1<<(mode-1)));
}
void Motor402::registerMode(uint16_t id, const ModeSharedPtr &m){
boost::mutex::scoped_lock map_lock(map_mutex_);
if(m && m->mode_id_ == id) modes_.insert(std::make_pair(id, m));
}
ModeSharedPtr Motor402::allocMode(uint16_t mode){
ModeSharedPtr res;
if(isModeSupportedByDevice(mode)){
boost::mutex::scoped_lock map_lock(map_mutex_);
std::unordered_map<uint16_t, ModeSharedPtr >::iterator it = modes_.find(mode);
if(it != modes_.end()){
res = it->second;
}
}
return res;
}
bool Motor402::switchMode(LayerStatus &status, uint16_t mode) {
if(mode == MotorBase::No_Mode){
boost::mutex::scoped_lock lock(mode_mutex_);
selected_mode_.reset();
try{ // try to set mode
op_mode_.set(mode);
}catch(...){}
return true;
}
ModeSharedPtr next_mode = allocMode(mode);
if(!next_mode){
status.error("Mode is not supported.");
return false;
}
if(!next_mode->start()){
status.error("Could not start mode.");
return false;
}
{ // disable mode handler
boost::mutex::scoped_lock lock(mode_mutex_);
if(mode_id_ == mode && selected_mode_ && selected_mode_->mode_id_ == mode){
// nothing to do
return true;
}
selected_mode_.reset();
}
if(!switchState(status, switching_state_)) return false;
op_mode_.set(mode);
bool okay = false;
{ // wait for switch
boost::mutex::scoped_lock lock(mode_mutex_);
time_point abstime = get_abs_time(boost::chrono::seconds(5));
if(monitor_mode_){
while(mode_id_ != mode && mode_cond_.wait_until(lock, abstime) == boost::cv_status::no_timeout) {}
}else{
while(mode_id_ != mode && get_abs_time() < abstime){
boost::reverse_lock<boost::mutex::scoped_lock> reverse(lock); // unlock inside loop
op_mode_display_.get(); // poll
boost::this_thread::sleep_for(boost::chrono::milliseconds(20)); // wait some time
}
}
if(mode_id_ == mode){
selected_mode_ = next_mode;
okay = true;
}else{
status.error("Mode switch timed out.");
op_mode_.set(mode_id_);
}
}
if(!switchState(status, State402::Operation_Enable)) return false;
return okay;
}
bool Motor402::switchState(LayerStatus &status, const State402::InternalState &target){
time_point abstime = get_abs_time(state_switch_timeout_);
State402::InternalState state = state_handler_.getState();
target_state_ = target;
while(state != target_state_){
boost::mutex::scoped_lock lock(cw_mutex_);
State402::InternalState next = State402::Unknown;
if(!Command402::setTransition(control_word_ ,state, target_state_ , &next)){
status.error("Could not set transition");
return false;
}
lock.unlock();
if(state != next && !state_handler_.waitForNewState(abstime, state)){
status.error("Transition timeout");
return false;
}
}
return state == target;
}
bool Motor402::readState(LayerStatus &status, const LayerState &current_state){
uint16_t old_sw, sw = status_word_entry_.get(); // TODO: added error handling
old_sw = status_word_.exchange(sw);
state_handler_.read(sw);
boost::mutex::scoped_lock lock(mode_mutex_);
uint16_t new_mode = monitor_mode_ ? op_mode_display_.get() : op_mode_display_.get_cached();
if(selected_mode_ && selected_mode_->mode_id_ == new_mode){
if(!selected_mode_->read(sw)){
status.error("Mode handler has error");
}
}
if(new_mode != mode_id_){
mode_id_ = new_mode;
mode_cond_.notify_all();
}
if(selected_mode_ && selected_mode_->mode_id_ != new_mode){
status.warn("mode does not match");
}
if(sw & (1<<State402::SW_Internal_limit)){
if(old_sw & (1<<State402::SW_Internal_limit) || current_state != Ready){
status.warn("Internal limit active");
}else{
status.error("Internal limit active");
}
}
return true;
}
void Motor402::handleRead(LayerStatus &status, const LayerState &current_state){
if(current_state > Off){
readState(status, current_state);
}
}
void Motor402::handleWrite(LayerStatus &status, const LayerState &current_state){
if(current_state > Off){
boost::mutex::scoped_lock lock(cw_mutex_);
control_word_ |= (1<<Command402::CW_Halt);
if(state_handler_.getState() == State402::Operation_Enable){
boost::mutex::scoped_lock lock(mode_mutex_);
Mode::OpModeAccesser cwa(control_word_);
bool okay = false;
if(selected_mode_ && selected_mode_->mode_id_ == mode_id_){
okay = selected_mode_->write(cwa);
} else {
cwa = 0;
}
if(okay) {
control_word_ &= ~(1<<Command402::CW_Halt);
}
}
if(start_fault_reset_.exchange(false)){
control_word_entry_.set_cached(control_word_ & ~(1<<Command402::CW_Fault_Reset));
}else{
control_word_entry_.set_cached(control_word_);
}
}
}
void Motor402::handleDiag(LayerReport &report){
uint16_t sw = status_word_;
State402::InternalState state = state_handler_.getState();
switch(state){
case State402::Not_Ready_To_Switch_On:
case State402::Switch_On_Disabled:
case State402::Ready_To_Switch_On:
case State402::Switched_On:
report.warn("Motor operation is not enabled");
case State402::Operation_Enable:
break;
case State402::Quick_Stop_Active:
report.error("Quick stop is active");
break;
case State402::Fault:
case State402::Fault_Reaction_Active:
report.error("Motor has fault");
break;
case State402::Unknown:
report.error("State is unknown");
report.add("status_word", sw);
break;
}
if(sw & (1<<State402::SW_Warning)){
report.warn("Warning bit is set");
}
if(sw & (1<<State402::SW_Internal_limit)){
report.error("Internal limit active");
}
}
void Motor402::handleInit(LayerStatus &status){
for(std::unordered_map<uint16_t, AllocFuncType>::iterator it = mode_allocators_.begin(); it != mode_allocators_.end(); ++it){
(it->second)();
}
if(!readState(status, Init)){
status.error("Could not read motor state");
return;
}
{
boost::mutex::scoped_lock lock(cw_mutex_);
control_word_ = 0;
start_fault_reset_ = true;
}
if(!switchState(status, State402::Operation_Enable)){
status.error("Could not enable motor");
return;
}
ModeSharedPtr m = allocMode(MotorBase::Homing);
if(!m){
return; // homing not supported
}
HomingMode *homing = dynamic_cast<HomingMode*>(m.get());
if(!homing){
status.error("Homing mode has incorrect handler");
return;
}
if(!switchMode(status, MotorBase::Homing)){
status.error("Could not enter homing mode");
return;
}
if(!homing->executeHoming(status)){
status.error("Homing failed");
return;
}
switchMode(status, No_Mode);
}
void Motor402::handleShutdown(LayerStatus &status){
switchMode(status, MotorBase::No_Mode);
switchState(status, State402::Switch_On_Disabled);
}
void Motor402::handleHalt(LayerStatus &status){
State402::InternalState state = state_handler_.getState();
boost::mutex::scoped_lock lock(cw_mutex_);
// do not demand quickstop in case of fault
if(state == State402::Fault_Reaction_Active || state == State402::Fault) return;
if(state != State402::Operation_Enable){
target_state_ = state;
}else{
target_state_ = State402::Quick_Stop_Active;
if(!Command402::setTransition(control_word_ ,state, State402::Quick_Stop_Active, 0)){
status.warn("Could not quick stop");
}
}
}
void Motor402::handleRecover(LayerStatus &status){
start_fault_reset_ = true;
{
boost::mutex::scoped_lock lock(mode_mutex_);
if(selected_mode_ && !selected_mode_->start()){
status.error("Could not restart mode.");
return;
}
}
if(!switchState(status, State402::Operation_Enable)){
status.error("Could not enable motor");
return;
}
}
} // namespace

8
Devices/Libraries/Ros/ros_canopen/canopen_402/src/plugin.cpp Executable file
View File

@@ -0,0 +1,8 @@
#include <class_loader/class_loader.hpp>
#include <canopen_402/motor.h>
canopen::MotorBaseSharedPtr canopen::Motor402::Allocator::allocate(const std::string &name, canopen::ObjectStorageSharedPtr storage, const canopen::Settings &settings) {
return std::make_shared<canopen::Motor402>(name, storage, settings);
}
CLASS_LOADER_REGISTER_CLASS(canopen::Motor402::Allocator, canopen::MotorBase::Allocator);

57
Devices/Libraries/Ros/ros_canopen/canopen_402/test/clamping.cpp Executable file
View File

@@ -0,0 +1,57 @@
#include <canopen_402/motor.h>
#include <gtest/gtest.h>
template<typename T> class ModeTargetHelperTest : public canopen::ModeTargetHelper<T>, public ::testing::Test{
public:
ModeTargetHelperTest() : canopen::ModeTargetHelper<T>(0) {}
virtual bool read(const uint16_t &sw) { return false; }
virtual bool write(canopen::Mode::OpModeAccesser& cw) { return false; }
};
typedef ::testing::Types<uint8_t, int8_t, uint16_t, int16_t, uint32_t, int32_t, uint64_t, int64_t> MyTypes;
TYPED_TEST_CASE(ModeTargetHelperTest, MyTypes);
TYPED_TEST(ModeTargetHelperTest, CheckNaN){
ASSERT_FALSE(this->setTarget(std::numeric_limits<double>::quiet_NaN()));
}
TYPED_TEST(ModeTargetHelperTest, CheckZero){
ASSERT_TRUE(this->setTarget(0.0));
}
TYPED_TEST(ModeTargetHelperTest, CheckOne){
ASSERT_TRUE(this->setTarget(1.0));
}
TYPED_TEST(ModeTargetHelperTest, CheckMax){
double max = static_cast<double>(std::numeric_limits<TypeParam>::max());
ASSERT_TRUE(this->setTarget(max));
ASSERT_EQ(max, this->getTarget());
ASSERT_TRUE(this->setTarget(max-1));
ASSERT_EQ(max-1,this->getTarget());
ASSERT_TRUE(this->setTarget(max+1));
ASSERT_EQ(max, this->getTarget());
}
TYPED_TEST(ModeTargetHelperTest, CheckMin){
double min = static_cast<double>(std::numeric_limits<TypeParam>::min());
ASSERT_TRUE(this->setTarget(min));
ASSERT_EQ(min, this->getTarget());
ASSERT_TRUE(this->setTarget(min-1));
ASSERT_EQ(min, this->getTarget());
ASSERT_TRUE(this->setTarget(min+1));
ASSERT_EQ(min+1,this->getTarget());
}
int main(int argc, char **argv){
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}