diff --git a/cob_driver/package.xml b/cob_driver/package.xml index 00f2ab6b4..0b7ec21f7 100644 --- a/cob_driver/package.xml +++ b/cob_driver/package.xml @@ -9,8 +9,6 @@ Florian Weisshardt - Nadia Hammoudeh Garcia - Florian Weisshardt catkin @@ -20,7 +18,6 @@ cob_canopen_motor cob_elmo_homing cob_generic_can - cob_head_axis cob_light cob_mimic cob_phidgets diff --git a/cob_head_axis/CHANGELOG.rst b/cob_head_axis/CHANGELOG.rst deleted file mode 100644 index 751c2514b..000000000 --- a/cob_head_axis/CHANGELOG.rst +++ /dev/null @@ -1,159 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package cob_head_axis -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.6.8 (2016-10-10) ------------------- - -0.6.7 (2016-04-02) ------------------- - -0.6.6 (2016-04-01) ------------------- - -0.6.5 (2015-08-31) ------------------- - -0.6.4 (2015-08-25) ------------------- -* rising no longer supported -* boost revision -* do not install headers in executable-only packages -* explicit dependency to boost -* more cleanup -* remove obsolete autogenerated mainpage.dox files -* catkin_package according to install tags -* remove trailing whitespaces -* add_dependencies EXPORTED_TARGETS -* migrate to package format 2 -* sort dependencies -* critically review dependencies -* Contributors: ipa-fxm, ipa-nhg - -0.6.3 (2015-06-17) ------------------- -* fix topic names -* use new Trigger from std_srvs -* cleanup cob_srvs -* adapt cob_head_axis to new namespaces -* Contributors: ipa-fxm, ipa-nhg - -0.6.2 (2014-12-15) ------------------- - -0.6.1 (2014-09-17) ------------------- - -0.6.0 (2014-09-09) ------------------- - -0.5.7 (2014-08-26) ------------------- -* Merge pull request `#163 `_ from ipa320/hydro_dev - updates from hydro_dev -* 0.5.6 -* update changelog -* added explicit default argument queue_size -* Cleaned up cob_driver with reduced deps to compile on indigo -* Contributors: Alexander Bubeck, Felix Messmer, Florian Weisshardt - -0.5.6 (2014-08-26) ------------------- -* Merge pull request `#163 `_ from ipa320/hydro_dev - updates from hydro_dev -* added explicit default argument queue_size -* Cleaned up cob_driver with reduced deps to compile on indigo -* Contributors: Alexander Bubeck, Felix Messmer, Florian Weisshardt - -0.5.3 (2014-03-31) ------------------- -* install tags -* cob_head_axis: tabs to spaces -* cob_head_axis: fix deprecated API in constructor -* Contributors: ipa-fxm, ipa-mig - -0.5.2 (2014-03-20) ------------------- - -0.5.1 (2014-03-20) ------------------- -* cherry-pick -* removed a lot of code related to packages not available in hydro anymore -* output long unsigned variables correctly -* upstream changes -* Merge branch 'groovy_dev' of git://github.com/ipa320/cob_driver into groovy_dev -* fixed build errors for gcc version >= 4.7 -* trying to fix quantal compilation -* merged with upstream -* Fixed diagnostic error for head_axis -* trying to get error state out of head axis -* cleaned up CMakeLists and added install directives -* further modifications for catkin, now everything is compiling and linking -* futher include and linkpath modifications -* compiling but still some linker errors -* Second catkinization push -* First catkinization, still need to update some CMakeLists.txt -* dirty hack for fixing joint direction -* fix finish bug in head axis -* fix finish bug in head axis -* added effort to joint states message -* remove deprecated link to urdf -* switched from pr2_controllers_msgs::JointTrajectoryAction to control_msgs::FollowJointTrajectory -* removed outdated file - not used anymore -* fix actionserver bug with early success -* beautyfing -* partial fix for head axis -* fix for crash if stop is executed before init -* add state topic for head_axis -* changes for fuerte compatibility -* remove deprecated tests -* delete old in iand yaml files -* added diagnotic topic for initialization states for sdh -* move to private namespace -* private nodehandle -* merge -* adde env ROBOT to test -* added roslaunch tests -* added cob3-4 configs -* removed compiler warnings -* cob_head_axis: turning to 0deg after homing -* cob_head_axis: removed homing-sleep -* added rostest -* modifications for fetch and carry -* update cob3-3 -* dummy head axis -* adaptions for cob_head_axis on cob3-3, included some new parameters instead of hard-coded settings -* update for cob3-3 -* rearranging cob_camera_sensors launch files -* update for cob3-3 -* add services and return true for recover after init -* changed test duration to 10s -* camera settings added for head -* changed head params for cob3-2 -* moved init test to cob_srvs -* wrong namespace in test file -* modified parameters -* modified parameters -* modified tests -* release update for cob3-1 -* merge -* changed device for head axis -* adjust devices for cob3-1 -* changed trigger service -* joint_state aggregator working on cob3-1, calibration script update -* cleanup in cob_driver -* Moved hard-coded lines for head_axis_homing from CanDriveHarmonica.cpp into ElmoCtrl.cpp. Removed debugger in base_drive_chain.launch and undercarriage_ctrl.launch -* Head axis working, tested on cob3-1 but adapted parameters (-files) should work on both robots -* merge -* HeadAxis working -* bugfix -* Cleaned cob_head_axis yaml-files -* merge -* removed unused parameters -* cob_head_axis: set offset via urdf and chose can-device-path via ini-File -* cob_camera_axis tested, now also is able to be shut down -* cob_head_axis working -* cob_head_axis working -* cob_head_axis: correctly working, but front and back is switched -* renamed camera_axis to head_axis and platform to base -* Contributors: Alexander Bubeck, Felix Messmer, Frederik Hegger, Richard Bormann, abubeck, cpc-pk, fmw-jk, ipa-bnm, ipa-cob3-4, ipa-cob3-5, ipa-fmw, ipa-fxm, ipa-goa, ipa-mig, ipa-uhr diff --git a/cob_head_axis/CMakeLists.txt b/cob_head_axis/CMakeLists.txt deleted file mode 100644 index 07f96c6d0..000000000 --- a/cob_head_axis/CMakeLists.txt +++ /dev/null @@ -1,24 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(cob_head_axis) - -find_package(catkin REQUIRED COMPONENTS actionlib cob_canopen_motor cob_generic_can cob_srvs cob_utilities control_msgs diagnostic_msgs roscpp sensor_msgs std_srvs urdf) - -catkin_package() - -### BUILD ### -include_directories(common/include ${catkin_INCLUDE_DIRS}) - -add_executable(${PROJECT_NAME}_node ros/src/${PROJECT_NAME}.cpp common/src/ElmoCtrl.cpp) -add_dependencies(${PROJECT_NAME}_node ${catkin_EXPORTED_TARGETS}) -target_link_libraries(${PROJECT_NAME}_node ${catkin_LIBRARIES}) - -### INSTALL ### -install(TARGETS ${PROJECT_NAME}_node - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -install(PROGRAMS ros/src/dummy_head.py - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) diff --git a/cob_head_axis/common/include/cob_head_axis/ElmoCtrl.h b/cob_head_axis/common/include/cob_head_axis/ElmoCtrl.h deleted file mode 100644 index ad2fb8ca0..000000000 --- a/cob_head_axis/common/include/cob_head_axis/ElmoCtrl.h +++ /dev/null @@ -1,255 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -#ifndef __ELMO_CTRL_H__ -#define __ELMO_CTRL_H__ -#define NTCAN_CLEAN_NAMESPACE -#include -#include -#include -#include -#include -#include - -// Headers provided by cob-packages which should be avoided/removed^M -#include -#include - - -typedef struct _CanOpenAddress -{ - int TxPDO1; - int TxPDO2; - int RxPDO2; - int TxSDO; - int RxSDO; -} CanOpenAddress; - -class ElmoCtrl; - -typedef struct -{ - ElmoCtrl * pElmoCtrl; -} ElmoThreadArgs; - - -class ElmoCtrlParams -{ - - public: - ElmoCtrlParams(){;} - - int Init(std::string CanDevice, int BaudRate, int ModuleID) - { - SetCanDevice(CanDevice); - SetBaudRate(BaudRate); - SetModuleID(ModuleID); - return 0; - } - - //Can Device - void SetCanDevice(std::string CanDevice){m_CanDevice = CanDevice;} - std::string GetCanDevice(){return m_CanDevice;} - - //BaudRate - void SetBaudRate(int BaudRate){m_BaudRate=BaudRate;} - int GetBaudRate(){return m_BaudRate;} - - //ModuleIDs - int GetModuleID(){return m_ModuleID;} - void SetModuleID(int id){m_ModuleID = id;} - - //Angular Constraints - void SetUpperLimit(double UpperLimit){m_UpperLimit = UpperLimit;} - void SetLowerLimit(double LowerLimit){m_LowerLimit = LowerLimit;} - void SetAngleOffset(double AngleOffset){m_Offset = AngleOffset;} - double GetUpperLimit(){return m_UpperLimit;} - double GetLowerLimit(){return m_LowerLimit;} - double GetAngleOffset(){return m_Offset;} - - //Velocity - void SetMaxVel(double maxVel){m_MaxVel = maxVel;} - double GetMaxVel(){return m_MaxVel;} - - //HomingDir - void SetHomingDir(int dir){m_HomingDir = dir;} - int GetHomingDir(){return m_HomingDir;} - - //HomingDigIn - void SetHomingDigIn(int dig_in){m_HomingDigIn = dig_in;} - int GetHomingDigIn(){return m_HomingDigIn;} - - //CanIniFile - void SetCanIniFile(std::string iniFile){m_CanIniFile=iniFile;} - std::string GetCanIniFile(){return m_CanIniFile;} - - //Encoder increments - void SetEncoderIncrements(int enc_incr){m_EncIncrPerRevMot = enc_incr;} - int GetEncoderIncrements(){return m_EncIncrPerRevMot;} - - //Belt & Gear ratio in one - void SetGearRatio(double gear_ratio){m_GearRatio = gear_ratio;} - double GetGearRatio(){return m_GearRatio;} - - //Motor direction - void SetMotorDirection(int motor_dir){ - if(motor_dir<0) m_MotorDirection = -1; - else m_MotorDirection = 1; - } - int GetMotorDirection(){return m_MotorDirection;} - - - - - private: - int m_ModuleID; - std::string m_CanDevice; - int m_BaudRate; - int m_HomingDir; - int m_HomingDigIn; - int m_EncIncrPerRevMot; - int m_MotorDirection; - double m_GearRatio; - double m_Offset; - double m_UpperLimit; - double m_LowerLimit; - double m_MaxVel; - std::string m_CanIniFile; - -}; - -class ElmoCtrl -{ - -public: - ElmoCtrl(); - ~ElmoCtrl(); - - bool Init(ElmoCtrlParams* params, bool home = true); - - - double MoveJointSpace (double PosRad); - - - bool Home(); - - bool RecoverAfterEmergencyStop(); - - bool Stop(); - - - void setMaxVelocity(float radpersec) - { - m_MaxVel = radpersec; - } - - /** - * Gets the position and velocity. - * @param pdAngleGearRad joint-position in radian - * @param pdVelGearRadS joint-velocity in radian per second - */ - int getGearPosVelRadS(double* pdAngleGearRad, double* pdVelGearRadS); - - /** - * Sets required position and veolocity. - * Use this function only in position mode. - */ - int setGearPosVelRadS(double dPosRad, double dVelRadS); - - /** - * Triggers evaluation of the can-buffer. - */ - int evalCanBuffer(); - - //_double getConfig(); - - double getJointVelocity(); - - - - bool SetMotionCtrlType(int type); - int GetMotionCtrlType(); - bool isError() ; - - - enum { - POS_CTRL, - VEL_CTRL - } MOTION_CTRL_TYPE; - - - CANPeakSysUSB* GetCanCtrl(){return m_CanCtrl;} - //CanESD* GetCanCtrl(){return m_CanCtrl;} - //bool m_ElmoCtrlThreadActive; - /// @brief joint mutexes - //pthread_mutex_t m_cs_elmoCtrlIO; - CanDriveHarmonica * m_Joint; - -private: - - bool sendNetStartCanOpen(CanItf* canCtrl); - - int m_MotionCtrlType; - - DriveParam * m_JointParams; - - int m_CanBaseAddress; - CanOpenAddress m_CanAddress; - - CANPeakSysUSB * m_CanCtrl; - //CanESD * m_CanCtrl; - - double m_MaxVel; - int m_HomingDir; - int m_HomingDigIn; - int m_EncIncrPerRevMot; - int m_MotorDirection; - double m_GearRatio; - - double m_UpperLimit; - double m_LowerLimit; - double m_JointOffset; - /* - Jointd* m_CurrentAngularVelocity; - /// @brief current joint angles - Jointd* m_CurrentJointAngles; - */ - /// @brief joint mutexes - pthread_mutex_t m_Mutex; - /// @brief joint velocity mutexes - //pthread_mutex_t m_AngularVel_Mutex; - /// @brief Thread IDs for PowerCube connection threads - //pthread_t m_ElmoThreadID; - /// @brief Arguments for Powercube connection Threads - //ElmoThreadArgs * m_ElmoCtrlThreadArgs; - ElmoCtrlParams * m_Params; - CanMsg m_CanMsgRec; - - - /// @brief the logfile for debugging info & errors: - -}; - - - - - - - - - -#endif //__ELMO_CTRL_H__ diff --git a/cob_head_axis/common/src/ElmoCtrl.cpp b/cob_head_axis/common/src/ElmoCtrl.cpp deleted file mode 100644 index 29d094d1c..000000000 --- a/cob_head_axis/common/src/ElmoCtrl.cpp +++ /dev/null @@ -1,436 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -#include -#include -#include -#include - -using namespace std; - -void Sleep(int msecs){usleep(1000*msecs);} - -bool ElmoCtrl::sendNetStartCanOpen(CanItf* canCtrl) { - bool ret = false; - - CanMsg msg; - - msg.m_iID = 0; - msg.m_iLen = 2; - msg.set(1,0,0,0,0,0,0,0); - ret = canCtrl->transmitMsg(msg, false); - - usleep(100000); - - return ret; -} - - -ElmoCtrl::ElmoCtrl() { - m_Joint = NULL; - m_JointParams = NULL; - m_CanCtrl = NULL; - m_CanBaseAddress = 0; - m_MotionCtrlType = POS_CTRL; - m_MaxVel = 2.0; - m_Params = NULL; -} - -ElmoCtrl::~ElmoCtrl() { - if (m_Joint) - m_Joint->shutdown(); - delete m_Joint; - if (m_JointParams) - delete m_JointParams; - if (m_CanCtrl) - delete m_CanCtrl; -} - -bool ElmoCtrl::Home() -{ - bool success = false; - if (m_Joint != NULL) { - //THIS is needed for head_axis on cob3-2! - //set input logic to 'general purpose' - m_Joint->IntprtSetInt(8, 'I', 'L', 2, 7); - usleep(20000); - - //THIS is needed for cob3-3 (it's using DIN1): - //set input logic to 'general purpose' - m_Joint->IntprtSetInt(8, 'I', 'L', 1, 6); - usleep(20000); - - m_Joint->initHoming(); - } - - //ToDo: UHR: necessary? - Sleep(10); - printf("ElmoCtrl: Home(): Homing Vel = %f\n",m_HomingDir*0.3); - m_Joint->setGearVelRadS(m_HomingDir*0.3); - //cpc-pk: removed this sleep, it's not necessary to leave home-sensor during homing - //Sleep(750); - success = m_Joint->execHoming(); - m_Joint->setGearVelRadS(0.0); - - return success; -} - - -int ElmoCtrl::evalCanBuffer() { - bool bRet; - - //pthread_mutex_lock(&(m_Mutex)); - - // as long as there is something in the can buffer -> read out next message - while(m_CanCtrl->receiveMsg(&m_CanMsgRec) == true) { - bRet = false; - // check if the message belongs to head_axis motor - bRet |= m_Joint->evalReceivedMsg(m_CanMsgRec); - - if (bRet == true) { - } else std::cout << "cob_head_axis: Unknown CAN-msg: " << m_CanMsgRec.m_iID << " " << (int)m_CanMsgRec.getAt(0) << " " << (int)m_CanMsgRec.getAt(1) << std::endl; - } - - //pthread_mutex_unlock(&(m_Mutex)); - - return 0; -} - -bool ElmoCtrl::RecoverAfterEmergencyStop() { - - bool success = false; - printf("Resetting motor ...\n"); - success = m_Joint->start(); - if (!success) { - printf("failed!\n"); - } else { - printf("successful\n"); - m_Joint->setGearVelRadS(0); - } - Sleep(1000); - return success; -} - - -bool ElmoCtrl::Init(ElmoCtrlParams * params, bool home) { //home = true by default - bool success = false; - string CanIniFile; - string CanDevice; - int baudrate = 0; - - m_Params = params; - - if (params == NULL) { - printf("ElmoCtrlParams:Error:%s:%d:, invalid parameters!\n",__FILE__,__LINE__); - success = false; - } else { - success = true; - } - - if (success) { - printf( "------------ ElmoCtrl Init ---------------\n"); - - //Allocate memory and read params e.g. gotten from ROS parameter server - m_Joint = new CanDriveHarmonica(); - m_JointParams = new DriveParam(); - m_CanBaseAddress = params->GetModuleID(); - CanIniFile = params->GetCanIniFile(); - m_MaxVel = params->GetMaxVel(); - m_HomingDir = params->GetHomingDir(); - m_HomingDigIn = params->GetHomingDigIn(); - - m_EncIncrPerRevMot = params->GetEncoderIncrements(); - m_MotorDirection = params->GetMotorDirection(); - m_GearRatio = params->GetGearRatio(); - - if (CanIniFile.length() == 0) { - printf("%s,%d:Error: Parameter 'CanIniFile' not given!\n",__FILE__,__LINE__); - success = false; - } - - CanDevice = params->GetCanDevice(); - if (CanDevice.length() == 0) { - printf("%s,%d:Error: Parameter 'Can-Device' not given!\n",__FILE__,__LINE__); - success = false; - } - - baudrate = params->GetBaudRate(); - if (baudrate == 0) { - printf("%s,%d:Error: Parameter 'Baud-Rate' not given!\n",__FILE__,__LINE__); - success = false; - } - - //Setting motor model data - if (success) { - m_JointOffset = params->GetAngleOffset(); - m_UpperLimit = params->GetUpperLimit(); - m_LowerLimit = params->GetLowerLimit(); - } - - if (success) { - printf("The following parameters were successfully read from the parameter server (given through *params): \n"); - printf("CanIniFile: %s\n",CanIniFile.c_str()); - printf("CanDieODvice: %s\n",CanDevice.c_str()); - printf("Baudrate: %d\n",baudrate); - printf("Module ID: %d\n",m_CanBaseAddress); - printf("Max Vel: %f\n",m_MaxVel); - printf("Homing Dir: %d\n",m_HomingDir); - printf("HomingDigIn: %d\n",m_HomingDigIn); - printf("Offset/Limit(min/max) %f/(%f,%f)\n",m_JointOffset,m_LowerLimit,m_UpperLimit); - } - } - - //Setting up CAN interface - - if (success) { - //m_CanCtrl = new CanESD(CanIniFile.c_str(), false); - m_CanCtrl = new CANPeakSysUSB(CanIniFile.c_str()); - if (m_CanCtrl == NULL) { - printf("%s,%d:Error: Could not open Can Device!\n",__FILE__,__LINE__); - success = false; - } - } - - if (success) { - /* WRONG CAN-identifiers - //m_CanBaseAddress = params->GetModulID(i); - m_CanAddress.TxPDO1 = 0x181 + m_CanBaseAddress -1; - m_CanAddress.TxPDO2 = 0x285 + m_CanBaseAddress -1; - m_CanAddress.RxPDO2 = 0x301 + m_CanBaseAddress -1; - m_CanAddress.TxSDO = 0x491 + m_CanBaseAddress -1; - m_CanAddress.RxSDO = 0x511 + m_CanBaseAddress -1; - */ - m_CanAddress.TxPDO1 = 0x181 + m_CanBaseAddress -1; - m_CanAddress.TxPDO2 = 0x281 + m_CanBaseAddress -1; - m_CanAddress.RxPDO2 = 0x301 + m_CanBaseAddress -1; - m_CanAddress.TxSDO = 0x581 + m_CanBaseAddress -1; - m_CanAddress.RxSDO = 0x601 + m_CanBaseAddress -1; - m_Joint->setCanItf(m_CanCtrl); - m_Joint->setCanOpenParam(m_CanAddress.TxPDO1, - m_CanAddress.TxPDO2, - m_CanAddress.RxPDO2, - m_CanAddress.TxSDO, - m_CanAddress.RxSDO ); - - printf("CanAdresses set to %d (Base), %x, %x, %x, %x, %x...\n", m_CanBaseAddress, - m_CanAddress.TxPDO1, - m_CanAddress.TxPDO2, - m_CanAddress.RxPDO2, - m_CanAddress.TxSDO, - m_CanAddress.RxSDO); - - } - - if (success) { - success = sendNetStartCanOpen(m_CanCtrl); - } - - if (success) { - //ToDo: Read from File! - /* - int iDriveIdent, - int iEncIncrPerRevMot, - double dVelMeasFrqHz, - double dBeltRatio, - double dGearRatio, - int iSign, - double dVelMaxEncIncrS, - double dAccIncrS2, - double dDecIncrS2, - int iEncOffsetIncr, - bool bIsSteer, - double dCurrToTorque, - double dCurrMax, - int iHomingDigIn - */ - -/* - m_JointParams->setParam( //parameters taken from CanCtrl.ini - 0, //int iDriveIdent, - 4096,//int iEncIncrPerRevMot, - 1,//double dVelMeasFrqHz, - 1,//double dBeltRatio, - 47.77,//double dGearRatio, - -1.0,//int iSign, - 740000,//double dVelMaxEncIncrS, - 1000000,//80000,//double dAccIncrS2, - 1000000,//80000//double dDecIncrS2), - 0, //int iEncOffsetIncr - true, // bool bIsSteer - 0, // double dCurrToTorque - 0, // double dCurrMax - m_HomingDigIn // int iHomingDigIn //cob3-2->11, cob3-1->9 - ); -*/ - - m_JointParams->setParam( //parameters taken from CanCtrl.ini - 0, //int iDriveIdent, - m_EncIncrPerRevMot,//int iEncIncrPerRevMot, - 1,//double dVelMeasFrqHz, - 1,//double dBeltRatio, - m_GearRatio,//double dGearRatio, - m_MotorDirection,//int iSign, - 74000000,//double dVelMaxEncIncrS, - 1000000,//80000,//double dAccIncrS2, - 1000000,//80000//double dDecIncrS2), - 0, //int iEncOffsetIncr - true, // bool bIsSteer - 0, // double dCurrToTorque - 0, // double dm_HomingDigInCurrMax - m_HomingDigIn // int iHomingDigIn //cob3-2->11, cob3-1->9 - ); - - - m_Joint->setDriveParam(*m_JointParams); - } - - if (success) - { - printf("Init motor ...\n"); - success = m_Joint->init(); - if (!success) - { - printf("failed!\n"); - } - else - { - printf("successful\n"); - } - } - - if (success) - success = SetMotionCtrlType(m_MotionCtrlType); - if(!success) std::cout << "Failed to SetMotionCtrlType to " << m_MotionCtrlType << std::endl; - - if (success) - { - printf("Starting motor ..\n"); - success = m_Joint->start(); - if (!success) - { - printf("failed!\n"); - } - else - { - printf("successful\n"); - m_Joint->setGearVelRadS(0); - } - } - //ToDo: UHR: necessary? - Sleep(1000); - - if (success && home) - { - std::cout << "Start homing procedure now.." << std::endl; - success = Home(); - } - //Thread init - if (success) - { - pthread_mutex_init(&m_Mutex,NULL); - } - - - return success; -} - -bool ElmoCtrl::SetMotionCtrlType(int type) -{ - m_MotionCtrlType = type; - bool success = false; - if (type == POS_CTRL) - { - success = m_Joint->shutdown(); - if (success) - success = m_Joint->setTypeMotion(CanDriveHarmonica::MOTIONTYPE_POSCTRL); - - //ToDo: necessary? - Sleep(100); - success = m_Joint->start(); - - } - else if (type == VEL_CTRL) - { - //UHR: ToDo - printf("%s%d:Error: Velocity control not implemented yet!\n",__FILE__,__LINE__); - success = false; - } - return success; -}; - - -int ElmoCtrl::GetMotionCtrlType() -{ - return m_MotionCtrlType; -} - -bool ElmoCtrl::isError() -{ - return m_Joint->isError(); -} - -int ElmoCtrl::getGearPosVelRadS( double* pdAngleGearRad, double* pdVelGearRadS) -{ - - // init default outputs - *pdAngleGearRad = 0; - *pdVelGearRadS = 0; - - m_Joint->getGearPosVelRadS(pdAngleGearRad, pdVelGearRadS); - *pdAngleGearRad = *pdAngleGearRad - m_JointOffset; - - return 0; -} - -//----------------------------------------------- - -int ElmoCtrl:: setGearPosVelRadS(double dPosRad, double dVelRadS) -{ - - if(dPosRad< m_LowerLimit) { - std::cout << "Position under LowerBound -> set up" << std::endl; - dPosRad = m_LowerLimit; - } else if(dPosRad > m_UpperLimit) { - std::cout << "Position over UpperBound -> set down" << std::endl; - dPosRad = m_UpperLimit; - } - - if(dVelRadS > m_MaxVel) - dVelRadS = m_MaxVel; - else if(dVelRadS < -m_MaxVel) - dVelRadS = -m_MaxVel; - - //COB3-2: m_Joint->setGearPosVelRadS(dPosRad + m_JointOffset, dVelRadS); - - //COB3-1: m_Joint->setGearPosVelRadS(-3.141592654 - dPosRad + m_JointOffset, dVelRadS); - printf("ElmoCtrl:setGearPosVelRadS: dPosRad %f with vel %f\n",dPosRad, dVelRadS); - m_Joint->setGearPosVelRadS(m_HomingDir * dPosRad + m_JointOffset, dVelRadS); - return 0; -} - -bool ElmoCtrl::Stop() -{ - //UHR: ToDo: what happens exactly in this method? Sudden stop? - double pos = 0.0; - double vel = 0.0; - m_Joint->getGearPosVelRadS(&pos,&vel); - m_Joint->setGearPosVelRadS(pos,0); - - return true; - //m_Joint[i]->shutdown(); - -} diff --git a/cob_head_axis/package.xml b/cob_head_axis/package.xml deleted file mode 100644 index 1b3691ca0..000000000 --- a/cob_head_axis/package.xml +++ /dev/null @@ -1,30 +0,0 @@ - - cob_head_axis - 0.6.8 - cob_head_axis - - Apache 2.0 - - http://ros.org/wiki/cob_head_axis - - - Matthias Gruhler - Ulrich Reiser - - catkin - - actionlib - cob_canopen_motor - cob_generic_can - cob_srvs - cob_utilities - control_msgs - diagnostic_msgs - roscpp - sensor_msgs - std_srvs - urdf - - rospy - - diff --git a/cob_head_axis/ros/src/cob_head_axis.cpp b/cob_head_axis/ros/src/cob_head_axis.cpp deleted file mode 100644 index 293c62d4f..000000000 --- a/cob_head_axis/ros/src/cob_head_axis.cpp +++ /dev/null @@ -1,560 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -//################## -//#### includes #### - -// standard includes -//-- - -// ROS includes -#include -#include -#include - -// ROS message includes -#include -#include -#include -#include - -// ROS service includes -#include -#include -#include - -// external includes -#include - -#include - -//#################### -//#### node class #### -class NodeClass -{ - // - public: - // create a handle for this node, initialize node - ros::NodeHandle n_; - ros::NodeHandle n_private_; - - // declaration of topics to publish - ros::Publisher topicPub_JointState_; - ros::Publisher topicPub_ControllerState_; - ros::Publisher topicPub_Diagnostic_; - - // declaration of topics to subscribe, callback is called for new messages arriving - ros::Subscriber topicSub_JointCommand_; - - // declaration of service servers - ros::ServiceServer srvServer_Init_; - ros::ServiceServer srvServer_Stop_; - ros::ServiceServer srvServer_Recover_; - ros::ServiceServer srvServer_SetOperationMode_; - ros::ServiceServer srvServer_SetDefaultVel_; - - // declaration of service clients - //-- - - // action lib server - actionlib::SimpleActionServer as_; - std::string action_name_; - control_msgs::FollowJointTrajectoryFeedback feedback_; - control_msgs::FollowJointTrajectoryResult result_; - - // global variables - ElmoCtrl * CamAxis_; - ElmoCtrlParams* CamAxisParams_; - - std::string CanDevice_; - std::string CanIniFile_; - int CanBaudrate_; - int HomingDir_; - int HomingDigIn_; - double MaxVel_; - int ModID_; - std::string operationMode_; - double LowerLimit_; - double UpperLimit_; - double Offset_; - int MotorDirection_; - int EnoderIncrementsPerRevMot_; - double GearRatio_; - - std::string JointName_; - bool isInitialized_; - bool isError_; - bool finished_; - double ActualPos_; - double ActualVel_; - double GoalPos_; - trajectory_msgs::JointTrajectory traj_; - trajectory_msgs::JointTrajectoryPoint traj_point_; - unsigned int traj_point_nr_; - - // Constructor - NodeClass(): - as_(n_, "joint_trajectory_controller/follow_joint_trajectory", boost::bind(&NodeClass::executeCB, this, _1), false), - action_name_("follow_joint_trajectory") - { - n_private_ = ros::NodeHandle("~"); - - isInitialized_ = false; - isError_ = false; - ActualPos_=0.0; - ActualVel_=0.0; - - CamAxis_ = new ElmoCtrl(); - CamAxisParams_ = new ElmoCtrlParams(); - - // implementation of topics to publish - topicPub_JointState_ = n_.advertise("joint_states", 1); - topicPub_ControllerState_ = n_.advertise("joint_trajectory_controller/state", 1); - topicPub_Diagnostic_ = n_.advertise("diagnostics", 1); - - - // implementation of topics to subscribe - - // implementation of service servers - srvServer_Init_ = n_.advertiseService("driver/init", &NodeClass::srvCallback_Init, this); - srvServer_Stop_ = n_.advertiseService("driver/stop", &NodeClass::srvCallback_Stop, this); - srvServer_Recover_ = n_.advertiseService("driver/recover", &NodeClass::srvCallback_Recover, this); - srvServer_SetOperationMode_ = n_.advertiseService("driver/set_operation_mode", &NodeClass::srvCallback_SetOperationMode, this); - srvServer_SetDefaultVel_ = n_.advertiseService("driver/set_default_vel", &NodeClass::srvCallback_SetDefaultVel, this); - - // implementation of service clients - //-- - - // read parameters from parameter server - ROS_INFO("Namespace: %s", n_private_.getNamespace().c_str()); - if(!n_private_.hasParam("EnoderIncrementsPerRevMot")) ROS_WARN("cob_head_axis: couldn't find parameter EnoderIncrementsPerRevMot, check if ALL parameters have been set correctly"); - - n_private_.param("CanDevice", CanDevice_, "PCAN"); - n_private_.param("CanBaudrate", CanBaudrate_, 500); - n_private_.param("HomingDir", HomingDir_, 1); - n_private_.param("HomingDigIn", HomingDigIn_, 11); - n_private_.param("ModId",ModID_, 17); - n_private_.param("JointName",JointName_, "head_axis_joint"); - n_private_.param("CanIniFile",CanIniFile_, "/"); - n_private_.param("operation_mode",operationMode_, "position"); - n_private_.param("MotorDirection",MotorDirection_, 1); - n_private_.param("GearRatio",GearRatio_, 62.5); - n_private_.param("EnoderIncrementsPerRevMot",EnoderIncrementsPerRevMot_, 4096); - - ROS_INFO("CanDevice=%s, CanBaudrate=%d, ModID=%d, HomingDigIn=%d",CanDevice_.c_str(),CanBaudrate_,ModID_,HomingDigIn_); - - - // load parameter server string for robot/model - std::string xml_string; - n_.getParam("/robot_description",xml_string); - if (xml_string.size()==0) - { - ROS_ERROR("Unable to load robot model from param server robot_description\n"); - exit(2); - } - - // extract limits and velocitys from urdf model - urdf::Model model; - if (!model.initString(xml_string)) - { - ROS_ERROR("Failed to parse urdf file"); - exit(2); - } - ROS_INFO("Successfully parsed urdf file"); - - // get LowerLimits out of urdf model - LowerLimit_ = model.getJoint(JointName_.c_str())->limits->lower; - //std::cout << "LowerLimits[" << JointNames[i].c_str() << "] = " << LowerLimits[i] << std::endl; - CamAxisParams_->SetLowerLimit(LowerLimit_); - - // get UpperLimits out of urdf model - UpperLimit_ = model.getJoint(JointName_.c_str())->limits->upper; - //std::cout << "LowerLimits[" << JointNames[i].c_str() << "] = " << LowerLimits[i] << std::endl; - CamAxisParams_->SetUpperLimit(UpperLimit_); - - // get Offset out of urdf model - //calibration rising no longer supported - //Offset_ = model.getJoint(JointName_.c_str())->calibration->rising.get()[0]; - Offset_ = 0.0; - //std::cout << "Offset[" << JointNames[i].c_str() << "] = " << Offsets[i] << std::endl; - CamAxisParams_->SetAngleOffset(Offset_); - - // get velocitiy out of urdf model - MaxVel_ = model.getJoint(JointName_.c_str())->limits->velocity; - ROS_INFO("Successfully read limits from urdf"); - - - //initializing and homing of camera_axis - CamAxisParams_->SetCanIniFile(CanIniFile_); - CamAxisParams_->SetHomingDir(HomingDir_); - CamAxisParams_->SetHomingDigIn(HomingDigIn_); - CamAxisParams_->SetMaxVel(MaxVel_); - CamAxisParams_->SetGearRatio(GearRatio_); - CamAxisParams_->SetMotorDirection(MotorDirection_); - CamAxisParams_->SetEncoderIncrements(EnoderIncrementsPerRevMot_); - - CamAxisParams_->Init(CanDevice_, CanBaudrate_, ModID_); - - - //finally start action_server - as_.start(); - } - - // Destructor - ~NodeClass() - { - delete CamAxis_; - } - - void executeCB(const control_msgs::FollowJointTrajectoryGoalConstPtr &goal) { - if(isInitialized_) { - ROS_INFO("Received new goal trajectory with %lu points",goal->trajectory.points.size()); - // saving goal into local variables - traj_ = goal->trajectory; - traj_point_nr_ = 0; - traj_point_ = traj_.points[traj_point_nr_]; - GoalPos_ = traj_point_.positions[0]; - finished_ = false; - - // stoping axis to prepare for new trajectory - CamAxis_->Stop(); - - // check that preempt has not been requested by the client - if (as_.isPreemptRequested()) - { - ROS_INFO("%s: Preempted %s", n_.getNamespace().c_str(), action_name_.c_str()); - // set the action state to preempted - as_.setPreempted(); - } - - usleep(2000000); // needed sleep until drive starts to change status from idle to moving - - while (not finished_) - { - if (as_.isNewGoalAvailable()) - { - ROS_WARN("%s: Aborted %s", n_.getNamespace().c_str(), action_name_.c_str()); - as_.setAborted(); - return; - } - usleep(10000); - //feedback_ = - //as_.send feedback_ - } - - // set the action state to succeed - //result_.result.data = "executing trajectory"; - ROS_INFO("%s: Succeeded %s", n_.getNamespace().c_str(), action_name_.c_str()); - // set the action state to succeeded - as_.setSucceeded(result_); - - } else { - as_.setAborted(); - ROS_WARN("Camera_axis not initialized yet!"); - } - } - - // service callback functions - // function will be called when a service is querried - bool srvCallback_Init(std_srvs::Trigger::Request &req, - std_srvs::Trigger::Response &res ) - { - if (isInitialized_ == false) { - ROS_INFO("...initializing camera axis..."); - // init axis - if (CamAxis_->Init(CamAxisParams_)) - { - CamAxis_->setGearPosVelRadS(0.0f, MaxVel_); - ROS_INFO("Initializing of camera axis successfully"); - isInitialized_ = true; - res.success = true; - res.message = "initializing camera axis successfully"; - } - else - { - ROS_ERROR("Initializing camera axis not successfully \n"); - res.success = false; - res.message = "initializing camera axis not successfully"; - } - } - else - { - ROS_WARN("...camera axis already initialized..."); - res.success = true; - res.message = "camera axis already initialized"; - } - - return true; - } - - bool srvCallback_Stop(std_srvs::Trigger::Request &req, - std_srvs::Trigger::Response &res ) - { - ROS_INFO("Stopping camera axis"); - if(isInitialized_) - { - // stopping all movements - if (CamAxis_->Stop()) { - ROS_INFO("Stopping camera axis successfully"); - res.success = true; - res.message = "camera axis stopped successfully"; - } - else { - ROS_ERROR("Stopping camera axis not successfully. error"); - res.success = false; - res.message = "stopping camera axis not successfully"; - } - } - return true; - } - - bool srvCallback_Recover(std_srvs::Trigger::Request &req, - std_srvs::Trigger::Response &res ) - { - if (isInitialized_) { - ROS_INFO("Recovering camera axis"); - - // stopping all arm movements - if (CamAxis_->RecoverAfterEmergencyStop()) { - ROS_INFO("Recovering camera axis successfully"); - res.success = true; - res.message = "camera axis successfully recovered"; - } else { - ROS_ERROR("Recovering camera axis not successfully. error"); - res.success = false; - res.message = "recovering camera axis failed"; - } - } else { - ROS_WARN("...camera axis already recovered..."); - res.success = true; - res.message = "camera axis already recovered"; - } - - return true; - } - - /*! - * \brief Executes the service callback for set_operation_mode. - * - * Changes the operation mode. - * \param req Service request - * \param res Service response - */ - bool srvCallback_SetOperationMode( cob_srvs::SetString::Request &req, - cob_srvs::SetString::Response &res ) - { - ROS_INFO("Set operation mode to [%s]", req.data.c_str()); - n_.setParam("operation_mode", req.data.c_str()); - res.success = true; // 0 = true, else = false - return true; - } - - /*! - * \brief Executes the service callback for set_default_vel. - * - * Sets the default velocity. - * \param req Service request - * \param res Service response - */ - bool srvCallback_SetDefaultVel( cob_srvs::SetFloat::Request &req, - cob_srvs::SetFloat::Response &res ) - { - ROS_INFO("Set default velocity to [%f]", req.data); - MaxVel_ = req.data; - CamAxisParams_->SetMaxVel(MaxVel_); - CamAxis_->setMaxVelocity(MaxVel_); - res.success = true; // 0 = true, else = false - return true; - } - - // other function declarations - void updateCommands() - { - if (isInitialized_ == true) - { - if (operationMode_ == "position") - { - ROS_DEBUG("moving head_axis in position mode"); - - if (fabs(ActualVel_) < 0.02) - { - //feedback_.isMoving = false; - - ROS_DEBUG("next point is %d from %lu",traj_point_nr_,traj_.points.size()); - - if (traj_point_nr_ < traj_.points.size()) - { - // if axis is not moving and not reached last point of trajectory, then send new target point - ROS_INFO("...moving to trajectory point[%d], %f",traj_point_nr_,traj_.points[traj_point_nr_].positions[0]); - traj_point_ = traj_.points[traj_point_nr_]; - CamAxis_->setGearPosVelRadS(traj_point_.positions[0], MaxVel_); - usleep(900000); - CamAxis_->m_Joint->requestPosVel(); - traj_point_nr_++; - //feedback_.isMoving = true; - //feedback_.pointNr = traj_point_nr; - //as_.publishFeedback(feedback_); - } - else if ( fabs( fabs(ActualPos_) - fabs(GoalPos_) ) < 0.5*M_PI/180.0 && !finished_ ) - { - ROS_DEBUG("...reached end of trajectory"); - finished_ = true; - } - else - { - //do nothing until GoalPos_ is reached - } - } - else - { - ROS_DEBUG("...axis still moving to point[%d]",traj_point_nr_); - } - } - else if (operationMode_ == "velocity") - { - ROS_WARN("Moving in velocity mode currently disabled"); - } - else - { - ROS_ERROR("axis neither in position nor in velocity mode. OperationMode = [%s]", operationMode_.c_str()); - } - } - else - { - ROS_DEBUG("axis not initialized"); - } - } - - void publishJointState() - { - - if (isInitialized_ == true) { - isError_ = CamAxis_->isError(); - - // create message - int DOF = 1; - - CamAxis_->evalCanBuffer(); - CamAxis_->getGearPosVelRadS(&ActualPos_,&ActualVel_); - CamAxis_->m_Joint->requestPosVel(); - - // really bad hack - ActualPos_ = HomingDir_ * ActualPos_; - ActualVel_ = HomingDir_ * ActualVel_; - - sensor_msgs::JointState msg; - msg.header.stamp = ros::Time::now(); - msg.name.resize(DOF); - msg.position.resize(DOF); - msg.velocity.resize(DOF); - msg.effort.resize(DOF); - - msg.name[0] = JointName_; - msg.position[0] = ActualPos_; - msg.velocity[0] = ActualVel_; - msg.effort[0] = 0.0; - - - //std::cout << "Joint " << msg.name[0] <<": pos="<< msg.position[0] << " vel=" << msg.velocity[0] << std::endl; - - // publish message - topicPub_JointState_.publish(msg); - - // publish controller state message - control_msgs::JointTrajectoryControllerState controllermsg; - controllermsg.header = msg.header; - controllermsg.joint_names.resize(DOF); - controllermsg.desired.positions.resize(DOF); - controllermsg.desired.velocities.resize(DOF); - controllermsg.actual.positions.resize(DOF); - controllermsg.actual.velocities.resize(DOF); - controllermsg.error.positions.resize(DOF); - controllermsg.error.velocities.resize(DOF); - - controllermsg.joint_names = msg.name; - controllermsg.desired.positions = msg.position; - controllermsg.desired.velocities = msg.velocity; - controllermsg.actual.positions = msg.position; - controllermsg.actual.velocities = msg.velocity; - // error, calculated out of desired and actual values - for (int i = 0; ihttp://ros.org/wiki/cob_mimic - Nadia Hammoudeh Garcia + Benjamin Maidel Nadia Hammoudeh Garcia Benjamin Maidel diff --git a/cob_scan_unifier/package.xml b/cob_scan_unifier/package.xml index 5e0c91ce3..9f702535a 100644 --- a/cob_scan_unifier/package.xml +++ b/cob_scan_unifier/package.xml @@ -5,11 +5,10 @@ The cob_scan_unifier package holds code to unify two or more laser-scans to one unified scan-message Benjamin Maidel + Florian Mirus Apache 2.0 - Florian Mirus - catkin laser_geometry diff --git a/cob_sound/package.xml b/cob_sound/package.xml index e3b135be2..73f551484 100644 --- a/cob_sound/package.xml +++ b/cob_sound/package.xml @@ -8,7 +8,7 @@ http://ros.org/wiki/cob_sound - Nadia Hammoudeh Garcia + Benjamin Maidel Florian Weisshardt Benjamin Maidel