From 9e71d55eecb99bfef1ca6728141037cf3aa7db40 Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Thu, 26 May 2016 19:11:01 +0900 Subject: [PATCH 01/13] Update LICENSE License changed (to BSD License) --- LICENSE | 365 ++++---------------------------------------------------- 1 file changed, 26 insertions(+), 339 deletions(-) diff --git a/LICENSE b/LICENSE index 23cb790..1d93559 100644 --- a/LICENSE +++ b/LICENSE @@ -1,339 +1,26 @@ - GNU GENERAL PUBLIC LICENSE - Version 2, June 1991 - - Copyright (C) 1989, 1991 Free Software Foundation, Inc., - 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA - Everyone is permitted to copy and distribute verbatim copies - of this license document, but changing it is not allowed. - - Preamble - - The licenses for most software are designed to take away your -freedom to share and change it. By contrast, the GNU General Public -License is intended to guarantee your freedom to share and change free -software--to make sure the software is free for all its users. This -General Public License applies to most of the Free Software -Foundation's software and to any other program whose authors commit to -using it. (Some other Free Software Foundation software is covered by -the GNU Lesser General Public License instead.) You can apply it to -your programs, too. - - When we speak of free software, we are referring to freedom, not -price. Our General Public Licenses are designed to make sure that you -have the freedom to distribute copies of free software (and charge for -this service if you wish), that you receive source code or can get it -if you want it, that you can change the software or use pieces of it -in new free programs; and that you know you can do these things. - - To protect your rights, we need to make restrictions that forbid -anyone to deny you these rights or to ask you to surrender the rights. -These restrictions translate to certain responsibilities for you if you -distribute copies of the software, or if you modify it. - - For example, if you distribute copies of such a program, whether -gratis or for a fee, you must give the recipients all the rights that -you have. You must make sure that they, too, receive or can get the -source code. And you must show them these terms so they know their -rights. - - We protect your rights with two steps: (1) copyright the software, and -(2) offer you this license which gives you legal permission to copy, -distribute and/or modify the software. - - Also, for each author's protection and ours, we want to make certain -that everyone understands that there is no warranty for this free -software. If the software is modified by someone else and passed on, we -want its recipients to know that what they have is not the original, so -that any problems introduced by others will not reflect on the original -authors' reputations. - - Finally, any free program is threatened constantly by software -patents. We wish to avoid the danger that redistributors of a free -program will individually obtain patent licenses, in effect making the -program proprietary. To prevent this, we have made it clear that any -patent must be licensed for everyone's free use or not licensed at all. - - The precise terms and conditions for copying, distribution and -modification follow. - - GNU GENERAL PUBLIC LICENSE - TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION - - 0. This License applies to any program or other work which contains -a notice placed by the copyright holder saying it may be distributed -under the terms of this General Public License. The "Program", below, -refers to any such program or work, and a "work based on the Program" -means either the Program or any derivative work under copyright law: -that is to say, a work containing the Program or a portion of it, -either verbatim or with modifications and/or translated into another -language. (Hereinafter, translation is included without limitation in -the term "modification".) Each licensee is addressed as "you". - -Activities other than copying, distribution and modification are not -covered by this License; they are outside its scope. The act of -running the Program is not restricted, and the output from the Program -is covered only if its contents constitute a work based on the -Program (independent of having been made by running the Program). -Whether that is true depends on what the Program does. - - 1. You may copy and distribute verbatim copies of the Program's -source code as you receive it, in any medium, provided that you -conspicuously and appropriately publish on each copy an appropriate -copyright notice and disclaimer of warranty; keep intact all the -notices that refer to this License and to the absence of any warranty; -and give any other recipients of the Program a copy of this License -along with the Program. - -You may charge a fee for the physical act of transferring a copy, and -you may at your option offer warranty protection in exchange for a fee. - - 2. You may modify your copy or copies of the Program or any portion -of it, thus forming a work based on the Program, and copy and -distribute such modifications or work under the terms of Section 1 -above, provided that you also meet all of these conditions: - - a) You must cause the modified files to carry prominent notices - stating that you changed the files and the date of any change. - - b) You must cause any work that you distribute or publish, that in - whole or in part contains or is derived from the Program or any - part thereof, to be licensed as a whole at no charge to all third - parties under the terms of this License. - - c) If the modified program normally reads commands interactively - when run, you must cause it, when started running for such - interactive use in the most ordinary way, to print or display an - announcement including an appropriate copyright notice and a - notice that there is no warranty (or else, saying that you provide - a warranty) and that users may redistribute the program under - these conditions, and telling the user how to view a copy of this - License. (Exception: if the Program itself is interactive but - does not normally print such an announcement, your work based on - the Program is not required to print an announcement.) - -These requirements apply to the modified work as a whole. If -identifiable sections of that work are not derived from the Program, -and can be reasonably considered independent and separate works in -themselves, then this License, and its terms, do not apply to those -sections when you distribute them as separate works. But when you -distribute the same sections as part of a whole which is a work based -on the Program, the distribution of the whole must be on the terms of -this License, whose permissions for other licensees extend to the -entire whole, and thus to each and every part regardless of who wrote it. - -Thus, it is not the intent of this section to claim rights or contest -your rights to work written entirely by you; rather, the intent is to -exercise the right to control the distribution of derivative or -collective works based on the Program. - -In addition, mere aggregation of another work not based on the Program -with the Program (or with a work based on the Program) on a volume of -a storage or distribution medium does not bring the other work under -the scope of this License. - - 3. You may copy and distribute the Program (or a work based on it, -under Section 2) in object code or executable form under the terms of -Sections 1 and 2 above provided that you also do one of the following: - - a) Accompany it with the complete corresponding machine-readable - source code, which must be distributed under the terms of Sections - 1 and 2 above on a medium customarily used for software interchange; or, - - b) Accompany it with a written offer, valid for at least three - years, to give any third party, for a charge no more than your - cost of physically performing source distribution, a complete - machine-readable copy of the corresponding source code, to be - distributed under the terms of Sections 1 and 2 above on a medium - customarily used for software interchange; or, - - c) Accompany it with the information you received as to the offer - to distribute corresponding source code. (This alternative is - allowed only for noncommercial distribution and only if you - received the program in object code or executable form with such - an offer, in accord with Subsection b above.) - -The source code for a work means the preferred form of the work for -making modifications to it. For an executable work, complete source -code means all the source code for all modules it contains, plus any -associated interface definition files, plus the scripts used to -control compilation and installation of the executable. However, as a -special exception, the source code distributed need not include -anything that is normally distributed (in either source or binary -form) with the major components (compiler, kernel, and so on) of the -operating system on which the executable runs, unless that component -itself accompanies the executable. - -If distribution of executable or object code is made by offering -access to copy from a designated place, then offering equivalent -access to copy the source code from the same place counts as -distribution of the source code, even though third parties are not -compelled to copy the source along with the object code. - - 4. You may not copy, modify, sublicense, or distribute the Program -except as expressly provided under this License. Any attempt -otherwise to copy, modify, sublicense or distribute the Program is -void, and will automatically terminate your rights under this License. -However, parties who have received copies, or rights, from you under -this License will not have their licenses terminated so long as such -parties remain in full compliance. - - 5. You are not required to accept this License, since you have not -signed it. However, nothing else grants you permission to modify or -distribute the Program or its derivative works. These actions are -prohibited by law if you do not accept this License. Therefore, by -modifying or distributing the Program (or any work based on the -Program), you indicate your acceptance of this License to do so, and -all its terms and conditions for copying, distributing or modifying -the Program or works based on it. - - 6. Each time you redistribute the Program (or any work based on the -Program), the recipient automatically receives a license from the -original licensor to copy, distribute or modify the Program subject to -these terms and conditions. You may not impose any further -restrictions on the recipients' exercise of the rights granted herein. -You are not responsible for enforcing compliance by third parties to -this License. - - 7. If, as a consequence of a court judgment or allegation of patent -infringement or for any other reason (not limited to patent issues), -conditions are imposed on you (whether by court order, agreement or -otherwise) that contradict the conditions of this License, they do not -excuse you from the conditions of this License. If you cannot -distribute so as to satisfy simultaneously your obligations under this -License and any other pertinent obligations, then as a consequence you -may not distribute the Program at all. For example, if a patent -license would not permit royalty-free redistribution of the Program by -all those who receive copies directly or indirectly through you, then -the only way you could satisfy both it and this License would be to -refrain entirely from distribution of the Program. - -If any portion of this section is held invalid or unenforceable under -any particular circumstance, the balance of the section is intended to -apply and the section as a whole is intended to apply in other -circumstances. - -It is not the purpose of this section to induce you to infringe any -patents or other property right claims or to contest validity of any -such claims; this section has the sole purpose of protecting the -integrity of the free software distribution system, which is -implemented by public license practices. Many people have made -generous contributions to the wide range of software distributed -through that system in reliance on consistent application of that -system; it is up to the author/donor to decide if he or she is willing -to distribute software through any other system and a licensee cannot -impose that choice. - -This section is intended to make thoroughly clear what is believed to -be a consequence of the rest of this License. - - 8. If the distribution and/or use of the Program is restricted in -certain countries either by patents or by copyrighted interfaces, the -original copyright holder who places the Program under this License -may add an explicit geographical distribution limitation excluding -those countries, so that distribution is permitted only in or among -countries not thus excluded. In such case, this License incorporates -the limitation as if written in the body of this License. - - 9. The Free Software Foundation may publish revised and/or new versions -of the General Public License from time to time. Such new versions will -be similar in spirit to the present version, but may differ in detail to -address new problems or concerns. - -Each version is given a distinguishing version number. If the Program -specifies a version number of this License which applies to it and "any -later version", you have the option of following the terms and conditions -either of that version or of any later version published by the Free -Software Foundation. If the Program does not specify a version number of -this License, you may choose any version ever published by the Free Software -Foundation. - - 10. If you wish to incorporate parts of the Program into other free -programs whose distribution conditions are different, write to the author -to ask for permission. For software which is copyrighted by the Free -Software Foundation, write to the Free Software Foundation; we sometimes -make exceptions for this. Our decision will be guided by the two goals -of preserving the free status of all derivatives of our free software and -of promoting the sharing and reuse of software generally. - - NO WARRANTY - - 11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY -FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN -OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES -PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED -OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF -MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS -TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE -PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING, -REPAIR OR CORRECTION. - - 12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING -WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR -REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, -INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING -OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED -TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY -YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER -PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE -POSSIBILITY OF SUCH DAMAGES. - - END OF TERMS AND CONDITIONS - - How to Apply These Terms to Your New Programs - - If you develop a new program, and you want it to be of the greatest -possible use to the public, the best way to achieve this is to make it -free software which everyone can redistribute and change under these terms. - - To do so, attach the following notices to the program. It is safest -to attach them to the start of each source file to most effectively -convey the exclusion of warranty; and each file should have at least -the "copyright" line and a pointer to where the full notice is found. - - {description} - Copyright (C) {year} {fullname} - - This program is free software; you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation; either version 2 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License along - with this program; if not, write to the Free Software Foundation, Inc., - 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - -Also add information on how to contact you by electronic and paper mail. - -If the program is interactive, make it output a short notice like this -when it starts in an interactive mode: - - Gnomovision version 69, Copyright (C) year name of author - Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'. - This is free software, and you are welcome to redistribute it - under certain conditions; type `show c' for details. - -The hypothetical commands `show w' and `show c' should show the appropriate -parts of the General Public License. Of course, the commands you use may -be called something other than `show w' and `show c'; they could even be -mouse-clicks or menu items--whatever suits your program. - -You should also get your employer (if you work as a programmer) or your -school, if any, to sign a "copyright disclaimer" for the program, if -necessary. Here is a sample; alter the names: - - Yoyodyne, Inc., hereby disclaims all copyright interest in the program - `Gnomovision' (which makes passes at compilers) written by James Hacker. - - {signature of Ty Coon}, 1 April 1989 - Ty Coon, President of Vice - -This General Public License does not permit incorporating your program into -proprietary programs. If your program is a subroutine library, you may -consider it more useful to permit linking proprietary applications with the -library. If this is what you want to do, use the GNU Lesser General -Public License instead of this License. +Software License Agreement (BSD License) + +Copyright (c) 2014, ROBOTIS Inc. +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of ROBOTIS nor the names of its contributors may be + used to endorse or promote products derived from this software + without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY ROBOTIS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF +MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. +IN NO EVENT SHALL ROBOTIS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, +PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; +OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, +WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR +OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. From 47a7028dd484a99e70d1b1ccf79444684c3bfb86 Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Thu, 26 May 2016 19:39:38 +0900 Subject: [PATCH 02/13] Setting the license to BSD. --- dynamixel_sdk/package.xml | 2 +- robotis_controller/package.xml | 2 +- robotis_controller_msgs/package.xml | 2 +- robotis_device/package.xml | 2 +- robotis_framework_common/package.xml | 2 +- robotis_math/package.xml | 2 +- 6 files changed, 6 insertions(+), 6 deletions(-) diff --git a/dynamixel_sdk/package.xml b/dynamixel_sdk/package.xml index f21a3d3..ae8e683 100644 --- a/dynamixel_sdk/package.xml +++ b/dynamixel_sdk/package.xml @@ -5,7 +5,7 @@ The dynamixel_sdk package robotis - GPLv2 + BSD ROBOTIS diff --git a/robotis_controller/package.xml b/robotis_controller/package.xml index c504764..8a25f7e 100644 --- a/robotis_controller/package.xml +++ b/robotis_controller/package.xml @@ -7,7 +7,7 @@ ROBOTIS - GPLv2 + BSD diff --git a/robotis_controller_msgs/package.xml b/robotis_controller_msgs/package.xml index 88a4148..3ce5db7 100644 --- a/robotis_controller_msgs/package.xml +++ b/robotis_controller_msgs/package.xml @@ -5,7 +5,7 @@ The robotis_controller_msgs package robotis - GPLv2 + BSD diff --git a/robotis_device/package.xml b/robotis_device/package.xml index 06de7e5..be72665 100644 --- a/robotis_device/package.xml +++ b/robotis_device/package.xml @@ -7,7 +7,7 @@ robotis - GPLv2 + BSD diff --git a/robotis_framework_common/package.xml b/robotis_framework_common/package.xml index 7863f09..feec7f8 100644 --- a/robotis_framework_common/package.xml +++ b/robotis_framework_common/package.xml @@ -13,7 +13,7 @@ - TODO + BSD diff --git a/robotis_math/package.xml b/robotis_math/package.xml index 895ab75..506f261 100644 --- a/robotis_math/package.xml +++ b/robotis_math/package.xml @@ -13,7 +13,7 @@ - TODO + BSD From 745144ccfa6f76f2bd65539dc0f67a7dd009b45b Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Fri, 12 Aug 2016 17:37:59 +0900 Subject: [PATCH 03/13] develop branch -> master branch --- dynamixel_sdk/CMakeLists.txt | 38 - dynamixel_sdk/include/DynamixelSDK.h | 29 - .../include/dynamixel_sdk/GroupBulkRead.h | 61 - .../include/dynamixel_sdk/GroupBulkWrite.h | 57 - .../include/dynamixel_sdk/GroupSyncRead.h | 61 - .../include/dynamixel_sdk/GroupSyncWrite.h | 56 - .../include/dynamixel_sdk/PacketHandler.h | 134 -- .../include/dynamixel_sdk/PortHandler.h | 60 - .../dynamixel_sdk/Protocol1PacketHandler.h | 98 - .../dynamixel_sdk/Protocol2PacketHandler.h | 103 - .../include/dynamixel_sdk/RobotisDef.h | 21 - .../dynamixel_sdk_linux/PortHandlerLinux.h | 62 - dynamixel_sdk/package.xml | 15 - .../src/dynamixel_sdk/GroupBulkRead.cpp | 199 -- .../src/dynamixel_sdk/GroupBulkWrite.cpp | 137 -- .../src/dynamixel_sdk/GroupSyncRead.cpp | 172 -- .../src/dynamixel_sdk/GroupSyncWrite.cpp | 117 - .../src/dynamixel_sdk/PacketHandler.cpp | 25 - .../src/dynamixel_sdk/PortHandler.cpp | 32 - .../dynamixel_sdk/Protocol1PacketHandler.cpp | 670 ------ .../dynamixel_sdk/Protocol2PacketHandler.cpp | 985 -------- .../dynamixel_sdk_linux/PortHandlerLinux.cpp | 245 -- robotis_controller/CMakeLists.txt | 22 +- .../robotis_controller/RobotisController.h | 145 -- .../robotis_controller/robotis_controller.h | 183 ++ robotis_controller/package.xml | 4 - .../robotis_controller/RobotisController.cpp | 1592 ------------ .../robotis_controller/robotis_controller.cpp | 2144 +++++++++++++++++ robotis_controller_msgs/CMakeLists.txt | 48 - .../msg/JointCtrlModule.msg | 2 - robotis_controller_msgs/msg/StatusMsg.msg | 10 - robotis_controller_msgs/msg/SyncWriteItem.msg | 3 - robotis_controller_msgs/package.xml | 30 - .../srv/GetJointModule.srv | 4 - robotis_device/CMakeLists.txt | 35 +- .../devices/dynamixel/GRIPPER.device | 3 + .../devices/dynamixel/H42-20-S300-R.device | 5 +- .../devices/dynamixel/H54-100-S500-R.device | 5 +- .../devices/dynamixel/H54-200-B500-R.device | 5 +- .../devices/dynamixel/H54-200-S500-R.device | 5 +- .../devices/dynamixel/L42-10-S300-R.device | 3 + .../devices/dynamixel/L54-30-S400-R.device | 5 +- .../devices/dynamixel/L54-30-S500-R.device | 5 +- .../devices/dynamixel/L54-50-S290-R.device | 5 +- .../devices/dynamixel/L54-50-S500-R.device | 5 +- .../devices/dynamixel/M42-10-S260-R.device | 5 +- .../devices/dynamixel/M54-40-S250-R.device | 5 +- .../devices/dynamixel/M54-60-S250-R.device | 5 +- .../devices/dynamixel/MX-106.device | 66 + robotis_device/devices/dynamixel/MX-28.device | 5 +- robotis_device/devices/dynamixel/MX-64.device | 66 + .../devices/dynamixel/XM-430-W210.device | 81 + .../devices/dynamixel/XM-430-W350.device | 81 + .../devices/dynamixel/XM-430.device | 9 +- .../include/robotis_device/ControlTableItem.h | 54 - .../include/robotis_device/Device.h | 37 - .../include/robotis_device/Dynamixel.h | 60 - .../include/robotis_device/DynamixelState.h | 55 - robotis_device/include/robotis_device/Robot.h | 38 - .../include/robotis_device/Sensor.h | 32 - .../include/robotis_device/SensorState.h | 35 - .../include/robotis_device/TimeStamp.h | 25 - .../robotis_device/control_table_item.h | 84 + .../include/robotis_device/device.h | 68 + .../include/robotis_device/dynamixel.h | 97 + .../include/robotis_device/dynamixel_state.h | 96 + robotis_device/include/robotis_device/robot.h | 78 + .../include/robotis_device/sensor.h | 63 + .../include/robotis_device/sensor_state.h | 64 + .../include/robotis_device/time_stamp.h | 59 + robotis_device/package.xml | 10 - .../src/robotis_device/Dynamixel.cpp | 110 - robotis_device/src/robotis_device/Robot.cpp | 408 ---- robotis_device/src/robotis_device/Sensor.cpp | 23 - .../src/robotis_device/dynamixel.cpp | 152 ++ robotis_device/src/robotis_device/robot.cpp | 457 ++++ robotis_device/src/robotis_device/sensor.cpp | 53 + robotis_framework_common/CMakeLists.txt | 38 - .../robotis_framework_common/MotionModule.h | 51 - .../robotis_framework_common/RobotisDef.h | 21 - .../robotis_framework_common/SensorModule.h | 38 - .../robotis_framework_common/Singleton.h | 49 - .../robotis_framework_common/motion_module.h | 101 + .../robotis_framework_common/sensor_module.h | 71 + .../robotis_framework_common/singleton.h | 79 + robotis_framework_common/package.xml | 45 +- robotis_math/CMakeLists.txt | 65 - .../robotis_math/RobotisLinearAlgebra.h | 52 - .../include/robotis_math/RobotisMath.h | 13 - .../include/robotis_math/RobotisMathBase.h | 33 - .../RobotisTrajectoryCalculator.h | 37 - robotis_math/package.xml | 37 - robotis_math/src/RobotisLinearAlgebra.cpp | 291 --- robotis_math/src/RobotisMathBase.cpp | 26 - .../src/RobotisTrajectoryCalculator.cpp | 325 --- 95 files changed, 4206 insertions(+), 7292 deletions(-) delete mode 100644 dynamixel_sdk/CMakeLists.txt delete mode 100644 dynamixel_sdk/include/DynamixelSDK.h delete mode 100644 dynamixel_sdk/include/dynamixel_sdk/GroupBulkRead.h delete mode 100644 dynamixel_sdk/include/dynamixel_sdk/GroupBulkWrite.h delete mode 100644 dynamixel_sdk/include/dynamixel_sdk/GroupSyncRead.h delete mode 100644 dynamixel_sdk/include/dynamixel_sdk/GroupSyncWrite.h delete mode 100644 dynamixel_sdk/include/dynamixel_sdk/PacketHandler.h delete mode 100644 dynamixel_sdk/include/dynamixel_sdk/PortHandler.h delete mode 100644 dynamixel_sdk/include/dynamixel_sdk/Protocol1PacketHandler.h delete mode 100644 dynamixel_sdk/include/dynamixel_sdk/Protocol2PacketHandler.h delete mode 100644 dynamixel_sdk/include/dynamixel_sdk/RobotisDef.h delete mode 100644 dynamixel_sdk/include/dynamixel_sdk_linux/PortHandlerLinux.h delete mode 100644 dynamixel_sdk/package.xml delete mode 100644 dynamixel_sdk/src/dynamixel_sdk/GroupBulkRead.cpp delete mode 100644 dynamixel_sdk/src/dynamixel_sdk/GroupBulkWrite.cpp delete mode 100644 dynamixel_sdk/src/dynamixel_sdk/GroupSyncRead.cpp delete mode 100644 dynamixel_sdk/src/dynamixel_sdk/GroupSyncWrite.cpp delete mode 100644 dynamixel_sdk/src/dynamixel_sdk/PacketHandler.cpp delete mode 100644 dynamixel_sdk/src/dynamixel_sdk/PortHandler.cpp delete mode 100644 dynamixel_sdk/src/dynamixel_sdk/Protocol1PacketHandler.cpp delete mode 100644 dynamixel_sdk/src/dynamixel_sdk/Protocol2PacketHandler.cpp delete mode 100644 dynamixel_sdk/src/dynamixel_sdk_linux/PortHandlerLinux.cpp delete mode 100644 robotis_controller/include/robotis_controller/RobotisController.h create mode 100644 robotis_controller/include/robotis_controller/robotis_controller.h delete mode 100644 robotis_controller/src/robotis_controller/RobotisController.cpp create mode 100644 robotis_controller/src/robotis_controller/robotis_controller.cpp delete mode 100644 robotis_controller_msgs/CMakeLists.txt delete mode 100644 robotis_controller_msgs/msg/JointCtrlModule.msg delete mode 100644 robotis_controller_msgs/msg/StatusMsg.msg delete mode 100644 robotis_controller_msgs/msg/SyncWriteItem.msg delete mode 100644 robotis_controller_msgs/package.xml delete mode 100644 robotis_controller_msgs/srv/GetJointModule.srv create mode 100644 robotis_device/devices/dynamixel/MX-106.device create mode 100644 robotis_device/devices/dynamixel/MX-64.device create mode 100644 robotis_device/devices/dynamixel/XM-430-W210.device create mode 100644 robotis_device/devices/dynamixel/XM-430-W350.device delete mode 100644 robotis_device/include/robotis_device/ControlTableItem.h delete mode 100644 robotis_device/include/robotis_device/Device.h delete mode 100644 robotis_device/include/robotis_device/Dynamixel.h delete mode 100644 robotis_device/include/robotis_device/DynamixelState.h delete mode 100644 robotis_device/include/robotis_device/Robot.h delete mode 100644 robotis_device/include/robotis_device/Sensor.h delete mode 100644 robotis_device/include/robotis_device/SensorState.h delete mode 100644 robotis_device/include/robotis_device/TimeStamp.h create mode 100644 robotis_device/include/robotis_device/control_table_item.h create mode 100644 robotis_device/include/robotis_device/device.h create mode 100644 robotis_device/include/robotis_device/dynamixel.h create mode 100644 robotis_device/include/robotis_device/dynamixel_state.h create mode 100644 robotis_device/include/robotis_device/robot.h create mode 100644 robotis_device/include/robotis_device/sensor.h create mode 100644 robotis_device/include/robotis_device/sensor_state.h create mode 100644 robotis_device/include/robotis_device/time_stamp.h delete mode 100644 robotis_device/src/robotis_device/Dynamixel.cpp delete mode 100644 robotis_device/src/robotis_device/Robot.cpp delete mode 100644 robotis_device/src/robotis_device/Sensor.cpp create mode 100644 robotis_device/src/robotis_device/dynamixel.cpp create mode 100644 robotis_device/src/robotis_device/robot.cpp create mode 100644 robotis_device/src/robotis_device/sensor.cpp delete mode 100644 robotis_framework_common/include/robotis_framework_common/MotionModule.h delete mode 100644 robotis_framework_common/include/robotis_framework_common/RobotisDef.h delete mode 100644 robotis_framework_common/include/robotis_framework_common/SensorModule.h delete mode 100644 robotis_framework_common/include/robotis_framework_common/Singleton.h create mode 100644 robotis_framework_common/include/robotis_framework_common/motion_module.h create mode 100644 robotis_framework_common/include/robotis_framework_common/sensor_module.h create mode 100644 robotis_framework_common/include/robotis_framework_common/singleton.h delete mode 100644 robotis_math/CMakeLists.txt delete mode 100644 robotis_math/include/robotis_math/RobotisLinearAlgebra.h delete mode 100644 robotis_math/include/robotis_math/RobotisMath.h delete mode 100644 robotis_math/include/robotis_math/RobotisMathBase.h delete mode 100644 robotis_math/include/robotis_math/RobotisTrajectoryCalculator.h delete mode 100644 robotis_math/package.xml delete mode 100644 robotis_math/src/RobotisLinearAlgebra.cpp delete mode 100644 robotis_math/src/RobotisMathBase.cpp delete mode 100644 robotis_math/src/RobotisTrajectoryCalculator.cpp diff --git a/dynamixel_sdk/CMakeLists.txt b/dynamixel_sdk/CMakeLists.txt deleted file mode 100644 index b3b5c84..0000000 --- a/dynamixel_sdk/CMakeLists.txt +++ /dev/null @@ -1,38 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(dynamixel_sdk) - -find_package(catkin REQUIRED COMPONENTS - roscpp -) - -catkin_package( - INCLUDE_DIRS include - LIBRARIES dynamixel_sdk -# CATKIN_DEPENDS roscpp -# DEPENDS system_lib -) - -# include_directories(include) -include_directories( - include - ${catkin_INCLUDE_DIRS} -) - -## Declare a C++ library -add_library(dynamixel_sdk - src/${PROJECT_NAME}/PacketHandler.cpp - src/${PROJECT_NAME}/Protocol1PacketHandler.cpp - src/${PROJECT_NAME}/Protocol2PacketHandler.cpp - src/${PROJECT_NAME}/GroupSyncRead.cpp - src/${PROJECT_NAME}/GroupSyncWrite.cpp - src/${PROJECT_NAME}/GroupBulkRead.cpp - src/${PROJECT_NAME}/GroupBulkWrite.cpp - src/${PROJECT_NAME}/PortHandler.cpp - src/${PROJECT_NAME}_linux/PortHandlerLinux.cpp -) - -## Specify libraries to link a library or executable target against -target_link_libraries(dynamixel_sdk - ${catkin_LIBRARIES} -) - diff --git a/dynamixel_sdk/include/DynamixelSDK.h b/dynamixel_sdk/include/DynamixelSDK.h deleted file mode 100644 index 0d34655..0000000 --- a/dynamixel_sdk/include/DynamixelSDK.h +++ /dev/null @@ -1,29 +0,0 @@ -/* - * DynamixelSDK.h - * - * Created on: 2016. 3. 8. - * Author: zerom, leon - */ - -#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_DYNAMIXELSDK_H_ -#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_DYNAMIXELSDK_H_ - - -#include "dynamixel_sdk/RobotisDef.h" -#include "dynamixel_sdk/GroupBulkRead.h" -#include "dynamixel_sdk/GroupBulkWrite.h" -#include "dynamixel_sdk/GroupSyncRead.h" -#include "dynamixel_sdk/GroupSyncWrite.h" -#include "dynamixel_sdk/Protocol1PacketHandler.h" -#include "dynamixel_sdk/Protocol2PacketHandler.h" - -#ifdef __linux__ - #include "dynamixel_sdk_linux/PortHandlerLinux.h" -#endif - -#if defined(_WIN32) || defined(_WIN64) - #include "dynamixel_sdk_windows/PortHandlerWindows.h" -#endif - - -#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_DYNAMIXELSDK_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/GroupBulkRead.h b/dynamixel_sdk/include/dynamixel_sdk/GroupBulkRead.h deleted file mode 100644 index bd3374b..0000000 --- a/dynamixel_sdk/include/dynamixel_sdk/GroupBulkRead.h +++ /dev/null @@ -1,61 +0,0 @@ -/* - * GroupBulkRead.h - * - * Created on: 2016. 1. 28. - * Author: zerom, leon - */ - -#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKREAD_H_ -#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKREAD_H_ - - -#include -#include -#include "RobotisDef.h" -#include "PortHandler.h" -#include "PacketHandler.h" - -namespace ROBOTIS -{ - -class WINDECLSPEC GroupBulkRead -{ -private: - PortHandler *port_; - PacketHandler *ph_; - - std::vector id_list_; - std::map address_list_; // - std::map length_list_; // - std::map data_list_; // - - bool last_result_; - bool is_param_changed_; - - UINT8_T *param_; - - void MakeParam(); - -public: - GroupBulkRead(PortHandler *port, PacketHandler *ph); - ~GroupBulkRead() { ClearParam(); } - - PortHandler *GetPortHandler() { return port_; } - PacketHandler *GetPacketHandler() { return ph_; } - - bool AddParam (UINT8_T id, UINT16_T start_address, UINT16_T data_length); - void RemoveParam (UINT8_T id); - void ClearParam (); - - int TxPacket(); - int RxPacket(); - int TxRxPacket(); - - bool IsAvailable (UINT8_T id, UINT16_T address, UINT16_T data_length); - UINT32_T GetData (UINT8_T id, UINT16_T address, UINT16_T data_length); -}; - -} - - -#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKREAD_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/GroupBulkWrite.h b/dynamixel_sdk/include/dynamixel_sdk/GroupBulkWrite.h deleted file mode 100644 index 3efbb80..0000000 --- a/dynamixel_sdk/include/dynamixel_sdk/GroupBulkWrite.h +++ /dev/null @@ -1,57 +0,0 @@ -/* - * GroupBulkWrite.h - * - * Created on: 2016. 2. 2. - * Author: zerom, leon - */ - -#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKWRITE_H_ -#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKWRITE_H_ - - -#include -#include -#include "RobotisDef.h" -#include "PortHandler.h" -#include "PacketHandler.h" - -namespace ROBOTIS -{ - -class WINDECLSPEC GroupBulkWrite -{ -private: - PortHandler *port_; - PacketHandler *ph_; - - std::vector id_list_; - std::map address_list_; // - std::map length_list_; // - std::map data_list_; // - - bool is_param_changed_; - - UINT8_T *param_; - UINT16_T param_length_; - - void MakeParam(); - -public: - GroupBulkWrite(PortHandler *port, PacketHandler *ph); - ~GroupBulkWrite() { ClearParam(); } - - PortHandler *GetPortHandler() { return port_; } - PacketHandler *GetPacketHandler() { return ph_; } - - bool AddParam (UINT8_T id, UINT16_T start_address, UINT16_T data_length, UINT8_T *data); - void RemoveParam (UINT8_T id); - bool ChangeParam (UINT8_T id, UINT16_T start_address, UINT16_T data_length, UINT8_T *data); - void ClearParam (); - - int TxPacket(); -}; - -} - - -#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKWRITE_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/GroupSyncRead.h b/dynamixel_sdk/include/dynamixel_sdk/GroupSyncRead.h deleted file mode 100644 index b6599de..0000000 --- a/dynamixel_sdk/include/dynamixel_sdk/GroupSyncRead.h +++ /dev/null @@ -1,61 +0,0 @@ -/* - * GroupSyncRead.h - * - * Created on: 2016. 2. 2. - * Author: zerom, leon - */ - -#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCREAD_H_ -#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCREAD_H_ - - -#include -#include -#include "RobotisDef.h" -#include "PortHandler.h" -#include "PacketHandler.h" - -namespace ROBOTIS -{ - -class WINDECLSPEC GroupSyncRead -{ -private: - PortHandler *port_; - PacketHandler *ph_; - - std::vector id_list_; - std::map data_list_; // - - bool last_result_; - bool is_param_changed_; - - UINT8_T *param_; - UINT16_T start_address_; - UINT16_T data_length_; - - void MakeParam(); - -public: - GroupSyncRead(PortHandler *port, PacketHandler *ph, UINT16_T start_address, UINT16_T data_length); - ~GroupSyncRead() { ClearParam(); } - - PortHandler *GetPortHandler() { return port_; } - PacketHandler *GetPacketHandler() { return ph_; } - - bool AddParam (UINT8_T id); - void RemoveParam (UINT8_T id); - void ClearParam (); - - int TxPacket(); - int RxPacket(); - int TxRxPacket(); - - bool IsAvailable (UINT8_T id, UINT16_T address, UINT16_T data_length); - UINT32_T GetData (UINT8_T id, UINT16_T address, UINT16_T data_length); -}; - -} - - -#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCREAD_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/GroupSyncWrite.h b/dynamixel_sdk/include/dynamixel_sdk/GroupSyncWrite.h deleted file mode 100644 index 1ff03b2..0000000 --- a/dynamixel_sdk/include/dynamixel_sdk/GroupSyncWrite.h +++ /dev/null @@ -1,56 +0,0 @@ -/* - * GroupSyncWrite.h - * - * Created on: 2016. 1. 28. - * Author: zerom, leon - */ - -#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCWRITE_H_ -#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCWRITE_H_ - - -#include -#include -#include "RobotisDef.h" -#include "PortHandler.h" -#include "PacketHandler.h" - -namespace ROBOTIS -{ - -class WINDECLSPEC GroupSyncWrite -{ -private: - PortHandler *port_; - PacketHandler *ph_; - - std::vector id_list_; - std::map data_list_; // - - bool is_param_changed_; - - UINT8_T *param_; - UINT16_T start_address_; - UINT16_T data_length_; - - void MakeParam(); - -public: - GroupSyncWrite(PortHandler *port, PacketHandler *ph, UINT16_T start_address, UINT16_T data_length); - ~GroupSyncWrite() { ClearParam(); } - - PortHandler *GetPortHandler() { return port_; } - PacketHandler *GetPacketHandler() { return ph_; } - - bool AddParam (UINT8_T id, UINT8_T *data); - void RemoveParam (UINT8_T id); - bool ChangeParam (UINT8_T id, UINT8_T *data); - void ClearParam (); - - int TxPacket(); -}; - -} - - -#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCWRITE_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/PacketHandler.h b/dynamixel_sdk/include/dynamixel_sdk/PacketHandler.h deleted file mode 100644 index 921d380..0000000 --- a/dynamixel_sdk/include/dynamixel_sdk/PacketHandler.h +++ /dev/null @@ -1,134 +0,0 @@ -/* - * PacketHandler.h - * - * Created on: 2016. 1. 26. - * Author: zerom, leon - */ - -#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PACKETHANDLER_H_ -#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PACKETHANDLER_H_ - - -#include -#include -#include "RobotisDef.h" -#include "PortHandler.h" - -#define BROADCAST_ID 0xFE // 254 -#define MAX_ID 0xFC // 252 - -/* Macro for Control Table Value */ -#define DXL_MAKEWORD(a, b) ((unsigned short)(((unsigned char)(((unsigned long)(a)) & 0xff)) | ((unsigned short)((unsigned char)(((unsigned long)(b)) & 0xff))) << 8)) -#define DXL_MAKEDWORD(a, b) ((unsigned int)(((unsigned short)(((unsigned long)(a)) & 0xffff)) | ((unsigned int)((unsigned short)(((unsigned long)(b)) & 0xffff))) << 16)) -#define DXL_LOWORD(l) ((unsigned short)(((unsigned long)(l)) & 0xffff)) -#define DXL_HIWORD(l) ((unsigned short)((((unsigned long)(l)) >> 16) & 0xffff)) -#define DXL_LOBYTE(w) ((unsigned char)(((unsigned long)(w)) & 0xff)) -#define DXL_HIBYTE(w) ((unsigned char)((((unsigned long)(w)) >> 8) & 0xff)) - -/* Instruction for DXL Protocol */ -#define INST_PING 1 -#define INST_READ 2 -#define INST_WRITE 3 -#define INST_REG_WRITE 4 -#define INST_ACTION 5 -#define INST_FACTORY_RESET 6 -#define INST_SYNC_WRITE 131 // 0x83 -#define INST_BULK_READ 146 // 0x92 -// --- Only for 2.0 --- // -#define INST_REBOOT 8 -#define INST_STATUS 85 // 0x55 -#define INST_SYNC_READ 130 // 0x82 -#define INST_BULK_WRITE 147 // 0x93 - -// Communication Result -#define COMM_SUCCESS 0 // tx or rx packet communication success -#define COMM_PORT_BUSY -1000 // Port is busy (in use) -#define COMM_TX_FAIL -1001 // Failed transmit instruction packet -#define COMM_RX_FAIL -1002 // Failed get status packet -#define COMM_TX_ERROR -2000 // Incorrect instruction packet -#define COMM_RX_WAITING -3000 // Now recieving status packet -#define COMM_RX_TIMEOUT -3001 // There is no status packet -#define COMM_RX_CORRUPT -3002 // Incorrect status packet -#define COMM_NOT_AVAILABLE -9000 // - -namespace ROBOTIS -{ - -class WINDECLSPEC PacketHandler -{ -protected: - PacketHandler() { } - -public: - static PacketHandler *GetPacketHandler(float protocol_version = 2.0); - - virtual ~PacketHandler() { } - - virtual float GetProtocolVersion() = 0; - - virtual void PrintTxRxResult(int result) = 0; - virtual void PrintRxPacketError(UINT8_T error) = 0; - - virtual int TxPacket (PortHandler *port, UINT8_T *txpacket) = 0; - virtual int RxPacket (PortHandler *port, UINT8_T *rxpacket) = 0; - virtual int TxRxPacket (PortHandler *port, UINT8_T *txpacket, UINT8_T *rxpacket, UINT8_T *error = 0) = 0; - - virtual int Ping (PortHandler *port, UINT8_T id, UINT8_T *error = 0) = 0; - virtual int Ping (PortHandler *port, UINT8_T id, UINT16_T *model_number, UINT8_T *error = 0) = 0; - - // BroadcastPing - virtual int BroadcastPing (PortHandler *port, std::vector &id_list) = 0; - - virtual int Action (PortHandler *port, UINT8_T id) = 0; - virtual int Reboot (PortHandler *port, UINT8_T id, UINT8_T *error = 0) = 0; - virtual int FactoryReset (PortHandler *port, UINT8_T id, UINT8_T option = 0, UINT8_T *error = 0) = 0; - - - virtual int ReadTx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length) = 0; - virtual int ReadRx (PortHandler *port, UINT16_T length, UINT8_T *data, UINT8_T *error = 0) = 0; - virtual int ReadTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0) = 0; - - virtual int Read1ByteTx (PortHandler *port, UINT8_T id, UINT16_T address) = 0; - virtual int Read1ByteRx (PortHandler *port, UINT8_T *data, UINT8_T *error = 0) = 0; - virtual int Read1ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T *data, UINT8_T *error = 0) = 0; - - virtual int Read2ByteTx (PortHandler *port, UINT8_T id, UINT16_T address) = 0; - virtual int Read2ByteRx (PortHandler *port, UINT16_T *data, UINT8_T *error = 0) = 0; - virtual int Read2ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T *data, UINT8_T *error = 0) = 0; - - virtual int Read4ByteTx (PortHandler *port, UINT8_T id, UINT16_T address) = 0; - virtual int Read4ByteRx (PortHandler *port, UINT32_T *data, UINT8_T *error = 0) = 0; - virtual int Read4ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T *data, UINT8_T *error = 0) = 0; - - virtual int WriteTxOnly (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data) = 0; - virtual int WriteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0) = 0; - - virtual int Write1ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T data) = 0; - virtual int Write1ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T data, UINT8_T *error = 0) = 0; - - virtual int Write2ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T data) = 0; - virtual int Write2ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T data, UINT8_T *error = 0) = 0; - - virtual int Write4ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T data) = 0; - virtual int Write4ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T data, UINT8_T *error = 0) = 0; - - virtual int RegWriteTxOnly (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data) = 0; - virtual int RegWriteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0) = 0; - - virtual int SyncReadTx (PortHandler *port, UINT16_T start_address, UINT16_T data_length, UINT8_T *param, UINT16_T param_length) = 0; - // SyncReadRx -> GroupSyncRead class - // SyncReadTxRx -> GroupSyncRead class - - virtual int SyncWriteTxOnly (PortHandler *port, UINT16_T start_address, UINT16_T data_length, UINT8_T *param, UINT16_T param_length) = 0; - - virtual int BulkReadTx (PortHandler *port, UINT8_T *param, UINT16_T param_length) = 0; - // BulkReadRx -> GroupBulkRead class - // BulkReadTxRx -> GroupBulkRead class - - virtual int BulkWriteTxOnly (PortHandler *port, UINT8_T *param, UINT16_T param_length) = 0; -}; - -} - - -#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PACKETHANDLER_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/PortHandler.h b/dynamixel_sdk/include/dynamixel_sdk/PortHandler.h deleted file mode 100644 index 6d3e6dd..0000000 --- a/dynamixel_sdk/include/dynamixel_sdk/PortHandler.h +++ /dev/null @@ -1,60 +0,0 @@ -/* - * PortHandler.h - * - * Created on: 2016. 1. 26. - * Author: zerom, leon - */ - -#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PORTHANDLER_H_ -#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PORTHANDLER_H_ - -#ifdef __linux__ -#define WINDECLSPEC -#elif defined(_WIN32) || defined(_WIN64) -#ifdef WINDLLEXPORT -#define WINDECLSPEC __declspec(dllexport) -#else -#define WINDECLSPEC __declspec(dllimport) -#endif -#endif - -#include "RobotisDef.h" - -namespace ROBOTIS -{ - -class WINDECLSPEC PortHandler -{ -public: - static const int DEFAULT_BAUDRATE = 1000000; - - static PortHandler *GetPortHandler(const char *port_name); - - bool is_using; - - virtual ~PortHandler() { } - - virtual bool OpenPort() = 0; - virtual void ClosePort() = 0; - virtual void ClearPort() = 0; - - virtual void SetPortName(const char* port_name) = 0; - virtual char *GetPortName() = 0; - - virtual bool SetBaudRate(const int baudrate) = 0; - virtual int GetBaudRate() = 0; - - virtual int GetBytesAvailable() = 0; - - virtual int ReadPort(UINT8_T *packet, int length) = 0; - virtual int WritePort(UINT8_T *packet, int length) = 0; - - virtual void SetPacketTimeout(UINT16_T packet_length) = 0; - virtual void SetPacketTimeout(double msec) = 0; - virtual bool IsPacketTimeout() = 0; -}; - -} - - -#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PORTHANDLER_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/Protocol1PacketHandler.h b/dynamixel_sdk/include/dynamixel_sdk/Protocol1PacketHandler.h deleted file mode 100644 index 1932d27..0000000 --- a/dynamixel_sdk/include/dynamixel_sdk/Protocol1PacketHandler.h +++ /dev/null @@ -1,98 +0,0 @@ -/* - * Protocol1PacketHandler.h - * - * Created on: 2016. 1. 26. - * Author: zerom, leon - */ - -#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL1PACKETHANDLER_H_ -#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL1PACKETHANDLER_H_ - - -#include "PacketHandler.h" - -namespace ROBOTIS -{ - -class WINDECLSPEC Protocol1PacketHandler : public PacketHandler -{ -private: - static Protocol1PacketHandler *unique_instance_; - - Protocol1PacketHandler(); - -public: - static Protocol1PacketHandler *GetInstance() { return unique_instance_; } - - virtual ~Protocol1PacketHandler() { } - - float GetProtocolVersion() { return 1.0; } - - void PrintTxRxResult(int result); - void PrintRxPacketError(UINT8_T error); - - int TxPacket (PortHandler *port, UINT8_T *txpacket); - int RxPacket (PortHandler *port, UINT8_T *rxpacket); - int TxRxPacket (PortHandler *port, UINT8_T *txpacket, UINT8_T *rxpacket, UINT8_T *error = 0); - - int Ping (PortHandler *port, UINT8_T id, UINT8_T *error = 0); - int Ping (PortHandler *port, UINT8_T id, UINT16_T *model_number, UINT8_T *error = 0); - - // BroadcastPing - int BroadcastPing (PortHandler *port, std::vector &id_list); - - int Action (PortHandler *port, UINT8_T id); - int Reboot (PortHandler *port, UINT8_T id, UINT8_T *error = 0); - int FactoryReset (PortHandler *port, UINT8_T id, UINT8_T option, UINT8_T *error = 0); - - - int ReadTx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length); - int ReadRx (PortHandler *port, UINT16_T length, UINT8_T *data, UINT8_T *error = 0); - int ReadTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0); - - int Read1ByteTx (PortHandler *port, UINT8_T id, UINT16_T address); - int Read1ByteRx (PortHandler *port, UINT8_T *data, UINT8_T *error = 0); - int Read1ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T *data, UINT8_T *error = 0); - - int Read2ByteTx (PortHandler *port, UINT8_T id, UINT16_T address); - int Read2ByteRx (PortHandler *port, UINT16_T *data, UINT8_T *error = 0); - int Read2ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T *data, UINT8_T *error = 0); - - int Read4ByteTx (PortHandler *port, UINT8_T id, UINT16_T address); - int Read4ByteRx (PortHandler *port, UINT32_T *data, UINT8_T *error = 0); - int Read4ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T *data, UINT8_T *error = 0); - - int WriteTxOnly (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data); - int WriteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0); - - int Write1ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T data); - int Write1ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T data, UINT8_T *error = 0); - - int Write2ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T data); - int Write2ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T data, UINT8_T *error = 0); - - int Write4ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T data); - int Write4ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T data, UINT8_T *error = 0); - - int RegWriteTxOnly (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data); - int RegWriteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0); - - int SyncReadTx (PortHandler *port, UINT16_T start_address, UINT16_T data_length, UINT8_T *param, UINT16_T param_length); - // SyncReadRx -> GroupSyncRead class - // SyncReadTxRx -> GroupSyncRead class - - // param : ID1 DATA0 DATA1 ... DATAn ID2 DATA0 DATA1 ... DATAn ID3 DATA0 DATA1 ... DATAn - int SyncWriteTxOnly (PortHandler *port, UINT16_T start_address, UINT16_T data_length, UINT8_T *param, UINT16_T param_length); - - // param : LEN1 ID1 ADDR1 LEN2 ID2 ADDR2 ... - int BulkReadTx (PortHandler *port, UINT8_T *param, UINT16_T param_length); - // BulkReadRx -> GroupBulkRead class - // BulkReadTxRx -> GroupBulkRead class - - int BulkWriteTxOnly (PortHandler *port, UINT8_T *param, UINT16_T param_length); -}; - -} - - -#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL1PACKETHANDLER_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/Protocol2PacketHandler.h b/dynamixel_sdk/include/dynamixel_sdk/Protocol2PacketHandler.h deleted file mode 100644 index 101413c..0000000 --- a/dynamixel_sdk/include/dynamixel_sdk/Protocol2PacketHandler.h +++ /dev/null @@ -1,103 +0,0 @@ -/* - * Protocol2PacketHandler.h - * - * Created on: 2016. 1. 26. - * Author: zerom, leon - */ - -#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL2PACKETHANDLER_H_ -#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL2PACKETHANDLER_H_ - - -#include "PacketHandler.h" - -namespace ROBOTIS -{ - -class WINDECLSPEC Protocol2PacketHandler : public PacketHandler -{ -private: - static Protocol2PacketHandler *unique_instance_; - - Protocol2PacketHandler(); - - UINT16_T UpdateCRC(UINT16_T crc_accum, UINT8_T *data_blk_ptr, UINT16_T data_blk_size); - void AddStuffing(UINT8_T *packet); - void RemoveStuffing(UINT8_T *packet); - -public: - static Protocol2PacketHandler *GetInstance() { return unique_instance_; } - - virtual ~Protocol2PacketHandler() { } - - float GetProtocolVersion() { return 2.0; } - - void PrintTxRxResult(int result); - void PrintRxPacketError(UINT8_T error); - - int TxPacket (PortHandler *port, UINT8_T *txpacket); - int RxPacket (PortHandler *port, UINT8_T *rxpacket); - int TxRxPacket (PortHandler *port, UINT8_T *txpacket, UINT8_T *rxpacket, UINT8_T *error = 0); - - int Ping (PortHandler *port, UINT8_T id, UINT8_T *error = 0); - int Ping (PortHandler *port, UINT8_T id, UINT16_T *model_number, UINT8_T *error = 0); - - // BroadcastPing - int BroadcastPing (PortHandler *port, std::vector &id_list); - - int Action (PortHandler *port, UINT8_T id); - int Reboot (PortHandler *port, UINT8_T id, UINT8_T *error = 0); - int FactoryReset (PortHandler *port, UINT8_T id, UINT8_T option, UINT8_T *error = 0); - - - int ReadTx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length); - int ReadRx (PortHandler *port, UINT16_T length, UINT8_T *data, UINT8_T *error = 0); - int ReadTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0); - - int Read1ByteTx (PortHandler *port, UINT8_T id, UINT16_T address); - int Read1ByteRx (PortHandler *port, UINT8_T *data, UINT8_T *error = 0); - int Read1ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T *data, UINT8_T *error = 0); - - int Read2ByteTx (PortHandler *port, UINT8_T id, UINT16_T address); - int Read2ByteRx (PortHandler *port, UINT16_T *data, UINT8_T *error = 0); - int Read2ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T *data, UINT8_T *error = 0); - - int Read4ByteTx (PortHandler *port, UINT8_T id, UINT16_T address); - int Read4ByteRx (PortHandler *port, UINT32_T *data, UINT8_T *error = 0); - int Read4ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T *data, UINT8_T *error = 0); - - int WriteTxOnly (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data); - int WriteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0); - - int Write1ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T data); - int Write1ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T data, UINT8_T *error = 0); - - int Write2ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T data); - int Write2ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T data, UINT8_T *error = 0); - - int Write4ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T data); - int Write4ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T data, UINT8_T *error = 0); - - int RegWriteTxOnly (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data); - int RegWriteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0); - - int SyncReadTx (PortHandler *port, UINT16_T start_address, UINT16_T data_length, UINT8_T *param, UINT16_T param_length); - // SyncReadRx -> GroupSyncRead class - // SyncReadTxRx -> GroupSyncRead class - - // param : ID1 DATA0 DATA1 ... DATAn ID2 DATA0 DATA1 ... DATAn ID3 DATA0 DATA1 ... DATAn - int SyncWriteTxOnly (PortHandler *port, UINT16_T start_address, UINT16_T data_length, UINT8_T *param, UINT16_T param_length); - - // param : ID1 ADDR_L1 ADDR_H1 LEN_L1 LEN_H1 ID2 ADDR_L2 ADDR_H2 LEN_L2 LEN_H2 ... - int BulkReadTx (PortHandler *port, UINT8_T *param, UINT16_T param_length); - // BulkReadRx -> GroupBulkRead class - // BulkReadTxRx -> GroupBulkRead class - - // param : ID1 START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H DATA0 DATA1 ... DATAn ID2 START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H DATA0 DATA1 ... DATAn - int BulkWriteTxOnly (PortHandler *port, UINT8_T *param, UINT16_T param_length); -}; - -} - - -#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL2PACKETHANDLER_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/RobotisDef.h b/dynamixel_sdk/include/dynamixel_sdk/RobotisDef.h deleted file mode 100644 index 95ab804..0000000 --- a/dynamixel_sdk/include/dynamixel_sdk/RobotisDef.h +++ /dev/null @@ -1,21 +0,0 @@ -/* - * RobotisDef.h - * - * Created on: 2016. 1. 27. - * Author: zerom, leon - */ - -#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_ROBOTISDEF_H_ -#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_ROBOTISDEF_H_ - - -typedef char INT8_T; -typedef short int INT16_T; -typedef int INT32_T; - -typedef unsigned char UINT8_T; -typedef unsigned short int UINT16_T; -typedef unsigned int UINT32_T; - - -#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_ROBOTISDEF_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk_linux/PortHandlerLinux.h b/dynamixel_sdk/include/dynamixel_sdk_linux/PortHandlerLinux.h deleted file mode 100644 index 6ff516e..0000000 --- a/dynamixel_sdk/include/dynamixel_sdk_linux/PortHandlerLinux.h +++ /dev/null @@ -1,62 +0,0 @@ -/* - * PortHandlerLinux.h - * - * Created on: 2016. 1. 26. - * Author: zerom, leon - */ - -#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_LINUX_PORTHANDLERLINUX_H_ -#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_LINUX_PORTHANDLERLINUX_H_ - - -#include "dynamixel_sdk/PortHandler.h" - -namespace ROBOTIS -{ - -class PortHandlerLinux : public PortHandler -{ -private: - int socket_fd_; - int baudrate_; - char port_name_[30]; - - double packet_start_time_; - double packet_timeout_; - double tx_time_per_byte; - - bool SetupPort(const int cflag_baud); - bool SetCustomBaudrate(int speed); - int GetCFlagBaud(const int baudrate); - - double GetCurrentTime(); - double GetTimeSinceStart(); - -public: - PortHandlerLinux(const char *port_name); - virtual ~PortHandlerLinux() { ClosePort(); } - - bool OpenPort(); - void ClosePort(); - void ClearPort(); - - void SetPortName(const char *port_name); - char *GetPortName(); - - bool SetBaudRate(const int baudrate); - int GetBaudRate(); - - int GetBytesAvailable(); - - int ReadPort(UINT8_T *packet, int length); - int WritePort(UINT8_T *packet, int length); - - void SetPacketTimeout(UINT16_T packet_length); - void SetPacketTimeout(double msec); - bool IsPacketTimeout(); -}; - -} - - -#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_LINUX_PORTHANDLERLINUX_H_ */ diff --git a/dynamixel_sdk/package.xml b/dynamixel_sdk/package.xml deleted file mode 100644 index ae8e683..0000000 --- a/dynamixel_sdk/package.xml +++ /dev/null @@ -1,15 +0,0 @@ - - - dynamixel_sdk - 0.1.0 - The dynamixel_sdk package - - robotis - BSD - - ROBOTIS - - catkin - roscpp - roscpp - \ No newline at end of file diff --git a/dynamixel_sdk/src/dynamixel_sdk/GroupBulkRead.cpp b/dynamixel_sdk/src/dynamixel_sdk/GroupBulkRead.cpp deleted file mode 100644 index f248ee8..0000000 --- a/dynamixel_sdk/src/dynamixel_sdk/GroupBulkRead.cpp +++ /dev/null @@ -1,199 +0,0 @@ -/* - * GroupBulkRead.cpp - * - * Created on: 2016. 1. 28. - * Author: zerom, leon - */ -#if defined(_WIN32) || defined(_WIN64) -#define WINDLLEXPORT -#endif - -#include -#include -#include "dynamixel_sdk/GroupBulkRead.h" - -using namespace ROBOTIS; - -GroupBulkRead::GroupBulkRead(PortHandler *port, PacketHandler *ph) - : port_(port), - ph_(ph), - last_result_(false), - is_param_changed_(false), - param_(0) -{ - ClearParam(); -} - -void GroupBulkRead::MakeParam() -{ - if(id_list_.size() == 0) - return; - - if(param_ != 0) - delete[] param_; - param_ = 0; - - if(ph_->GetProtocolVersion() == 1.0) - param_ = new UINT8_T[id_list_.size() * 3]; // ID(1) + ADDR(1) + LENGTH(1) - else // 2.0 - param_ = new UINT8_T[id_list_.size() * 5]; // ID(1) + ADDR(2) + LENGTH(2) - - int _idx = 0; - for(unsigned int _i = 0; _i < id_list_.size(); _i++) - { - UINT8_T _id = id_list_[_i]; - if(ph_->GetProtocolVersion() == 1.0) - { - param_[_idx++] = (UINT8_T)length_list_[_id]; // LEN - param_[_idx++] = _id; // ID - param_[_idx++] = (UINT8_T)address_list_[_id]; // ADDR - } - else // 2.0 - { - param_[_idx++] = _id; // ID - param_[_idx++] = DXL_LOBYTE(address_list_[_id]); // ADDR_L - param_[_idx++] = DXL_HIBYTE(address_list_[_id]); // ADDR_H - param_[_idx++] = DXL_LOBYTE(length_list_[_id]); // LEN_L - param_[_idx++] = DXL_HIBYTE(length_list_[_id]); // LEN_H - } - } -} - -bool GroupBulkRead::AddParam(UINT8_T id, UINT16_T start_address, UINT16_T data_length) -{ - if(std::find(id_list_.begin(), id_list_.end(), id) != id_list_.end()) // id already exist - return false; - - id_list_.push_back(id); - length_list_[id] = data_length; - address_list_[id] = start_address; - data_list_[id] = new UINT8_T[data_length]; - - is_param_changed_ = true; - return true; -} - -void GroupBulkRead::RemoveParam(UINT8_T id) -{ - std::vector::iterator it = std::find(id_list_.begin(), id_list_.end(), id); - if(it == id_list_.end()) // NOT exist - return; - - id_list_.erase(it); - address_list_.erase(id); - length_list_.erase(id); - delete[] data_list_[id]; - data_list_.erase(id); - - is_param_changed_ = true; -} - -void GroupBulkRead::ClearParam() -{ - if(id_list_.size() == 0) - return; - - for(unsigned int _i = 0; _i < id_list_.size(); _i++) - delete[] data_list_[id_list_[_i]]; - - id_list_.clear(); - address_list_.clear(); - length_list_.clear(); - data_list_.clear(); - if(param_ != 0) - delete[] param_; - param_ = 0; -} - -int GroupBulkRead::TxPacket() -{ - if(id_list_.size() == 0) - return COMM_NOT_AVAILABLE; - - if(is_param_changed_ == true) - MakeParam(); - - if(ph_->GetProtocolVersion() == 1.0) - return ph_->BulkReadTx(port_, param_, id_list_.size() * 3); - else // 2.0 - return ph_->BulkReadTx(port_, param_, id_list_.size() * 5); -} - -int GroupBulkRead::RxPacket() -{ - int _cnt = id_list_.size(); - int _result = COMM_RX_FAIL; - - last_result_ = false; - - if(_cnt == 0) - return COMM_NOT_AVAILABLE; - - for(int _i = 0; _i < _cnt; _i++) - { - UINT8_T _id = id_list_[_i]; - - _result = ph_->ReadRx(port_, length_list_[_id], data_list_[_id]); - if(_result != COMM_SUCCESS) - { - fprintf(stderr, "[GroupBulkRead::RxPacket] ID %d result : %d !!!!!!!!!!\n", _id, _result); - return _result; - } - } - - if(_result == COMM_SUCCESS) - last_result_ = true; - - return _result; -} - -int GroupBulkRead::TxRxPacket() -{ - int _result = COMM_TX_FAIL; - - _result = TxPacket(); - if(_result != COMM_SUCCESS) - return _result; - - return RxPacket(); -} - -bool GroupBulkRead::IsAvailable(UINT8_T id, UINT16_T address, UINT16_T data_length) -{ - UINT16_T _start_addr, _data_length; - - if(last_result_ == false || data_list_.find(id) == data_list_.end()) - return false; - - _start_addr = address_list_[id]; - _data_length = length_list_[id]; - - if(address < _start_addr || _start_addr + _data_length - data_length < address) - return false; - - return true; -} - -UINT32_T GroupBulkRead::GetData(UINT8_T id, UINT16_T address, UINT16_T data_length) -{ - if(IsAvailable(id, address, data_length) == false) - return 0; - - UINT16_T _start_addr = address_list_[id]; - - switch(data_length) - { - case 1: - return data_list_[id][address - _start_addr]; - - case 2: - return DXL_MAKEWORD(data_list_[id][address - _start_addr], data_list_[id][address - _start_addr + 1]); - - case 4: - return DXL_MAKEDWORD(DXL_MAKEWORD(data_list_[id][address - _start_addr + 0], data_list_[id][address - _start_addr + 1]), - DXL_MAKEWORD(data_list_[id][address - _start_addr + 2], data_list_[id][address - _start_addr + 3])); - - default: - return 0; - } -} diff --git a/dynamixel_sdk/src/dynamixel_sdk/GroupBulkWrite.cpp b/dynamixel_sdk/src/dynamixel_sdk/GroupBulkWrite.cpp deleted file mode 100644 index 973793b..0000000 --- a/dynamixel_sdk/src/dynamixel_sdk/GroupBulkWrite.cpp +++ /dev/null @@ -1,137 +0,0 @@ -/* - * GroupBulkWrite.cpp - * - * Created on: 2016. 2. 2. - * Author: zerom, leon - */ -#if defined(_WIN32) || defined(_WIN64) -#define WINDLLEXPORT -#endif - -#include -#include "dynamixel_sdk/GroupBulkWrite.h" - -using namespace ROBOTIS; - -GroupBulkWrite::GroupBulkWrite(PortHandler *port, PacketHandler *ph) - : port_(port), - ph_(ph), - is_param_changed_(false), - param_(0), - param_length_(0) -{ - ClearParam(); -} - -void GroupBulkWrite::MakeParam() -{ - if(ph_->GetProtocolVersion() == 1.0 || id_list_.size() == 0) - return; - - if(param_ != 0) - delete[] param_; - param_ = 0; - - param_length_ = 0; - for(unsigned int _i = 0; _i < id_list_.size(); _i++) - param_length_ += 1 + 2 + 2 + length_list_[id_list_[_i]]; - - param_ = new UINT8_T[param_length_]; - - int _idx = 0; - for(unsigned int _i = 0; _i < id_list_.size(); _i++) - { - UINT8_T _id = id_list_[_i]; - if(data_list_[_id] == 0) - return; - - param_[_idx++] = _id; - param_[_idx++] = DXL_LOBYTE(address_list_[_id]); - param_[_idx++] = DXL_HIBYTE(address_list_[_id]); - param_[_idx++] = DXL_LOBYTE(length_list_[_id]); - param_[_idx++] = DXL_HIBYTE(length_list_[_id]); - for(int _c = 0; _c < length_list_[_id]; _c++) - param_[_idx++] = (data_list_[_id])[_c]; - } -} - -bool GroupBulkWrite::AddParam(UINT8_T id, UINT16_T start_address, UINT16_T data_length, UINT8_T *data) -{ - if(ph_->GetProtocolVersion() == 1.0) - return false; - - if(std::find(id_list_.begin(), id_list_.end(), id) != id_list_.end()) // id already exist - return false; - - id_list_.push_back(id); - address_list_[id] = start_address; - length_list_[id] = data_length; - data_list_[id] = new UINT8_T[data_length]; - for(int _c = 0; _c < data_length; _c++) - data_list_[id][_c] = data[_c]; - - is_param_changed_ = true; - return true; -} -void GroupBulkWrite::RemoveParam(UINT8_T id) -{ - if(ph_->GetProtocolVersion() == 1.0) - return; - - std::vector::iterator it = std::find(id_list_.begin(), id_list_.end(), id); - if(it == id_list_.end()) // NOT exist - return; - - id_list_.erase(it); - address_list_.erase(id); - length_list_.erase(id); - delete[] data_list_[id]; - data_list_.erase(id); - - is_param_changed_ = true; -} -bool GroupBulkWrite::ChangeParam(UINT8_T id, UINT16_T start_address, UINT16_T data_length, UINT8_T *data) -{ - if(ph_->GetProtocolVersion() == 1.0) - return false; - - std::vector::iterator it = std::find(id_list_.begin(), id_list_.end(), id); - if(it == id_list_.end()) // NOT exist - return false; - - address_list_[id] = start_address; - length_list_[id] = data_length; - delete[] data_list_[id]; - data_list_[id] = new UINT8_T[data_length]; - for(int _c = 0; _c < data_length; _c++) - data_list_[id][_c] = data[_c]; - - is_param_changed_ = true; - return true; -} -void GroupBulkWrite::ClearParam() -{ - if(ph_->GetProtocolVersion() == 1.0 || id_list_.size() == 0) - return; - - for(unsigned int _i = 0; _i < id_list_.size(); _i++) - delete[] data_list_[id_list_[_i]]; - - id_list_.clear(); - address_list_.clear(); - length_list_.clear(); - data_list_.clear(); - if(param_ != 0) - delete[] param_; - param_ = 0; -} -int GroupBulkWrite::TxPacket() -{ - if(ph_->GetProtocolVersion() == 1.0 || id_list_.size() == 0) - return COMM_NOT_AVAILABLE; - - if(is_param_changed_ == true) - MakeParam(); - - return ph_->BulkWriteTxOnly(port_, param_, param_length_); -} diff --git a/dynamixel_sdk/src/dynamixel_sdk/GroupSyncRead.cpp b/dynamixel_sdk/src/dynamixel_sdk/GroupSyncRead.cpp deleted file mode 100644 index fede283..0000000 --- a/dynamixel_sdk/src/dynamixel_sdk/GroupSyncRead.cpp +++ /dev/null @@ -1,172 +0,0 @@ -/* - * GroupSyncRead.cpp - * - * Created on: 2016. 2. 2. - * Author: zerom, leon - */ -#if defined(_WIN32) || defined(_WIN64) -#define WINDLLEXPORT -#endif - -#include -#include "dynamixel_sdk/GroupSyncRead.h" - -using namespace ROBOTIS; - -GroupSyncRead::GroupSyncRead(PortHandler *port, PacketHandler *ph, UINT16_T start_address, UINT16_T data_length) - : port_(port), - ph_(ph), - last_result_(false), - is_param_changed_(false), - param_(0), - start_address_(start_address), - data_length_(data_length) -{ - ClearParam(); -} - -void GroupSyncRead::MakeParam() -{ - if(ph_->GetProtocolVersion() == 1.0 || id_list_.size() == 0) - return; - - if(param_ != 0) - delete[] param_; - param_ = 0; - - param_ = new UINT8_T[id_list_.size() * 1]; // ID(1) - - int _idx = 0; - for(unsigned int _i = 0; _i < id_list_.size(); _i++) - param_[_idx++] = id_list_[_i]; -} - -bool GroupSyncRead::AddParam(UINT8_T id) -{ - if(ph_->GetProtocolVersion() == 1.0) - return false; - - if(std::find(id_list_.begin(), id_list_.end(), id) != id_list_.end()) // id already exist - return false; - - id_list_.push_back(id); - data_list_[id] = new UINT8_T[data_length_]; - - is_param_changed_ = true; - return true; -} -void GroupSyncRead::RemoveParam(UINT8_T id) -{ - if(ph_->GetProtocolVersion() == 1.0) - return; - - std::vector::iterator it = std::find(id_list_.begin(), id_list_.end(), id); - if(it == id_list_.end()) // NOT exist - return; - - id_list_.erase(it); - delete[] data_list_[id]; - data_list_.erase(id); - - is_param_changed_ = true; -} -void GroupSyncRead::ClearParam() -{ - if(ph_->GetProtocolVersion() == 1.0 || id_list_.size() == 0) - return; - - for(unsigned int _i = 0; _i < id_list_.size(); _i++) - delete[] data_list_[id_list_[_i]]; - - id_list_.clear(); - data_list_.clear(); - if(param_ != 0) - delete[] param_; - param_ = 0; -} - -int GroupSyncRead::TxPacket() -{ - if(ph_->GetProtocolVersion() == 1.0 || id_list_.size() == 0) - return COMM_NOT_AVAILABLE; - - if(is_param_changed_ == true) - MakeParam(); - - return ph_->SyncReadTx(port_, start_address_, data_length_, param_, (UINT16_T)id_list_.size() * 1); -} - -int GroupSyncRead::RxPacket() -{ - last_result_ = false; - - if(ph_->GetProtocolVersion() == 1.0) - return COMM_NOT_AVAILABLE; - - int _cnt = id_list_.size(); - int _result = COMM_RX_FAIL; - - if(_cnt == 0) - return COMM_NOT_AVAILABLE; - - for(int _i = 0; _i < _cnt; _i++) - { - UINT8_T _id = id_list_[_i]; - - _result = ph_->ReadRx(port_, data_length_, data_list_[_id]); - if(_result != COMM_SUCCESS) - return _result; - } - - if(_result == COMM_SUCCESS) - last_result_ = true; - - return _result; -} - -int GroupSyncRead::TxRxPacket() -{ - if(ph_->GetProtocolVersion() == 1.0) - return COMM_NOT_AVAILABLE; - - int _result = COMM_TX_FAIL; - - _result = TxPacket(); - if(_result != COMM_SUCCESS) - return _result; - - return RxPacket(); -} - -bool GroupSyncRead::IsAvailable(UINT8_T id, UINT16_T address, UINT16_T data_length) -{ - if(ph_->GetProtocolVersion() == 1.0 || last_result_ == false || data_list_.find(id) == data_list_.end()) - return false; - - if(address < start_address_ || start_address_ + data_length_ - data_length < address) - return false; - - return true; -} - -UINT32_T GroupSyncRead::GetData(UINT8_T id, UINT16_T address, UINT16_T data_length) -{ - if(IsAvailable(id, address, data_length) == false) - return 0; - - switch(data_length) - { - case 1: - return data_list_[id][address - start_address_]; - - case 2: - return DXL_MAKEWORD(data_list_[id][address - start_address_], data_list_[id][address - start_address_ + 1]); - - case 4: - return DXL_MAKEDWORD(DXL_MAKEWORD(data_list_[id][address - start_address_ + 0], data_list_[id][address - start_address_ + 1]), - DXL_MAKEWORD(data_list_[id][address - start_address_ + 2], data_list_[id][address - start_address_ + 3])); - - default: - return 0; - } -} diff --git a/dynamixel_sdk/src/dynamixel_sdk/GroupSyncWrite.cpp b/dynamixel_sdk/src/dynamixel_sdk/GroupSyncWrite.cpp deleted file mode 100644 index 8a61435..0000000 --- a/dynamixel_sdk/src/dynamixel_sdk/GroupSyncWrite.cpp +++ /dev/null @@ -1,117 +0,0 @@ -/* - * GroupSyncWrite.cpp - * - * Created on: 2016. 1. 28. - * Author: zerom, leon - */ -#if defined(_WIN32) || defined(_WIN64) -#define WINDLLEXPORT -#endif - -#include -#include "dynamixel_sdk/GroupSyncWrite.h" - -using namespace ROBOTIS; - -GroupSyncWrite::GroupSyncWrite(PortHandler *port, PacketHandler *ph, UINT16_T start_address, UINT16_T data_length) - : port_(port), - ph_(ph), - is_param_changed_(false), - param_(0), - start_address_(start_address), - data_length_(data_length) -{ - ClearParam(); -} - -void GroupSyncWrite::MakeParam() -{ - if(id_list_.size() == 0) - return; - - if(param_ != 0) - delete[] param_; - param_ = 0; - - param_ = new UINT8_T[id_list_.size() * (1 + data_length_)]; // ID(1) + DATA(data_length) - - int _idx = 0; - for(unsigned int _i = 0; _i < id_list_.size(); _i++) - { - UINT8_T _id = id_list_[_i]; - if(data_list_[_id] == 0) - return; - - param_[_idx++] = _id; - for(int _c = 0; _c < data_length_; _c++) - param_[_idx++] = (data_list_[_id])[_c]; - } -} - -bool GroupSyncWrite::AddParam(UINT8_T id, UINT8_T *data) -{ - if(std::find(id_list_.begin(), id_list_.end(), id) != id_list_.end()) // id already exist - return false; - - id_list_.push_back(id); - data_list_[id] = new UINT8_T[data_length_]; - for(int _c = 0; _c < data_length_; _c++) - data_list_[id][_c] = data[_c]; - - is_param_changed_ = true; - return true; -} - -void GroupSyncWrite::RemoveParam(UINT8_T id) -{ - std::vector::iterator it = std::find(id_list_.begin(), id_list_.end(), id); - if(it == id_list_.end()) // NOT exist - return; - - id_list_.erase(it); - delete[] data_list_[id]; - data_list_.erase(id); - - is_param_changed_ = true; -} - -bool GroupSyncWrite::ChangeParam(UINT8_T id, UINT8_T *data) -{ - std::vector::iterator it = std::find(id_list_.begin(), id_list_.end(), id); - if(it == id_list_.end()) // NOT exist - return false; - - delete[] data_list_[id]; - data_list_[id] = new UINT8_T[data_length_]; - for(int _c = 0; _c < data_length_; _c++) - data_list_[id][_c] = data[_c]; - - is_param_changed_ = true; - return true; -} - -void GroupSyncWrite::ClearParam() -{ - if(id_list_.size() == 0) - return; - - for(unsigned int _i = 0; _i < id_list_.size(); _i++) - delete[] data_list_[id_list_[_i]]; - - id_list_.clear(); - data_list_.clear(); - if(param_ != 0) - delete[] param_; - param_ = 0; -} - -int GroupSyncWrite::TxPacket() -{ - if(id_list_.size() == 0) - return COMM_NOT_AVAILABLE; - - if(is_param_changed_ == true) - MakeParam(); - - return ph_->SyncWriteTxOnly(port_, start_address_, data_length_, param_, id_list_.size() * (1 + data_length_)); -} diff --git a/dynamixel_sdk/src/dynamixel_sdk/PacketHandler.cpp b/dynamixel_sdk/src/dynamixel_sdk/PacketHandler.cpp deleted file mode 100644 index 8ad1c48..0000000 --- a/dynamixel_sdk/src/dynamixel_sdk/PacketHandler.cpp +++ /dev/null @@ -1,25 +0,0 @@ -/* - * PacketHandler.cpp - * - * Created on: 2016. 1. 26. - * Author: zerom, leon - */ -#if defined(_WIN32) || defined(_WIN64) -#define WINDLLEXPORT -#endif - -#include "dynamixel_sdk/PacketHandler.h" -#include "dynamixel_sdk/Protocol1PacketHandler.h" -#include "dynamixel_sdk/Protocol2PacketHandler.h" - -using namespace ROBOTIS; - -PacketHandler *PacketHandler::GetPacketHandler(float protocol_version) -{ - if(protocol_version == 1.0) - return (PacketHandler *)(Protocol1PacketHandler::GetInstance()); - else if(protocol_version == 2.0) - return (PacketHandler *)(Protocol2PacketHandler::GetInstance()); - - return (PacketHandler *)(Protocol2PacketHandler::GetInstance()); -} diff --git a/dynamixel_sdk/src/dynamixel_sdk/PortHandler.cpp b/dynamixel_sdk/src/dynamixel_sdk/PortHandler.cpp deleted file mode 100644 index ff4355c..0000000 --- a/dynamixel_sdk/src/dynamixel_sdk/PortHandler.cpp +++ /dev/null @@ -1,32 +0,0 @@ -/* - * PortHandler.cpp - * - * Created on: 2016. 2. 5. - * Author: zerom, leon - */ -#if defined(_WIN32) || defined(_WIN64) -#define WINDLLEXPORT -#endif - -#include "dynamixel_sdk/PortHandler.h" - -#ifdef __linux__ - #include "dynamixel_sdk_linux/PortHandlerLinux.h" -#endif - -#if defined(_WIN32) || defined(_WIN64) - #include "dynamixel_sdk_windows/PortHandlerWindows.h" -#endif - -using namespace ROBOTIS; - -PortHandler *PortHandler::GetPortHandler(const char *port_name) -{ -#ifdef __linux__ - return (PortHandler *)(new PortHandlerLinux(port_name)); -#endif - -#if defined(_WIN32) || defined(_WIN64) - return (PortHandler *)(new PortHandlerWindows(port_name)); -#endif -} diff --git a/dynamixel_sdk/src/dynamixel_sdk/Protocol1PacketHandler.cpp b/dynamixel_sdk/src/dynamixel_sdk/Protocol1PacketHandler.cpp deleted file mode 100644 index 66877e7..0000000 --- a/dynamixel_sdk/src/dynamixel_sdk/Protocol1PacketHandler.cpp +++ /dev/null @@ -1,670 +0,0 @@ -/* - * Protocol1PacketHandler.cpp - * - * Created on: 2016. 1. 26. - * Author: zerom, leon - */ -#if defined(_WIN32) || defined(_WIN64) -#define WINDLLEXPORT -#endif - -#include -#include -#include "dynamixel_sdk/Protocol1PacketHandler.h" - -#define TXPACKET_MAX_LEN (250) -#define RXPACKET_MAX_LEN (250) - -///////////////// for Protocol 1.0 Packet ///////////////// -#define PKT_HEADER0 0 -#define PKT_HEADER1 1 -#define PKT_ID 2 -#define PKT_LENGTH 3 -#define PKT_INSTRUCTION 4 -#define PKT_ERROR 4 -#define PKT_PARAMETER0 5 - -///////////////// Protocol 1.0 Error bit ///////////////// -#define ERRBIT_VOLTAGE 1 // Supplied voltage is out of the range (operating volatage set in the control table) -#define ERRBIT_ANGLE 2 // Goal position is written out of the range (from CW angle limit to CCW angle limit) -#define ERRBIT_OVERHEAT 4 // Temperature is out of the range (operating temperature set in the control table) -#define ERRBIT_RANGE 8 // Command(setting value) is out of the range for use. -#define ERRBIT_CHECKSUM 16 // Instruction packet checksum is incorrect. -#define ERRBIT_OVERLOAD 32 // The current load cannot be controlled by the set torque. -#define ERRBIT_INSTRUCTION 64 // Undefined instruction or delivering the action command without the reg_write command. - -using namespace ROBOTIS; - -Protocol1PacketHandler *Protocol1PacketHandler::unique_instance_ = new Protocol1PacketHandler(); - -Protocol1PacketHandler::Protocol1PacketHandler() { } - -void Protocol1PacketHandler::PrintTxRxResult(int result) -{ - switch(result) - { - case COMM_SUCCESS: - printf("[TxRxResult] Communication success.\n"); - break; - - case COMM_PORT_BUSY: - printf("[TxRxResult] Port is in use!\n"); - break; - - case COMM_TX_FAIL: - printf("[TxRxResult] Failed transmit instruction packet!\n"); - break; - - case COMM_RX_FAIL: - printf("[TxRxResult] Failed get status packet from device!\n"); - break; - - case COMM_TX_ERROR: - printf("[TxRxResult] Incorrect instruction packet!\n"); - break; - - case COMM_RX_WAITING: - printf("[TxRxResult] Now recieving status packet!\n"); - break; - - case COMM_RX_TIMEOUT: - printf("[TxRxResult] There is no status packet!\n"); - break; - - case COMM_RX_CORRUPT: - printf("[TxRxResult] Incorrect status packet!\n"); - break; - - case COMM_NOT_AVAILABLE: - printf("[TxRxResult] Protocol does not support This function!\n"); - break; - - default: - break; - } -} - -void Protocol1PacketHandler::PrintRxPacketError(UINT8_T error) -{ - if(error & ERRBIT_VOLTAGE) - printf("[RxPacketError] Input voltage error!\n"); - - if(error & ERRBIT_ANGLE) - printf("[RxPacketError] Angle limit error!\n"); - - if(error & ERRBIT_OVERHEAT) - printf("[RxPacketError] Overheat error!\n"); - - if(error & ERRBIT_RANGE) - printf("[RxPacketError] Out of range error!\n"); - - if(error & ERRBIT_CHECKSUM) - printf("[RxPacketError] Checksum error!\n"); - - if(error & ERRBIT_OVERLOAD) - printf("[RxPacketError] Overload error!\n"); - - if(error & ERRBIT_INSTRUCTION) - printf("[RxPacketError] Instruction code error!\n"); -} - -int Protocol1PacketHandler::TxPacket(PortHandler *port, UINT8_T *txpacket) -{ - UINT8_T _checksum = 0; - UINT8_T _total_packet_length = txpacket[PKT_LENGTH] + 4; // 4: HEADER0 HEADER1 ID LENGTH - UINT8_T _written_packet_length = 0; - - if(port->is_using) - return COMM_PORT_BUSY; - port->is_using = true; - - // check max packet length - if(_total_packet_length > TXPACKET_MAX_LEN) - { - port->is_using = false; - return COMM_TX_ERROR; - } - - // make packet header - txpacket[PKT_HEADER0] = 0xFF; - txpacket[PKT_HEADER1] = 0xFF; - - // add a checksum to the packet - for(int _idx = 2; _idx < _total_packet_length - 1; _idx++) // except header, checksum - _checksum += txpacket[_idx]; - txpacket[_total_packet_length - 1] = ~_checksum; - - // tx packet - port->ClearPort(); - _written_packet_length = port->WritePort(txpacket, _total_packet_length); - if(_total_packet_length != _written_packet_length) - { - port->is_using = false; - return COMM_TX_FAIL; - } - - return COMM_SUCCESS; -} - -int Protocol1PacketHandler::RxPacket(PortHandler *port, UINT8_T *rxpacket) -{ - int _result = COMM_TX_FAIL; - - UINT8_T _checksum = 0; - UINT8_T _rx_length = 0; - UINT8_T _wait_length = 6; // minimum length ( HEADER0 HEADER1 ID LENGTH ERROR CHKSUM ) - - while(true) - { - _rx_length += port->ReadPort(&rxpacket[_rx_length], _wait_length - _rx_length); - if(_rx_length >= _wait_length) - { - UINT8_T _idx = 0; - - // find packet header - for(_idx = 0; _idx < (_rx_length - 1); _idx++) - { - if(rxpacket[_idx] == 0xFF && rxpacket[_idx+1] == 0xFF) - break; - } - - if(_idx == 0) // found at the beginning of the packet - { - if(rxpacket[PKT_ID] > 0xFD || // unavailable ID - rxpacket[PKT_LENGTH] > RXPACKET_MAX_LEN || // unavailable Length - rxpacket[PKT_ERROR] >= 0x64) // unavailable Error - { - // remove the first byte in the packet - for(UINT8_T _s = 0; _s < _rx_length - 1; _s++) - rxpacket[_s] = rxpacket[1 + _s]; - //memcpy(&rxpacket[0], &rxpacket[_idx], _rx_length - _idx); - _rx_length -= 1; - continue; - } - - // re-calculate the exact length of the rx packet - if(_wait_length != rxpacket[PKT_LENGTH] + PKT_LENGTH + 1) - { - _wait_length = rxpacket[PKT_LENGTH] + PKT_LENGTH + 1; - continue; - } - - if(_rx_length < _wait_length) - { - // check timeout - if(port->IsPacketTimeout() == true) - { - if(_rx_length == 0) - _result = COMM_RX_TIMEOUT; - else - _result = COMM_RX_CORRUPT; - break; - } - else - continue; - } - - // calculate checksum - for(int _i = 2; _i < _wait_length - 1; _i++) // except header, checksum - _checksum += rxpacket[_i]; - _checksum = ~_checksum; - - // verify checksum - if(rxpacket[_wait_length - 1] == _checksum) - _result = COMM_SUCCESS; - else - _result = COMM_RX_CORRUPT; - break; - } - else - { - // remove unnecessary packets - for(UINT8_T _s = 0; _s < _rx_length - _idx; _s++) - rxpacket[_s] = rxpacket[_idx + _s]; - //memcpy(&rxpacket[0], &rxpacket[_idx], _rx_length - _idx); - _rx_length -= _idx; - } - } - else - { - // check timeout - if(port->IsPacketTimeout() == true) - { - if(_rx_length == 0) - _result = COMM_RX_TIMEOUT; - else - _result = COMM_RX_CORRUPT; - break; - } - } - } - port->is_using = false; - - return _result; -} - -// NOT for BulkRead instruction -int Protocol1PacketHandler::TxRxPacket(PortHandler *port, UINT8_T *txpacket, UINT8_T *rxpacket, UINT8_T *error) -{ - int _result = COMM_TX_FAIL; - - // tx packet - _result = TxPacket(port, txpacket); - if(_result != COMM_SUCCESS) - return _result; - - // (ID == Broadcast ID && NOT BulkRead) == no need to wait for status packet - // (Instruction == Action) == no need to wait for status packet - if((txpacket[PKT_ID] == BROADCAST_ID && txpacket[PKT_INSTRUCTION] != INST_BULK_READ) || - (txpacket[PKT_INSTRUCTION] == INST_ACTION)) - { - port->is_using = false; - return _result; - } - - // set packet timeout - if(txpacket[PKT_INSTRUCTION] == INST_READ) - port->SetPacketTimeout((UINT16_T)(txpacket[PKT_PARAMETER0+1] + 6)); - else - port->SetPacketTimeout((UINT16_T)6); - - // rx packet - _result = RxPacket(port, rxpacket); - // check txpacket ID == rxpacket ID - if(txpacket[PKT_ID] != rxpacket[PKT_ID]) - _result = RxPacket(port, rxpacket); - - if(_result == COMM_SUCCESS && txpacket[PKT_ID] != BROADCAST_ID) - { - if(error != 0) - *error = (UINT8_T)rxpacket[PKT_ERROR]; - } - - return _result; -} - -int Protocol1PacketHandler::Ping(PortHandler *port, UINT8_T id, UINT8_T *error) -{ - return Ping(port, id, 0, error); -} - -int Protocol1PacketHandler::Ping(PortHandler *port, UINT8_T id, UINT16_T *model_number, UINT8_T *error) -{ - int _result = COMM_TX_FAIL; - - UINT8_T txpacket[6] = {0}; - UINT8_T rxpacket[6] = {0}; - - if(id >= BROADCAST_ID) - return COMM_NOT_AVAILABLE; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH] = 2; - txpacket[PKT_INSTRUCTION] = INST_PING; - - _result = TxRxPacket(port, txpacket, rxpacket, error); - if(_result == COMM_SUCCESS && model_number != 0) - { - UINT8_T _data[2] = {0}; - _result = ReadTxRx(port, id, 0, 2, _data); // Address 0 : Model Number - if(_result == COMM_SUCCESS) - *model_number = DXL_MAKEWORD(_data[0], _data[1]); - } - - return _result; -} - -int Protocol1PacketHandler::BroadcastPing(PortHandler *port, std::vector &id_list) -{ - return COMM_NOT_AVAILABLE; -} - -int Protocol1PacketHandler::Action(PortHandler *port, UINT8_T id) -{ - UINT8_T txpacket[6] = {0}; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH] = 2; - txpacket[PKT_INSTRUCTION] = INST_ACTION; - - return TxRxPacket(port, txpacket, 0); -} - -int Protocol1PacketHandler::Reboot(PortHandler *port, UINT8_T id, UINT8_T *error) -{ - return COMM_NOT_AVAILABLE; -} - -int Protocol1PacketHandler::FactoryReset(PortHandler *port, UINT8_T id, UINT8_T option, UINT8_T *error) -{ - UINT8_T txpacket[6] = {0}; - UINT8_T rxpacket[6] = {0}; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH] = 2; - txpacket[PKT_INSTRUCTION] = INST_FACTORY_RESET; - - return TxRxPacket(port, txpacket, rxpacket, error); -} - -int Protocol1PacketHandler::ReadTx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length) -{ - int _result = COMM_TX_FAIL; - - UINT8_T txpacket[8] = {0}; - - if(id >= BROADCAST_ID) - return COMM_NOT_AVAILABLE; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH] = 4; - txpacket[PKT_INSTRUCTION] = INST_READ; - txpacket[PKT_PARAMETER0+0] = (UINT8_T)address; - txpacket[PKT_PARAMETER0+1] = (UINT8_T)length; - - _result = TxPacket(port, txpacket); - - // set packet timeout - if(_result == COMM_SUCCESS) - port->SetPacketTimeout((UINT16_T)(length+6)); - - return _result; -} - -int Protocol1PacketHandler::ReadRx(PortHandler *port, UINT16_T length, UINT8_T *data, UINT8_T *error) -{ - int _result = COMM_TX_FAIL; - UINT8_T *rxpacket = (UINT8_T *)malloc(RXPACKET_MAX_LEN);//(length+6); - //UINT8_T *rxpacket = new UINT8_T[length+6]; - - _result = RxPacket(port, rxpacket); - if(_result == COMM_SUCCESS) - { - if(error != 0) - *error = (UINT8_T)rxpacket[PKT_ERROR]; - for(UINT8_T _s = 0; _s < length; _s++) - data[_s] = rxpacket[PKT_PARAMETER0 + _s]; - //memcpy(data, &rxpacket[PKT_PARAMETER0], length); - } - - free(rxpacket); - //delete[] rxpacket; - return _result; -} - -int Protocol1PacketHandler::ReadTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error) -{ - int _result = COMM_TX_FAIL; - - UINT8_T txpacket[8] = {0}; - UINT8_T *rxpacket = (UINT8_T *)malloc(RXPACKET_MAX_LEN);//(length+6); - - if(id >= BROADCAST_ID) - return COMM_NOT_AVAILABLE; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH] = 4; - txpacket[PKT_INSTRUCTION] = INST_READ; - txpacket[PKT_PARAMETER0+0] = (UINT8_T)address; - txpacket[PKT_PARAMETER0+1] = (UINT8_T)length; - - _result = TxRxPacket(port, txpacket, rxpacket, error); - if(_result == COMM_SUCCESS) - { - if(error != 0) - *error = (UINT8_T)rxpacket[PKT_ERROR]; - for(UINT8_T _s = 0; _s < length; _s++) - data[_s] = rxpacket[PKT_PARAMETER0 + _s]; - //memcpy(data, &rxpacket[PKT_PARAMETER0], length); - } - - free(rxpacket); - //delete[] rxpacket; - return _result; -} - -int Protocol1PacketHandler::Read1ByteTx(PortHandler *port, UINT8_T id, UINT16_T address) -{ - return ReadTx(port, id, address, 1); -} -int Protocol1PacketHandler::Read1ByteRx(PortHandler *port, UINT8_T *data, UINT8_T *error) -{ - UINT8_T _data[1] = {0}; - int _result = ReadRx(port, 1, _data, error); - if(_result == COMM_SUCCESS) - *data = _data[0]; - return _result; -} -int Protocol1PacketHandler::Read1ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T *data, UINT8_T *error) -{ - UINT8_T _data[1] = {0}; - int _result = ReadTxRx(port, id, address, 1, _data, error); - if(_result == COMM_SUCCESS) - *data = _data[0]; - return _result; -} - -int Protocol1PacketHandler::Read2ByteTx(PortHandler *port, UINT8_T id, UINT16_T address) -{ - return ReadTx(port, id, address, 2); -} -int Protocol1PacketHandler::Read2ByteRx(PortHandler *port, UINT16_T *data, UINT8_T *error) -{ - UINT8_T _data[2] = {0}; - int _result = ReadRx(port, 2, _data, error); - if(_result == COMM_SUCCESS) - *data = DXL_MAKEWORD(_data[0], _data[1]); - return _result; -} -int Protocol1PacketHandler::Read2ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T *data, UINT8_T *error) -{ - UINT8_T _data[2] = {0}; - int _result = ReadTxRx(port, id, address, 2, _data, error); - if(_result == COMM_SUCCESS) - *data = DXL_MAKEWORD(_data[0], _data[1]); - return _result; -} - -int Protocol1PacketHandler::Read4ByteTx(PortHandler *port, UINT8_T id, UINT16_T address) -{ - return COMM_NOT_AVAILABLE; -} -int Protocol1PacketHandler::Read4ByteRx(PortHandler *port, UINT32_T *data, UINT8_T *error) -{ - return COMM_NOT_AVAILABLE; -} -int Protocol1PacketHandler::Read4ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T *data, UINT8_T *error) -{ - return COMM_NOT_AVAILABLE; -} - -int Protocol1PacketHandler::WriteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data) -{ - int _result = COMM_TX_FAIL; - - UINT8_T *txpacket = (UINT8_T *)malloc(length+7); - //UINT8_T *txpacket = new UINT8_T[length+7]; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH] = length+3; - txpacket[PKT_INSTRUCTION] = INST_WRITE; - txpacket[PKT_PARAMETER0] = (UINT8_T)address; - - for(UINT8_T _s = 0; _s < length; _s++) - txpacket[PKT_PARAMETER0+1+_s] = data[_s]; - //memcpy(&txpacket[PKT_PARAMETER0+1], data, length); - - _result = TxPacket(port, txpacket); - port->is_using = false; - - free(txpacket); - //delete[] txpacket; - return _result; -} - -int Protocol1PacketHandler::WriteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error) -{ - int _result = COMM_TX_FAIL; - - UINT8_T *txpacket = (UINT8_T *)malloc(length+6); - //UINT8_T *txpacket = new UINT8_T[length+6]; - UINT8_T rxpacket[6] = {0}; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH] = length+3; - txpacket[PKT_INSTRUCTION] = INST_WRITE; - txpacket[PKT_PARAMETER0] = (UINT8_T)address; - - for(UINT8_T _s = 0; _s < length; _s++) - txpacket[PKT_PARAMETER0+1+_s] = data[_s]; - //memcpy(&txpacket[PKT_PARAMETER0+1], data, length); - - _result = TxRxPacket(port, txpacket, rxpacket, error); - - free(txpacket); - //delete[] txpacket; - return _result; -} - -int Protocol1PacketHandler::Write1ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T data) -{ - UINT8_T _data[1] = { data }; - return WriteTxOnly(port, id, address, 1, _data); -} -int Protocol1PacketHandler::Write1ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T data, UINT8_T *error) -{ - UINT8_T _data[1] = { data }; - return WriteTxRx(port, id, address, 1, _data, error); -} - -int Protocol1PacketHandler::Write2ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T data) -{ - UINT8_T _data[2] = { DXL_LOBYTE(data), DXL_HIBYTE(data) }; - return WriteTxOnly(port, id, address, 2, _data); -} -int Protocol1PacketHandler::Write2ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T data, UINT8_T *error) -{ - UINT8_T _data[2] = { DXL_LOBYTE(data), DXL_HIBYTE(data) }; - return WriteTxRx(port, id, address, 2, _data, error); -} - -int Protocol1PacketHandler::Write4ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T data) -{ - return COMM_NOT_AVAILABLE; -} -int Protocol1PacketHandler::Write4ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T data, UINT8_T *error) -{ - return COMM_NOT_AVAILABLE; -} - -int Protocol1PacketHandler::RegWriteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data) -{ - int _result = COMM_TX_FAIL; - - UINT8_T *txpacket = (UINT8_T *)malloc(length+6); - //UINT8_T *txpacket = new UINT8_T[length+6]; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH] = length+3; - txpacket[PKT_INSTRUCTION] = INST_REG_WRITE; - txpacket[PKT_PARAMETER0] = (UINT8_T)address; - - for(UINT8_T _s = 0; _s < length; _s++) - txpacket[PKT_PARAMETER0+1+_s] = data[_s]; - //memcpy(&txpacket[PKT_PARAMETER0+1], data, length); - - _result = TxPacket(port, txpacket); - port->is_using = false; - - free(txpacket); - //delete[] txpacket; - return _result; -} - -int Protocol1PacketHandler::RegWriteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error) -{ - int _result = COMM_TX_FAIL; - - UINT8_T *txpacket = (UINT8_T *)malloc(length+6); - //UINT8_T *txpacket = new UINT8_T[length+6]; - UINT8_T rxpacket[6] = {0}; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH] = length+3; - txpacket[PKT_INSTRUCTION] = INST_REG_WRITE; - txpacket[PKT_PARAMETER0] = (UINT8_T)address; - - for(UINT8_T _s = 0; _s < length; _s++) - txpacket[PKT_PARAMETER0+1+_s] = data[_s]; - //memcpy(&txpacket[PKT_PARAMETER0+1], data, length); - - _result = TxRxPacket(port, txpacket, rxpacket, error); - - free(txpacket); - //delete[] txpacket; - return _result; -} - -int Protocol1PacketHandler::SyncReadTx(PortHandler *port, UINT16_T start_address, UINT16_T data_length, UINT8_T *param, UINT16_T param_length) -{ - return COMM_NOT_AVAILABLE; -} - -int Protocol1PacketHandler::SyncWriteTxOnly(PortHandler *port, UINT16_T start_address, UINT16_T data_length, UINT8_T *param, UINT16_T param_length) -{ - int _result = COMM_TX_FAIL; - - UINT8_T *txpacket = (UINT8_T *)malloc(param_length+8); // 8: HEADER0 HEADER1 ID LEN INST START_ADDR DATA_LEN ... CHKSUM - //UINT8_T *txpacket = new UINT8_T[param_length + 8]; // 8: HEADER0 HEADER1 ID LEN INST START_ADDR DATA_LEN ... CHKSUM - - txpacket[PKT_ID] = BROADCAST_ID; - txpacket[PKT_LENGTH] = param_length + 4; // 4: INST START_ADDR DATA_LEN ... CHKSUM - txpacket[PKT_INSTRUCTION] = INST_SYNC_WRITE; - txpacket[PKT_PARAMETER0+0] = start_address; - txpacket[PKT_PARAMETER0+1] = data_length; - - for(UINT8_T _s = 0; _s < param_length; _s++) - txpacket[PKT_PARAMETER0+2+_s] = param[_s]; - //memcpy(&txpacket[PKT_PARAMETER0+2], param, param_length); - - _result = TxRxPacket(port, txpacket, 0, 0); - - free(txpacket); - //delete[] txpacket; - return _result; -} - -int Protocol1PacketHandler::BulkReadTx(PortHandler *port, UINT8_T *param, UINT16_T param_length) -{ - int _result = COMM_TX_FAIL; - - UINT8_T *txpacket = (UINT8_T *)malloc(param_length+7); // 7: HEADER0 HEADER1 ID LEN INST 0x00 ... CHKSUM - //UINT8_T *txpacket = new UINT8_T[param_length + 7]; // 7: HEADER0 HEADER1 ID LEN INST 0x00 ... CHKSUM - - txpacket[PKT_ID] = BROADCAST_ID; - txpacket[PKT_LENGTH] = param_length + 3; // 3: INST 0x00 ... CHKSUM - txpacket[PKT_INSTRUCTION] = INST_BULK_READ; - txpacket[PKT_PARAMETER0+0] = 0x00; - - for(UINT8_T _s = 0; _s < param_length; _s++) - txpacket[PKT_PARAMETER0+1+_s] = param[_s]; - //memcpy(&txpacket[PKT_PARAMETER0+1], param, param_length); - - _result = TxPacket(port, txpacket); - if(_result == COMM_SUCCESS) - { - int _wait_length = 0; - for(int _i = 0; _i < param_length; _i += 3) - _wait_length += param[_i] + 7; - port->SetPacketTimeout((UINT16_T)_wait_length); - } - - free(txpacket); - //delete[] txpacket; - return _result; -} - -int Protocol1PacketHandler::BulkWriteTxOnly(PortHandler *port, UINT8_T *param, UINT16_T param_length) -{ - return COMM_NOT_AVAILABLE; -} diff --git a/dynamixel_sdk/src/dynamixel_sdk/Protocol2PacketHandler.cpp b/dynamixel_sdk/src/dynamixel_sdk/Protocol2PacketHandler.cpp deleted file mode 100644 index 771fee7..0000000 --- a/dynamixel_sdk/src/dynamixel_sdk/Protocol2PacketHandler.cpp +++ /dev/null @@ -1,985 +0,0 @@ -/* - * Protocol2PacketHandler.cpp - * - * Created on: 2016. 1. 26. - * Author: zerom, leon - */ - -#if defined(_WIN32) || defined(_WIN64) -#define WINDLLEXPORT -#endif - -#include -#include -#include -#include "dynamixel_sdk/Protocol2PacketHandler.h" - -#define TXPACKET_MAX_LEN (4*1024) -#define RXPACKET_MAX_LEN (4*1024) - -///////////////// for Protocol 2.0 Packet ///////////////// -#define PKT_HEADER0 0 -#define PKT_HEADER1 1 -#define PKT_HEADER2 2 -#define PKT_RESERVED 3 -#define PKT_ID 4 -#define PKT_LENGTH_L 5 -#define PKT_LENGTH_H 6 -#define PKT_INSTRUCTION 7 -#define PKT_ERROR 8 -#define PKT_PARAMETER0 8 - -///////////////// Protocol 2.0 Error bit ///////////////// -#define ERRNUM_RESULT_FAIL 1 // Failed to process the instruction packet. -#define ERRNUM_INSTRUCTION 2 // Instruction error -#define ERRNUM_CRC 3 // CRC check error -#define ERRNUM_DATA_RANGE 4 // Data range error -#define ERRNUM_DATA_LENGTH 5 // Data length error -#define ERRNUM_DATA_LIMIT 6 // Data limit error -#define ERRNUM_ACCESS 7 // Access error - -#define ERRBIT_ALERT 128 //When the device has a problem, this bit is set to 1. Check "Device Status Check" value. - -using namespace ROBOTIS; - -Protocol2PacketHandler *Protocol2PacketHandler::unique_instance_ = new Protocol2PacketHandler(); - -Protocol2PacketHandler::Protocol2PacketHandler() { } - -void Protocol2PacketHandler::PrintTxRxResult(int result) -{ - switch(result) - { - case COMM_SUCCESS: - printf("[TxRxResult] Communication success.\n"); - break; - - case COMM_PORT_BUSY: - printf("[TxRxResult] Port is in use!\n"); - break; - - case COMM_TX_FAIL: - printf("[TxRxResult] Failed transmit instruction packet!\n"); - break; - - case COMM_RX_FAIL: - printf("[TxRxResult] Failed get status packet from device!\n"); - break; - - case COMM_TX_ERROR: - printf("[TxRxResult] Incorrect instruction packet!\n"); - break; - - case COMM_RX_WAITING: - printf("[TxRxResult] Now recieving status packet!\n"); - break; - - case COMM_RX_TIMEOUT: - printf("[TxRxResult] There is no status packet!\n"); - break; - - case COMM_RX_CORRUPT: - printf("[TxRxResult] Incorrect status packet!\n"); - break; - - case COMM_NOT_AVAILABLE: - printf("[TxRxResult] Protocol does not support This function!\n"); - break; - - default: - break; - } -} - -void Protocol2PacketHandler::PrintRxPacketError(UINT8_T error) -{ - if(error & ERRBIT_ALERT) - printf("[RxPacketError] Hardware error occurred. Check the error at Control Table (Hardware Error Status)!\n"); - - int _error = error & ~ERRBIT_ALERT; - - switch(_error) - { - case 0: - break; - - case ERRNUM_RESULT_FAIL: - printf("[RxPacketError] Failed to process the instruction packet!\n"); - break; - - case ERRNUM_INSTRUCTION: - printf("[RxPacketError] Undefined instruction or incorrect instruction!\n"); - break; - - case ERRNUM_CRC: - printf("[RxPacketError] CRC doesn't match!\n"); - break; - - case ERRNUM_DATA_RANGE: - printf("[RxPacketError] The data value is out of range!\n"); - break; - - case ERRNUM_DATA_LENGTH: - printf("[RxPacketError] The data length does not match as expected!\n"); - break; - - case ERRNUM_DATA_LIMIT: - printf("[RxPacketError] The data value exceeds the limit value!\n"); - break; - - case ERRNUM_ACCESS: - printf("[RxPacketError] Writing or Reading is not available to target address!\n"); - break; - - default: - printf("[RxPacketError] Unknown error code!\n"); - break; - } -} - -unsigned short Protocol2PacketHandler::UpdateCRC(UINT16_T crc_accum, UINT8_T *data_blk_ptr, UINT16_T data_blk_size) -{ - UINT16_T i, j; - UINT16_T crc_table[256] = {0x0000, - 0x8005, 0x800F, 0x000A, 0x801B, 0x001E, 0x0014, 0x8011, - 0x8033, 0x0036, 0x003C, 0x8039, 0x0028, 0x802D, 0x8027, - 0x0022, 0x8063, 0x0066, 0x006C, 0x8069, 0x0078, 0x807D, - 0x8077, 0x0072, 0x0050, 0x8055, 0x805F, 0x005A, 0x804B, - 0x004E, 0x0044, 0x8041, 0x80C3, 0x00C6, 0x00CC, 0x80C9, - 0x00D8, 0x80DD, 0x80D7, 0x00D2, 0x00F0, 0x80F5, 0x80FF, - 0x00FA, 0x80EB, 0x00EE, 0x00E4, 0x80E1, 0x00A0, 0x80A5, - 0x80AF, 0x00AA, 0x80BB, 0x00BE, 0x00B4, 0x80B1, 0x8093, - 0x0096, 0x009C, 0x8099, 0x0088, 0x808D, 0x8087, 0x0082, - 0x8183, 0x0186, 0x018C, 0x8189, 0x0198, 0x819D, 0x8197, - 0x0192, 0x01B0, 0x81B5, 0x81BF, 0x01BA, 0x81AB, 0x01AE, - 0x01A4, 0x81A1, 0x01E0, 0x81E5, 0x81EF, 0x01EA, 0x81FB, - 0x01FE, 0x01F4, 0x81F1, 0x81D3, 0x01D6, 0x01DC, 0x81D9, - 0x01C8, 0x81CD, 0x81C7, 0x01C2, 0x0140, 0x8145, 0x814F, - 0x014A, 0x815B, 0x015E, 0x0154, 0x8151, 0x8173, 0x0176, - 0x017C, 0x8179, 0x0168, 0x816D, 0x8167, 0x0162, 0x8123, - 0x0126, 0x012C, 0x8129, 0x0138, 0x813D, 0x8137, 0x0132, - 0x0110, 0x8115, 0x811F, 0x011A, 0x810B, 0x010E, 0x0104, - 0x8101, 0x8303, 0x0306, 0x030C, 0x8309, 0x0318, 0x831D, - 0x8317, 0x0312, 0x0330, 0x8335, 0x833F, 0x033A, 0x832B, - 0x032E, 0x0324, 0x8321, 0x0360, 0x8365, 0x836F, 0x036A, - 0x837B, 0x037E, 0x0374, 0x8371, 0x8353, 0x0356, 0x035C, - 0x8359, 0x0348, 0x834D, 0x8347, 0x0342, 0x03C0, 0x83C5, - 0x83CF, 0x03CA, 0x83DB, 0x03DE, 0x03D4, 0x83D1, 0x83F3, - 0x03F6, 0x03FC, 0x83F9, 0x03E8, 0x83ED, 0x83E7, 0x03E2, - 0x83A3, 0x03A6, 0x03AC, 0x83A9, 0x03B8, 0x83BD, 0x83B7, - 0x03B2, 0x0390, 0x8395, 0x839F, 0x039A, 0x838B, 0x038E, - 0x0384, 0x8381, 0x0280, 0x8285, 0x828F, 0x028A, 0x829B, - 0x029E, 0x0294, 0x8291, 0x82B3, 0x02B6, 0x02BC, 0x82B9, - 0x02A8, 0x82AD, 0x82A7, 0x02A2, 0x82E3, 0x02E6, 0x02EC, - 0x82E9, 0x02F8, 0x82FD, 0x82F7, 0x02F2, 0x02D0, 0x82D5, - 0x82DF, 0x02DA, 0x82CB, 0x02CE, 0x02C4, 0x82C1, 0x8243, - 0x0246, 0x024C, 0x8249, 0x0258, 0x825D, 0x8257, 0x0252, - 0x0270, 0x8275, 0x827F, 0x027A, 0x826B, 0x026E, 0x0264, - 0x8261, 0x0220, 0x8225, 0x822F, 0x022A, 0x823B, 0x023E, - 0x0234, 0x8231, 0x8213, 0x0216, 0x021C, 0x8219, 0x0208, - 0x820D, 0x8207, 0x0202 }; - - for(j = 0; j < data_blk_size; j++) - { - i = ((UINT16_T)(crc_accum >> 8) ^ *data_blk_ptr++) & 0xFF; - crc_accum = (crc_accum << 8) ^ crc_table[i]; - } - - return crc_accum; -} - -void Protocol2PacketHandler::AddStuffing(UINT8_T *packet) -{ - int i = 0, index = 0; - int packet_length_in = DXL_MAKEWORD(packet[PKT_LENGTH_L], packet[PKT_LENGTH_H]); - int packet_length_out = packet_length_in; - UINT8_T temp[TXPACKET_MAX_LEN] = {0}; - - for(UINT8_T _s = PKT_HEADER0; _s <= PKT_LENGTH_H; _s++) - temp[_s] = packet[_s]; // FF FF FD XX ID LEN_L LEN_H - //memcpy(temp, packet, PKT_LENGTH_H+1); - index = PKT_INSTRUCTION; - for( i = 0; i < packet_length_in - 2; i++) // except CRC - { - temp[index++] = packet[i+PKT_INSTRUCTION]; - if(packet[i+PKT_INSTRUCTION] == 0xFD && packet[i+PKT_INSTRUCTION-1] == 0xFF && packet[i+PKT_INSTRUCTION-2] == 0xFF) - { // FF FF FD - temp[index++] = 0xFD; - packet_length_out++; - } - } - temp[index++] = packet[PKT_INSTRUCTION+packet_length_in-2]; - temp[index++] = packet[PKT_INSTRUCTION+packet_length_in-1]; - - - ////////////////////////// - if(packet_length_in != packet_length_out) - packet = (UINT8_T *)realloc(packet, index * sizeof(UINT8_T)); - - /////////////////////////// - - for(UINT8_T _s = 0; _s < index; _s++) - packet[_s] = temp[_s]; - //memcpy(packet, temp, index); - packet[PKT_LENGTH_L] = DXL_LOBYTE(packet_length_out); - packet[PKT_LENGTH_H] = DXL_HIBYTE(packet_length_out); -} - -void Protocol2PacketHandler::RemoveStuffing(UINT8_T *packet) -{ - int i = 0, index = 0; - int packet_length_in = DXL_MAKEWORD(packet[PKT_LENGTH_L], packet[PKT_LENGTH_H]); - int packet_length_out = packet_length_in; - - index = PKT_INSTRUCTION; - for( i = 0; i < packet_length_in - 2; i++) // except CRC - { - if(packet[i+PKT_INSTRUCTION] == 0xFD && packet[i+PKT_INSTRUCTION+1] == 0xFD && packet[i+PKT_INSTRUCTION-1] == 0xFF && packet[i+PKT_INSTRUCTION-2] == 0xFF) - { // FF FF FD FD - packet_length_out--; - i++; - } - packet[index++] = packet[i+PKT_INSTRUCTION]; - } - packet[index++] = packet[PKT_INSTRUCTION+packet_length_in-2]; - packet[index++] = packet[PKT_INSTRUCTION+packet_length_in-1]; - - packet[PKT_LENGTH_L] = DXL_LOBYTE(packet_length_out); - packet[PKT_LENGTH_H] = DXL_HIBYTE(packet_length_out); -} - -int Protocol2PacketHandler::TxPacket(PortHandler *port, UINT8_T *txpacket) -{ - UINT16_T _total_packet_length = 0; - UINT16_T _written_packet_length = 0; - - if(port->is_using) - return COMM_PORT_BUSY; - port->is_using = true; - - // byte stuffing for header - AddStuffing(txpacket); - - // check max packet length - _total_packet_length = DXL_MAKEWORD(txpacket[PKT_LENGTH_L], txpacket[PKT_LENGTH_H]) + 7; - // 7: HEADER0 HEADER1 HEADER2 RESERVED ID LENGTH_L LENGTH_H - if(_total_packet_length > TXPACKET_MAX_LEN) - { - port->is_using = false; - return COMM_TX_ERROR; - } - - // make packet header - txpacket[PKT_HEADER0] = 0xFF; - txpacket[PKT_HEADER1] = 0xFF; - txpacket[PKT_HEADER2] = 0xFD; - txpacket[PKT_RESERVED] = 0x00; - - // add CRC16 - UINT16_T crc = UpdateCRC(0, txpacket, _total_packet_length - 2); // 2: CRC16 - txpacket[_total_packet_length - 2] = DXL_LOBYTE(crc); - txpacket[_total_packet_length - 1] = DXL_HIBYTE(crc); - - // tx packet - port->ClearPort(); - _written_packet_length = port->WritePort(txpacket, _total_packet_length); - if(_total_packet_length != _written_packet_length) - { - port->is_using = false; - return COMM_TX_FAIL; - } - - return COMM_SUCCESS; -} - -int Protocol2PacketHandler::RxPacket(PortHandler *port, UINT8_T *rxpacket) -{ - int _result = COMM_TX_FAIL; - - UINT16_T _rx_length = 0; - UINT16_T _wait_length = 11; - // minimum length ( HEADER0 HEADER1 HEADER2 RESERVED ID LENGTH_L LENGTH_H INST ERROR CRC16_L CRC16_H ) - - while(true) - { - _rx_length += port->ReadPort(&rxpacket[_rx_length], _wait_length - _rx_length); - if(_rx_length >= _wait_length) - { - UINT16_T _idx = 0; - - // find packet header - for(_idx = 0; _idx < (_rx_length - 3); _idx++) - { - if((rxpacket[_idx] == 0xFF) && (rxpacket[_idx+1] == 0xFF) && (rxpacket[_idx+2] == 0xFD) && (rxpacket[_idx+3] != 0xFD)) - break; - } - - if(_idx == 0) // found at the beginning of the packet - { - if(rxpacket[PKT_RESERVED] != 0x00 || - rxpacket[PKT_ID] > 0xFC || - DXL_MAKEWORD(rxpacket[PKT_LENGTH_L], rxpacket[PKT_LENGTH_H]) > RXPACKET_MAX_LEN || - rxpacket[PKT_INSTRUCTION] != 0x55) - { - // remove the first byte in the packet - for(UINT8_T _s = 0; _s < _rx_length - 1; _s++) - rxpacket[_s] = rxpacket[1 + _s]; - //memcpy(&rxpacket[0], &rxpacket[_idx], _rx_length - _idx); - _rx_length -= 1; - continue; - } - - // re-calculate the exact length of the rx packet - if(_wait_length != DXL_MAKEWORD(rxpacket[PKT_LENGTH_L], rxpacket[PKT_LENGTH_H]) + PKT_LENGTH_H + 1) - { - _wait_length = DXL_MAKEWORD(rxpacket[PKT_LENGTH_L], rxpacket[PKT_LENGTH_H]) + PKT_LENGTH_H + 1; - continue; - } - - if(_rx_length < _wait_length) - { - // check timeout - if(port->IsPacketTimeout() == true) - { - if(_rx_length == 0) - _result = COMM_RX_TIMEOUT; - else - _result = COMM_RX_CORRUPT; - break; - } - else - continue; - } - - // verify CRC16 - UINT16_T crc = DXL_MAKEWORD(rxpacket[_wait_length-2], rxpacket[_wait_length-1]); - if(UpdateCRC(0, rxpacket, _wait_length - 2) == crc) - _result = COMM_SUCCESS; - else - _result = COMM_RX_CORRUPT; - break; - } - else - { - // remove unnecessary packets - for(UINT8_T _s = 0; _s < _rx_length - _idx; _s++) - rxpacket[_s] = rxpacket[_idx + _s]; - //memcpy(&rxpacket[0], &rxpacket[_idx], _rx_length - _idx); - _rx_length -= _idx; - } - } - else - { - // check timeout - if(port->IsPacketTimeout() == true) - { - if(_rx_length == 0) - _result = COMM_RX_TIMEOUT; - else - _result = COMM_RX_CORRUPT; - break; - } - } - } - port->is_using = false; - - if(_result == COMM_SUCCESS) - RemoveStuffing(rxpacket); - - return _result; -} - -// NOT for BulkRead / SyncRead instruction -int Protocol2PacketHandler::TxRxPacket(PortHandler *port, UINT8_T *txpacket, UINT8_T *rxpacket, UINT8_T *error) -{ - int _result = COMM_TX_FAIL; - - // tx packet - _result = TxPacket(port, txpacket); - if(_result != COMM_SUCCESS) - return _result; - - // (ID == Broadcast ID && NOT BulkRead) == no need to wait for status packet - // (Instruction == Action) == no need to wait for status packet - if((txpacket[PKT_ID] == BROADCAST_ID && txpacket[PKT_INSTRUCTION] != INST_BULK_READ) || - (txpacket[PKT_ID] == BROADCAST_ID && txpacket[PKT_INSTRUCTION] != INST_SYNC_READ) || - (txpacket[PKT_INSTRUCTION] == INST_ACTION)) - { - port->is_using = false; - return _result; - } - - // set packet timeout - if(txpacket[PKT_INSTRUCTION] == INST_READ) - port->SetPacketTimeout((UINT16_T)(DXL_MAKEWORD(txpacket[PKT_PARAMETER0+2], txpacket[PKT_PARAMETER0+3]) + 11)); - else - port->SetPacketTimeout((UINT16_T)11); // HEADER0 HEADER1 HEADER2 RESERVED ID LENGTH_L LENGTH_H INST ERROR CRC16_L CRC16_H - - // rx packet - _result = RxPacket(port, rxpacket); - // check txpacket ID == rxpacket ID - if(txpacket[PKT_ID] != rxpacket[PKT_ID]) - _result = RxPacket(port, rxpacket); - - if(_result == COMM_SUCCESS && txpacket[PKT_ID] != BROADCAST_ID) - { - if(error != 0) - *error = (UINT8_T)rxpacket[PKT_ERROR]; - } - - return _result; -} - -int Protocol2PacketHandler::Ping(PortHandler *port, UINT8_T id, UINT8_T *error) -{ - return Ping(port, id, 0, error); -} - -int Protocol2PacketHandler::Ping(PortHandler *port, UINT8_T id, UINT16_T *model_number, UINT8_T *error) -{ - int _result = COMM_TX_FAIL; - - UINT8_T txpacket[10] = {0}; - UINT8_T rxpacket[14] = {0}; - - if(id >= BROADCAST_ID) - return COMM_NOT_AVAILABLE; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = 3; - txpacket[PKT_LENGTH_H] = 0; - txpacket[PKT_INSTRUCTION] = INST_PING; - - _result = TxRxPacket(port, txpacket, rxpacket, error); - if(_result == COMM_SUCCESS && model_number != 0) - *model_number = DXL_MAKEWORD(rxpacket[PKT_PARAMETER0+1], rxpacket[PKT_PARAMETER0+2]); - - return _result; -} - -int Protocol2PacketHandler::BroadcastPing(PortHandler *port, std::vector &id_list) -{ - const int STATUS_LENGTH = 14; - int _result = COMM_TX_FAIL; - - id_list.clear(); - - UINT16_T _rx_length = 0; - UINT16_T _wait_length = STATUS_LENGTH * MAX_ID; - - UINT8_T txpacket[10] = {0}; - UINT8_T rxpacket[STATUS_LENGTH * MAX_ID] = {0}; - - txpacket[PKT_ID] = BROADCAST_ID; - txpacket[PKT_LENGTH_L] = 3; - txpacket[PKT_LENGTH_H] = 0; - txpacket[PKT_INSTRUCTION] = INST_PING; - - _result = TxPacket(port, txpacket); - if(_result != COMM_SUCCESS) - { - port->is_using = false; - return _result; - } - - // set rx timeout - port->SetPacketTimeout((UINT16_T)(_wait_length * 30)); - - while(1) - { - _rx_length += port->ReadPort(&rxpacket[_rx_length], _wait_length - _rx_length); - if(port->IsPacketTimeout() == true)// || _rx_length >= _wait_length) - break; - } - - port->is_using = false; - - if(_rx_length == 0) - return COMM_RX_TIMEOUT; - - while(1) - { - if(_rx_length < STATUS_LENGTH) - return COMM_RX_CORRUPT; - - UINT16_T _idx = 0; - - // find packet header - for(_idx = 0; _idx < (_rx_length - 2); _idx++) - { - if(rxpacket[_idx] == 0xFF && rxpacket[_idx+1] == 0xFF && rxpacket[_idx+2] == 0xFD) - break; - } - - if(_idx == 0) // found at the beginning of the packet - { - // verify CRC16 - UINT16_T crc = DXL_MAKEWORD(rxpacket[STATUS_LENGTH-2], rxpacket[STATUS_LENGTH-1]); - - if(UpdateCRC(0, rxpacket, STATUS_LENGTH - 2) == crc) - { - _result = COMM_SUCCESS; - - id_list.push_back(rxpacket[PKT_ID]); - - for(UINT8_T _s = 0; _s < _rx_length - STATUS_LENGTH; _s++) - rxpacket[_s] = rxpacket[STATUS_LENGTH + _s]; - _rx_length -= STATUS_LENGTH; - - if(_rx_length == 0) - return _result; - } - else - { - _result = COMM_RX_CORRUPT; - - // remove header (0xFF 0xFF 0xFD) - for(UINT8_T _s = 0; _s < _rx_length - 3; _s++) - rxpacket[_s] = rxpacket[3 + _s]; - _rx_length -= 3; - } - } - else - { - // remove unnecessary packets - for(UINT8_T _s = 0; _s < _rx_length - _idx; _s++) - rxpacket[_s] = rxpacket[_idx + _s]; - _rx_length -= _idx; - } - } - - return _result; -} - -int Protocol2PacketHandler::Action(PortHandler *port, UINT8_T id) -{ - UINT8_T txpacket[10] = {0}; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = 3; - txpacket[PKT_LENGTH_H] = 0; - txpacket[PKT_INSTRUCTION] = INST_ACTION; - - return TxRxPacket(port, txpacket, 0); -} - -int Protocol2PacketHandler::Reboot(PortHandler *port, UINT8_T id, UINT8_T *error) -{ - UINT8_T txpacket[10] = {0}; - UINT8_T rxpacket[11] = {0}; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = 3; - txpacket[PKT_LENGTH_H] = 0; - txpacket[PKT_INSTRUCTION] = INST_REBOOT; - - return TxRxPacket(port, txpacket, rxpacket, error); -} - -int Protocol2PacketHandler::FactoryReset(PortHandler *port, UINT8_T id, UINT8_T option, UINT8_T *error) -{ - UINT8_T txpacket[11] = {0}; - UINT8_T rxpacket[11] = {0}; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = 4; - txpacket[PKT_LENGTH_H] = 0; - txpacket[PKT_INSTRUCTION] = INST_FACTORY_RESET; - txpacket[PKT_PARAMETER0] = option; - - return TxRxPacket(port, txpacket, rxpacket, error); -} - -int Protocol2PacketHandler::ReadTx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length) -{ - int _result = COMM_TX_FAIL; - - UINT8_T txpacket[14] = {0}; - - if(id >= BROADCAST_ID) - return COMM_NOT_AVAILABLE; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = 7; - txpacket[PKT_LENGTH_H] = 0; - txpacket[PKT_INSTRUCTION] = INST_READ; - txpacket[PKT_PARAMETER0+0] = (UINT8_T)DXL_LOBYTE(address); - txpacket[PKT_PARAMETER0+1] = (UINT8_T)DXL_HIBYTE(address); - txpacket[PKT_PARAMETER0+2] = (UINT8_T)DXL_LOBYTE(length); - txpacket[PKT_PARAMETER0+3] = (UINT8_T)DXL_HIBYTE(length); - - _result = TxPacket(port, txpacket); - - // set packet timeout - if(_result == COMM_SUCCESS) - port->SetPacketTimeout((UINT16_T)(length + 11)); - - return _result; -} - -int Protocol2PacketHandler::ReadRx(PortHandler *port, UINT16_T length, UINT8_T *data, UINT8_T *error) -{ - int _result = COMM_TX_FAIL; - UINT8_T *rxpacket = (UINT8_T *)malloc(RXPACKET_MAX_LEN);//(length + 11 + (length/3)); // (length/3): consider stuffing - //UINT8_T *rxpacket = new UINT8_T[length + 11 + (length/3)]; // (length/3): consider stuffing - - _result = RxPacket(port, rxpacket); - if(_result == COMM_SUCCESS) - { - if(error != 0) - *error = (UINT8_T)rxpacket[PKT_ERROR]; - for(UINT8_T _s = 0; _s < length; _s++) - data[_s] = rxpacket[PKT_PARAMETER0 + 1 + _s]; - //memcpy(data, &rxpacket[PKT_PARAMETER0+1], length); - } - - free(rxpacket); - //delete[] rxpacket; - return _result; -} - -int Protocol2PacketHandler::ReadTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error) -{ - int _result = COMM_TX_FAIL; - - UINT8_T txpacket[14] = {0}; - UINT8_T *rxpacket = (UINT8_T *)malloc(RXPACKET_MAX_LEN);//(length + 11 + (length/3)); // (length/3): consider stuffing - - if(id >= BROADCAST_ID) - return COMM_NOT_AVAILABLE; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = 7; - txpacket[PKT_LENGTH_H] = 0; - txpacket[PKT_INSTRUCTION] = INST_READ; - txpacket[PKT_PARAMETER0+0] = (UINT8_T)DXL_LOBYTE(address); - txpacket[PKT_PARAMETER0+1] = (UINT8_T)DXL_HIBYTE(address); - txpacket[PKT_PARAMETER0+2] = (UINT8_T)DXL_LOBYTE(length); - txpacket[PKT_PARAMETER0+3] = (UINT8_T)DXL_HIBYTE(length); - - _result = TxRxPacket(port, txpacket, rxpacket, error); - if(_result == COMM_SUCCESS) - { - if(error != 0) - *error = (UINT8_T)rxpacket[PKT_ERROR]; - for(UINT8_T _s = 0; _s < length; _s++) - data[_s] = rxpacket[PKT_PARAMETER0 + 1 + _s]; - //memcpy(data, &rxpacket[PKT_PARAMETER0+1], length); - } - - free(rxpacket); - //delete[] rxpacket; - return _result; -} - -int Protocol2PacketHandler::Read1ByteTx(PortHandler *port, UINT8_T id, UINT16_T address) -{ - return ReadTx(port, id, address, 1); -} -int Protocol2PacketHandler::Read1ByteRx(PortHandler *port, UINT8_T *data, UINT8_T *error) -{ - UINT8_T _data[1] = {0}; - int _result = ReadRx(port, 1, _data, error); - if(_result == COMM_SUCCESS) - *data = _data[0]; - return _result; -} -int Protocol2PacketHandler::Read1ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T *data, UINT8_T *error) -{ - UINT8_T _data[1] = {0}; - int _result = ReadTxRx(port, id, address, 1, _data, error); - if(_result == COMM_SUCCESS) - *data = _data[0]; - return _result; -} - -int Protocol2PacketHandler::Read2ByteTx(PortHandler *port, UINT8_T id, UINT16_T address) -{ - return ReadTx(port, id, address, 2); -} -int Protocol2PacketHandler::Read2ByteRx(PortHandler *port, UINT16_T *data, UINT8_T *error) -{ - UINT8_T _data[2] = {0}; - int _result = ReadRx(port, 2, _data, error); - if(_result == COMM_SUCCESS) - *data = DXL_MAKEWORD(_data[0], _data[1]); - return _result; -} -int Protocol2PacketHandler::Read2ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T *data, UINT8_T *error) -{ - UINT8_T _data[2] = {0}; - int _result = ReadTxRx(port, id, address, 2, _data, error); - if(_result == COMM_SUCCESS) - *data = DXL_MAKEWORD(_data[0], _data[1]); - return _result; -} - -int Protocol2PacketHandler::Read4ByteTx(PortHandler *port, UINT8_T id, UINT16_T address) -{ - return ReadTx(port, id, address, 4); -} -int Protocol2PacketHandler::Read4ByteRx(PortHandler *port, UINT32_T *data, UINT8_T *error) -{ - UINT8_T _data[4] = {0}; - int _result = ReadRx(port, 4, _data, error); - if(_result == COMM_SUCCESS) - *data = DXL_MAKEDWORD(DXL_MAKEWORD(_data[0], _data[1]), DXL_MAKEWORD(_data[2], _data[3])); - return _result; -} -int Protocol2PacketHandler::Read4ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T *data, UINT8_T *error) -{ - UINT8_T _data[4] = {0}; - int _result = ReadTxRx(port, id, address, 4, _data, error); - if(_result == COMM_SUCCESS) - *data = DXL_MAKEDWORD(DXL_MAKEWORD(_data[0], _data[1]), DXL_MAKEWORD(_data[2], _data[3])); - return _result; -} - - -int Protocol2PacketHandler::WriteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data) -{ - int _result = COMM_TX_FAIL; - - UINT8_T *txpacket = (UINT8_T *)malloc(length+12); - //UINT8_T *txpacket = new UINT8_T[length+12]; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); - txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5); - txpacket[PKT_INSTRUCTION] = INST_WRITE; - txpacket[PKT_PARAMETER0+0] = (UINT8_T)DXL_LOBYTE(address); - txpacket[PKT_PARAMETER0+1] = (UINT8_T)DXL_HIBYTE(address); - - for(UINT8_T _s = 0; _s < length; _s++) - txpacket[PKT_PARAMETER0+2+_s] = data[_s]; - //memcpy(&txpacket[PKT_PARAMETER0+2], data, length); - - _result = TxPacket(port, txpacket); - port->is_using = false; - - free(txpacket); - //delete[] txpacket; - return _result; -} - -int Protocol2PacketHandler::WriteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error) -{ - int _result = COMM_TX_FAIL; - - UINT8_T *txpacket = (UINT8_T *)malloc(length + 12); - //UINT8_T *txpacket = new UINT8_T[length+12]; - UINT8_T rxpacket[11] = {0}; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); - txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5); - txpacket[PKT_INSTRUCTION] = INST_WRITE; - txpacket[PKT_PARAMETER0+0] = (UINT8_T)DXL_LOBYTE(address); - txpacket[PKT_PARAMETER0+1] = (UINT8_T)DXL_HIBYTE(address); - - for(UINT8_T _s = 0; _s < length; _s++) - txpacket[PKT_PARAMETER0+2+_s] = data[_s]; - //memcpy(&txpacket[PKT_PARAMETER0+2], data, length); - - _result = TxRxPacket(port, txpacket, rxpacket, error); - - free(txpacket); - //delete[] txpacket; - return _result; -} - -int Protocol2PacketHandler::Write1ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T data) -{ - UINT8_T _data[1] = { data }; - return WriteTxOnly(port, id, address, 1, _data); -} -int Protocol2PacketHandler::Write1ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T data, UINT8_T *error) -{ - UINT8_T _data[1] = { data }; - return WriteTxRx(port, id, address, 1, _data, error); -} - -int Protocol2PacketHandler::Write2ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T data) -{ - UINT8_T _data[2] = { DXL_LOBYTE(data), DXL_HIBYTE(data) }; - return WriteTxOnly(port, id, address, 2, _data); -} -int Protocol2PacketHandler::Write2ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T data, UINT8_T *error) -{ - UINT8_T _data[2] = { DXL_LOBYTE(data), DXL_HIBYTE(data) }; - return WriteTxRx(port, id, address, 2, _data, error); -} - -int Protocol2PacketHandler::Write4ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T data) -{ - UINT8_T _data[4] = { DXL_LOBYTE(DXL_LOWORD(data)), DXL_HIBYTE(DXL_LOWORD(data)), DXL_LOBYTE(DXL_HIWORD(data)), DXL_HIBYTE(DXL_HIWORD(data)) }; - return WriteTxOnly(port, id, address, 4, _data); -} -int Protocol2PacketHandler::Write4ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T data, UINT8_T *error) -{ - UINT8_T _data[4] = { DXL_LOBYTE(DXL_LOWORD(data)), DXL_HIBYTE(DXL_LOWORD(data)), DXL_LOBYTE(DXL_HIWORD(data)), DXL_HIBYTE(DXL_HIWORD(data)) }; - return WriteTxRx(port, id, address, 4, _data, error); -} - -int Protocol2PacketHandler::RegWriteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data) -{ - int _result = COMM_TX_FAIL; - - UINT8_T *txpacket = (UINT8_T *)malloc(length + 12); - //UINT8_T *txpacket = new UINT8_T[length+12]; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); - txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5); - txpacket[PKT_INSTRUCTION] = INST_REG_WRITE; - txpacket[PKT_PARAMETER0+0] = (UINT8_T)DXL_LOBYTE(address); - txpacket[PKT_PARAMETER0+1] = (UINT8_T)DXL_HIBYTE(address); - - for(UINT8_T _s = 0; _s < length; _s++) - txpacket[PKT_PARAMETER0+2+_s] = data[_s]; - //memcpy(&txpacket[PKT_PARAMETER0+2], data, length); - - _result = TxPacket(port, txpacket); - port->is_using = false; - - free(txpacket); - //delete[] txpacket; - return _result; -} - -int Protocol2PacketHandler::RegWriteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error) -{ - int _result = COMM_TX_FAIL; - - UINT8_T *txpacket = (UINT8_T *)malloc(length + 12); - //UINT8_T *txpacket = new UINT8_T[length+12]; - UINT8_T rxpacket[11] = {0}; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); - txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5); - txpacket[PKT_INSTRUCTION] = INST_REG_WRITE; - txpacket[PKT_PARAMETER0+0] = (UINT8_T)DXL_LOBYTE(address); - txpacket[PKT_PARAMETER0+1] = (UINT8_T)DXL_HIBYTE(address); - - for(UINT8_T _s = 0; _s < length; _s++) - txpacket[PKT_PARAMETER0+2+_s] = data[_s]; - //memcpy(&txpacket[PKT_PARAMETER0+2], data, length); - - _result = TxRxPacket(port, txpacket, rxpacket, error); - - free(txpacket); - //delete[] txpacket; - return _result; -} - -int Protocol2PacketHandler::SyncReadTx(PortHandler *port, UINT16_T start_address, UINT16_T data_length, UINT8_T *param, UINT16_T param_length) -{ - int _result = COMM_TX_FAIL; - - UINT8_T *txpacket = (UINT8_T *)malloc(param_length + 14); - // 14: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H - - txpacket[PKT_ID] = BROADCAST_ID; - txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H - txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H - txpacket[PKT_INSTRUCTION] = INST_SYNC_READ; - txpacket[PKT_PARAMETER0+0] = DXL_LOBYTE(start_address); - txpacket[PKT_PARAMETER0+1] = DXL_HIBYTE(start_address); - txpacket[PKT_PARAMETER0+2] = DXL_LOBYTE(data_length); - txpacket[PKT_PARAMETER0+3] = DXL_HIBYTE(data_length); - - for(UINT8_T _s = 0; _s < param_length; _s++) - txpacket[PKT_PARAMETER0+4+_s] = param[_s]; - //memcpy(&txpacket[PKT_PARAMETER0+4], param, param_length); - - _result = TxPacket(port, txpacket); - if(_result == COMM_SUCCESS) - port->SetPacketTimeout((UINT16_T)((11 + data_length) * param_length)); - - free(txpacket); - return _result; -} - -int Protocol2PacketHandler::SyncWriteTxOnly(PortHandler *port, UINT16_T start_address, UINT16_T data_length, UINT8_T *param, UINT16_T param_length) -{ - int _result = COMM_TX_FAIL; - - UINT8_T *txpacket = (UINT8_T *)malloc(param_length + 14); - //UINT8_T *txpacket = new UINT8_T[param_length + 14]; - // 14: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H - - txpacket[PKT_ID] = BROADCAST_ID; - txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H - txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H - txpacket[PKT_INSTRUCTION] = INST_SYNC_WRITE; - txpacket[PKT_PARAMETER0+0] = DXL_LOBYTE(start_address); - txpacket[PKT_PARAMETER0+1] = DXL_HIBYTE(start_address); - txpacket[PKT_PARAMETER0+2] = DXL_LOBYTE(data_length); - txpacket[PKT_PARAMETER0+3] = DXL_HIBYTE(data_length); - - for(UINT8_T _s = 0; _s < param_length; _s++) - txpacket[PKT_PARAMETER0+4+_s] = param[_s]; - //memcpy(&txpacket[PKT_PARAMETER0+4], param, param_length); - - _result = TxRxPacket(port, txpacket, 0, 0); - - free(txpacket); - //delete[] txpacket; - return _result; -} - -int Protocol2PacketHandler::BulkReadTx(PortHandler *port, UINT8_T *param, UINT16_T param_length) -{ - int _result = COMM_TX_FAIL; - - UINT8_T *txpacket = (UINT8_T *)malloc(param_length + 10); - //UINT8_T *txpacket = new UINT8_T[param_length + 10]; - // 10: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST CRC16_L CRC16_H - - txpacket[PKT_ID] = BROADCAST_ID; - txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H - txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H - txpacket[PKT_INSTRUCTION] = INST_BULK_READ; - - for(UINT8_T _s = 0; _s < param_length; _s++) - txpacket[PKT_PARAMETER0+_s] = param[_s]; - //memcpy(&txpacket[PKT_PARAMETER0], param, param_length); - - _result = TxPacket(port, txpacket); - if(_result == COMM_SUCCESS) - { - int _wait_length = 0; - for(int _i = 0; _i < param_length; _i += 5) - _wait_length += DXL_MAKEWORD(param[_i+3], param[_i+4]) + 10; - port->SetPacketTimeout((UINT16_T)_wait_length); - } - - free(txpacket); - //delete[] txpacket; - return _result; -} - -int Protocol2PacketHandler::BulkWriteTxOnly(PortHandler *port, UINT8_T *param, UINT16_T param_length) -{ - int _result = COMM_TX_FAIL; - - UINT8_T *txpacket = (UINT8_T *)malloc(param_length + 10); - //UINT8_T *txpacket = new UINT8_T[param_length + 10]; - // 10: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST CRC16_L CRC16_H - - txpacket[PKT_ID] = BROADCAST_ID; - txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H - txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H - txpacket[PKT_INSTRUCTION] = INST_BULK_WRITE; - - for(UINT8_T _s = 0; _s < param_length; _s++) - txpacket[PKT_PARAMETER0+_s] = param[_s]; - //memcpy(&txpacket[PKT_PARAMETER0], param, param_length); - - _result = TxRxPacket(port, txpacket, 0, 0); - - free(txpacket); - //delete[] txpacket; - return _result; -} diff --git a/dynamixel_sdk/src/dynamixel_sdk_linux/PortHandlerLinux.cpp b/dynamixel_sdk/src/dynamixel_sdk_linux/PortHandlerLinux.cpp deleted file mode 100644 index 96c3f75..0000000 --- a/dynamixel_sdk/src/dynamixel_sdk_linux/PortHandlerLinux.cpp +++ /dev/null @@ -1,245 +0,0 @@ -/* - * PortHandlerLinux.cpp - * - * Created on: 2016. 1. 26. - * Author: zerom, leon - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "dynamixel_sdk_linux/PortHandlerLinux.h" - -#define LATENCY_TIMER 4 // msec (USB latency timer) - -using namespace ROBOTIS; - -PortHandlerLinux::PortHandlerLinux(const char *port_name) - : socket_fd_(-1), - baudrate_(DEFAULT_BAUDRATE), - packet_start_time_(0.0), - packet_timeout_(0.0), - tx_time_per_byte(0.0) -{ - is_using = false; - SetPortName(port_name); -} - -bool PortHandlerLinux::OpenPort() -{ - return SetBaudRate(baudrate_); -} - -void PortHandlerLinux::ClosePort() -{ - if(socket_fd_ != -1) - close(socket_fd_); - socket_fd_ = -1; -} - -void PortHandlerLinux::ClearPort() -{ - tcflush(socket_fd_, TCIOFLUSH); -} - -void PortHandlerLinux::SetPortName(const char *port_name) -{ - strcpy(port_name_, port_name); -} - -char *PortHandlerLinux::GetPortName() -{ - return port_name_; -} - -// TODO: baud number ?? -bool PortHandlerLinux::SetBaudRate(const int baudrate) -{ - int _baud = GetCFlagBaud(baudrate); - - ClosePort(); - - if(_baud <= 0) // custom baudrate - { - SetupPort(B38400); - baudrate_ = baudrate; - return SetCustomBaudrate(baudrate); - } - else - { - baudrate_ = baudrate; - return SetupPort(_baud); - } -} - -int PortHandlerLinux::GetBaudRate() -{ - return baudrate_; -} - -int PortHandlerLinux::GetBytesAvailable() -{ - int _bytes_available; - ioctl(socket_fd_, FIONREAD, &_bytes_available); - return _bytes_available; -} - -int PortHandlerLinux::ReadPort(UINT8_T *packet, int length) -{ - return read(socket_fd_, packet, length); -} - -int PortHandlerLinux::WritePort(UINT8_T *packet, int length) -{ - return write(socket_fd_, packet, length); -} - -void PortHandlerLinux::SetPacketTimeout(UINT16_T packet_length) -{ - packet_start_time_ = GetCurrentTime(); - packet_timeout_ = (tx_time_per_byte * (double)packet_length) + (LATENCY_TIMER * 2.0) + 2.0; -} - -void PortHandlerLinux::SetPacketTimeout(double msec) -{ - packet_start_time_ = GetCurrentTime(); - packet_timeout_ = msec; -} - -bool PortHandlerLinux::IsPacketTimeout() -{ - if(GetTimeSinceStart() > packet_timeout_) - { - packet_timeout_ = 0; - return true; - } - return false; -} - -double PortHandlerLinux::GetCurrentTime() -{ - struct timespec _tv; - clock_gettime( CLOCK_REALTIME, &_tv); - return ((double)_tv.tv_sec*1000.0 + (double)_tv.tv_nsec*0.001*0.001); -} - -double PortHandlerLinux::GetTimeSinceStart() -{ - double _time; - - _time = GetCurrentTime() - packet_start_time_; - if(_time < 0.0) - packet_start_time_ = GetCurrentTime(); - - return _time; -} - -bool PortHandlerLinux::SetupPort(int cflag_baud) -{ - struct termios newtio; - - socket_fd_ = open(port_name_, O_RDWR|O_NOCTTY|O_NONBLOCK); - if(socket_fd_ < 0) - { - printf("[PortHandlerLinux::SetupPort] Error opening serial port!\n"); - return false; - } - - bzero(&newtio, sizeof(newtio)); // clear struct for new port settings - - newtio.c_cflag = cflag_baud | CS8 | CLOCAL | CREAD; - newtio.c_iflag = IGNPAR; - newtio.c_oflag = 0; - newtio.c_lflag = 0; - newtio.c_cc[VTIME] = 0; - newtio.c_cc[VMIN] = 0; - - // clean the buffer and activate the settings for the port - tcflush(socket_fd_, TCIFLUSH); - tcsetattr(socket_fd_, TCSANOW, &newtio); - - tx_time_per_byte = (1000.0 / (double)baudrate_) * 10.0; - return true; -} - -bool PortHandlerLinux::SetCustomBaudrate(int speed) -{ - // try to set a custom divisor - struct serial_struct ss; - if(ioctl(socket_fd_, TIOCGSERIAL, &ss) != 0) - { - printf("[PortHandlerLinux::SetCustomBaudrate] TIOCGSERIAL failed!\n"); - return false; - } - - ss.flags = (ss.flags & ~ASYNC_SPD_MASK) | ASYNC_SPD_CUST; - ss.custom_divisor = (ss.baud_base + (speed / 2)) / speed; - int closest_speed = ss.baud_base / ss.custom_divisor; - - if(closest_speed < speed * 98 / 100 || closest_speed > speed * 102 / 100) - { - printf("[PortHandlerLinux::SetCustomBaudrate] Cannot set speed to %d, closest is %d \n", speed, closest_speed); - return false; - } - - if(ioctl(socket_fd_, TIOCSSERIAL, &ss) < 0) - { - printf("[PortHandlerLinux::SetCustomBaudrate] TIOCSSERIAL failed!\n"); - return false; - } - - tx_time_per_byte = (1000.0 / (double)speed) * 10.0; - return true; -} - -int PortHandlerLinux::GetCFlagBaud(int baudrate) -{ - switch(baudrate) - { - case 9600: - return B9600; - case 19200: - return B19200; - case 38400: - return B38400; - case 57600: - return B57600; - case 115200: - return B115200; - case 230400: - return B230400; - case 460800: - return B460800; - case 500000: - return B500000; - case 576000: - return B576000; - case 921600: - return B921600; - case 1000000: - return B1000000; - case 1152000: - return B1152000; - case 1500000: - return B1500000; - case 2000000: - return B2000000; - case 2500000: - return B2500000; - case 3000000: - return B3000000; - case 3500000: - return B3500000; - case 4000000: - return B4000000; - default: - return -1; - } -} diff --git a/robotis_controller/CMakeLists.txt b/robotis_controller/CMakeLists.txt index 5865116..97c4a6d 100644 --- a/robotis_controller/CMakeLists.txt +++ b/robotis_controller/CMakeLists.txt @@ -1,6 +1,8 @@ cmake_minimum_required(VERSION 2.8.3) project(robotis_controller) +set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}") + find_package(catkin REQUIRED COMPONENTS roscpp roslib @@ -13,15 +15,6 @@ find_package(catkin REQUIRED COMPONENTS dynamixel_sdk ) -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if you package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( INCLUDE_DIRS include LIBRARIES robotis_controller @@ -29,24 +22,15 @@ catkin_package( # DEPENDS system_lib ) -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -# include_directories(include) include_directories( include ${catkin_INCLUDE_DIRS} ) -## Declare a cpp library add_library(robotis_controller - src/robotis_controller/RobotisController.cpp + src/robotis_controller/robotis_controller.cpp ) -## Specify libraries to link a library or executable target against target_link_libraries(robotis_controller yaml-cpp ${catkin_LIBRARIES} diff --git a/robotis_controller/include/robotis_controller/RobotisController.h b/robotis_controller/include/robotis_controller/RobotisController.h deleted file mode 100644 index 46eb104..0000000 --- a/robotis_controller/include/robotis_controller/RobotisController.h +++ /dev/null @@ -1,145 +0,0 @@ -/* - * RobotisController.h - * - * Created on: 2016. 1. 15. - * Author: zerom - */ - -#ifndef ROBOTIS_FRAMEWORK_ROBOTIS_CONTROLLER_INCLUDE_ROBOTIS_CONTROLLER_ROBOTISCONTROLLER_H_ -#define ROBOTIS_FRAMEWORK_ROBOTIS_CONTROLLER_INCLUDE_ROBOTIS_CONTROLLER_ROBOTISCONTROLLER_H_ - - -#include -#include -#include -#include -#include -#include - -#include "robotis_device/Robot.h" -#include "robotis_framework_common/MotionModule.h" -#include "robotis_framework_common/SensorModule.h" - -#include "robotis_controller_msgs/SyncWriteItem.h" -#include "robotis_controller_msgs/JointCtrlModule.h" -#include "robotis_controller_msgs/GetJointModule.h" - -// TODO: TEMPORARY CODE !! -#include "dynamixel_sdk/GroupBulkRead.h" -#include "dynamixel_sdk/GroupSyncWrite.h" - -namespace ROBOTIS -{ - -enum CONTROLLER_MODE -{ - MOTION_MODULE_MODE, - DIRECT_CONTROL_MODE -}; - -class RobotisController : public Singleton -{ -private: - boost::thread queue_thread_; - boost::thread gazebo_thread_; - boost::thread set_module_thread_; - boost::mutex queue_mutex_; - - bool init_pose_loaded_; - bool is_timer_running_; - bool stop_timer_; - pthread_t timer_thread_; - CONTROLLER_MODE controller_mode_; - - std::list motion_modules_; - std::list sensor_modules_; - std::vector direct_sync_write_; - - std::map sensor_result_; - - void QueueThread(); - void GazeboThread(); - void SetCtrlModuleThread(std::string ctrl_module); - - bool CheckTimerStop(); - void InitSyncWrite(); - -public: - static const int CONTROL_CYCLE_MSEC = 8; // 8 msec - - bool DEBUG_PRINT; - Robot *robot; - - bool gazebo_mode; - std::string gazebo_robot_name; - - // TODO: TEMPORARY CODE !! - std::map port_to_bulk_read; - std::map port_to_sync_write_position; - std::map port_to_sync_write_velocity; - std::map port_to_sync_write_torque; - - /* publisher */ - ros::Publisher goal_joint_state_pub; - ros::Publisher present_joint_state_pub; - ros::Publisher current_module_pub; - - std::map gazebo_joint_pub; - - static void *ThreadProc(void *param); - - RobotisController(); - - bool Initialize(const std::string robot_file_path, const std::string init_file_path); - void InitDevice(const std::string init_file_path); - void Process(); - - void AddMotionModule(MotionModule *module); - void RemoveMotionModule(MotionModule *module); - void AddSensorModule(SensorModule *module); - void RemoveSensorModule(SensorModule *module); - - void StartTimer(); - void StopTimer(); - bool IsTimerRunning(); - - void LoadOffset(const std::string path); - - /* ROS Topic Callback Functions */ - void SyncWriteItemCallback(const robotis_controller_msgs::SyncWriteItem::ConstPtr &msg); - void SetControllerModeCallback(const std_msgs::String::ConstPtr &msg); - void SetJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg); - void SetJointCtrlModuleCallback(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg); - void SetCtrlModuleCallback(const std_msgs::String::ConstPtr &msg); - bool GetCtrlModuleCallback(robotis_controller_msgs::GetJointModule::Request &req, robotis_controller_msgs::GetJointModule::Response &res); - - void GazeboJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg); - - int Ping (const std::string joint_name, UINT8_T *error = 0); - int Ping (const std::string joint_name, UINT16_T* model_number, UINT8_T *error = 0); - - int Action (const std::string joint_name); - int Reboot (const std::string joint_name, UINT8_T *error = 0); - int FactoryReset(const std::string joint_name, UINT8_T option = 0, UINT8_T *error = 0); - - int Read (const std::string joint_name, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0); - int ReadCtrlItem(const std::string joint_name, const std::string item_name, UINT32_T *data, UINT8_T *error = 0); - - int Read1Byte (const std::string joint_name, UINT16_T address, UINT8_T *data, UINT8_T *error = 0); - int Read2Byte (const std::string joint_name, UINT16_T address, UINT16_T *data, UINT8_T *error = 0); - int Read4Byte (const std::string joint_name, UINT16_T address, UINT32_T *data, UINT8_T *error = 0); - - int Write (const std::string joint_name, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0); - int WriteCtrlItem(const std::string joint_name, const std::string item_name, UINT32_T data, UINT8_T *error = 0); - - int Write1Byte (const std::string joint_name, UINT16_T address, UINT8_T data, UINT8_T *error = 0); - int Write2Byte (const std::string joint_name, UINT16_T address, UINT16_T data, UINT8_T *error = 0); - int Write4Byte (const std::string joint_name, UINT16_T address, UINT32_T data, UINT8_T *error = 0); - - int RegWrite (const std::string joint_name, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0); -}; - -} - - -#endif /* ROBOTIS_FRAMEWORK_ROBOTIS_CONTROLLER_INCLUDE_ROBOTIS_CONTROLLER_ROBOTISCONTROLLER_H_ */ diff --git a/robotis_controller/include/robotis_controller/robotis_controller.h b/robotis_controller/include/robotis_controller/robotis_controller.h new file mode 100644 index 0000000..65d7821 --- /dev/null +++ b/robotis_controller/include/robotis_controller/robotis_controller.h @@ -0,0 +1,183 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of ROBOTIS nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *******************************************************************************/ + +/* + * robotis_controller.h + * + * Created on: 2016. 1. 15. + * Author: zerom + */ + +#ifndef ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_ +#define ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_ + + +#include +#include +#include +#include +#include +#include + +#include "robotis_controller_msgs/SyncWriteItem.h" +#include "robotis_controller_msgs/JointCtrlModule.h" +#include "robotis_controller_msgs/GetJointModule.h" + +#include "robotis_device/robot.h" +#include "robotis_framework_common/motion_module.h" +#include "robotis_framework_common/sensor_module.h" +#include "dynamixel_sdk/group_bulk_read.h" +#include "dynamixel_sdk/group_sync_write.h" + +namespace robotis_framework +{ + +enum ControllerMode +{ + MotionModuleMode, + DirectControlMode +}; + +class RobotisController : public Singleton +{ +private: + boost::thread queue_thread_; + boost::thread gazebo_thread_; + boost::thread set_module_thread_; + boost::mutex queue_mutex_; + + bool init_pose_loaded_; + bool is_timer_running_; + bool stop_timer_; + pthread_t timer_thread_; + ControllerMode controller_mode_; + + std::list motion_modules_; + std::list sensor_modules_; + std::vector direct_sync_write_; + + std::map sensor_result_; + + void gazeboTimerThread(); + void msgQueueThread(); + void setCtrlModuleThread(std::string ctrl_module); + + bool isTimerStopped(); + void initializeSyncWrite(); + +public: + static const int CONTROL_CYCLE_MSEC = 8; // 8 msec + + bool DEBUG_PRINT; + Robot *robot_; + + bool gazebo_mode_; + std::string gazebo_robot_name_; + + /* bulk read */ + std::map port_to_bulk_read_; + + /* sync write */ + std::map port_to_sync_write_position_; + std::map port_to_sync_write_velocity_; + std::map port_to_sync_write_current_; + std::map port_to_sync_write_position_p_gain_; + std::map port_to_sync_write_position_i_gain_; + std::map port_to_sync_write_position_d_gain_; + std::map port_to_sync_write_velocity_p_gain_; + std::map port_to_sync_write_velocity_i_gain_; + std::map port_to_sync_write_velocity_d_gain_; + + /* publisher */ + ros::Publisher goal_joint_state_pub_; + ros::Publisher present_joint_state_pub_; + ros::Publisher current_module_pub_; + + std::map gazebo_joint_position_pub_; + std::map gazebo_joint_velocity_pub_; + std::map gazebo_joint_effort_pub_; + + static void *timerThread(void *param); + + RobotisController(); + + bool initialize(const std::string robot_file_path, const std::string init_file_path); + void initializeDevice(const std::string init_file_path); + void process(); + + void addMotionModule(MotionModule *module); + void removeMotionModule(MotionModule *module); + void addSensorModule(SensorModule *module); + void removeSensorModule(SensorModule *module); + + void startTimer(); + void stopTimer(); + bool isTimerRunning(); + + void loadOffset(const std::string path); + + /* ROS Topic Callback Functions */ + void syncWriteItemCallback(const robotis_controller_msgs::SyncWriteItem::ConstPtr &msg); + void setControllerModeCallback(const std_msgs::String::ConstPtr &msg); + void setJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg); + void setJointCtrlModuleCallback(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg); + void setCtrlModuleCallback(const std_msgs::String::ConstPtr &msg); + bool getCtrlModuleCallback(robotis_controller_msgs::GetJointModule::Request &req, robotis_controller_msgs::GetJointModule::Response &res); + + void gazeboJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg); + + int ping (const std::string joint_name, uint8_t *error = 0); + int ping (const std::string joint_name, uint16_t* model_number, uint8_t *error = 0); + + int action (const std::string joint_name); + int reboot (const std::string joint_name, uint8_t *error = 0); + int factoryReset(const std::string joint_name, uint8_t option = 0, uint8_t *error = 0); + + int read (const std::string joint_name, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0); + int readCtrlItem(const std::string joint_name, const std::string item_name, uint32_t *data, uint8_t *error = 0); + + int read1Byte (const std::string joint_name, uint16_t address, uint8_t *data, uint8_t *error = 0); + int read2Byte (const std::string joint_name, uint16_t address, uint16_t *data, uint8_t *error = 0); + int read4Byte (const std::string joint_name, uint16_t address, uint32_t *data, uint8_t *error = 0); + + int write (const std::string joint_name, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0); + int writeCtrlItem(const std::string joint_name, const std::string item_name, uint32_t data, uint8_t *error = 0); + + int write1Byte (const std::string joint_name, uint16_t address, uint8_t data, uint8_t *error = 0); + int write2Byte (const std::string joint_name, uint16_t address, uint16_t data, uint8_t *error = 0); + int write4Byte (const std::string joint_name, uint16_t address, uint32_t data, uint8_t *error = 0); + + int regWrite (const std::string joint_name, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0); +}; + +} + + +#endif /* ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_ */ diff --git a/robotis_controller/package.xml b/robotis_controller/package.xml index 8a25f7e..6dcf6d2 100644 --- a/robotis_controller/package.xml +++ b/robotis_controller/package.xml @@ -6,16 +6,12 @@ ROBOTIS - BSD - - ROBOTIS - catkin roscpp diff --git a/robotis_controller/src/robotis_controller/RobotisController.cpp b/robotis_controller/src/robotis_controller/RobotisController.cpp deleted file mode 100644 index e93e58c..0000000 --- a/robotis_controller/src/robotis_controller/RobotisController.cpp +++ /dev/null @@ -1,1592 +0,0 @@ -/* - * RobotisController.cpp - * - * Created on: 2016. 1. 15. - * Author: zerom - */ - -#include -#include - -#include "robotis_controller/RobotisController.h" - -using namespace ROBOTIS; - -RobotisController::RobotisController() -: is_timer_running_(false), - stop_timer_(false), - init_pose_loaded_(false), - timer_thread_(0), - controller_mode_(MOTION_MODULE_MODE), - DEBUG_PRINT(false), - robot(0), - gazebo_mode(false), - gazebo_robot_name("robotis") -{ - direct_sync_write_.clear(); -} - -void RobotisController::InitSyncWrite() -{ - if(gazebo_mode == true) - return; - - ROS_INFO("FIRST BULKREAD"); - // bulkread twice - for(std::map::iterator _it = port_to_bulk_read.begin(); _it != port_to_bulk_read.end(); _it++) - _it->second->TxRxPacket(); - for(std::map::iterator _it = port_to_bulk_read.begin(); _it != port_to_bulk_read.end(); _it++) - { - int _error_cnt = 0; - int _result = COMM_SUCCESS; - do { - if(++_error_cnt > 10) - { - ROS_ERROR("[RobotisController] bulk read fail!!"); - exit(-1); - } - usleep(10*1000); - _result = _it->second->TxRxPacket(); - } while (_result != COMM_SUCCESS); - } - init_pose_loaded_ = true; - ROS_INFO("FIRST BULKREAD END"); - - // clear syncwrite param setting - for(std::map::iterator _it = port_to_sync_write_position.begin(); _it != port_to_sync_write_position.end(); _it++) - _it->second->ClearParam(); - for(std::map::iterator _it = port_to_sync_write_velocity.begin(); _it != port_to_sync_write_velocity.end(); _it++) - _it->second->ClearParam(); - for(std::map::iterator _it = port_to_sync_write_torque.begin(); _it != port_to_sync_write_torque.end(); _it++) - _it->second->ClearParam(); - - // set init syncwrite param(from data of bulkread) - for(std::map::iterator _it = robot->dxls.begin(); _it != robot->dxls.end(); _it++) - { - std::string _joint_name = _it->first; - Dynamixel *_dxl = _it->second; - - for(int _i = 0; _i < _dxl->bulk_read_items.size(); _i++) - { - UINT32_T _read_data = 0; - UINT8_T _sync_write_data[4]; - - if(port_to_bulk_read[_dxl->port_name]->IsAvailable(_dxl->id, - _dxl->bulk_read_items[_i]->address, - _dxl->bulk_read_items[_i]->data_length) == true) - { - _read_data = port_to_bulk_read[_dxl->port_name]->GetData(_dxl->id, - _dxl->bulk_read_items[_i]->address, - _dxl->bulk_read_items[_i]->data_length); - - _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_read_data)); - _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_read_data)); - _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_read_data)); - _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_read_data)); - - if(_dxl->present_position_item != 0 && _dxl->bulk_read_items[_i]->item_name == _dxl->present_position_item->item_name) - { - _dxl->dxl_state->present_position = _dxl->ConvertValue2Radian(_read_data) - _dxl->dxl_state->position_offset; // remove offset - _dxl->dxl_state->goal_position = _dxl->dxl_state->present_position; - - port_to_sync_write_position[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); - } - else if(_dxl->present_velocity_item != 0 && _dxl->bulk_read_items[_i]->item_name == _dxl->present_velocity_item->item_name) - { - _dxl->dxl_state->present_velocity = _dxl->ConvertValue2Velocity(_read_data); - _dxl->dxl_state->goal_velocity = _dxl->dxl_state->present_velocity; - } - else if(_dxl->present_current_item != 0 && _dxl->bulk_read_items[_i]->item_name == _dxl->present_current_item->item_name) - { - _dxl->dxl_state->present_current = _dxl->ConvertValue2Current(_read_data); - _dxl->dxl_state->goal_current = _dxl->dxl_state->present_current; - } - } - } - } -} - -bool RobotisController::Initialize(const std::string robot_file_path, const std::string init_file_path) -{ - std::string _dev_desc_dir_path = ros::package::getPath("robotis_device") + "/devices"; - - // load robot info : port , device - robot = new Robot(robot_file_path, _dev_desc_dir_path); - - if(gazebo_mode == true) - { - queue_thread_ = boost::thread(boost::bind(&RobotisController::QueueThread, this)); - return true; - } - - for(std::map::iterator _it = robot->ports.begin(); _it != robot->ports.end(); _it++) - { - std::string _port_name = _it->first; - PortHandler *_port = _it->second; - - PacketHandler *_default_pkt_handler = PacketHandler::GetPacketHandler(2.0); - - if(_port->SetBaudRate(_port->GetBaudRate()) == false) - { - ROS_ERROR("PORT [%s] SETUP ERROR! (baudrate: %d)", _port_name.c_str(), _port->GetBaudRate()); - exit(-1); - } - - // get the default device info of the port - std::string _default_device_name = robot->port_default_device[_port_name]; - std::map::iterator _dxl_it = robot->dxls.find(_default_device_name); - std::map::iterator _sensor_it = robot->sensors.find(_default_device_name); - if(_dxl_it != robot->dxls.end()) - { - Dynamixel *_default_device = _dxl_it->second; - _default_pkt_handler = PacketHandler::GetPacketHandler(_default_device->protocol_version); - - port_to_sync_write_position[_port_name] = new GroupSyncWrite(_port, - _default_pkt_handler, - _default_device->goal_position_item->address, - _default_device->goal_position_item->data_length); - - port_to_sync_write_velocity[_port_name] = new GroupSyncWrite(_port, - _default_pkt_handler, - _default_device->goal_velocity_item->address, - _default_device->goal_velocity_item->data_length); - - port_to_sync_write_torque[_port_name] = new GroupSyncWrite(_port, - _default_pkt_handler, - _default_device->goal_current_item->address, - _default_device->goal_current_item->data_length); - } - else if(_sensor_it != robot->sensors.end()) - { - Sensor *_default_device = _sensor_it->second; - _default_pkt_handler = PacketHandler::GetPacketHandler(_default_device->protocol_version); - } - - port_to_bulk_read[_port_name] = new GroupBulkRead(_port, _default_pkt_handler); - } - - // (for loop) check all dxls are connected. - for(std::map::iterator _it = robot->dxls.begin(); _it != robot->dxls.end(); _it++) - { - std::string _joint_name = _it->first; - Dynamixel *_dxl = _it->second; - - if(Ping(_joint_name) != 0) - { - usleep(10*1000); - if(Ping(_joint_name) != 0) - ROS_ERROR("JOINT[%s] does NOT respond!!", _joint_name.c_str()); - } - } - - InitDevice(init_file_path); - - // [ BulkRead ] StartAddress : Present Position , Length : 10 ( Position/Velocity/Current ) - for(std::map::iterator _it = robot->dxls.begin(); _it != robot->dxls.end(); _it++) - { - std::string _joint_name = _it->first; - Dynamixel *_dxl = _it->second; - - if(_dxl == NULL) - continue; - - int _bulkread_start_addr = 0; - int _bulkread_data_length = 0; - - // bulk read default : present position - if(_dxl->present_position_item != 0) - { - _bulkread_start_addr = _dxl->present_position_item->address; - _bulkread_data_length = _dxl->present_position_item->data_length; - } - - // TODO: modifing - std::map::iterator _indirect_addr_it = _dxl->ctrl_table.find(INDIRECT_ADDRESS_1); - if(_indirect_addr_it != _dxl->ctrl_table.end()) // INDIRECT_ADDRESS_1 exist - { - if(_dxl->bulk_read_items.size() != 0) - { - _bulkread_start_addr = _dxl->bulk_read_items[0]->address; - _bulkread_data_length = 0; - - // set indirect address - int _indirect_addr = _indirect_addr_it->second->address; - for(int _i = 0; _i < _dxl->bulk_read_items.size(); _i++) - { - int _addr_leng = _dxl->bulk_read_items[_i]->data_length; - - _bulkread_data_length += _addr_leng; - for(int _l = 0; _l < _addr_leng; _l++) - { -// ROS_WARN("[%12s] INDIR_ADDR: %d, ITEM_ADDR: %d", _joint_name.c_str(), _indirect_addr, _dxl->ctrl_table[_dxl->bulk_read_items[_i]->item_name]->address + _l); - Write2Byte(_joint_name, _indirect_addr, _dxl->ctrl_table[_dxl->bulk_read_items[_i]->item_name]->address + _l); - _indirect_addr += 2; - } - } - } - } - else // INDIRECT_ADDRESS_1 NOT exist - { - if(_dxl->bulk_read_items.size() != 0) - { - _bulkread_start_addr = _dxl->bulk_read_items[0]->address; - _bulkread_data_length = 0; - - ControlTableItem *_last_item = _dxl->bulk_read_items[0]; - - for(int _i = 0; _i < _dxl->bulk_read_items.size(); _i++) - { - int _addr = _dxl->bulk_read_items[_i]->address; - if(_addr < _bulkread_start_addr) - _bulkread_start_addr = _addr; - else if(_last_item->address < _addr) - _last_item = _dxl->bulk_read_items[_i]; - } - - _bulkread_data_length = _last_item->address - _bulkread_start_addr + _last_item->data_length; - } - } - -// ROS_WARN("[%12s] start_addr: %d, data_length: %d", _joint_name.c_str(), _bulkread_start_addr, _bulkread_data_length); - port_to_bulk_read[_dxl->port_name]->AddParam(_dxl->id, _bulkread_start_addr, _bulkread_data_length); - - // Torque ON - if(WriteCtrlItem(_joint_name, _dxl->torque_enable_item->item_name, 1) != COMM_SUCCESS) - WriteCtrlItem(_joint_name, _dxl->torque_enable_item->item_name, 1); - } - - queue_thread_ = boost::thread(boost::bind(&RobotisController::QueueThread, this)); - return true; -} - -void RobotisController::InitDevice(const std::string init_file_path) -{ - // device initialize - if(DEBUG_PRINT) ROS_WARN("INIT FILE LOAD"); - - YAML::Node _doc; - try{ - _doc = YAML::LoadFile(init_file_path.c_str()); - - for(YAML::const_iterator _it_doc = _doc.begin(); _it_doc != _doc.end(); _it_doc++) - { - std::string _joint_name = _it_doc->first.as(); - - YAML::Node _joint_node = _doc[_joint_name]; - if(_joint_node.size() == 0) - continue; - - Dynamixel *_dxl = NULL; - std::map::iterator _dxl_it = robot->dxls.find(_joint_name); - if(_dxl_it != robot->dxls.end()) - _dxl = _dxl_it->second; - - if(_dxl == NULL) - { - ROS_WARN("Joint [%s] does not found.", _joint_name.c_str()); - continue; - } - if(DEBUG_PRINT) ROS_INFO("JOINT_NAME: %s", _joint_name.c_str()); - - for(YAML::const_iterator _it_joint = _joint_node.begin(); _it_joint != _joint_node.end(); _it_joint++) - { - std::string _item_name = _it_joint->first.as(); - - if(DEBUG_PRINT) ROS_INFO(" ITEM_NAME: %s", _item_name.c_str()); - - UINT32_T _value = _it_joint->second.as(); - - ControlTableItem *_item = _dxl->ctrl_table[_item_name]; - if(_item == NULL) - { - ROS_WARN("Control Item [%s] does not found.", _item_name.c_str()); - continue; - } - - if(_item->memory_type == EEPROM) - { - UINT8_T _data8 = 0; - UINT16_T _data16 = 0; - UINT32_T _data32 = 0; - - switch(_item->data_length) - { - case 1: - Read1Byte(_joint_name, _item->address, &_data8); - if(_data8 == _value) - continue; - break; - case 2: - Read2Byte(_joint_name, _item->address, &_data16); - if(_data16 == _value) - continue; - break; - case 4: - Read4Byte(_joint_name, _item->address, &_data32); - if(_data32 == _value) - continue; - break; - default: - break; - } - } - - switch(_item->data_length) - { - case 1: - Write1Byte(_joint_name, _item->address, (UINT8_T)_value); - break; - case 2: - Write2Byte(_joint_name, _item->address, (UINT16_T)_value); - break; - case 4: - Write4Byte(_joint_name, _item->address, _value); - break; - default: - break; - } - - if(_item->memory_type == EEPROM) - { - // Write to EEPROM -> delay is required (max delay: 55 msec per byte) - usleep(_item->data_length * 55 * 1000); - } - } - } - } catch(const std::exception& e) { - ROS_INFO("Dynamixel Init file not found."); - } -} - -void RobotisController::GazeboThread() -{ - ros::Rate gazebo_rate(1000 / CONTROL_CYCLE_MSEC); - - while(!stop_timer_) - { - if(init_pose_loaded_ == true) - Process(); - gazebo_rate.sleep(); - } -} - -void RobotisController::QueueThread() -{ - ros::NodeHandle _ros_node; - ros::CallbackQueue _callback_queue; - - _ros_node.setCallbackQueue(&_callback_queue); - - /* subscriber */ - ros::Subscriber _sync_write_item_sub = _ros_node.subscribe("/robotis/sync_write_item", 10, &RobotisController::SyncWriteItemCallback, this); - ros::Subscriber _joint_ctrl_modules_sub = _ros_node.subscribe("/robotis/set_joint_ctrl_modules", 10, &RobotisController::SetJointCtrlModuleCallback, this); - ros::Subscriber _enable_ctrl_module_sub = _ros_node.subscribe("/robotis/enable_ctrl_module", 10, &RobotisController::SetCtrlModuleCallback, this); - ros::Subscriber _control_mode_sub = _ros_node.subscribe("/robotis/set_control_mode", 10, &RobotisController::SetControllerModeCallback, this); - ros::Subscriber _joint_states_sub = _ros_node.subscribe("/robotis/set_joint_states", 10, &RobotisController::SetJointStatesCallback, this); - - /* publisher */ - goal_joint_state_pub = _ros_node.advertise("/robotis/goal_joint_states", 10); - present_joint_state_pub = _ros_node.advertise("/robotis/present_joint_states", 10); - current_module_pub = _ros_node.advertise("/robotis/present_joint_ctrl_modules", 10); - - ros::Subscriber _gazebo_joint_states_sub; - if(gazebo_mode == true) - { - _gazebo_joint_states_sub = _ros_node.subscribe("/" + gazebo_robot_name + "/joint_states", 10, &RobotisController::GazeboJointStatesCallback, this); - - for(std::map::iterator _it = robot->dxls.begin(); _it != robot->dxls.end(); _it++) - gazebo_joint_pub[_it->first] = _ros_node.advertise("/" + gazebo_robot_name + "/" + _it->first + "_pos/command", 1); - } - - /* service */ - ros::ServiceServer _joint_module_server = _ros_node.advertiseService("/robotis/get_present_joint_ctrl_modules", &RobotisController::GetCtrlModuleCallback, this); - - while(_ros_node.ok()) - { - _callback_queue.callAvailable(); - usleep(100); - } -} - -void *RobotisController::ThreadProc(void *param) -{ - RobotisController *controller = (RobotisController *)param; - static struct timespec next_time; - static struct timespec curr_time; - - ROS_INFO("controller::thread_proc"); - - clock_gettime(CLOCK_MONOTONIC, &next_time); - - while(!controller->stop_timer_) - { - next_time.tv_sec += (next_time.tv_nsec + CONTROL_CYCLE_MSEC * 1000000) / 1000000000; - next_time.tv_nsec = (next_time.tv_nsec + CONTROL_CYCLE_MSEC * 1000000) % 1000000000; - - controller->Process(); - - clock_gettime(CLOCK_MONOTONIC, &curr_time); - long delta_nsec = (next_time.tv_sec - curr_time.tv_sec) * 1000000000 + (next_time.tv_nsec - curr_time.tv_nsec); - if(delta_nsec < -100000 ) - { - if(controller->DEBUG_PRINT == true) - ROS_WARN("[RobotisController::ThreadProc] NEXT TIME < CURR TIME.. (%f)[%ld.%09ld / %ld.%09ld]", delta_nsec/1000000.0, (long)next_time.tv_sec, (long)next_time.tv_nsec, (long)curr_time.tv_sec, (long)curr_time.tv_nsec); - // next_time = curr_time + 3 msec - next_time.tv_sec = curr_time.tv_sec + (curr_time.tv_nsec + 3000000) / 1000000000; - next_time.tv_nsec = (curr_time.tv_nsec + 3000000) % 1000000000; - } - - clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &next_time, NULL); - } - return 0; -} - -void RobotisController::StartTimer() -{ - if(this->is_timer_running_ == true) - return; - - if(this->gazebo_mode == true) - { - // create and start the thread - gazebo_thread_ = boost::thread(boost::bind(&RobotisController::GazeboThread, this)); - } - else - { - InitSyncWrite(); - - for(std::map::iterator _it = port_to_bulk_read.begin(); _it != port_to_bulk_read.end(); _it++) - _it->second->TxPacket(); - - int error; - struct sched_param param; - pthread_attr_t attr; - - pthread_attr_init(&attr); - - error = pthread_attr_setschedpolicy(&attr, SCHED_RR); - if(error != 0) - ROS_ERROR("pthread_attr_setschedpolicy error = %d\n",error); - error = pthread_attr_setinheritsched(&attr,PTHREAD_EXPLICIT_SCHED); - if(error != 0) - ROS_ERROR("pthread_attr_setinheritsched error = %d\n",error); - - memset(¶m, 0, sizeof(param)); - param.sched_priority = 31;// RT - error = pthread_attr_setschedparam(&attr, ¶m); - if(error != 0) - ROS_ERROR("pthread_attr_setschedparam error = %d\n",error); - - // create and start the thread - if((error = pthread_create(&this->timer_thread_, &attr, this->ThreadProc, this))!= 0) { - ROS_ERROR("timer thread create fail!!"); - exit(-1); - } - } - - this->is_timer_running_ = true; -} - -void RobotisController::StopTimer() -{ - int error = 0; - - // set the flag to stop the thread - if(this->is_timer_running_) - { - this->stop_timer_ = true; - - if(this->gazebo_mode == false) - { - // wait until the thread is stopped. - if((error = pthread_join(this->timer_thread_, NULL)) != 0) - exit(-1); - - for(std::map::iterator _it = port_to_bulk_read.begin(); _it != port_to_bulk_read.end(); _it++) - _it->second->RxPacket(); - - for(std::map::iterator _it = port_to_sync_write_position.begin(); _it != port_to_sync_write_position.end(); _it++) - _it->second->ClearParam(); - for(std::map::iterator _it = port_to_sync_write_velocity.begin(); _it != port_to_sync_write_velocity.end(); _it++) - _it->second->ClearParam(); - for(std::map::iterator _it = port_to_sync_write_torque.begin(); _it != port_to_sync_write_torque.end(); _it++) - _it->second->ClearParam(); - } - else - { - // wait until the thread is stopped. - gazebo_thread_.join(); - } - - this->stop_timer_ = false; - this->is_timer_running_ = false; - } -} - -bool RobotisController::IsTimerRunning() -{ - return this->is_timer_running_; -} - -void RobotisController::LoadOffset(const std::string path) -{ - YAML::Node _doc; - try{ - _doc = YAML::LoadFile(path.c_str()); - } catch(const std::exception& e) { - ROS_ERROR("Fail to load offset yaml."); - return; - } - - YAML::Node _offset_node = _doc["offset"]; - if(_offset_node.size() == 0) - return; - - ROS_INFO("Load offsets..."); - for(YAML::const_iterator _it = _offset_node.begin(); _it != _offset_node.end(); _it++) - { - std::string _joint_name = _it->first.as(); - double _offset = _it->second.as(); - - std::map::iterator _dxl_it = robot->dxls.find(_joint_name); - if(_dxl_it != robot->dxls.end()) - _dxl_it->second->dxl_state->position_offset = _offset; - } -} - -void RobotisController::Process() -{ - // avoid duplicated function call - static bool _is_process_running = false; - if(_is_process_running == true) - return; - _is_process_running = true; - - // ROS_INFO("Controller::Process()"); - bool _do_sync_write = false; - - sensor_msgs::JointState _goal_state; - sensor_msgs::JointState _present_state; - - -// ros::Time _now = ros::Time::now(); - - - // BulkRead Rx - if(gazebo_mode == false) - { - for(std::map::iterator _it = port_to_bulk_read.begin(); _it != port_to_bulk_read.end(); _it++) - _it->second->RxPacket(); - } - -// ros::Duration _dur = ros::Time::now() - _now; -// double _msec = _dur.nsec * 0.000001; -// -// if(_msec > 8) std::cout << "Process duration : " << _msec << std::endl; - - // -> save to Robot->dxls[]->dynamixel_state.present_position - if(robot->dxls.size() > 0) - { - for(std::map::iterator dxl_it = robot->dxls.begin(); dxl_it != robot->dxls.end(); dxl_it++) - { - UINT32_T _data = 0; - - std::string _port_name = dxl_it->second->port_name; - std::string _joint_name = dxl_it->first; - Dynamixel *_dxl = dxl_it->second; - - _present_state.header.stamp = ros::Time::now(); - _goal_state.header.stamp = _present_state.header.stamp; - - if(gazebo_mode == false && _dxl->bulk_read_items.size() > 0) - { - for(int _i = 0; _i < _dxl->bulk_read_items.size(); _i++) - { - ControlTableItem *_item = _dxl->bulk_read_items[_i]; - if(port_to_bulk_read[_port_name]->IsAvailable(_dxl->id, _item->address, _item->data_length) == true) - { - _data = port_to_bulk_read[_port_name]->GetData(_dxl->id, _item->address, _item->data_length); - - // TODO: change dxl_state - if(_dxl->present_position_item != 0 && _item->item_name == _dxl->present_position_item->item_name) - _dxl->dxl_state->present_position = _dxl->ConvertValue2Radian(_data) - _dxl->dxl_state->position_offset; // remove offset - else if(_dxl->present_velocity_item != 0 && _item->item_name == _dxl->present_velocity_item->item_name) - _dxl->dxl_state->present_velocity = _dxl->ConvertValue2Velocity(_data); - else if(_dxl->present_current_item != 0 && _item->item_name == _dxl->present_current_item->item_name) - _dxl->dxl_state->present_current = _dxl->ConvertValue2Current(_data); - else if(_dxl->goal_position_item != 0 && _item->item_name == _dxl->goal_position_item->item_name) - _dxl->dxl_state->goal_position = _dxl->ConvertValue2Radian(_data) - _dxl->dxl_state->position_offset; // remove offset - else if(_dxl->goal_velocity_item != 0 && _item->item_name == _dxl->goal_velocity_item->item_name) - _dxl->dxl_state->goal_velocity = _dxl->ConvertValue2Velocity(_data); - else if(_dxl->goal_current_item != 0 && _item->item_name == _dxl->goal_current_item->item_name) - _dxl->dxl_state->goal_current = _dxl->ConvertValue2Current(_data); - else - _dxl->dxl_state->bulk_read_table[_item->item_name] = _data; - } - } - - // -> update time stamp to Robot->dxls[]->dynamixel_state.update_time_stamp - _dxl->dxl_state->update_time_stamp = TimeStamp(_present_state.header.stamp.sec, _present_state.header.stamp.nsec); - } - - _present_state.name.push_back(_joint_name); - // TODO: should be converted to rad, rad/s, Nm - _present_state.position.push_back(_dxl->dxl_state->present_position); - _present_state.velocity.push_back(_dxl->dxl_state->present_velocity); - _present_state.effort.push_back(_dxl->dxl_state->present_current); - - _goal_state.name.push_back(_joint_name); - _goal_state.position.push_back(_dxl->dxl_state->goal_position); - _goal_state.velocity.push_back(_dxl->dxl_state->goal_velocity); - _goal_state.effort.push_back(_dxl->dxl_state->goal_current); - } - } - - // -> publish present joint_states & goal joint states topic - present_joint_state_pub.publish(_present_state); - goal_joint_state_pub.publish(_goal_state); - - if(robot->sensors.size() > 0) - { - for(std::map::iterator sensor_it = robot->sensors.begin(); sensor_it != robot->sensors.end(); sensor_it++) - { - UINT32_T _data = 0; - - std::string _port_name = sensor_it->second->port_name; - std::string _sensor_name = sensor_it->first; - Sensor *_sensor = sensor_it->second; - - if(gazebo_mode == false && _sensor->bulk_read_items.size() > 0) - { - for(int _i = 0; _i < _sensor->bulk_read_items.size(); _i++) - { - ControlTableItem *_item = _sensor->bulk_read_items[_i]; - if(port_to_bulk_read[_port_name]->IsAvailable(_sensor->id, _item->address, _item->data_length) == true) - { - _data = port_to_bulk_read[_port_name]->GetData(_sensor->id, _item->address, _item->data_length); - - // TODO: change sensor_state - _sensor->sensor_state->bulk_read_table[_item->item_name] = _data; - } - } - - // -> update time stamp to Robot->dxls[]->dynamixel_state.update_time_stamp - ros::Time _now = ros::Time::now(); - _sensor->sensor_state->update_time_stamp = TimeStamp(_now.sec, _now.nsec); - } - } - } - - // Call SensorModule Process() - // -> for loop : call SensorModule list -> Process() - if(sensor_modules_.size() > 0) - { - for(std::list::iterator _module_it = sensor_modules_.begin(); _module_it != sensor_modules_.end(); _module_it++) - { - (*_module_it)->Process(robot->dxls, robot->sensors); - - for(std::map::iterator _it = (*_module_it)->result.begin(); _it != (*_module_it)->result.end(); _it++) - sensor_result_[_it->first] = _it->second; - } - } - - if(controller_mode_ == MOTION_MODULE_MODE) - { - // Call MotionModule Process() - // -> for loop : call MotionModule list -> Process() - if(motion_modules_.size() > 0) - { - queue_mutex_.lock(); - - for(std::list::iterator module_it = motion_modules_.begin(); module_it != motion_modules_.end(); module_it++) - { -// ros::Time _before = ros::Time::now(); - - if((*module_it)->enable == false) - continue; - - (*module_it)->Process(robot->dxls, sensor_result_); - -// ros::Duration _dur = ros::Time::now() - _before; -// double _msec = _dur.sec * 1e+3 + _dur.nsec * 0.000001; - -// std::cout << "Process duration ["<< (*module_it)->module_name << "] : " << _msec << std::endl; - - - ros::Time _before = ros::Time::now(); - - // for loop : joint list - for(std::map::iterator dxl_it = robot->dxls.begin(); dxl_it != robot->dxls.end(); dxl_it++) - { - std::string _joint_name = dxl_it->first; - Dynamixel *_dxl = dxl_it->second; - - DynamixelState *_dxl_state = _dxl->dxl_state; - if(_dxl->ctrl_module_name == (*module_it)->module_name) - { - _do_sync_write = true; - // ROS_INFO("Set goal value"); - DynamixelState *_result_state = (*module_it)->result[_joint_name]; - - if(_result_state == NULL) { - ROS_INFO("[%s] %s", (*module_it)->module_name.c_str(), _joint_name.c_str()); - continue; - } - - // TODO: check update time stamp ? - - if((*module_it)->control_mode == POSITION_CONTROL) - { -// if(_result_state->goal_position == 0 && _dxl->id == 3) -// ROS_INFO("[MODULE:%s][ID:3] goal position = %f", (*module_it)->module_name.c_str(), _dxl_state->goal_position); - _dxl_state->goal_position = _result_state->goal_position; - - if(gazebo_mode == false) - { - // add offset - UINT32_T _pos_data = _dxl->ConvertRadian2Value(_dxl_state->goal_position + _dxl_state->position_offset); - UINT8_T _sync_write_data[4]; - _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_pos_data)); - _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_pos_data)); - _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_pos_data)); - _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_pos_data)); - - if(abs(_pos_data) > 151800) - printf("goal_pos : %f | position_offset : %f | pos_data : %d\n", _dxl_state->goal_position , _dxl_state->position_offset, _pos_data); - - port_to_sync_write_position[_dxl->port_name]->ChangeParam(_dxl->id, _sync_write_data); - } - } - else if((*module_it)->control_mode == VELOCITY_CONTROL) - { - _dxl_state->goal_velocity = _result_state->goal_velocity; - - if(gazebo_mode == false) - { - UINT32_T _vel_data = _dxl->ConvertVelocity2Value(_dxl_state->goal_velocity); - UINT8_T _sync_write_data[4]; - _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_vel_data)); - _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_vel_data)); - _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_vel_data)); - _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_vel_data)); - - port_to_sync_write_velocity[_dxl->port_name]->ChangeParam(_dxl->id, _sync_write_data); - } - } - else if((*module_it)->control_mode == CURRENT_CONTROL) - { - _dxl_state->goal_current = _result_state->goal_current; - - if(gazebo_mode == false) - { - UINT32_T _torq_data = _dxl->ConvertCurrent2Value(_dxl_state->goal_current); - UINT8_T _sync_write_data[2]; - _sync_write_data[0] = DXL_LOBYTE(_torq_data); - _sync_write_data[1] = DXL_HIBYTE(_torq_data); - - port_to_sync_write_torque[_dxl->port_name]->ChangeParam(_dxl->id, _sync_write_data); - } - } - } - } - - ros::Duration _dur = ros::Time::now() - _before; - double _msec = _dur.sec * 1e+3 + _dur.nsec * 0.000001; - - //std::cout << "Process duration ["<< (*module_it)->module_name << "] : " << _msec << std::endl; - - } - - // std::cout << " ------------------------------------------- " << std::endl; - queue_mutex_.unlock(); - } - - // TODO: Combine the result && SyncWrite - // -> SyncWrite - if(gazebo_mode == false && _do_sync_write) - { - for(std::map::iterator _it = port_to_sync_write_position.begin(); _it != port_to_sync_write_position.end(); _it++) - _it->second->TxPacket(); - for(std::map::iterator _it = port_to_sync_write_velocity.begin(); _it != port_to_sync_write_velocity.end(); _it++) - _it->second->TxPacket(); - for(std::map::iterator _it = port_to_sync_write_torque.begin(); _it != port_to_sync_write_torque.end(); _it++) - _it->second->TxPacket(); - } - else if(gazebo_mode == true) - { - std_msgs::Float64 _joint_msg; - - for(std::map::iterator dxl_it = robot->dxls.begin(); dxl_it != robot->dxls.end(); dxl_it++) - { - _joint_msg.data = dxl_it->second->dxl_state->goal_position; - gazebo_joint_pub[dxl_it->first].publish(_joint_msg); - } - } - } - else if(controller_mode_ == DIRECT_CONTROL_MODE) - { - queue_mutex_.lock(); - - for(std::map::iterator _it = port_to_sync_write_position.begin(); _it != port_to_sync_write_position.end(); _it++) - { - _it->second->TxPacket(); - _it->second->ClearParam(); - } - - if(direct_sync_write_.size() > 0) - { - for(int _i = 0; _i < direct_sync_write_.size(); _i++) - { - direct_sync_write_[_i]->TxPacket(); - direct_sync_write_[_i]->ClearParam(); - } - direct_sync_write_.clear(); - } - - queue_mutex_.unlock(); - } - - // TODO: User Read/Write - - // BulkRead Tx - if(gazebo_mode == false) - { - for(std::map::iterator _it = port_to_bulk_read.begin(); _it != port_to_bulk_read.end(); _it++) - _it->second->TxPacket(); - } - - // ros::Duration _dur = ros::Time::now() - _now; - // double _msec = _dur.nsec * 0.000001; - // - // if(_msec > 8) std::cout << "Process duration : " << _msec << std::endl; - - _is_process_running = false; -} - -void RobotisController::AddMotionModule(MotionModule *module) -{ - module->Initialize(CONTROL_CYCLE_MSEC, robot); - motion_modules_.push_back(module); - motion_modules_.unique(); -} - -void RobotisController::RemoveMotionModule(MotionModule *module) -{ - motion_modules_.remove(module); -} - -void RobotisController::AddSensorModule(SensorModule *module) -{ - module->Initialize(CONTROL_CYCLE_MSEC, robot); - sensor_modules_.push_back(module); - sensor_modules_.unique(); -} - -void RobotisController::RemoveSensorModule(SensorModule *module) -{ - sensor_modules_.remove(module); -} - -void RobotisController::SyncWriteItemCallback(const robotis_controller_msgs::SyncWriteItem::ConstPtr &msg) -{ - for(int _i = 0; _i < msg->joint_name.size(); _i++) - { - Dynamixel *_dxl = robot->dxls[msg->joint_name[_i]]; - ControlTableItem *_item = _dxl->ctrl_table[msg->item_name]; - - PortHandler *_port = robot->ports[_dxl->port_name]; - PacketHandler *_packet_handler= PacketHandler::GetPacketHandler(_dxl->protocol_version); - - if(_item->access_type == READ) - continue; - - int _idx = 0; - if(direct_sync_write_.size() == 0) - { - direct_sync_write_.push_back(new GroupSyncWrite(_port, _packet_handler, _item->address, _item->data_length)); - _idx = 0; - } - else - { - for(_idx = 0; _idx < direct_sync_write_.size(); _idx++) - { - if(direct_sync_write_[_idx]->GetPortHandler() == _port && - direct_sync_write_[_idx]->GetPacketHandler() == _packet_handler) - break; - } - - if(_idx == direct_sync_write_.size()) - direct_sync_write_.push_back(new GroupSyncWrite(_port, _packet_handler, _item->address, _item->data_length)); - } - - UINT8_T *_data = new UINT8_T[_item->data_length]; - if(_item->data_length == 1) - _data[0] = (UINT8_T)msg->value[_i]; - else if(_item->data_length == 2) - { - _data[0] = DXL_LOBYTE((UINT16_T)msg->value[_i]); - _data[1] = DXL_HIBYTE((UINT16_T)msg->value[_i]); - } - else if(_item->data_length == 4) - { - _data[0] = DXL_LOBYTE(DXL_LOWORD((UINT32_T)msg->value[_i])); - _data[1] = DXL_HIBYTE(DXL_LOWORD((UINT32_T)msg->value[_i])); - _data[2] = DXL_LOBYTE(DXL_HIWORD((UINT32_T)msg->value[_i])); - _data[3] = DXL_HIBYTE(DXL_HIWORD((UINT32_T)msg->value[_i])); - } - direct_sync_write_[_idx]->AddParam(_dxl->id, _data); - delete[] _data; - } -} - -void RobotisController::SetControllerModeCallback(const std_msgs::String::ConstPtr &msg) -{ - if(msg->data == "DIRECT_CONTROL_MODE") - controller_mode_ = DIRECT_CONTROL_MODE; - else if(msg->data == "MOTION_MODULE_MODE") - controller_mode_ = MOTION_MODULE_MODE; -} - -void RobotisController::SetJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg) -{ - if(controller_mode_ != DIRECT_CONTROL_MODE) - return; - - queue_mutex_.lock(); - - for(int _i = 0; _i < msg->name.size(); _i++) - { - INT32_T _pos = 0; - - Dynamixel *_dxl = robot->dxls[msg->name[_i]]; - if(_dxl == NULL) - continue; - - _dxl->dxl_state->goal_position = msg->position[_i]; - _pos = _dxl->ConvertRadian2Value((double)msg->position[_i]); - - UINT8_T _sync_write_data[4]; - _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_pos)); - _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_pos)); - _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_pos)); - _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_pos)); - - port_to_sync_write_position[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); - } - - queue_mutex_.unlock(); -} - -void RobotisController::SetCtrlModuleCallback(const std_msgs::String::ConstPtr &msg) -{ - std::string _module_name_to_set = msg->data; - - set_module_thread_ = boost::thread(boost::bind(&RobotisController::SetCtrlModuleThread, this, _module_name_to_set)); -} - -void RobotisController::SetJointCtrlModuleCallback(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg) -{ - if(msg->joint_name.size() != msg->module_name.size()) - return; - - queue_mutex_.lock(); - - for(unsigned int idx = 0; idx < msg->joint_name.size(); idx++) - { - Dynamixel *_dxl = NULL; - std::map::iterator _dxl_it = robot->dxls.find((std::string)(msg->joint_name[idx])); - if(_dxl_it != robot->dxls.end()) - _dxl = _dxl_it->second; - else - continue; - - // none - if(msg->module_name[idx] == "" || msg->module_name[idx] == "none") - { - _dxl->ctrl_module_name = msg->module_name[idx]; - continue; - } - - // check whether the module is using this joint - for(std::list::iterator _m_it = motion_modules_.begin(); _m_it != motion_modules_.end(); _m_it++) - { - if((*_m_it)->module_name == msg->module_name[idx]) - { - if((*_m_it)->result.find(msg->joint_name[idx]) != (*_m_it)->result.end()) - { - _dxl->ctrl_module_name = msg->module_name[idx]; - break; - } - } - } - } - - for(std::list::iterator _m_it = motion_modules_.begin(); _m_it != motion_modules_.end(); _m_it++) - { - // set all modules -> disable - (*_m_it)->enable = false; - - // set all used modules -> enable - for(std::map::iterator _d_it = robot->dxls.begin(); _d_it != robot->dxls.end(); _d_it++) - { - if(_d_it->second->ctrl_module_name == (*_m_it)->module_name) - { - (*_m_it)->enable = true; - break; - } - } - } - - // TODO: set indirect address - // -> check module's control_mode - - queue_mutex_.unlock(); - - robotis_controller_msgs::JointCtrlModule _current_module_msg; - for(std::map::iterator _dxl_iter = robot->dxls.begin(); _dxl_iter != robot->dxls.end(); ++_dxl_iter) - { - _current_module_msg.joint_name.push_back(_dxl_iter->first); - _current_module_msg.module_name.push_back(_dxl_iter->second->ctrl_module_name); - } - - if(_current_module_msg.joint_name.size() == _current_module_msg.module_name.size()) - current_module_pub.publish(_current_module_msg); -} - -bool RobotisController::GetCtrlModuleCallback(robotis_controller_msgs::GetJointModule::Request &req, robotis_controller_msgs::GetJointModule::Response &res) -{ - for(unsigned int idx = 0; idx < req.joint_name.size(); idx++) - { - std::map::iterator _d_it = robot->dxls.find((std::string)(req.joint_name[idx])); - if(_d_it != robot->dxls.end()) - { - res.joint_name.push_back(req.joint_name[idx]); - res.module_name.push_back(_d_it->second->ctrl_module_name); - } - } - - if(res.joint_name.size() == 0) return false; - - return true; -} - -void RobotisController::SetCtrlModuleThread(std::string ctrl_module) -{ - // stop module - std::list _stop_modules; - - if(ctrl_module == "" || ctrl_module == "none") - { - // enqueue all modules in order to stop - for(std::list::iterator _m_it = motion_modules_.begin(); _m_it != motion_modules_.end(); _m_it++) - { - if((*_m_it)->enable == true) _stop_modules.push_back(*_m_it); - } - } - else - { - for(std::list::iterator _m_it = motion_modules_.begin(); _m_it != motion_modules_.end(); _m_it++) - { - // if it exist - if((*_m_it)->module_name == ctrl_module) - { - // enqueue the module which lost control of joint in order to stop - for(std::map::iterator _result_it = (*_m_it)->result.begin(); _result_it != (*_m_it)->result.end(); _result_it++) - { - std::map::iterator _d_it = robot->dxls.find(_result_it->first); - - if(_d_it != robot->dxls.end()) - { - // enqueue - if(_d_it->second->ctrl_module_name != ctrl_module) - { - for(std::list::iterator _stop_m_it = motion_modules_.begin(); _stop_m_it != motion_modules_.end(); _stop_m_it++) - { - if((*_stop_m_it)->module_name == _d_it->second->ctrl_module_name && (*_stop_m_it)->enable == true) - _stop_modules.push_back(*_stop_m_it); - } - } - } - } - - break; - } - } - } - - // stop the module - _stop_modules.unique(); - for(std::list::iterator _stop_m_it = _stop_modules.begin(); _stop_m_it != _stop_modules.end(); _stop_m_it++) - { - (*_stop_m_it)->Stop(); - } - - // wait to stop - for(std::list::iterator _stop_m_it = _stop_modules.begin(); _stop_m_it != _stop_modules.end(); _stop_m_it++) - { - while((*_stop_m_it)->IsRunning()) - usleep(CONTROL_CYCLE_MSEC * 1000); - } - - // set ctrl module - queue_mutex_.lock(); - - if(DEBUG_PRINT) ROS_INFO_STREAM("set module : " << ctrl_module); - - // none - if(ctrl_module == "" || ctrl_module == "none") - { - // set all modules -> disable - for(std::list::iterator _m_it = motion_modules_.begin(); _m_it != motion_modules_.end(); _m_it++) - { - (*_m_it)->enable = false; - } - - // set dxl's control module to "none" - for(std::map::iterator _d_it = robot->dxls.begin(); _d_it != robot->dxls.end(); _d_it++) - { - Dynamixel *_dxl = _d_it->second; - _dxl->ctrl_module_name = "none"; - - UINT32_T _pos_data = _dxl->ConvertRadian2Value(_dxl->dxl_state->goal_position + _dxl->dxl_state->position_offset); - UINT8_T _sync_write_data[4]; - _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_pos_data)); - _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_pos_data)); - _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_pos_data)); - _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_pos_data)); - - port_to_sync_write_position[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); - - port_to_sync_write_torque[_dxl->port_name]->RemoveParam(_dxl->id); - port_to_sync_write_velocity[_dxl->port_name]->RemoveParam(_dxl->id); - } - } - else - { - // check whether the module exist - for(std::list::iterator _m_it = motion_modules_.begin(); _m_it != motion_modules_.end(); _m_it++) - { - // if it exist - if((*_m_it)->module_name == ctrl_module) - { - CONTROL_MODE _mode = (*_m_it)->control_mode; - for(std::map::iterator _result_it = (*_m_it)->result.begin(); _result_it != (*_m_it)->result.end(); _result_it++) - { - std::map::iterator _d_it = robot->dxls.find(_result_it->first); - if(_d_it != robot->dxls.end()) - { - Dynamixel *_dxl = _d_it->second; - _dxl->ctrl_module_name = ctrl_module; - - if(_mode == POSITION_CONTROL) - { - UINT32_T _pos_data = _dxl->ConvertRadian2Value(_dxl->dxl_state->goal_position + _dxl->dxl_state->position_offset); - UINT8_T _sync_write_data[4]; - _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_pos_data)); - _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_pos_data)); - _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_pos_data)); - _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_pos_data)); - - port_to_sync_write_position[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); - - port_to_sync_write_torque[_dxl->port_name]->RemoveParam(_dxl->id); - port_to_sync_write_velocity[_dxl->port_name]->RemoveParam(_dxl->id); - } - else if(_mode == VELOCITY_CONTROL) - { - UINT32_T _vel_data = _dxl->ConvertVelocity2Value(_dxl->dxl_state->goal_velocity); - UINT8_T _sync_write_data[4]; - _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_vel_data)); - _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_vel_data)); - _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_vel_data)); - _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_vel_data)); - - port_to_sync_write_velocity[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); - - port_to_sync_write_torque[_dxl->port_name]->RemoveParam(_dxl->id); - port_to_sync_write_position[_dxl->port_name]->RemoveParam(_dxl->id); - } - else if(_mode == CURRENT_CONTROL) - { - UINT32_T _curr_data = _dxl->ConvertCurrent2Value(_dxl->dxl_state->goal_current); - UINT8_T _sync_write_data[4]; - _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_curr_data)); - _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_curr_data)); - _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_curr_data)); - _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_curr_data)); - - port_to_sync_write_torque[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); - - port_to_sync_write_velocity[_dxl->port_name]->RemoveParam(_dxl->id); - port_to_sync_write_position[_dxl->port_name]->RemoveParam(_dxl->id); - } - } - } - - break; - } - } - } - - for(std::list::iterator _m_it = motion_modules_.begin(); _m_it != motion_modules_.end(); _m_it++) - { - // set all modules -> disable - (*_m_it)->enable = false; - - // set all used modules -> enable - for(std::map::iterator _d_it = robot->dxls.begin(); _d_it != robot->dxls.end(); _d_it++) - { - if(_d_it->second->ctrl_module_name == (*_m_it)->module_name) - { - (*_m_it)->enable = true; - break; - } - } - } - - // TODO: set indirect address - // -> check module's control_mode - - queue_mutex_.unlock(); - - // publish current module - robotis_controller_msgs::JointCtrlModule _current_module_msg; - for(std::map::iterator _dxl_iter = robot->dxls.begin(); _dxl_iter != robot->dxls.end(); ++_dxl_iter) - { - _current_module_msg.joint_name.push_back(_dxl_iter->first); - _current_module_msg.module_name.push_back(_dxl_iter->second->ctrl_module_name); - } - - if(_current_module_msg.joint_name.size() == _current_module_msg.module_name.size()) - current_module_pub.publish(_current_module_msg); -} - -void RobotisController::GazeboJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg) -{ - for(unsigned int _i = 0; _i < msg->name.size(); _i++) - { - std::map::iterator _d_it = robot->dxls.find((std::string)msg->name[_i]); - if(_d_it != robot->dxls.end()) - { - _d_it->second->dxl_state->present_position = msg->position[_i]; - _d_it->second->dxl_state->present_velocity = msg->velocity[_i]; - _d_it->second->dxl_state->present_current = msg->effort[_i]; - } - } - - if(init_pose_loaded_ == false) - { - for(std::map::iterator _it = robot->dxls.begin(); _it != robot->dxls.end(); _it++) - _it->second->dxl_state->goal_position = _it->second->dxl_state->present_position; - init_pose_loaded_ = true; - } -} - -bool RobotisController::CheckTimerStop() -{ - if(this->is_timer_running_) - { - if(DEBUG_PRINT == true) - ROS_WARN("Process Timer is running.. STOP the timer first."); - return false; - } - return true; -} - -int RobotisController::Ping(const std::string joint_name, UINT8_T *error) -{ - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; - - Dynamixel *_dxl = robot->dxls[joint_name]; - if(_dxl == NULL) - return COMM_NOT_AVAILABLE; - - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; - - return _pkt_handler->Ping(_port_handler, _dxl->id, error); -} -int RobotisController::Ping(const std::string joint_name, UINT16_T* model_number, UINT8_T *error) -{ - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; - - Dynamixel *_dxl = robot->dxls[joint_name]; - if(_dxl == NULL) - return COMM_NOT_AVAILABLE; - - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; - - return _pkt_handler->Ping(_port_handler, _dxl->id, model_number, error); -} - -int RobotisController::Action(const std::string joint_name) -{ - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; - - Dynamixel *_dxl = robot->dxls[joint_name]; - if(_dxl == NULL) - return COMM_NOT_AVAILABLE; - - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; - - return _pkt_handler->Action(_port_handler, _dxl->id); -} -int RobotisController::Reboot(const std::string joint_name, UINT8_T *error) -{ - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; - - Dynamixel *_dxl = robot->dxls[joint_name]; - if(_dxl == NULL) - return COMM_NOT_AVAILABLE; - - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; - - return _pkt_handler->Reboot(_port_handler, _dxl->id, error); -} -int RobotisController::FactoryReset(const std::string joint_name, UINT8_T option, UINT8_T *error) -{ - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; - - Dynamixel *_dxl = robot->dxls[joint_name]; - if(_dxl == NULL) - return COMM_NOT_AVAILABLE; - - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; - - return _pkt_handler->FactoryReset(_port_handler, _dxl->id, option, error); -} - -int RobotisController::Read(const std::string joint_name, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error) -{ - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; - - Dynamixel *_dxl = robot->dxls[joint_name]; - if(_dxl == NULL) - return COMM_NOT_AVAILABLE; - - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; - - return _pkt_handler->ReadTxRx(_port_handler, _dxl->id, address, length, data, error); -} - -int RobotisController::ReadCtrlItem(const std::string joint_name, const std::string item_name, UINT32_T *data, UINT8_T *error) -{ - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; - - Dynamixel *_dxl = robot->dxls[joint_name]; - if(_dxl == NULL) - return COMM_NOT_AVAILABLE; - - ControlTableItem*_item = _dxl->ctrl_table[item_name]; - if(_item == NULL) - return COMM_NOT_AVAILABLE; - - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; - - int _result = COMM_NOT_AVAILABLE; - switch(_item->data_length) - { - case 1: - { - UINT8_T _data = 0; - _result = _pkt_handler->Read1ByteTxRx(_port_handler, _dxl->id, _item->address, &_data, error); - if(_result == COMM_SUCCESS) - *data = _data; - break; - } - case 2: - { - UINT16_T _data = 0; - _result = _pkt_handler->Read2ByteTxRx(_port_handler, _dxl->id, _item->address, &_data, error); - if(_result == COMM_SUCCESS) - *data = _data; - break; - } - case 4: - { - UINT32_T _data = 0; - _result = _pkt_handler->Read4ByteTxRx(_port_handler, _dxl->id, _item->address, &_data, error); - if(_result == COMM_SUCCESS) - *data = _data; - break; - } - default: - break; - } - return _result; -} - -int RobotisController::Read1Byte(const std::string joint_name, UINT16_T address, UINT8_T *data, UINT8_T *error) -{ - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; - - Dynamixel *_dxl = robot->dxls[joint_name]; - if(_dxl == NULL) - return COMM_NOT_AVAILABLE; - - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; - - return _pkt_handler->Read1ByteTxRx(_port_handler, _dxl->id, address, data, error); -} - -int RobotisController::Read2Byte(const std::string joint_name, UINT16_T address, UINT16_T *data, UINT8_T *error) -{ - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; - - Dynamixel *_dxl = robot->dxls[joint_name]; - if(_dxl == NULL) - return COMM_NOT_AVAILABLE; - - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; - - return _pkt_handler->Read2ByteTxRx(_port_handler, _dxl->id, address, data, error); -} - -int RobotisController::Read4Byte(const std::string joint_name, UINT16_T address, UINT32_T *data, UINT8_T *error) -{ - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; - - Dynamixel *_dxl = robot->dxls[joint_name]; - if(_dxl == NULL) - return COMM_NOT_AVAILABLE; - - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; - - return _pkt_handler->Read4ByteTxRx(_port_handler, _dxl->id, address, data, error); -} - -int RobotisController::Write(const std::string joint_name, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error) -{ - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; - - Dynamixel *_dxl = robot->dxls[joint_name]; - if(_dxl == NULL) - return COMM_NOT_AVAILABLE; - - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; - - return _pkt_handler->WriteTxRx(_port_handler, _dxl->id, address, length, data, error); -} - -int RobotisController::WriteCtrlItem(const std::string joint_name, const std::string item_name, UINT32_T data, UINT8_T *error) -{ - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; - - Dynamixel *_dxl = robot->dxls[joint_name]; - if(_dxl == NULL) - return COMM_NOT_AVAILABLE; - - ControlTableItem*_item = _dxl->ctrl_table[item_name]; - if(_item == NULL) - return COMM_NOT_AVAILABLE; - - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; - - int _result = COMM_NOT_AVAILABLE; - UINT8_T *_data = new UINT8_T[_item->data_length]; - if(_item->data_length == 1) - { - _data[0] = (UINT8_T)data; - _result = _pkt_handler->Write1ByteTxRx(_port_handler, _dxl->id, _item->address, data, error); - } - else if(_item->data_length == 2) - { - _data[0] = DXL_LOBYTE((UINT16_T)data); - _data[1] = DXL_HIBYTE((UINT16_T)data); - _result = _pkt_handler->Write2ByteTxRx(_port_handler, _dxl->id, _item->address, data, error); - } - else if(_item->data_length == 4) - { - _data[0] = DXL_LOBYTE(DXL_LOWORD((UINT32_T)data)); - _data[1] = DXL_HIBYTE(DXL_LOWORD((UINT32_T)data)); - _data[2] = DXL_LOBYTE(DXL_HIWORD((UINT32_T)data)); - _data[3] = DXL_HIBYTE(DXL_HIWORD((UINT32_T)data)); - _result = _pkt_handler->Write4ByteTxRx(_port_handler, _dxl->id, _item->address, data, error); - } - delete[] _data; - return _result; -} - -int RobotisController::Write1Byte(const std::string joint_name, UINT16_T address, UINT8_T data, UINT8_T *error) -{ - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; - - Dynamixel *_dxl = robot->dxls[joint_name]; - if(_dxl == NULL) - return COMM_NOT_AVAILABLE; - - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; - - return _pkt_handler->Write1ByteTxRx(_port_handler, _dxl->id, address, data, error); -} - -int RobotisController::Write2Byte(const std::string joint_name, UINT16_T address, UINT16_T data, UINT8_T *error) -{ - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; - - Dynamixel *_dxl = robot->dxls[joint_name]; - if(_dxl == NULL) - return COMM_NOT_AVAILABLE; - - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; - - return _pkt_handler->Write2ByteTxRx(_port_handler, _dxl->id, address, data, error); -} - -int RobotisController::Write4Byte(const std::string joint_name, UINT16_T address, UINT32_T data, UINT8_T *error) -{ - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; - - Dynamixel *_dxl = robot->dxls[joint_name]; - if(_dxl == NULL) - return COMM_NOT_AVAILABLE; - - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; - - return _pkt_handler->Write4ByteTxRx(_port_handler, _dxl->id, address, data, error); -} - -int RobotisController::RegWrite(const std::string joint_name, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error) -{ - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; - - Dynamixel *_dxl = robot->dxls[joint_name]; - if(_dxl == NULL) - return COMM_NOT_AVAILABLE; - - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; - - return _pkt_handler->RegWriteTxRx(_port_handler, _dxl->id, address, length, data, error); -} - diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp new file mode 100644 index 0000000..35c81b1 --- /dev/null +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -0,0 +1,2144 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of ROBOTIS nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *******************************************************************************/ + +/* + * robotis_controller.cpp + * + * Created on: 2016. 1. 15. + * Author: zerom + */ + +#include +#include + +#include "robotis_controller/robotis_controller.h" + +using namespace robotis_framework; + +RobotisController::RobotisController() + : is_timer_running_(false), + stop_timer_(false), + init_pose_loaded_(false), + timer_thread_(0), + controller_mode_(MotionModuleMode), + DEBUG_PRINT(false), + robot_(0), + gazebo_mode_(false), + gazebo_robot_name_("robotis") +{ + direct_sync_write_.clear(); +} + +void RobotisController::initializeSyncWrite() +{ + if (gazebo_mode_ == true) + return; + + ROS_INFO("FIRST BULKREAD"); + for (auto& it : port_to_bulk_read_) + it.second->txRxPacket(); + for(auto& it : port_to_bulk_read_) + { + int error_count = 0; + int result = COMM_SUCCESS; + do + { + if (++error_count > 10) + { + ROS_ERROR("[RobotisController] bulk read fail!!"); + exit(-1); + } + usleep(10 * 1000); + result = it.second->txRxPacket(); + } while (result != COMM_SUCCESS); + } + init_pose_loaded_ = true; + ROS_INFO("FIRST BULKREAD END"); + + // clear syncwrite param setting + for (auto& it : port_to_sync_write_position_) + { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto& it : port_to_sync_write_position_p_gain_) + { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto& it : port_to_sync_write_position_i_gain_) + { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto& it : port_to_sync_write_position_d_gain_) + { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto& it : port_to_sync_write_velocity_) + { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto& it : port_to_sync_write_velocity_p_gain_) + { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto& it : port_to_sync_write_velocity_i_gain_) + { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto& it : port_to_sync_write_velocity_d_gain_) + { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto& it : port_to_sync_write_current_) + { + if (it.second != NULL) + it.second->clearParam(); + } + + // set init syncwrite param(from data of bulkread) + for (auto& it : robot_->dxls_) + { + std::string joint_name = it.first; + Dynamixel *dxl = it.second; + + for (int i = 0; i < dxl->bulk_read_items_.size(); i++) + { + uint32_t read_data = 0; + uint8_t sync_write_data[4]; + + if (port_to_bulk_read_[dxl->port_name_]->isAvailable(dxl->id_, + dxl->bulk_read_items_[i]->address_, + dxl->bulk_read_items_[i]->data_length_) == true) + { + read_data = port_to_bulk_read_[dxl->port_name_]->getData(dxl->id_, + dxl->bulk_read_items_[i]->address_, + dxl->bulk_read_items_[i]->data_length_); + + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(read_data)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(read_data)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(read_data)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(read_data)); + + if ((dxl->present_position_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == dxl->present_position_item_->item_name_)) + { + dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(read_data) - dxl->dxl_state_->position_offset_; // remove offset + dxl->dxl_state_->goal_position_ = dxl->dxl_state_->present_position_; + + port_to_sync_write_position_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); + } + else if ((dxl->position_p_gain_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == dxl->position_p_gain_item_->item_name_)) + { + dxl->dxl_state_->position_p_gain_ = read_data; + } + else if ((dxl->position_i_gain_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == dxl->position_i_gain_item_->item_name_)) + { + dxl->dxl_state_->position_i_gain_ = read_data; + } + else if ((dxl->position_d_gain_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == dxl->position_d_gain_item_->item_name_)) + { + dxl->dxl_state_->position_d_gain_ = read_data; + } + else if ((dxl->present_velocity_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == dxl->present_velocity_item_->item_name_)) + { + dxl->dxl_state_->present_velocity_ = dxl->convertValue2Velocity(read_data); + dxl->dxl_state_->goal_velocity_ = dxl->dxl_state_->present_velocity_; + } + else if ((dxl->velocity_p_gain_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == dxl->velocity_p_gain_item_->item_name_)) + { + dxl->dxl_state_->velocity_p_gain_ = read_data; + } + else if ((dxl->velocity_i_gain_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == dxl->velocity_i_gain_item_->item_name_)) + { + dxl->dxl_state_->velocity_i_gain_ = read_data; + } + else if ((dxl->velocity_d_gain_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == dxl->velocity_d_gain_item_->item_name_)) + { + dxl->dxl_state_->velocity_d_gain_ = read_data; + } + else if ((dxl->present_current_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == dxl->present_current_item_->item_name_)) + { + dxl->dxl_state_->present_torque_ = dxl->convertValue2Torque(read_data); + dxl->dxl_state_->goal_torque_ = dxl->dxl_state_->present_torque_; + } + } + } + } +} + +bool RobotisController::initialize(const std::string robot_file_path, const std::string init_file_path) +{ + std::string dev_desc_dir_path = ros::package::getPath("robotis_device") + "/devices"; + + // load robot info : port , device + robot_ = new Robot(robot_file_path, dev_desc_dir_path); + + if (gazebo_mode_ == true) + { + queue_thread_ = boost::thread(boost::bind(&RobotisController::msgQueueThread, this)); + return true; + } + + for (auto& it : robot_->ports_) + { + std::string port_name = it.first; + dynamixel::PortHandler *port = it.second; + dynamixel::PacketHandler *default_pkt_handler = dynamixel::PacketHandler::getPacketHandler(2.0); + + if (port->setBaudRate(port->getBaudRate()) == false) + { + ROS_ERROR("PORT [%s] SETUP ERROR! (baudrate: %d)", port_name.c_str(), port->getBaudRate()); + exit(-1); + } + + // get the default device info of the port + std::string default_device_name = robot_->port_default_device_[port_name]; + auto dxl_it = robot_->dxls_.find(default_device_name); + auto sensor_it = robot_->sensors_.find(default_device_name); + if (dxl_it != robot_->dxls_.end()) + { + Dynamixel *default_device = dxl_it->second; + default_pkt_handler = dynamixel::PacketHandler::getPacketHandler(default_device->protocol_version_); + + if (default_device->goal_position_item_ != 0) + { + port_to_sync_write_position_[port_name] + = new dynamixel::GroupSyncWrite(port, + default_pkt_handler, + default_device->goal_position_item_->address_, + default_device->goal_position_item_->data_length_); + } + + if (default_device->position_p_gain_item_ != 0) + { + port_to_sync_write_position_p_gain_[port_name] + = new dynamixel::GroupSyncWrite(port, + default_pkt_handler, + default_device->position_p_gain_item_->address_, + default_device->position_p_gain_item_->data_length_); + } + + if (default_device->position_i_gain_item_ != 0) + { + port_to_sync_write_position_i_gain_[port_name] + = new dynamixel::GroupSyncWrite(port, + default_pkt_handler, + default_device->position_i_gain_item_->address_, + default_device->position_i_gain_item_->data_length_); + } + + if (default_device->position_d_gain_item_ != 0) + { + port_to_sync_write_position_d_gain_[port_name] + = new dynamixel::GroupSyncWrite(port, + default_pkt_handler, + default_device->position_d_gain_item_->address_, + default_device->position_d_gain_item_->data_length_); + } + + if (default_device->goal_velocity_item_ != 0) + { + port_to_sync_write_velocity_[port_name] + = new dynamixel::GroupSyncWrite(port, + default_pkt_handler, + default_device->goal_velocity_item_->address_, + default_device->goal_velocity_item_->data_length_); + } + + if (default_device->velocity_p_gain_item_ != 0) + { + port_to_sync_write_velocity_p_gain_[port_name] + = new dynamixel::GroupSyncWrite(port, + default_pkt_handler, + default_device->velocity_p_gain_item_->address_, + default_device->velocity_p_gain_item_->data_length_); + } + + if (default_device->velocity_i_gain_item_ != 0) + { + port_to_sync_write_velocity_i_gain_[port_name] + = new dynamixel::GroupSyncWrite(port, + default_pkt_handler, + default_device->velocity_i_gain_item_->address_, + default_device->velocity_i_gain_item_->data_length_); + } + + if (default_device->velocity_d_gain_item_ != 0) + { + port_to_sync_write_velocity_d_gain_[port_name] + = new dynamixel::GroupSyncWrite(port, + default_pkt_handler, + default_device->velocity_d_gain_item_->address_, + default_device->velocity_d_gain_item_->data_length_); + } + + if (default_device->goal_current_item_ != 0) + { + port_to_sync_write_current_[port_name] + = new dynamixel::GroupSyncWrite(port, + default_pkt_handler, + default_device->goal_current_item_->address_, + default_device->goal_current_item_->data_length_); + } + } + else if (sensor_it != robot_->sensors_.end()) + { + Sensor *_default_device = sensor_it->second; + default_pkt_handler = dynamixel::PacketHandler::getPacketHandler(_default_device->protocol_version_); + } + + port_to_bulk_read_[port_name] = new dynamixel::GroupBulkRead(port, default_pkt_handler); + } + + // (for loop) check all dxls are connected. + for (auto& it : robot_->dxls_) + { + std::string joint_name = it.first; + Dynamixel *dxl = it.second; + + if (ping(joint_name) != 0) + { + usleep(10 * 1000); + if (ping(joint_name) != 0) + ROS_ERROR("JOINT[%s] does NOT respond!!", joint_name.c_str()); + } + } + + initializeDevice(init_file_path); + + // [ BulkRead ] StartAddress : Present Position , Length : 10 ( Position/Velocity/Current ) + for (auto& it : robot_->dxls_) + { + std::string joint_name = it.first; + Dynamixel *dxl = it.second; + + if (dxl == NULL) + continue; + + int bulkread_start_addr = 0; + int bulkread_data_length = 0; + +// // bulk read default : present position +// if(dxl->present_position_item != 0) +// { +// bulkread_start_addr = dxl->present_position_item->address; +// bulkread_data_length = dxl->present_position_item->data_length; +// } + + // calculate bulk read start address & data length + auto indirect_addr_it = dxl->ctrl_table_.find(INDIRECT_ADDRESS_1); + if (indirect_addr_it != dxl->ctrl_table_.end()) // INDIRECT_ADDRESS_1 exist + { + if (dxl->bulk_read_items_.size() != 0) + { + bulkread_start_addr = dxl->bulk_read_items_[0]->address_; + bulkread_data_length = 0; + + // set indirect address + int indirect_addr = indirect_addr_it->second->address_; + for (int i = 0; i < dxl->bulk_read_items_.size(); i++) + { + int addr_leng = dxl->bulk_read_items_[i]->data_length_; + + bulkread_data_length += addr_leng; + for (int l = 0; l < addr_leng; l++) + { +// ROS_WARN("[%12s] INDIR_ADDR: %d, ITEM_ADDR: %d", joint_name.c_str(), indirect_addr, dxl->ctrl_table[dxl->bulk_read_items[i]->item_name]->address + _l); + write2Byte(joint_name, indirect_addr, dxl->ctrl_table_[dxl->bulk_read_items_[i]->item_name_]->address_ + l); + indirect_addr += 2; + } + } + } + } + else // INDIRECT_ADDRESS_1 NOT exist + { + if (dxl->bulk_read_items_.size() != 0) + { + bulkread_start_addr = dxl->bulk_read_items_[0]->address_; + bulkread_data_length = 0; + + ControlTableItem *last_item = dxl->bulk_read_items_[0]; + + for (int i = 0; i < dxl->bulk_read_items_.size(); i++) + { + int addr = dxl->bulk_read_items_[i]->address_; + if (addr < bulkread_start_addr) + bulkread_start_addr = addr; + else if (last_item->address_ < addr) + last_item = dxl->bulk_read_items_[i]; + } + + bulkread_data_length = last_item->address_ - bulkread_start_addr + last_item->data_length_; + } + } + +// ROS_WARN("[%12s] start_addr: %d, data_length: %d", joint_name.c_str(), bulkread_start_addr, bulkread_data_length); + if (bulkread_start_addr != 0) + port_to_bulk_read_[dxl->port_name_]->addParam(dxl->id_, bulkread_start_addr, bulkread_data_length); + + // Torque ON + if (writeCtrlItem(joint_name, dxl->torque_enable_item_->item_name_, 1) != COMM_SUCCESS) + writeCtrlItem(joint_name, dxl->torque_enable_item_->item_name_, 1); + } + + for (auto& it : robot_->sensors_) + { + std::string sensor_name = it.first; + Sensor *sensor = it.second; + + if (sensor == NULL) + continue; + + int bulkread_start_addr = 0; + int bulkread_data_length = 0; + + // calculate bulk read start address & data length + auto indirect_addr_it = sensor->ctrl_table_.find(INDIRECT_ADDRESS_1); + if (indirect_addr_it != sensor->ctrl_table_.end()) // INDIRECT_ADDRESS_1 exist + { + if (sensor->bulk_read_items_.size() != 0) + { + bulkread_start_addr = sensor->bulk_read_items_[0]->address_; + bulkread_data_length = 0; + + // set indirect address + int indirect_addr = indirect_addr_it->second->address_; + for (int i = 0; i < sensor->bulk_read_items_.size(); i++) + { + int addr_leng = sensor->bulk_read_items_[i]->data_length_; + + bulkread_data_length += addr_leng; + for (int l = 0; l < addr_leng; l++) + { +// ROS_WARN("[%12s] INDIR_ADDR: %d, ITEM_ADDR: %d", sensor_name.c_str(), indirect_addr, sensor->ctrl_table[sensor->bulk_read_items[i]->item_name]->address + _l); + write2Byte(sensor_name, + indirect_addr, + sensor->ctrl_table_[sensor->bulk_read_items_[i]->item_name_]->address_ + l); + indirect_addr += 2; + } + } + } + } + else // INDIRECT_ADDRESS_1 NOT exist + { + if (sensor->bulk_read_items_.size() != 0) + { + bulkread_start_addr = sensor->bulk_read_items_[0]->address_; + bulkread_data_length = 0; + + ControlTableItem *last_item = sensor->bulk_read_items_[0]; + + for (int i = 0; i < sensor->bulk_read_items_.size(); i++) + { + int addr = sensor->bulk_read_items_[i]->address_; + if (addr < bulkread_start_addr) + bulkread_start_addr = addr; + else if (last_item->address_ < addr) + last_item = sensor->bulk_read_items_[i]; + } + + bulkread_data_length = last_item->address_ - bulkread_start_addr + last_item->data_length_; + } + } + + //ROS_WARN("[%12s] start_addr: %d, data_length: %d", sensor_name.c_str(), bulkread_start_addr, bulkread_data_length); + if (bulkread_start_addr != 0) + port_to_bulk_read_[sensor->port_name_]->addParam(sensor->id_, bulkread_start_addr, bulkread_data_length); + } + + queue_thread_ = boost::thread(boost::bind(&RobotisController::msgQueueThread, this)); + return true; +} + +void RobotisController::initializeDevice(const std::string init_file_path) +{ + // device initialize + if (DEBUG_PRINT) + ROS_WARN("INIT FILE LOAD"); + + YAML::Node doc; + try + { + doc = YAML::LoadFile(init_file_path.c_str()); + + for (YAML::const_iterator it_doc = doc.begin(); it_doc != doc.end(); it_doc++) + { + std::string joint_name = it_doc->first.as(); + + YAML::Node joint_node = doc[joint_name]; + if (joint_node.size() == 0) + continue; + + Dynamixel *dxl = NULL; + auto dxl_it = robot_->dxls_.find(joint_name); + if (dxl_it != robot_->dxls_.end()) + dxl = dxl_it->second; + + if (dxl == NULL) + { + ROS_WARN("Joint [%s] was not found.", joint_name.c_str()); + continue; + } + if (DEBUG_PRINT) + ROS_INFO("JOINT_NAME: %s", joint_name.c_str()); + + for (YAML::const_iterator it_joint = joint_node.begin(); it_joint != joint_node.end(); it_joint++) + { + std::string item_name = it_joint->first.as(); + + if (DEBUG_PRINT) + ROS_INFO(" ITEM_NAME: %s", item_name.c_str()); + + uint32_t value = it_joint->second.as(); + + ControlTableItem *item = dxl->ctrl_table_[item_name]; + if (item == NULL) + { + ROS_WARN("Control Item [%s] was not found.", item_name.c_str()); + continue; + } + + if (item->memory_type_ == EEPROM) + { + uint8_t data8 = 0; + uint16_t data16 = 0; + uint32_t data32 = 0; + + switch (item->data_length_) + { + case 1: + read1Byte(joint_name, item->address_, &data8); + if (data8 == value) + continue; + break; + case 2: + read2Byte(joint_name, item->address_, &data16); + if (data16 == value) + continue; + break; + case 4: + read4Byte(joint_name, item->address_, &data32); + if (data32 == value) + continue; + break; + default: + break; + } + } + + switch (item->data_length_) + { + case 1: + write1Byte(joint_name, item->address_, (uint8_t) value); + break; + case 2: + write2Byte(joint_name, item->address_, (uint16_t) value); + break; + case 4: + write4Byte(joint_name, item->address_, value); + break; + default: + break; + } + + if (item->memory_type_ == EEPROM) + { + // Write to EEPROM -> delay is required (max delay: 55 msec per byte) + usleep(item->data_length_ * 55 * 1000); + } + } + } + } catch (const std::exception& e) + { + ROS_INFO("Dynamixel Init file not found."); + } +} + +void RobotisController::gazeboTimerThread() +{ + ros::Rate gazebo_rate(1000 / CONTROL_CYCLE_MSEC); + + while (!stop_timer_) + { + if (init_pose_loaded_ == true) + process(); + gazebo_rate.sleep(); + } +} + +void RobotisController::msgQueueThread() +{ + ros::NodeHandle ros_node; + ros::CallbackQueue callback_queue; + + ros_node.setCallbackQueue(&callback_queue); + + /* subscriber */ + ros::Subscriber sync_write_item_sub = ros_node.subscribe("/robotis/sync_write_item", 10, + &RobotisController::syncWriteItemCallback, this); + ros::Subscriber joint_ctrl_modules_sub = ros_node.subscribe("/robotis/set_joint_ctrl_modules", 10, + &RobotisController::setJointCtrlModuleCallback, this); + ros::Subscriber enable_ctrl_module_sub = ros_node.subscribe("/robotis/enable_ctrl_module", 10, + &RobotisController::setCtrlModuleCallback, this); + ros::Subscriber control_mode_sub = ros_node.subscribe("/robotis/set_control_mode", 10, + &RobotisController::setControllerModeCallback, this); + ros::Subscriber joint_states_sub = ros_node.subscribe("/robotis/set_joint_states", 10, + &RobotisController::setJointStatesCallback, this); + + ros::Subscriber gazebo_joint_states_sub; + if (gazebo_mode_ == true) + gazebo_joint_states_sub = ros_node.subscribe("/" + gazebo_robot_name_ + "/joint_states", 10, + &RobotisController::gazeboJointStatesCallback, this); + + /* publisher */ + goal_joint_state_pub_ = ros_node.advertise("/robotis/goal_joint_states", 10); + present_joint_state_pub_ = ros_node.advertise("/robotis/present_joint_states", 10); + current_module_pub_ = ros_node.advertise( + "/robotis/present_joint_ctrl_modules", 10); + + if (gazebo_mode_ == true) + { + for (auto& it : robot_->dxls_) + { + gazebo_joint_position_pub_[it.first] = ros_node.advertise( + "/" + gazebo_robot_name_ + "/" + it.first + "_position/command", 1); + gazebo_joint_velocity_pub_[it.first] = ros_node.advertise( + "/" + gazebo_robot_name_ + "/" + it.first + "_velocity/command", 1); + gazebo_joint_effort_pub_[it.first] = ros_node.advertise( + "/" + gazebo_robot_name_ + "/" + it.first + "_effort/command", 1); + } + } + + /* service */ + ros::ServiceServer joint_module_server = ros_node.advertiseService("/robotis/get_present_joint_ctrl_modules", + &RobotisController::getCtrlModuleCallback, this); + + while (ros_node.ok()) + { + callback_queue.callAvailable(); + usleep(100); + } +} + +void *RobotisController::timerThread(void *param) +{ + RobotisController *controller = (RobotisController *) param; + static struct timespec next_time; + static struct timespec curr_time; + + ROS_INFO("controller::thread_proc"); + + clock_gettime(CLOCK_MONOTONIC, &next_time); + + while (!controller->stop_timer_) + { + next_time.tv_sec += (next_time.tv_nsec + CONTROL_CYCLE_MSEC * 1000000) / 1000000000; + next_time.tv_nsec = (next_time.tv_nsec + CONTROL_CYCLE_MSEC * 1000000) % 1000000000; + + controller->process(); + + clock_gettime(CLOCK_MONOTONIC, &curr_time); + long delta_nsec = (next_time.tv_sec - curr_time.tv_sec) * 1000000000 + (next_time.tv_nsec - curr_time.tv_nsec); + if (delta_nsec < -100000) + { + if (controller->DEBUG_PRINT == true) + { + fprintf(stderr, "[RobotisController::ThreadProc] NEXT TIME < CURR TIME.. (%f)[%ld.%09ld / %ld.%09ld]", + delta_nsec / 1000000.0, (long )next_time.tv_sec, (long )next_time.tv_nsec, + (long )curr_time.tv_sec, (long )curr_time.tv_nsec); + } + + // next_time = curr_time + 3 msec + next_time.tv_sec = curr_time.tv_sec + (curr_time.tv_nsec + 3000000) / 1000000000; + next_time.tv_nsec = (curr_time.tv_nsec + 3000000) % 1000000000; + } + + clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &next_time, NULL); + } + return 0; +} + +void RobotisController::startTimer() +{ + if (this->is_timer_running_ == true) + return; + + if (this->gazebo_mode_ == true) + { + // create and start the thread + gazebo_thread_ = boost::thread(boost::bind(&RobotisController::gazeboTimerThread, this)); + } + else + { + initializeSyncWrite(); + + for (auto& it : port_to_bulk_read_) + { + it.second->txPacket(); + } + + usleep(8 * 1000); + + int error; + struct sched_param param; + pthread_attr_t attr; + + pthread_attr_init(&attr); + + error = pthread_attr_setschedpolicy(&attr, SCHED_RR); + if (error != 0) + ROS_ERROR("pthread_attr_setschedpolicy error = %d\n", error); + error = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED); + if (error != 0) + ROS_ERROR("pthread_attr_setinheritsched error = %d\n", error); + + memset(¶m, 0, sizeof(param)); + param.sched_priority = 31; // RT + error = pthread_attr_setschedparam(&attr, ¶m); + if (error != 0) + ROS_ERROR("pthread_attr_setschedparam error = %d\n", error); + + // create and start the thread + if ((error = pthread_create(&this->timer_thread_, &attr, this->timerThread, this)) != 0) + { + ROS_ERROR("timer thread create fail!!"); + exit(-1); + } + } + + this->is_timer_running_ = true; +} + +void RobotisController::stopTimer() +{ + int error = 0; + + // set the flag to stop the thread + if (this->is_timer_running_) + { + this->stop_timer_ = true; + + if (this->gazebo_mode_ == false) + { + // wait until the thread is stopped. + if ((error = pthread_join(this->timer_thread_, NULL)) != 0) + exit(-1); + + for (auto& it : port_to_bulk_read_) + { + if (it.second != NULL) + it.second->rxPacket(); + } + + for (auto& it : port_to_sync_write_position_) + { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto& it : port_to_sync_write_position_p_gain_) + { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto& it : port_to_sync_write_position_i_gain_) + { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto& it : port_to_sync_write_position_d_gain_) + { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto& it : port_to_sync_write_velocity_) + { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto& it : port_to_sync_write_velocity_p_gain_) + { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto& it : port_to_sync_write_velocity_i_gain_) + { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto& it : port_to_sync_write_velocity_d_gain_) + { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto& it : port_to_sync_write_current_) + { + if (it.second != NULL) + it.second->clearParam(); + } + } + else + { + // wait until the thread is stopped. + gazebo_thread_.join(); + } + + this->stop_timer_ = false; + this->is_timer_running_ = false; + } +} + +bool RobotisController::isTimerRunning() +{ + return this->is_timer_running_; +} + +void RobotisController::loadOffset(const std::string path) +{ + YAML::Node doc; + try + { + doc = YAML::LoadFile(path.c_str()); + } catch (const std::exception& e) + { + ROS_ERROR("Fail to load offset yaml."); + return; + } + + YAML::Node offset_node = doc["offset"]; + if (offset_node.size() == 0) + return; + + ROS_INFO("Load offsets..."); + for (YAML::const_iterator it = offset_node.begin(); it != offset_node.end(); it++) + { + std::string joint_name = it->first.as(); + double offset = it->second.as(); + + auto dxl_it = robot_->dxls_.find(joint_name); + if (dxl_it != robot_->dxls_.end()) + dxl_it->second->dxl_state_->position_offset_ = offset; + } +} + +void RobotisController::process() +{ + // avoid duplicated function call + static bool is_process_running = false; + if (is_process_running == true) + return; + is_process_running = true; + + // ROS_INFO("Controller::Process()"); + bool do_sync_write = false; + + ros::Time start_time; + ros::Duration time_duration; + + if (DEBUG_PRINT) + start_time = ros::Time::now(); + + sensor_msgs::JointState goal_state; + sensor_msgs::JointState present_state; + + present_state.header.stamp = ros::Time::now(); + goal_state.header.stamp = present_state.header.stamp; + + // BulkRead Rx + // -> save to Robot->dxls[]->dynamixel_state.present_position + if (gazebo_mode_ == false) + { + // BulkRead Rx + for (auto& it : port_to_bulk_read_) + { + robot_->ports_[it.first]->setPacketTimeout(0.0); + it.second->rxPacket(); + } + + if (robot_->dxls_.size() > 0) + { + for (auto& dxl_it : robot_->dxls_) + { + Dynamixel *dxl = dxl_it.second; + std::string port_name = dxl_it.second->port_name_; + std::string joint_name = dxl_it.first; + + if (dxl->bulk_read_items_.size() > 0) + { + uint32_t data = 0; + for (int i = 0; i < dxl->bulk_read_items_.size(); i++) + { + ControlTableItem *item = dxl->bulk_read_items_[i]; + if (port_to_bulk_read_[port_name]->isAvailable(dxl->id_, item->address_, item->data_length_) == true) + { + data = port_to_bulk_read_[port_name]->getData(dxl->id_, item->address_, item->data_length_); + + // change dxl_state + if (dxl->present_position_item_ != 0 && item->item_name_ == dxl->present_position_item_->item_name_) + dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_; // remove offset + else if (dxl->present_velocity_item_ != 0 && item->item_name_ == dxl->present_velocity_item_->item_name_) + dxl->dxl_state_->present_velocity_ = dxl->convertValue2Velocity(data); + else if (dxl->present_current_item_ != 0 && item->item_name_ == dxl->present_current_item_->item_name_) + dxl->dxl_state_->present_torque_ = dxl->convertValue2Torque(data); + else if (dxl->goal_position_item_ != 0 && item->item_name_ == dxl->goal_position_item_->item_name_) + dxl->dxl_state_->goal_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_; // remove offset + else if (dxl->goal_velocity_item_ != 0 && item->item_name_ == dxl->goal_velocity_item_->item_name_) + dxl->dxl_state_->goal_velocity_ = dxl->convertValue2Velocity(data); + else if (dxl->goal_current_item_ != 0 && item->item_name_ == dxl->goal_current_item_->item_name_) + dxl->dxl_state_->goal_torque_ = dxl->convertValue2Torque(data); + else + dxl->dxl_state_->bulk_read_table_[item->item_name_] = data; + } + } + + // -> update time stamp to Robot->dxls[]->dynamixel_state.update_time_stamp + dxl->dxl_state_->update_time_stamp_ = TimeStamp(present_state.header.stamp.sec, present_state.header.stamp.nsec); + } + } + } + + if (robot_->sensors_.size() > 0) + { + for (auto& sensor_it : robot_->sensors_) + { + Sensor *sensor = sensor_it.second; + std::string port_name = sensor_it.second->port_name_; + std::string sensor_name = sensor_it.first; + + if (sensor->bulk_read_items_.size() > 0) + { + uint32_t data = 0; + for (int i = 0; i < sensor->bulk_read_items_.size(); i++) + { + ControlTableItem *item = sensor->bulk_read_items_[i]; + if (port_to_bulk_read_[port_name]->isAvailable(sensor->id_, item->address_, item->data_length_) == true) + { + data = port_to_bulk_read_[port_name]->getData(sensor->id_, item->address_, item->data_length_); + + // change sensor_state + sensor->sensor_state_->bulk_read_table_[item->item_name_] = data; + } + } + + // -> update time stamp to Robot->dxls[]->dynamixel_state.update_time_stamp + sensor->sensor_state_->update_time_stamp_ = TimeStamp(present_state.header.stamp.sec, present_state.header.stamp.nsec); + } + } + } + } + + if (DEBUG_PRINT) + { + time_duration = ros::Time::now() - start_time; + fprintf(stderr, "(%2.6f) BulkRead Rx & update state \n", time_duration.nsec * 0.000001); + } + + // Call SensorModule Process() + // -> for loop : call SensorModule list -> Process() + if (sensor_modules_.size() > 0) + { + for (auto module_it = sensor_modules_.begin(); module_it != sensor_modules_.end(); module_it++) + { + (*module_it)->process(robot_->dxls_, robot_->sensors_); + + for (auto& it : (*module_it)->result_) + sensor_result_[it.first] = it.second; + } + } + + if (DEBUG_PRINT) + { + time_duration = ros::Time::now() - start_time; + fprintf(stderr, "(%2.6f) SensorModule Process() & save result \n", time_duration.nsec * 0.000001); + } + + if (controller_mode_ == MotionModuleMode) + { + // Call MotionModule Process() + // -> for loop : call MotionModule list -> Process() + if (motion_modules_.size() > 0) + { + queue_mutex_.lock(); + + for (auto module_it = motion_modules_.begin(); module_it != motion_modules_.end(); module_it++) + { + if ((*module_it)->getModuleEnable() == false) + continue; + + (*module_it)->process(robot_->dxls_, sensor_result_); + + // for loop : joint list + for (auto& dxl_it : robot_->dxls_) + { + std::string joint_name = dxl_it.first; + Dynamixel *dxl = dxl_it.second; + DynamixelState *dxl_state = dxl_it.second->dxl_state_; + + if (dxl->ctrl_module_name_ == (*module_it)->getModuleName()) + { + do_sync_write = true; + DynamixelState *result_state = (*module_it)->result_[joint_name]; + + if (result_state == NULL) + { + fprintf(stderr, "[%s] %s", (*module_it)->getModuleName().c_str(), joint_name.c_str()); + continue; + } + + // TODO: check update time stamp ? + + if ((*module_it)->getControlMode() == PositionControl) + { +// if(result_state->goal_position == 0 && dxl->id == 3) +// ROS_INFO("[MODULE:%s][ID:%2d] goal position = %f", (*module_it)->GetModuleName().c_str(), dxl->id, dxl_state->goal_position); + dxl_state->goal_position_ = result_state->goal_position_; + + if (gazebo_mode_ == false) + { + // add offset + uint32_t pos_data = dxl->convertRadian2Value(dxl_state->goal_position_ + dxl_state->position_offset_); + uint8_t sync_write_data[4] = { 0 }; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(pos_data)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(pos_data)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(pos_data)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(pos_data)); + + if (abs(pos_data) > 151800) + { + printf("goal_pos : %f | position_offset : %f | pos_data : %d\n", + dxl_state->goal_position_, dxl_state->position_offset_, pos_data); + } + + if (port_to_sync_write_position_[dxl->port_name_] != NULL) + port_to_sync_write_position_[dxl->port_name_]->changeParam(dxl->id_, sync_write_data); + + // if position p gain value is changed -> sync write + if (dxl_state->position_p_gain_ != result_state->position_p_gain_) + { + dxl_state->position_p_gain_ = result_state->position_p_gain_; + uint8_t sync_write_data[4] = { 0 }; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(dxl_state->position_p_gain_)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(dxl_state->position_p_gain_)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(dxl_state->position_p_gain_)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(dxl_state->position_p_gain_)); + + if (port_to_sync_write_position_p_gain_[dxl->port_name_] != NULL) + port_to_sync_write_position_p_gain_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); + } + + // if position i gain value is changed -> sync write + if (dxl_state->position_i_gain_ != result_state->position_i_gain_) + { + dxl_state->position_i_gain_ = result_state->position_i_gain_; + uint8_t sync_write_data[4] = { 0 }; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(dxl_state->position_i_gain_)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(dxl_state->position_i_gain_)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(dxl_state->position_i_gain_)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(dxl_state->position_i_gain_)); + + if (port_to_sync_write_position_i_gain_[dxl->port_name_] != NULL) + port_to_sync_write_position_i_gain_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); + } + + // if position d gain value is changed -> sync write + if (dxl_state->position_d_gain_ != result_state->position_d_gain_) + { + dxl_state->position_d_gain_ = result_state->position_d_gain_; + uint8_t sync_write_data[4] = { 0 }; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(dxl_state->position_d_gain_)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(dxl_state->position_d_gain_)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(dxl_state->position_d_gain_)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(dxl_state->position_d_gain_)); + + if (port_to_sync_write_position_d_gain_[dxl->port_name_] != NULL) + port_to_sync_write_position_d_gain_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); + } + } + } + else if ((*module_it)->getControlMode() == VelocityControl) + { + dxl_state->goal_velocity_ = result_state->goal_velocity_; + + if (gazebo_mode_ == false) + { + uint32_t vel_data = dxl->convertVelocity2Value(dxl_state->goal_velocity_); + uint8_t sync_write_data[4] = { 0 }; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(vel_data)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(vel_data)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(vel_data)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(vel_data)); + + if (port_to_sync_write_velocity_[dxl->port_name_] != NULL) + port_to_sync_write_velocity_[dxl->port_name_]->changeParam(dxl->id_, sync_write_data); + + // if velocity p gain gain value is changed -> sync write + if (dxl_state->velocity_p_gain_ != result_state->velocity_p_gain_) + { + dxl_state->velocity_p_gain_ = result_state->velocity_p_gain_; + uint8_t sync_write_data[4] = { 0 }; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(dxl_state->velocity_p_gain_)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(dxl_state->velocity_p_gain_)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(dxl_state->velocity_p_gain_)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(dxl_state->velocity_p_gain_)); + + if (port_to_sync_write_velocity_p_gain_[dxl->port_name_] != NULL) + port_to_sync_write_velocity_p_gain_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); + } + + // if velocity i gain value is changed -> sync write + if (dxl_state->velocity_i_gain_ != result_state->velocity_i_gain_) + { + dxl_state->velocity_i_gain_ = result_state->velocity_i_gain_; + uint8_t sync_write_data[4] = { 0 }; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(dxl_state->velocity_i_gain_)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(dxl_state->velocity_i_gain_)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(dxl_state->velocity_i_gain_)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(dxl_state->velocity_i_gain_)); + + if (port_to_sync_write_velocity_i_gain_[dxl->port_name_] != NULL) + port_to_sync_write_velocity_i_gain_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); + } + + // if velocity d gain value is changed -> sync write + if (dxl_state->velocity_d_gain_ != result_state->velocity_d_gain_) + { + dxl_state->velocity_d_gain_ = result_state->velocity_d_gain_; + uint8_t sync_write_data[4] = { 0 }; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(dxl_state->velocity_d_gain_)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(dxl_state->velocity_d_gain_)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(dxl_state->velocity_d_gain_)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(dxl_state->velocity_d_gain_)); + + if (port_to_sync_write_velocity_d_gain_[dxl->port_name_] != NULL) + port_to_sync_write_velocity_d_gain_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); + } + } + } + else if ((*module_it)->getControlMode() == TorqueControl) + { + dxl_state->goal_torque_ = result_state->goal_torque_; + + if (gazebo_mode_ == false) + { + uint32_t curr_data = dxl->convertTorque2Value(dxl_state->goal_torque_); + uint8_t sync_write_data[2] = { 0 }; + sync_write_data[0] = DXL_LOBYTE(curr_data); + sync_write_data[1] = DXL_HIBYTE(curr_data); + + if (port_to_sync_write_current_[dxl->port_name_] != NULL) + port_to_sync_write_current_[dxl->port_name_]->changeParam(dxl->id_, sync_write_data); + } + } + } + } + } + + queue_mutex_.unlock(); + } + + if (DEBUG_PRINT) + { + time_duration = ros::Time::now() - start_time; + fprintf(stderr, "(%2.6f) MotionModule Process() & save result \n", time_duration.nsec * 0.000001); + } + + // SyncWrite + if (gazebo_mode_ == false && do_sync_write) + { + if (port_to_sync_write_position_p_gain_.size() > 0) + { + for (auto& it : port_to_sync_write_position_p_gain_) + { + it.second->txPacket(); + it.second->clearParam(); + } + } + if (port_to_sync_write_position_i_gain_.size() > 0) + { + for (auto& it : port_to_sync_write_position_i_gain_) + { + it.second->txPacket(); + it.second->clearParam(); + } + } + if (port_to_sync_write_position_d_gain_.size() > 0) + { + for (auto& it : port_to_sync_write_position_d_gain_) + { + it.second->txPacket(); + it.second->clearParam(); + } + } + if (port_to_sync_write_velocity_p_gain_.size() > 0) + { + for (auto& it : port_to_sync_write_velocity_p_gain_) + { + it.second->txPacket(); + it.second->clearParam(); + } + } + if (port_to_sync_write_velocity_i_gain_.size() > 0) + { + for (auto& it : port_to_sync_write_velocity_i_gain_) + { + it.second->txPacket(); + it.second->clearParam(); + } + } + if (port_to_sync_write_velocity_d_gain_.size() > 0) + { + for (auto& it : port_to_sync_write_velocity_d_gain_) + { + it.second->txPacket(); + it.second->clearParam(); + } + } + for (auto& it : port_to_sync_write_position_) + { + if (it.second != NULL) + it.second->txPacket(); + } + for (auto& it : port_to_sync_write_velocity_) + { + if (it.second != NULL) + it.second->txPacket(); + } + for (auto& it : port_to_sync_write_current_) + { + if (it.second != NULL) + it.second->txPacket(); + } + } + else if (gazebo_mode_ == true) + { + for (auto module_it = motion_modules_.begin(); module_it != motion_modules_.end(); module_it++) + { + if ((*module_it)->getModuleEnable() == false) + continue; + + std_msgs::Float64 joint_msg; + + for (auto& dxl_it : robot_->dxls_) + { + std::string joint_name = dxl_it.first; + Dynamixel *dxl = dxl_it.second; + DynamixelState *dxl_state = dxl_it.second->dxl_state_; + + if (dxl->ctrl_module_name_ == (*module_it)->getModuleName()) + { + if ((*module_it)->getControlMode() == PositionControl) + { + joint_msg.data = dxl_state->goal_position_; + gazebo_joint_position_pub_[joint_name].publish(joint_msg); + } + else if ((*module_it)->getControlMode() == VelocityControl) + { + joint_msg.data = dxl_state->goal_velocity_; + gazebo_joint_velocity_pub_[joint_name].publish(joint_msg); + } + else if ((*module_it)->getControlMode() == TorqueControl) + { + joint_msg.data = dxl_state->goal_torque_; + gazebo_joint_effort_pub_[joint_name].publish(joint_msg); + } + } + } + } + } + } + else if (controller_mode_ == DirectControlMode) + { + queue_mutex_.lock(); + + for (auto& it : port_to_sync_write_position_) + { + it.second->txPacket(); + it.second->clearParam(); + } + + if (direct_sync_write_.size() > 0) + { + for (int i = 0; i < direct_sync_write_.size(); i++) + { + direct_sync_write_[i]->txPacket(); + direct_sync_write_[i]->clearParam(); + } + direct_sync_write_.clear(); + } + + queue_mutex_.unlock(); + } + + // TODO: User Read/Write + + // BulkRead Tx + if (gazebo_mode_ == false) + { + for (auto& it : port_to_bulk_read_) + it.second->txPacket(); + } + + if (DEBUG_PRINT) + { + time_duration = ros::Time::now() - start_time; + fprintf(stderr, "(%2.6f) SyncWrite & BulkRead Tx \n", time_duration.nsec * 0.000001); + } + + // publish present & goal position + for (auto& dxl_it : robot_->dxls_) + { + std::string joint_name = dxl_it.first; + Dynamixel *dxl = dxl_it.second; + + present_state.name.push_back(joint_name); + present_state.position.push_back(dxl->dxl_state_->present_position_); + present_state.velocity.push_back(dxl->dxl_state_->present_velocity_); + present_state.effort.push_back(dxl->dxl_state_->present_torque_); + + goal_state.name.push_back(joint_name); + goal_state.position.push_back(dxl->dxl_state_->goal_position_); + goal_state.velocity.push_back(dxl->dxl_state_->goal_velocity_); + goal_state.effort.push_back(dxl->dxl_state_->goal_torque_); + } + + // -> publish present joint_states & goal joint states topic + present_joint_state_pub_.publish(present_state); + goal_joint_state_pub_.publish(goal_state); + + if (DEBUG_PRINT) + { + time_duration = ros::Time::now() - start_time; + fprintf(stderr, "(%2.6f) Process() DONE \n", time_duration.nsec * 0.000001); + } + + is_process_running = false; +} + +void RobotisController::addMotionModule(MotionModule *module) +{ + // check whether the module name already exists + for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) + { + if ((*m_it)->getModuleName() == module->getModuleName()) + { + ROS_ERROR("Motion Module Name [%s] already exist !!", module->getModuleName().c_str()); + return; + } + } + + module->initialize(CONTROL_CYCLE_MSEC, robot_); + motion_modules_.push_back(module); + motion_modules_.unique(); +} + +void RobotisController::removeMotionModule(MotionModule *module) +{ + motion_modules_.remove(module); +} + +void RobotisController::addSensorModule(SensorModule *module) +{ + // check whether the module name already exists + for (auto m_it = sensor_modules_.begin(); m_it != sensor_modules_.end(); m_it++) + { + if ((*m_it)->getModuleName() == module->getModuleName()) + { + ROS_ERROR("Sensor Module Name [%s] already exist !!", module->getModuleName().c_str()); + return; + } + } + + module->initialize(CONTROL_CYCLE_MSEC, robot_); + sensor_modules_.push_back(module); + sensor_modules_.unique(); +} + +void RobotisController::removeSensorModule(SensorModule *module) +{ + sensor_modules_.remove(module); +} + +void RobotisController::syncWriteItemCallback(const robotis_controller_msgs::SyncWriteItem::ConstPtr &msg) +{ + for (int i = 0; i < msg->joint_name.size(); i++) + { + Device *device; + + auto d_it1 = robot_->dxls_.find(msg->joint_name[i]); + if (d_it1 != robot_->dxls_.end()) + { + device = d_it1->second; + } + else + { + auto d_it2 = robot_->sensors_.find(msg->joint_name[i]); + if (d_it2 != robot_->sensors_.end()) + { + device = d_it2->second; + } + else + { + // could not find the device + continue; + } + } + + ControlTableItem *item = device->ctrl_table_[msg->item_name]; + + dynamixel::PortHandler *port = robot_->ports_[device->port_name_]; + dynamixel::PacketHandler *packet_handler = dynamixel::PacketHandler::getPacketHandler(device->protocol_version_); + + if (item->access_type_ == Read) + continue; + + int idx = 0; + if (direct_sync_write_.size() == 0) + { + direct_sync_write_.push_back(new dynamixel::GroupSyncWrite(port, packet_handler, item->address_, item->data_length_)); + idx = 0; + } + else + { + for (idx = 0; idx < direct_sync_write_.size(); idx++) + { + if (direct_sync_write_[idx]->getPortHandler() == port && direct_sync_write_[idx]->getPacketHandler() == packet_handler) + break; + } + + if (idx == direct_sync_write_.size()) + direct_sync_write_.push_back(new dynamixel::GroupSyncWrite(port, packet_handler, item->address_, item->data_length_)); + } + + uint8_t *data = new uint8_t[item->data_length_]; + if (item->data_length_ == 1) + data[0] = (uint8_t) msg->value[i]; + else if (item->data_length_ == 2) + { + data[0] = DXL_LOBYTE((uint16_t )msg->value[i]); + data[1] = DXL_HIBYTE((uint16_t )msg->value[i]); + } + else if (item->data_length_ == 4) + { + data[0] = DXL_LOBYTE(DXL_LOWORD((uint32_t)msg->value[i])); + data[1] = DXL_HIBYTE(DXL_LOWORD((uint32_t)msg->value[i])); + data[2] = DXL_LOBYTE(DXL_HIWORD((uint32_t)msg->value[i])); + data[3] = DXL_HIBYTE(DXL_HIWORD((uint32_t)msg->value[i])); + } + direct_sync_write_[idx]->addParam(device->id_, data); + delete[] data; + } +} + +void RobotisController::setControllerModeCallback(const std_msgs::String::ConstPtr &msg) +{ + if (msg->data == "DirectControlMode") + controller_mode_ = DirectControlMode; + else if (msg->data == "MotionModuleMode") + controller_mode_ = MotionModuleMode; +} + +void RobotisController::setJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg) +{ + if (controller_mode_ != DirectControlMode) + return; + + queue_mutex_.lock(); + + for (int i = 0; i < msg->name.size(); i++) + { + int32_t pos = 0; + + Dynamixel *dxl = robot_->dxls_[msg->name[i]]; + if (dxl == NULL) + continue; + + dxl->dxl_state_->goal_position_ = msg->position[i]; + pos = dxl->convertRadian2Value((double) msg->position[i]); + + uint8_t sync_write_data[4]; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(pos)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(pos)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(pos)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(pos)); + + if (port_to_sync_write_position_[dxl->port_name_] != NULL) + port_to_sync_write_position_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); + } + + queue_mutex_.unlock(); +} + +void RobotisController::setCtrlModuleCallback(const std_msgs::String::ConstPtr &msg) +{ + std::string _module_name_to_set = msg->data; + + set_module_thread_ = boost::thread(boost::bind(&RobotisController::setCtrlModuleThread, this, _module_name_to_set)); +} + +void RobotisController::setJointCtrlModuleCallback(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg) +{ + if (msg->joint_name.size() != msg->module_name.size()) + return; + + queue_mutex_.lock(); + + for (unsigned int idx = 0; idx < msg->joint_name.size(); idx++) + { + Dynamixel *dxl = NULL; + auto dxl_it = robot_->dxls_.find((std::string) (msg->joint_name[idx])); + if (dxl_it != robot_->dxls_.end()) + dxl = dxl_it->second; + else + continue; + + // none + if (msg->module_name[idx] == "" || msg->module_name[idx] == "none") + { + dxl->ctrl_module_name_ = msg->module_name[idx]; + continue; + } + + // check whether the module is using this joint + for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) + { + if ((*m_it)->getModuleName() == msg->module_name[idx]) + { + if ((*m_it)->result_.find(msg->joint_name[idx]) != (*m_it)->result_.end()) + { + dxl->ctrl_module_name_ = msg->module_name[idx]; + break; + } + } + } + } + + for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) + { + // set all modules -> disable + (*m_it)->setModuleEnable(false); + + // set all used modules -> enable + for (auto& d_it : robot_->dxls_) + { + if (d_it.second->ctrl_module_name_ == (*m_it)->getModuleName()) + { + (*m_it)->setModuleEnable(true); + break; + } + } + } + + // TODO: set indirect address + // -> check module's control_mode + + queue_mutex_.unlock(); + + robotis_controller_msgs::JointCtrlModule current_module_msg; + for (auto& dxl_iter : robot_->dxls_) + { + current_module_msg.joint_name.push_back(dxl_iter.first); + current_module_msg.module_name.push_back(dxl_iter.second->ctrl_module_name_); + } + + if (current_module_msg.joint_name.size() == current_module_msg.module_name.size()) + current_module_pub_.publish(current_module_msg); +} + +bool RobotisController::getCtrlModuleCallback(robotis_controller_msgs::GetJointModule::Request &req, + robotis_controller_msgs::GetJointModule::Response &res) +{ + for (unsigned int idx = 0; idx < req.joint_name.size(); idx++) + { + auto d_it = robot_->dxls_.find((std::string) (req.joint_name[idx])); + if (d_it != robot_->dxls_.end()) + { + res.joint_name.push_back(req.joint_name[idx]); + res.module_name.push_back(d_it->second->ctrl_module_name_); + } + } + + if (res.joint_name.size() == 0) + return false; + + return true; +} + +void RobotisController::setCtrlModuleThread(std::string ctrl_module) +{ + // stop module + std::list stop_modules; + + if (ctrl_module == "" || ctrl_module == "none") + { + // enqueue all modules in order to stop + for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) + { + if ((*m_it)->getModuleEnable() == true) + stop_modules.push_back(*m_it); + } + } + else + { + for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) + { + // if it exist + if ((*m_it)->getModuleName() == ctrl_module) + { + // enqueue the module which lost control of joint in order to stop + for (auto& result_it : (*m_it)->result_) + { + auto d_it = robot_->dxls_.find(result_it.first); + + if (d_it != robot_->dxls_.end()) + { + // enqueue + if (d_it->second->ctrl_module_name_ != ctrl_module) + { + for (auto stop_m_it = motion_modules_.begin(); stop_m_it != motion_modules_.end(); stop_m_it++) + { + if (((*stop_m_it)->getModuleName() == d_it->second->ctrl_module_name_) && + ((*stop_m_it)->getModuleEnable() == true)) + { + stop_modules.push_back(*stop_m_it); + } + } + } + } + } + + break; + } + } + } + + // stop the module + stop_modules.unique(); + for (auto stop_m_it = stop_modules.begin(); stop_m_it != stop_modules.end(); stop_m_it++) + { + (*stop_m_it)->stop(); + } + + // wait to stop + for (auto stop_m_it = stop_modules.begin(); stop_m_it != stop_modules.end(); stop_m_it++) + { + while ((*stop_m_it)->isRunning()) + usleep(CONTROL_CYCLE_MSEC * 1000); + } + + // set ctrl module + queue_mutex_.lock(); + + if (DEBUG_PRINT) + ROS_INFO_STREAM("set module : " << ctrl_module); + + // none + if ((ctrl_module == "") || (ctrl_module == "none")) + { + // set all modules -> disable + for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) + { + (*m_it)->setModuleEnable(false); + } + + // set dxl's control module to "none" + for (auto& d_it : robot_->dxls_) + { + Dynamixel *dxl = d_it.second; + dxl->ctrl_module_name_ = "none"; + + if (gazebo_mode_ == true) + continue; + + uint32_t pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_ + dxl->dxl_state_->position_offset_); + uint8_t sync_write_data[4] = { 0 }; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(pos_data)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(pos_data)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(pos_data)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(pos_data)); + + if (port_to_sync_write_position_[dxl->port_name_] != NULL) + port_to_sync_write_position_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); + + if (port_to_sync_write_current_[dxl->port_name_] != NULL) + port_to_sync_write_current_[dxl->port_name_]->removeParam(dxl->id_); + if (port_to_sync_write_velocity_[dxl->port_name_] != NULL) + port_to_sync_write_velocity_[dxl->port_name_]->removeParam(dxl->id_); + } + } + else + { + // check whether the module exist + for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) + { + // if it exist + if ((*m_it)->getModuleName() == ctrl_module) + { + ControlMode mode = (*m_it)->getControlMode(); + for (auto& result_it : (*m_it)->result_) + { + auto d_it = robot_->dxls_.find(result_it.first); + if (d_it != robot_->dxls_.end()) + { + Dynamixel *dxl = d_it->second; + dxl->ctrl_module_name_ = ctrl_module; + + if (gazebo_mode_ == true) + continue; + + if (mode == PositionControl) + { + uint32_t pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_ + dxl->dxl_state_->position_offset_); + uint8_t sync_write_data[4] = { 0 }; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(pos_data)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(pos_data)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(pos_data)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(pos_data)); + + if (port_to_sync_write_position_[dxl->port_name_] != NULL) + port_to_sync_write_position_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); + + if (port_to_sync_write_current_[dxl->port_name_] != NULL) + port_to_sync_write_current_[dxl->port_name_]->removeParam(dxl->id_); + if (port_to_sync_write_velocity_[dxl->port_name_] != NULL) + port_to_sync_write_velocity_[dxl->port_name_]->removeParam(dxl->id_); + } + else if (mode == VelocityControl) + { + uint32_t vel_data = dxl->convertVelocity2Value(dxl->dxl_state_->goal_velocity_); + uint8_t sync_write_data[4] = { 0 }; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(vel_data)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(vel_data)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(vel_data)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(vel_data)); + + if (port_to_sync_write_velocity_[dxl->port_name_] != NULL) + port_to_sync_write_velocity_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); + + if (port_to_sync_write_current_[dxl->port_name_] != NULL) + port_to_sync_write_current_[dxl->port_name_]->removeParam(dxl->id_); + if (port_to_sync_write_position_[dxl->port_name_] != NULL) + port_to_sync_write_position_[dxl->port_name_]->removeParam(dxl->id_); + } + else if (mode == TorqueControl) + { + uint32_t curr_data = dxl->convertTorque2Value(dxl->dxl_state_->goal_torque_); + uint8_t sync_write_data[4] = { 0 }; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(curr_data)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(curr_data)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(curr_data)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(curr_data)); + + if (port_to_sync_write_current_[dxl->port_name_] != NULL) + port_to_sync_write_current_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); + + if (port_to_sync_write_velocity_[dxl->port_name_] != NULL) + port_to_sync_write_velocity_[dxl->port_name_]->removeParam(dxl->id_); + if (port_to_sync_write_position_[dxl->port_name_] != NULL) + port_to_sync_write_position_[dxl->port_name_]->removeParam(dxl->id_); + } + } + } + + break; + } + } + } + + for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) + { + // set all modules -> disable + (*m_it)->setModuleEnable(false); + + // set all used modules -> enable + for (auto& d_it : robot_->dxls_) + { + if (d_it.second->ctrl_module_name_ == (*m_it)->getModuleName()) + { + (*m_it)->setModuleEnable(true); + break; + } + } + } + + // TODO: set indirect address + // -> check module's control_mode + + queue_mutex_.unlock(); + + // publish current module + robotis_controller_msgs::JointCtrlModule current_module_msg; + for (auto& dxl_iter : robot_->dxls_) + { + current_module_msg.joint_name.push_back(dxl_iter.first); + current_module_msg.module_name.push_back(dxl_iter.second->ctrl_module_name_); + } + + if (current_module_msg.joint_name.size() == current_module_msg.module_name.size()) + current_module_pub_.publish(current_module_msg); +} + +void RobotisController::gazeboJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg) +{ + queue_mutex_.lock(); + + for (unsigned int i = 0; i < msg->name.size(); i++) + { + auto d_it = robot_->dxls_.find((std::string) msg->name[i]); + if (d_it != robot_->dxls_.end()) + { + d_it->second->dxl_state_->present_position_ = msg->position[i]; + d_it->second->dxl_state_->present_velocity_ = msg->velocity[i]; + d_it->second->dxl_state_->present_torque_ = msg->effort[i]; + } + } + + if (init_pose_loaded_ == false) + { + for (auto& it : robot_->dxls_) + it.second->dxl_state_->goal_position_ = it.second->dxl_state_->present_position_; + init_pose_loaded_ = true; + } + + queue_mutex_.unlock(); +} + +bool RobotisController::isTimerStopped() +{ + if (this->is_timer_running_) + { + if (DEBUG_PRINT == true) + ROS_WARN("Process Timer is running.. STOP the timer first."); + return false; + } + return true; +} + +int RobotisController::ping(const std::string joint_name, uint8_t *error) +{ + return ping(joint_name, 0, error); +} +int RobotisController::ping(const std::string joint_name, uint16_t* model_number, uint8_t *error) +{ + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->ping(port_handler, dxl->id_, model_number, error); +} + +int RobotisController::action(const std::string joint_name) +{ + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->action(port_handler, dxl->id_); +} +int RobotisController::reboot(const std::string joint_name, uint8_t *error) +{ + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->reboot(port_handler, dxl->id_, error); +} +int RobotisController::factoryReset(const std::string joint_name, uint8_t option, uint8_t *error) +{ + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->factoryReset(port_handler, dxl->id_, option, error); +} + +int RobotisController::read(const std::string joint_name, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error) +{ + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->readTxRx(port_handler, dxl->id_, address, length, data, error); +} + +int RobotisController::readCtrlItem(const std::string joint_name, const std::string item_name, uint32_t *data, uint8_t *error) +{ + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + ControlTableItem *item = dxl->ctrl_table_[item_name]; + if (item == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + int result = COMM_NOT_AVAILABLE; + switch (item->data_length_) + { + case 1: + { + uint8_t read_data = 0; + result = pkt_handler->read1ByteTxRx(port_handler, dxl->id_, item->address_, &read_data, error); + if (result == COMM_SUCCESS) + *data = read_data; + break; + } + case 2: + { + uint16_t read_data = 0; + result = pkt_handler->read2ByteTxRx(port_handler, dxl->id_, item->address_, &read_data, error); + if (result == COMM_SUCCESS) + *data = read_data; + break; + } + case 4: + { + uint32_t read_data = 0; + result = pkt_handler->read4ByteTxRx(port_handler, dxl->id_, item->address_, &read_data, error); + if (result == COMM_SUCCESS) + *data = read_data; + break; + } + default: + break; + } + return result; +} + +int RobotisController::read1Byte(const std::string joint_name, uint16_t address, uint8_t *data, uint8_t *error) +{ + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->read1ByteTxRx(port_handler, dxl->id_, address, data, error); +} + +int RobotisController::read2Byte(const std::string joint_name, uint16_t address, uint16_t *data, uint8_t *error) +{ + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->read2ByteTxRx(port_handler, dxl->id_, address, data, error); +} + +int RobotisController::read4Byte(const std::string joint_name, uint16_t address, uint32_t *data, uint8_t *error) +{ + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->read4ByteTxRx(port_handler, dxl->id_, address, data, error); +} + +int RobotisController::write(const std::string joint_name, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error) +{ + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->writeTxRx(port_handler, dxl->id_, address, length, data, error); +} + +int RobotisController::writeCtrlItem(const std::string joint_name, const std::string item_name, uint32_t data, uint8_t *error) +{ + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + ControlTableItem *item = dxl->ctrl_table_[item_name]; + if (item == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + int result = COMM_NOT_AVAILABLE; + uint8_t *write_data = new uint8_t[item->data_length_]; + if (item->data_length_ == 1) + { + write_data[0] = (uint8_t) data; + result = pkt_handler->write1ByteTxRx(port_handler, dxl->id_, item->address_, data, error); + } + else if (item->data_length_ == 2) + { + write_data[0] = DXL_LOBYTE((uint16_t )data); + write_data[1] = DXL_HIBYTE((uint16_t )data); + result = pkt_handler->write2ByteTxRx(port_handler, dxl->id_, item->address_, data, error); + } + else if (item->data_length_ == 4) + { + write_data[0] = DXL_LOBYTE(DXL_LOWORD((uint32_t)data)); + write_data[1] = DXL_HIBYTE(DXL_LOWORD((uint32_t)data)); + write_data[2] = DXL_LOBYTE(DXL_HIWORD((uint32_t)data)); + write_data[3] = DXL_HIBYTE(DXL_HIWORD((uint32_t)data)); + result = pkt_handler->write4ByteTxRx(port_handler, dxl->id_, item->address_, data, error); + } + delete[] write_data; + return result; +} + +int RobotisController::write1Byte(const std::string joint_name, uint16_t address, uint8_t data, uint8_t *error) +{ + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->write1ByteTxRx(port_handler, dxl->id_, address, data, error); +} + +int RobotisController::write2Byte(const std::string joint_name, uint16_t address, uint16_t data, uint8_t *error) +{ + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->write2ByteTxRx(port_handler, dxl->id_, address, data, error); +} + +int RobotisController::write4Byte(const std::string joint_name, uint16_t address, uint32_t data, uint8_t *error) +{ + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->write4ByteTxRx(port_handler, dxl->id_, address, data, error); +} + +int RobotisController::regWrite(const std::string joint_name, uint16_t address, uint16_t length, uint8_t *data, + uint8_t *error) +{ + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->regWriteTxRx(port_handler, dxl->id_, address, length, data, error); +} + diff --git a/robotis_controller_msgs/CMakeLists.txt b/robotis_controller_msgs/CMakeLists.txt deleted file mode 100644 index d9f015c..0000000 --- a/robotis_controller_msgs/CMakeLists.txt +++ /dev/null @@ -1,48 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(robotis_controller_msgs) - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - sensor_msgs - std_msgs - message_generation -) - -## Generate messages in the 'msg' folder -add_message_files( - FILES - SyncWriteItem.msg - JointCtrlModule.msg - StatusMsg.msg -) - -## Generate services in the 'srv' folder -add_service_files( - FILES - GetJointModule.srv -) - # Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -generate_messages( - DEPENDENCIES - std_msgs - sensor_msgs -) - -catkin_package( -# INCLUDE_DIRS include - CATKIN_DEPENDS sensor_msgs std_msgs -) - diff --git a/robotis_controller_msgs/msg/JointCtrlModule.msg b/robotis_controller_msgs/msg/JointCtrlModule.msg deleted file mode 100644 index b91eb4d..0000000 --- a/robotis_controller_msgs/msg/JointCtrlModule.msg +++ /dev/null @@ -1,2 +0,0 @@ -string[] joint_name -string[] module_name \ No newline at end of file diff --git a/robotis_controller_msgs/msg/StatusMsg.msg b/robotis_controller_msgs/msg/StatusMsg.msg deleted file mode 100644 index 47b706c..0000000 --- a/robotis_controller_msgs/msg/StatusMsg.msg +++ /dev/null @@ -1,10 +0,0 @@ -# Status Constants -uint8 STATUS_UNKNOWN = 0 -uint8 STATUS_INFO = 1 -uint8 STATUS_WARN = 2 -uint8 STATUS_ERROR = 3 - -std_msgs/Header header -uint8 type -string module_name -string status_msg \ No newline at end of file diff --git a/robotis_controller_msgs/msg/SyncWriteItem.msg b/robotis_controller_msgs/msg/SyncWriteItem.msg deleted file mode 100644 index 4d602b6..0000000 --- a/robotis_controller_msgs/msg/SyncWriteItem.msg +++ /dev/null @@ -1,3 +0,0 @@ -string item_name -string[] joint_name -uint32[] value \ No newline at end of file diff --git a/robotis_controller_msgs/package.xml b/robotis_controller_msgs/package.xml deleted file mode 100644 index 3ce5db7..0000000 --- a/robotis_controller_msgs/package.xml +++ /dev/null @@ -1,30 +0,0 @@ - - - robotis_controller_msgs - 0.1.0 - The robotis_controller_msgs package - robotis - - BSD - - - - robotis --> - - catkin - - sensor_msgs - std_msgs - message_generation - - sensor_msgs - std_msgs - message_runtime - - - - - - - - \ No newline at end of file diff --git a/robotis_controller_msgs/srv/GetJointModule.srv b/robotis_controller_msgs/srv/GetJointModule.srv deleted file mode 100644 index bedde91..0000000 --- a/robotis_controller_msgs/srv/GetJointModule.srv +++ /dev/null @@ -1,4 +0,0 @@ -string[] joint_name ---- -string[] joint_name -string[] module_name \ No newline at end of file diff --git a/robotis_device/CMakeLists.txt b/robotis_device/CMakeLists.txt index 8bd7be5..e492450 100644 --- a/robotis_device/CMakeLists.txt +++ b/robotis_device/CMakeLists.txt @@ -5,18 +5,8 @@ find_package(catkin REQUIRED COMPONENTS roscpp rospy dynamixel_sdk - robotis_framework_common ) -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if you package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( INCLUDE_DIRS include LIBRARIES robotis_device @@ -24,38 +14,19 @@ catkin_package( # DEPENDS system_lib ) -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -# include_directories(include) include_directories( include ${catkin_INCLUDE_DIRS} ) -## Declare a C++ library add_library(robotis_device - src/robotis_device/Robot.cpp - src/robotis_device/Sensor.cpp - src/robotis_device/Dynamixel.cpp + src/robotis_device/robot.cpp + src/robotis_device/sensor.cpp + src/robotis_device/dynamixel.cpp ) -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure add_dependencies(robotis_device ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -## Declare a C++ executable -# add_executable(robotis_device_node src/robotis_device_node.cpp) - -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(robotis_device_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against target_link_libraries(robotis_device ${catkin_LIBRARIES} ) diff --git a/robotis_device/devices/dynamixel/GRIPPER.device b/robotis_device/devices/dynamixel/GRIPPER.device index e95ff17..4267c99 100644 --- a/robotis_device/devices/dynamixel/GRIPPER.device +++ b/robotis_device/devices/dynamixel/GRIPPER.device @@ -19,6 +19,9 @@ goal_current_item_name = goal_torque position_d_gain_item_name = position_i_gain_item_name = position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/H42-20-S300-R.device b/robotis_device/devices/dynamixel/H42-20-S300-R.device index 1ed385f..95686ff 100644 --- a/robotis_device/devices/dynamixel/H42-20-S300-R.device +++ b/robotis_device/devices/dynamixel/H42-20-S300-R.device @@ -3,7 +3,7 @@ model_name = H42-20-S300-R device_type = dynamixel [type info] -current_ratio = 4.0283203125 +torque_to_current_value_ratio = 27.15146 value_of_0_radian_position = 0 value_of_min_radian_position = -151900 @@ -21,6 +21,9 @@ goal_current_item_name = goal_torque position_d_gain_item_name = position_i_gain_item_name = position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/H54-100-S500-R.device b/robotis_device/devices/dynamixel/H54-100-S500-R.device index 09ef828..a251ddd 100644 --- a/robotis_device/devices/dynamixel/H54-100-S500-R.device +++ b/robotis_device/devices/dynamixel/H54-100-S500-R.device @@ -3,7 +3,7 @@ model_name = H54-100-S500-R device_type = dynamixel [type info] -current_ratio = 16.11328125 +torque_to_current_value_ratio = 9.66026 value_of_0_radian_position = 0 value_of_min_radian_position = -250950 @@ -21,6 +21,9 @@ goal_current_item_name = goal_torque position_d_gain_item_name = position_i_gain_item_name = position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/H54-200-B500-R.device b/robotis_device/devices/dynamixel/H54-200-B500-R.device index 8791d7c..20a280a 100644 --- a/robotis_device/devices/dynamixel/H54-200-B500-R.device +++ b/robotis_device/devices/dynamixel/H54-200-B500-R.device @@ -3,7 +3,7 @@ model_name = H54-200-B500-R device_type = dynamixel [type info] -current_ratio = 16.11328125 +torque_to_current_value_ratio = 9.09201 value_of_0_radian_position = 0 value_of_min_radian_position = -250950 @@ -21,6 +21,9 @@ goal_current_item_name = goal_torque position_d_gain_item_name = position_i_gain_item_name = position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/H54-200-S500-R.device b/robotis_device/devices/dynamixel/H54-200-S500-R.device index dba268c..5b7dc76 100644 --- a/robotis_device/devices/dynamixel/H54-200-S500-R.device +++ b/robotis_device/devices/dynamixel/H54-200-S500-R.device @@ -3,7 +3,7 @@ model_name = H54-200-S500-R device_type = dynamixel [type info] -current_ratio = 16.11328125 +torque_to_current_value_ratio = 9.09201 value_of_0_radian_position = 0 value_of_min_radian_position = -250950 @@ -21,6 +21,9 @@ goal_current_item_name = goal_torque position_d_gain_item_name = position_i_gain_item_name = position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/L42-10-S300-R.device b/robotis_device/devices/dynamixel/L42-10-S300-R.device index 8a4ca5a..84e556d 100644 --- a/robotis_device/devices/dynamixel/L42-10-S300-R.device +++ b/robotis_device/devices/dynamixel/L42-10-S300-R.device @@ -19,6 +19,9 @@ goal_current_item_name = goal_torque position_d_gain_item_name = position_i_gain_item_name = position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/L54-30-S400-R.device b/robotis_device/devices/dynamixel/L54-30-S400-R.device index 8cdcff3..9dc5d36 100644 --- a/robotis_device/devices/dynamixel/L54-30-S400-R.device +++ b/robotis_device/devices/dynamixel/L54-30-S400-R.device @@ -3,8 +3,6 @@ model_name = L54-30-S400-R device_type = dynamixel [type info] -current_ratio = 16.11328125 - value_of_0_radian_position = 0 value_of_min_radian_position = -144198 value_of_max_radian_position = 144198 @@ -21,6 +19,9 @@ goal_current_item_name = goal_torque position_d_gain_item_name = position_i_gain_item_name = position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/L54-30-S500-R.device b/robotis_device/devices/dynamixel/L54-30-S500-R.device index db5a0b4..fa15941 100644 --- a/robotis_device/devices/dynamixel/L54-30-S500-R.device +++ b/robotis_device/devices/dynamixel/L54-30-S500-R.device @@ -3,8 +3,6 @@ model_name = L54-30-S500-R device_type = dynamixel [type info] -current_ratio = 16.11328125 - value_of_0_radian_position = 0 value_of_min_radian_position = -180684 value_of_max_radian_position = 180684 @@ -21,6 +19,9 @@ goal_current_item_name = goal_torque position_d_gain_item_name = position_i_gain_item_name = position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/L54-50-S290-R.device b/robotis_device/devices/dynamixel/L54-50-S290-R.device index cbd0453..8c2f1a2 100644 --- a/robotis_device/devices/dynamixel/L54-50-S290-R.device +++ b/robotis_device/devices/dynamixel/L54-50-S290-R.device @@ -3,8 +3,6 @@ model_name = L54-50-S290-R device_type = dynamixel [type info] -current_ratio = 16.11328125 - value_of_0_radian_position = 0 value_of_min_radian_position = -103860 value_of_max_radian_position = 103860 @@ -21,6 +19,9 @@ goal_current_item_name = goal_torque position_d_gain_item_name = position_i_gain_item_name = position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/L54-50-S500-R.device b/robotis_device/devices/dynamixel/L54-50-S500-R.device index d72f2ef..991e159 100644 --- a/robotis_device/devices/dynamixel/L54-50-S500-R.device +++ b/robotis_device/devices/dynamixel/L54-50-S500-R.device @@ -3,8 +3,6 @@ model_name = L54-50-S500-R device_type = dynamixel [type info] -current_ratio = 16.11328125 - value_of_0_radian_position = 0 value_of_min_radian_position = -180684 value_of_max_radian_position = 180684 @@ -21,6 +19,9 @@ goal_current_item_name = goal_torque position_d_gain_item_name = position_i_gain_item_name = position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/M42-10-S260-R.device b/robotis_device/devices/dynamixel/M42-10-S260-R.device index 8c96988..4c25631 100644 --- a/robotis_device/devices/dynamixel/M42-10-S260-R.device +++ b/robotis_device/devices/dynamixel/M42-10-S260-R.device @@ -3,8 +3,6 @@ model_name = M42-10-S260-R device_type = dynamixel [type info] -current_ratio = 4.0283203125 - value_of_0_radian_position = 0 value_of_min_radian_position = -131584 value_of_max_radian_position = 131584 @@ -21,6 +19,9 @@ goal_current_item_name = goal_torque position_d_gain_item_name = position_i_gain_item_name = position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/M54-40-S250-R.device b/robotis_device/devices/dynamixel/M54-40-S250-R.device index 2bc7c1c..76796b1 100644 --- a/robotis_device/devices/dynamixel/M54-40-S250-R.device +++ b/robotis_device/devices/dynamixel/M54-40-S250-R.device @@ -3,8 +3,6 @@ model_name = M54-40-S250-R device_type = dynamixel [type info] -current_ratio = 16.11328125 - value_of_0_radian_position = 0 value_of_min_radian_position = -125700 value_of_max_radian_position = 125700 @@ -21,6 +19,9 @@ goal_current_item_name = goal_torque position_d_gain_item_name = position_i_gain_item_name = position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/M54-60-S250-R.device b/robotis_device/devices/dynamixel/M54-60-S250-R.device index 72382de..a6cfa2a 100644 --- a/robotis_device/devices/dynamixel/M54-60-S250-R.device +++ b/robotis_device/devices/dynamixel/M54-60-S250-R.device @@ -3,8 +3,6 @@ model_name = M54-60-S250-R device_type = dynamixel [type info] -current_ratio = 16.11328125 - value_of_0_radian_position = 0 value_of_min_radian_position = -125700 value_of_max_radian_position = 125700 @@ -21,6 +19,9 @@ goal_current_item_name = goal_torque position_d_gain_item_name = position_i_gain_item_name = position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/MX-106.device b/robotis_device/devices/dynamixel/MX-106.device new file mode 100644 index 0000000..4b8c63d --- /dev/null +++ b/robotis_device/devices/dynamixel/MX-106.device @@ -0,0 +1,66 @@ +[device info] +model_name = MX-106 +device_type = dynamixel + +[type info] +value_of_0_radian_position = 2048 +value_of_min_radian_position = 0 +value_of_max_radian_position = 4095 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = +velocity_p_gain_item_name = + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 2 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 3 | ID | 1 | RW | EEPROM | 0 | 252 | N + 4 | baudrate | 1 | RW | EEPROM | 0 | 252 | N + 5 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 6 | CW_angle_limit | 2 | RW | EEPROM | 0 | 4095 | N + 8 | CCW_angle_limit | 2 | RW | EEPROM | 0 | 4095 | N + 10 | drive_mode | 1 | RW | EEPROM | 0 | 3 | N + 11 | max_temperature_limit | 1 | RW | EEPROM | 0 | 99 | N + 12 | min_voltage_limit | 1 | RW | EEPROM | 0 | 250 | N + 13 | max_voltage_limit | 1 | RW | EEPROM | 0 | 250 | N + 14 | max_torque | 2 | RW | EEPROM | 0 | 1023 | N + 16 | status_return_level | 1 | RW | EEPROM | 0 | 2 | N + 17 | alarm_LED | 1 | RW | EEPROM | 0 | 127 | N + 18 | alarm_shutdown | 1 | RW | EEPROM | 0 | 127 | N + 20 | multi_turn_offset | 2 | RW | EEPROM | -26624 | 26624 | Y + 22 | resolution_dividor | 1 | RW | EEPROM | 1 | 255 | N + 24 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 25 | LED | 1 | RW | RAM | 0 | 1 | N + 26 | position_d_gain | 1 | RW | RAM | 0 | 254 | N + 27 | position_i_gain | 1 | RW | RAM | 0 | 254 | N + 28 | position_p_gain | 1 | RW | RAM | 0 | 254 | N + 30 | goal_position | 2 | RW | RAM | -28672 | 28672 | Y + 32 | goal_velocity | 2 | RW | RAM | 0 | 1023 | N + 34 | goal_torque | 2 | RW | RAM | 0 | 1023 | N + 36 | present_position | 2 | R | RAM | -32768 | 32767 | Y + 38 | present_velocity | 2 | R | RAM | 0 | 2048 | N + 40 | present_load | 2 | R | RAM | 0 | 2048 | N + 42 | present_voltage | 1 | R | RAM | 50 | 250 | N + 43 | present_temperature | 1 | R | RAM | 0 | 99 | N + 44 | registered_instruction | 1 | R | RAM | 0 | 1 | N + 46 | is_moving | 1 | R | RAM | 0 | 1 | N + 47 | EEPROM_lock | 1 | RW | RAM | 0 | 1 | N + 48 | punch | 2 | RW | RAM | 0 | 1023 | N + 68 | current_consumption | 2 | RW | RAM | 0 | 4095 | N + 70 | torque_control_mode | 1 | RW | RAM | 0 | 1 | N + 71 | torque_control_goal | 2 | RW | RAM | 0 | 2047 | N + 73 | goal_acceleration | 1 | RW | RAM | 0 | 254 | N + diff --git a/robotis_device/devices/dynamixel/MX-28.device b/robotis_device/devices/dynamixel/MX-28.device index f1cfc21..ff67f2a 100644 --- a/robotis_device/devices/dynamixel/MX-28.device +++ b/robotis_device/devices/dynamixel/MX-28.device @@ -19,6 +19,9 @@ goal_current_item_name = position_d_gain_item_name = position_d_gain position_i_gain_item_name = position_i_gain position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = +velocity_p_gain_item_name = [control table] # addr | item name | length | access | memory | min value | max value | signed @@ -47,7 +50,7 @@ position_p_gain_item_name = position_p_gain 30 | goal_position | 2 | RW | RAM | -28672 | 28672 | Y 32 | goal_velocity | 2 | RW | RAM | 0 | 1023 | N 34 | goal_torque | 2 | RW | RAM | 0 | 1023 | N - 36 | present_position | 2 | R | RAM | 0 | 4095 | N + 36 | present_position | 2 | R | RAM | -32768 | 32767 | Y 38 | present_velocity | 2 | R | RAM | 0 | 2048 | N 40 | present_load | 2 | R | RAM | 0 | 2048 | N 42 | present_voltage | 1 | R | RAM | 50 | 250 | N diff --git a/robotis_device/devices/dynamixel/MX-64.device b/robotis_device/devices/dynamixel/MX-64.device new file mode 100644 index 0000000..06c0a4a --- /dev/null +++ b/robotis_device/devices/dynamixel/MX-64.device @@ -0,0 +1,66 @@ +[device info] +model_name = MX-64 +device_type = dynamixel + +[type info] +value_of_0_radian_position = 2048 +value_of_min_radian_position = 0 +value_of_max_radian_position = 4095 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = +velocity_p_gain_item_name = + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 2 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 3 | ID | 1 | RW | EEPROM | 0 | 252 | N + 4 | baudrate | 1 | RW | EEPROM | 0 | 252 | N + 5 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 6 | CW_angle_limit | 2 | RW | EEPROM | 0 | 4095 | N + 8 | CCW_angle_limit | 2 | RW | EEPROM | 0 | 4095 | N + 10 | drive_mode | 1 | RW | EEPROM | 0 | 3 | N + 11 | max_temperature_limit | 1 | RW | EEPROM | 0 | 99 | N + 12 | min_voltage_limit | 1 | RW | EEPROM | 0 | 250 | N + 13 | max_voltage_limit | 1 | RW | EEPROM | 0 | 250 | N + 14 | max_torque | 2 | RW | EEPROM | 0 | 1023 | N + 16 | status_return_level | 1 | RW | EEPROM | 0 | 2 | N + 17 | alarm_LED | 1 | RW | EEPROM | 0 | 127 | N + 18 | alarm_shutdown | 1 | RW | EEPROM | 0 | 127 | N + 20 | multi_turn_offset | 2 | RW | EEPROM | -26624 | 26624 | Y + 22 | resolution_dividor | 1 | RW | EEPROM | 1 | 255 | N + 24 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 25 | LED | 1 | RW | RAM | 0 | 1 | N + 26 | position_d_gain | 1 | RW | RAM | 0 | 254 | N + 27 | position_i_gain | 1 | RW | RAM | 0 | 254 | N + 28 | position_p_gain | 1 | RW | RAM | 0 | 254 | N + 30 | goal_position | 2 | RW | RAM | -28672 | 28672 | Y + 32 | goal_velocity | 2 | RW | RAM | 0 | 1023 | N + 34 | goal_torque | 2 | RW | RAM | 0 | 1023 | N + 36 | present_position | 2 | R | RAM | -32768 | 32767 | Y + 38 | present_velocity | 2 | R | RAM | 0 | 2048 | N + 40 | present_load | 2 | R | RAM | 0 | 2048 | N + 42 | present_voltage | 1 | R | RAM | 50 | 250 | N + 43 | present_temperature | 1 | R | RAM | 0 | 99 | N + 44 | registered_instruction | 1 | R | RAM | 0 | 1 | N + 46 | is_moving | 1 | R | RAM | 0 | 1 | N + 47 | EEPROM_lock | 1 | RW | RAM | 0 | 1 | N + 48 | punch | 2 | RW | RAM | 0 | 1023 | N + 68 | current_consumption | 2 | RW | RAM | 0 | 4095 | N + 70 | torque_control_mode | 1 | RW | RAM | 0 | 1 | N + 71 | torque_control_goal | 2 | RW | RAM | 0 | 2047 | N + 73 | goal_acceleration | 1 | RW | RAM | 0 | 254 | N + diff --git a/robotis_device/devices/dynamixel/XM-430-W210.device b/robotis_device/devices/dynamixel/XM-430-W210.device new file mode 100644 index 0000000..f42ae57 --- /dev/null +++ b/robotis_device/devices/dynamixel/XM-430-W210.device @@ -0,0 +1,81 @@ +[device info] +model_name = XM-430-W210 +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 235.53647082 +velocity_to_value_ratio = 41.707853 + +value_of_0_radian_position = 2048 +value_of_min_radian_position = 0 +value_of_max_radian_position = 4095 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_current +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 7 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 16 | N + 20 | homing_offset | 4 | RW | EEPROM | -1044479 | 1044479 | Y + 24 | moving_threshold | 4 | RW | EEPROM | 0 | 1023 | N + 31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 32 | max_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N + 34 | min_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N + 36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N + 38 | current_limit | 2 | RW | EEPROM | 0 | 1193 | N + 40 | acceleration_limit | 4 | RW | EEPROM | 0 | 32767 | N + 44 | velocity_limit | 4 | RW | EEPROM | 0 | 1023 | N + 48 | max_position_limit | 4 | RW | EEPROM | 0 | 4095 | N + 52 | min_position_limit | 4 | RW | EEPROM | 0 | 4095 | N + 63 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 64 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 65 | LED | 1 | RW | RAM | 0 | 1 | N + 68 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 69 | registered_instruction | 1 | R | RAM | 0 | 1 | N + 70 | hardware_error_status | 1 | R | RAM | 0 | 255 | N + 76 | velocity_i_gain | 2 | RW | RAM | 0 | 32767 | N + 78 | velocity_p_gain | 2 | RW | RAM | 0 | 32767 | N + 80 | position_d_gain | 2 | RW | RAM | 0 | 32767 | N + 82 | position_i_gain | 2 | RW | RAM | 0 | 32767 | N + 84 | position_p_gain | 2 | RW | RAM | 0 | 32767 | N + 88 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N + 90 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N + 100 | goal_pwm | 2 | RW | RAM | 0 | 885 | N + 102 | goal_current | 2 | RW | RAM | 0 | 1193 | N + 104 | goal_velocity | 4 | RW | RAM | 0 | 1023 | N + 108 | profile_acceleration | 4 | RW | RAM | 0 | 32767 | N + 112 | profile_velocity | 4 | RW | RAM | 0 | 1023 | N + 116 | goal_position | 4 | RW | RAM | -1048575 | 1048575 | Y + 120 | realtime_tick | 2 | R | RAM | 0 | 32767 | N + 122 | moving | 1 | R | RAM | 0 | 1 | N + 123 | moving_status | 1 | R | RAM | 0 | 255 | N + 124 | present_pwm | 2 | R | RAM | 0 | 885 | N + 126 | present_current | 2 | R | RAM | 0 | 1193 | N + 128 | present_velocity | 4 | R | RAM | 0 | 1023 | N + 132 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 136 | velocity_trajectory | 4 | R | RAM | 0 | 1023 | N + 140 | position_trajectory | 4 | R | RAM | 0 | 4095 | N + 144 | present_input_voltage | 2 | R | RAM | 95 | 160 | N + 146 | present_temperature | 1 | R | RAM | 0 | 100 | N + 168 | indirect_address_1 | 2 | RW | RAM | 0 | 65535 | N + 224 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 578 | indirect_address_29 | 2 | RW | RAM | 0 | 65535 | N + 634 | indirect_data_29 | 1 | RW | RAM | 0 | 255 | N diff --git a/robotis_device/devices/dynamixel/XM-430-W350.device b/robotis_device/devices/dynamixel/XM-430-W350.device new file mode 100644 index 0000000..ee14157 --- /dev/null +++ b/robotis_device/devices/dynamixel/XM-430-W350.device @@ -0,0 +1,81 @@ +[device info] +model_name = XM-430-W350 +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 149.795386991 +velocity_to_value_ratio = 41.707853 + +value_of_0_radian_position = 2048 +value_of_min_radian_position = 0 +value_of_max_radian_position = 4095 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_current +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 7 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 16 | N + 20 | homing_offset | 4 | RW | EEPROM | -1044479 | 1044479 | Y + 24 | moving_threshold | 4 | RW | EEPROM | 0 | 1023 | N + 31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 32 | max_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N + 34 | min_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N + 36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N + 38 | current_limit | 2 | RW | EEPROM | 0 | 1193 | N + 40 | acceleration_limit | 4 | RW | EEPROM | 0 | 32767 | N + 44 | velocity_limit | 4 | RW | EEPROM | 0 | 1023 | N + 48 | max_position_limit | 4 | RW | EEPROM | 0 | 4095 | N + 52 | min_position_limit | 4 | RW | EEPROM | 0 | 4095 | N + 63 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 64 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 65 | LED | 1 | RW | RAM | 0 | 1 | N + 68 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 69 | registered_instruction | 1 | R | RAM | 0 | 1 | N + 70 | hardware_error_status | 1 | R | RAM | 0 | 255 | N + 76 | velocity_i_gain | 2 | RW | RAM | 0 | 32767 | N + 78 | velocity_p_gain | 2 | RW | RAM | 0 | 32767 | N + 80 | position_d_gain | 2 | RW | RAM | 0 | 32767 | N + 82 | position_i_gain | 2 | RW | RAM | 0 | 32767 | N + 84 | position_p_gain | 2 | RW | RAM | 0 | 32767 | N + 88 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N + 90 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N + 100 | goal_pwm | 2 | RW | RAM | 0 | 885 | N + 102 | goal_current | 2 | RW | RAM | 0 | 1193 | N + 104 | goal_velocity | 4 | RW | RAM | 0 | 1023 | N + 108 | profile_acceleration | 4 | RW | RAM | 0 | 32767 | N + 112 | profile_velocity | 4 | RW | RAM | 0 | 1023 | N + 116 | goal_position | 4 | RW | RAM | -1048575 | 1048575 | Y + 120 | realtime_tick | 2 | R | RAM | 0 | 32767 | N + 122 | moving | 1 | R | RAM | 0 | 1 | N + 123 | moving_status | 1 | R | RAM | 0 | 255 | N + 124 | present_pwm | 2 | R | RAM | 0 | 885 | N + 126 | present_current | 2 | R | RAM | 0 | 1193 | N + 128 | present_velocity | 4 | R | RAM | 0 | 1023 | N + 132 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 136 | velocity_trajectory | 4 | R | RAM | 0 | 1023 | N + 140 | position_trajectory | 4 | R | RAM | 0 | 4095 | N + 144 | present_input_voltage | 2 | R | RAM | 95 | 160 | N + 146 | present_temperature | 1 | R | RAM | 0 | 100 | N + 168 | indirect_address_1 | 2 | RW | RAM | 0 | 65535 | N + 224 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 578 | indirect_address_29 | 2 | RW | RAM | 0 | 65535 | N + 634 | indirect_data_29 | 1 | RW | RAM | 0 | 255 | N diff --git a/robotis_device/devices/dynamixel/XM-430.device b/robotis_device/devices/dynamixel/XM-430.device index 9f17d37..c3a7bad 100644 --- a/robotis_device/devices/dynamixel/XM-430.device +++ b/robotis_device/devices/dynamixel/XM-430.device @@ -3,7 +3,7 @@ model_name = XM-430 device_type = dynamixel [type info] -current_ratio = 2.69 +torque_to_current_value_ratio = 149.795386991 value_of_0_radian_position = 2048 value_of_min_radian_position = 0 @@ -21,6 +21,9 @@ goal_current_item_name = goal_current position_d_gain_item_name = position_d_gain position_i_gain_item_name = position_i_gain position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed @@ -59,14 +62,14 @@ position_p_gain_item_name = position_p_gain 104 | goal_velocity | 4 | RW | RAM | 0 | 1023 | N 108 | profile_acceleration | 4 | RW | RAM | 0 | 32767 | N 112 | profile_velocity | 4 | RW | RAM | 0 | 1023 | N - 116 | goal_position | 4 | RW | RAM | 0 | 4095 | N + 116 | goal_position | 4 | RW | RAM | -1048575 | 1048575 | Y 120 | realtime_tick | 2 | R | RAM | 0 | 32767 | N 122 | moving | 1 | R | RAM | 0 | 1 | N 123 | moving_status | 1 | R | RAM | 0 | 255 | N 124 | present_pwm | 2 | R | RAM | 0 | 885 | N 126 | present_current | 2 | R | RAM | 0 | 1193 | N 128 | present_velocity | 4 | R | RAM | 0 | 1023 | N - 132 | present_position | 4 | R | RAM | 0 | 4095 | N + 132 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y 136 | velocity_trajectory | 4 | R | RAM | 0 | 1023 | N 140 | position_trajectory | 4 | R | RAM | 0 | 4095 | N 144 | present_input_voltage | 2 | R | RAM | 95 | 160 | N diff --git a/robotis_device/include/robotis_device/ControlTableItem.h b/robotis_device/include/robotis_device/ControlTableItem.h deleted file mode 100644 index e7b75fd..0000000 --- a/robotis_device/include/robotis_device/ControlTableItem.h +++ /dev/null @@ -1,54 +0,0 @@ -/* - * ControlTableItem.h - * - * Created on: 2015. 12. 16. - * Author: zerom - */ - -#ifndef ROBOTIS_FRAMEWORK_ROBOTIS_DEVICE_INCLUDE_DEVICE_CONTROLTABLEITEM_H_ -#define ROBOTIS_FRAMEWORK_ROBOTIS_DEVICE_INCLUDE_DEVICE_CONTROLTABLEITEM_H_ - - -#include - -namespace ROBOTIS -{ - -enum ACCESS_TYPE { - READ, - READ_WRITE -}; - -enum MEMORY_TYPE { - EEPROM, - RAM -}; - -class ControlTableItem -{ -public: - std::string item_name; - UINT16_T address; - ACCESS_TYPE access_type; - MEMORY_TYPE memory_type; - UINT8_T data_length; - INT32_T data_min_value; - INT32_T data_max_value; - bool is_signed; - - ControlTableItem() - : item_name(""), - address(0), - access_type(READ), - memory_type(RAM), - data_length(0), - data_min_value(0), - data_max_value(0), - is_signed(false) - { } -}; - -} - - -#endif /* ROBOTIS_FRAMEWORK_ROBOTIS_DEVICE_INCLUDE_DEVICE_CONTROLTABLEITEM_H_ */ diff --git a/robotis_device/include/robotis_device/Device.h b/robotis_device/include/robotis_device/Device.h deleted file mode 100644 index d9fd0bf..0000000 --- a/robotis_device/include/robotis_device/Device.h +++ /dev/null @@ -1,37 +0,0 @@ -/* - * Device.h - * - * Created on: 2016. 5. 12. - * Author: zerom - */ - -#ifndef ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DEVICE_H_ -#define ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DEVICE_H_ - - -#include -#include -#include -#include "ControlTableItem.h" - -namespace ROBOTIS -{ - -class Device -{ -public: - UINT8_T id; - float protocol_version; - std::string model_name; - std::string port_name; - - std::map ctrl_table; - std::vector bulk_read_items; - - virtual ~Device() { } -}; - -} - - -#endif /* ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DEVICE_H_ */ diff --git a/robotis_device/include/robotis_device/Dynamixel.h b/robotis_device/include/robotis_device/Dynamixel.h deleted file mode 100644 index 64d4a4a..0000000 --- a/robotis_device/include/robotis_device/Dynamixel.h +++ /dev/null @@ -1,60 +0,0 @@ -/* - * Dynamixel.h - * - * Created on: 2015. 12. 8. - * Author: zerom - */ - -#ifndef ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DYNAMIXEL_H_ -#define ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DYNAMIXEL_H_ - - -#include -#include -#include -#include "Device.h" -#include "DynamixelState.h" -#include "ControlTableItem.h" - -namespace ROBOTIS -{ - -class Dynamixel : public Device -{ -public: - std::string ctrl_module_name; - DynamixelState *dxl_state; - - double current_ratio; - double velocity_ratio; - - INT32_T value_of_0_radian_position; - INT32_T value_of_min_radian_position; - INT32_T value_of_max_radian_position; - double min_radian; - double max_radian; - - ControlTableItem *torque_enable_item; - ControlTableItem *present_position_item; - ControlTableItem *present_velocity_item; - ControlTableItem *present_current_item; - ControlTableItem *goal_position_item; - ControlTableItem *goal_velocity_item; - ControlTableItem *goal_current_item; - - Dynamixel(int id, std::string model_name, float protocol_version); - - double ConvertValue2Radian(INT32_T value); - INT32_T ConvertRadian2Value(double radian); - - double ConvertValue2Velocity(INT32_T value); - INT32_T ConvertVelocity2Value(double velocity); - - double ConvertValue2Current(INT16_T value); - INT16_T ConvertCurrent2Value(double torque); -}; - -} - - -#endif /* ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DYNAMIXEL_H_ */ diff --git a/robotis_device/include/robotis_device/DynamixelState.h b/robotis_device/include/robotis_device/DynamixelState.h deleted file mode 100644 index 29212c4..0000000 --- a/robotis_device/include/robotis_device/DynamixelState.h +++ /dev/null @@ -1,55 +0,0 @@ -/* - * DynamixelState.h - * - * Created on: 2015. 12. 8. - * Author: zerom - */ - -#ifndef ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DYNAMIXELSTATE_H_ -#define ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DYNAMIXELSTATE_H_ - -#include - -#include "robotis_device/TimeStamp.h" -#include "robotis_framework_common/RobotisDef.h" - -#define INDIRECT_DATA_1 "indirect_data_1" -#define INDIRECT_ADDRESS_1 "indirect_address_1" - -namespace ROBOTIS -{ - -class DynamixelState -{ -public: - TimeStamp update_time_stamp; - - double present_position; - double present_velocity; - double present_current; - double goal_position; - double goal_velocity; - double goal_current; - - std::map bulk_read_table; - - double position_offset; - - DynamixelState() - : update_time_stamp(0, 0), - present_position(0.0), - present_velocity(0.0), - present_current(0.0), - goal_position(0.0), - goal_velocity(0.0), - goal_current(0.0), - position_offset(0.0) - { - bulk_read_table.clear(); - } -}; - -} - - -#endif /* ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DYNAMIXELSTATE_H_ */ diff --git a/robotis_device/include/robotis_device/Robot.h b/robotis_device/include/robotis_device/Robot.h deleted file mode 100644 index 2473e7c..0000000 --- a/robotis_device/include/robotis_device/Robot.h +++ /dev/null @@ -1,38 +0,0 @@ -/* - * Robot.h - * - * Created on: 2015. 12. 11. - * Author: zerom - */ - -#ifndef ROBOTIS_FRAMEWORK_ROBOTIS_DEVICE_INCLUDE_DEVICE_ROBOT_H_ -#define ROBOTIS_FRAMEWORK_ROBOTIS_DEVICE_INCLUDE_DEVICE_ROBOT_H_ - - -#include -#include "Sensor.h" -#include "Dynamixel.h" -#include "dynamixel_sdk/PortHandler.h" - -namespace ROBOTIS -{ - -class Robot -{ -public: - std::map ports; // string: port name - std::map port_default_device; // port name, default device name - - std::map dxls; // string: joint name - std::map sensors; // string: sensor name - - Robot(std::string robot_file_path, std::string dev_desc_dir_path); - - Sensor *getSensor(std::string path, int id, std::string port, float protocol_version); - Dynamixel *getDynamixel(std::string path, int id, std::string port, float protocol_version); -}; - -} - - -#endif /* ROBOTIS_FRAMEWORK_ROBOTIS_DEVICE_INCLUDE_DEVICE_ROBOT_H_ */ diff --git a/robotis_device/include/robotis_device/Sensor.h b/robotis_device/include/robotis_device/Sensor.h deleted file mode 100644 index 54df44f..0000000 --- a/robotis_device/include/robotis_device/Sensor.h +++ /dev/null @@ -1,32 +0,0 @@ -/* - * Sensor.h - * - * Created on: 2015. 12. 16. - * Author: zerom - */ - -#ifndef ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_SENSOR_H_ -#define ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_SENSOR_H_ - -#include -#include -#include -#include "Device.h" -#include "SensorState.h" -#include "ControlTableItem.h" - -namespace ROBOTIS -{ - -class Sensor : public Device -{ -public: - SensorState *sensor_state; - - Sensor(int id, std::string model_name, float protocol_version); -}; - -} - - -#endif /* ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_SENSOR_H_ */ diff --git a/robotis_device/include/robotis_device/SensorState.h b/robotis_device/include/robotis_device/SensorState.h deleted file mode 100644 index 4137b14..0000000 --- a/robotis_device/include/robotis_device/SensorState.h +++ /dev/null @@ -1,35 +0,0 @@ -/* - * SensorState.h - * - * Created on: 2016. 5. 16. - * Author: zerom - */ - -#ifndef ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_SENSORSTATE_H_ -#define ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_SENSORSTATE_H_ - - -#include "robotis_device/TimeStamp.h" -#include "robotis_framework_common/RobotisDef.h" - -namespace ROBOTIS -{ - -class SensorState -{ -public: - TimeStamp update_time_stamp; - - std::map bulk_read_table; - - SensorState() - : update_time_stamp(0, 0) - { - bulk_read_table.clear(); - } -}; - -} - - -#endif /* ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_SENSORSTATE_H_ */ diff --git a/robotis_device/include/robotis_device/TimeStamp.h b/robotis_device/include/robotis_device/TimeStamp.h deleted file mode 100644 index 17b749a..0000000 --- a/robotis_device/include/robotis_device/TimeStamp.h +++ /dev/null @@ -1,25 +0,0 @@ -/* - * TimeStamp.h - * - * Created on: 2016. 5. 16. - * Author: zerom - */ - -#ifndef ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_TIMESTAMP_H_ -#define ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_TIMESTAMP_H_ - - -namespace ROBOTIS -{ - -class TimeStamp { -public: - long sec; - long nsec; - TimeStamp(long sec, long nsec) : sec(sec), nsec(nsec) { } -}; - -} - - -#endif /* ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_TIMESTAMP_H_ */ diff --git a/robotis_device/include/robotis_device/control_table_item.h b/robotis_device/include/robotis_device/control_table_item.h new file mode 100644 index 0000000..28a840a --- /dev/null +++ b/robotis_device/include/robotis_device/control_table_item.h @@ -0,0 +1,84 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of ROBOTIS nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *******************************************************************************/ + +/* + * control_table_item.h + * + * Created on: 2015. 12. 16. + * Author: zerom + */ + +#ifndef ROBOTIS_DEVICE_CONTROL_TABLE_ITEM_H_ +#define ROBOTIS_DEVICE_CONTROL_TABLE_ITEM_H_ + + +#include + +namespace robotis_framework +{ + +enum AccessType { + Read, + ReadWrite +}; + +enum MemoryType { + EEPROM, + RAM +}; + +class ControlTableItem +{ +public: + std::string item_name_; + uint16_t address_; + AccessType access_type_; + MemoryType memory_type_; + uint8_t data_length_; + int32_t data_min_value_; + int32_t data_max_value_; + bool is_signed_; + + ControlTableItem() + : item_name_(""), + address_(0), + access_type_(Read), + memory_type_(RAM), + data_length_(0), + data_min_value_(0), + data_max_value_(0), + is_signed_(false) + { } +}; + +} + + +#endif /* ROBOTIS_DEVICE_CONTROL_TABLE_ITEM_H_ */ diff --git a/robotis_device/include/robotis_device/device.h b/robotis_device/include/robotis_device/device.h new file mode 100644 index 0000000..0a5556e --- /dev/null +++ b/robotis_device/include/robotis_device/device.h @@ -0,0 +1,68 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of ROBOTIS nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *******************************************************************************/ + +/* + * device.h + * + * Created on: 2016. 5. 12. + * Author: zerom + */ + +#ifndef ROBOTIS_DEVICE_DEVICE_H_ +#define ROBOTIS_DEVICE_DEVICE_H_ + + +#include +#include +#include + +#include "control_table_item.h" + +namespace robotis_framework +{ + +class Device +{ +public: + uint8_t id_; + float protocol_version_; + std::string model_name_; + std::string port_name_; + + std::map ctrl_table_; + std::vector bulk_read_items_; + + virtual ~Device() { } +}; + +} + + +#endif /* ROBOTIS_DEVICE_DEVICE_H_ */ diff --git a/robotis_device/include/robotis_device/dynamixel.h b/robotis_device/include/robotis_device/dynamixel.h new file mode 100644 index 0000000..adff01b --- /dev/null +++ b/robotis_device/include/robotis_device/dynamixel.h @@ -0,0 +1,97 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of ROBOTIS nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *******************************************************************************/ + +/* + * dynamixel.h + * + * Created on: 2015. 12. 8. + * Author: zerom + */ + +#ifndef ROBOTIS_DEVICE_DYNAMIXEL_H_ +#define ROBOTIS_DEVICE_DYNAMIXEL_H_ + + +#include +#include +#include + +#include "control_table_item.h" +#include "device.h" +#include "dynamixel_state.h" + +namespace robotis_framework +{ + +class Dynamixel : public Device +{ +public: + std::string ctrl_module_name_; + DynamixelState *dxl_state_; + + double velocity_to_value_ratio_; + double torque_to_current_value_ratio_; + + int32_t value_of_0_radian_position_; + int32_t value_of_min_radian_position_; + int32_t value_of_max_radian_position_; + double min_radian_; + double max_radian_; + + ControlTableItem *torque_enable_item_; + ControlTableItem *present_position_item_; + ControlTableItem *present_velocity_item_; + ControlTableItem *present_current_item_; + ControlTableItem *goal_position_item_; + ControlTableItem *goal_velocity_item_; + ControlTableItem *goal_current_item_; + ControlTableItem *position_p_gain_item_; + ControlTableItem *position_i_gain_item_; + ControlTableItem *position_d_gain_item_; + ControlTableItem *velocity_p_gain_item_; + ControlTableItem *velocity_i_gain_item_; + ControlTableItem *velocity_d_gain_item_; + + Dynamixel(int id, std::string model_name, float protocol_version); + + double convertValue2Radian(int32_t value); + int32_t convertRadian2Value(double radian); + + double convertValue2Velocity(int32_t value); + int32_t convertVelocity2Value(double velocity); + + double convertValue2Torque(int16_t value); + int16_t convertTorque2Value(double torque); +}; + +} + + +#endif /* ROBOTIS_DEVICE_DYNAMIXEL_H_ */ diff --git a/robotis_device/include/robotis_device/dynamixel_state.h b/robotis_device/include/robotis_device/dynamixel_state.h new file mode 100644 index 0000000..5f4ae36 --- /dev/null +++ b/robotis_device/include/robotis_device/dynamixel_state.h @@ -0,0 +1,96 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of ROBOTIS nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *******************************************************************************/ + +/* + * dynamixel_state.h + * + * Created on: 2015. 12. 8. + * Author: zerom + */ + +#ifndef ROBOTIS_DEVICE_DYNAMIXEL_STATE_H_ +#define ROBOTIS_DEVICE_DYNAMIXEL_STATE_H_ + +#include + +#include "time_stamp.h" + +#define INDIRECT_DATA_1 "indirect_data_1" +#define INDIRECT_ADDRESS_1 "indirect_address_1" + +namespace robotis_framework +{ + +class DynamixelState +{ +public: + TimeStamp update_time_stamp_; + + double present_position_; + double present_velocity_; + double present_torque_; + double goal_position_; + double goal_velocity_; + double goal_torque_; + double position_p_gain_; + double position_i_gain_; + double position_d_gain_; + double velocity_p_gain_; + double velocity_i_gain_; + double velocity_d_gain_; + + std::map bulk_read_table_; + + double position_offset_; + + DynamixelState() + : update_time_stamp_(0, 0), + present_position_(0.0), + present_velocity_(0.0), + present_torque_(0.0), + goal_position_(0.0), + goal_velocity_(0.0), + goal_torque_(0.0), + position_p_gain_(0.0), + position_i_gain_(0.0), + position_d_gain_(0.0), + velocity_p_gain_(0.0), + velocity_i_gain_(0.0), + velocity_d_gain_(0.0), + position_offset_(0.0) + { + bulk_read_table_.clear(); + } +}; + +} + + +#endif /* ROBOTIS_DEVICE_DYNAMIXEL_STATE_H_ */ diff --git a/robotis_device/include/robotis_device/robot.h b/robotis_device/include/robotis_device/robot.h new file mode 100644 index 0000000..4dfe5bc --- /dev/null +++ b/robotis_device/include/robotis_device/robot.h @@ -0,0 +1,78 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of ROBOTIS nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *******************************************************************************/ + +/* + * robot.h + * + * Created on: 2015. 12. 11. + * Author: zerom + */ + +#ifndef ROBOTIS_DEVICE_ROBOT_H_ +#define ROBOTIS_DEVICE_ROBOT_H_ + + +#include + +#include "sensor.h" +#include "dynamixel.h" +#include "dynamixel_sdk/port_handler.h" + +#define DYNAMIXEL "dynamixel" +#define SENSOR "sensor" + +#define SESSION_PORT_INFO "port info" +#define SESSION_DEVICE_INFO "device info" + +#define SESSION_TYPE_INFO "type info" +#define SESSION_CONTROL_TABLE "control table" + +namespace robotis_framework +{ + +class Robot +{ +public: + std::map ports_; // string: port name + std::map port_default_device_; // port name, default device name + + std::map dxls_; // string: joint name + std::map sensors_; // string: sensor name + + Robot(std::string robot_file_path, std::string dev_desc_dir_path); + + Sensor *getSensor(std::string path, int id, std::string port, float protocol_version); + Dynamixel *getDynamixel(std::string path, int id, std::string port, float protocol_version); +}; + +} + + +#endif /* ROBOTIS_DEVICE_ROBOT_H_ */ diff --git a/robotis_device/include/robotis_device/sensor.h b/robotis_device/include/robotis_device/sensor.h new file mode 100644 index 0000000..2d1b757 --- /dev/null +++ b/robotis_device/include/robotis_device/sensor.h @@ -0,0 +1,63 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of ROBOTIS nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *******************************************************************************/ + +/* + * sensor.h + * + * Created on: 2015. 12. 16. + * Author: zerom + */ + +#ifndef ROBOTIS_DEVICE_SENSOR_H_ +#define ROBOTIS_DEVICE_SENSOR_H_ + +#include +#include +#include + +#include "device.h" +#include "sensor_state.h" +#include "control_table_item.h" + +namespace robotis_framework +{ + +class Sensor : public Device +{ +public: + SensorState *sensor_state_; + + Sensor(int id, std::string model_name, float protocol_version); +}; + +} + + +#endif /* ROBOTIS_DEVICE_SENSOR_H_ */ diff --git a/robotis_device/include/robotis_device/sensor_state.h b/robotis_device/include/robotis_device/sensor_state.h new file mode 100644 index 0000000..f40afa4 --- /dev/null +++ b/robotis_device/include/robotis_device/sensor_state.h @@ -0,0 +1,64 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of ROBOTIS nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *******************************************************************************/ + +/* + * sensor_state.h + * + * Created on: 2016. 5. 16. + * Author: zerom + */ + +#ifndef ROBOTIS_DEVICE_SENSOR_STATE_H_ +#define ROBOTIS_DEVICE_SENSOR_STATE_H_ + + +#include "time_stamp.h" + +namespace robotis_framework +{ + +class SensorState +{ +public: + TimeStamp update_time_stamp_; + + std::map bulk_read_table_; + + SensorState() + : update_time_stamp_(0, 0) + { + bulk_read_table_.clear(); + } +}; + +} + + +#endif /* ROBOTIS_DEVICE_SENSOR_STATE_H_ */ diff --git a/robotis_device/include/robotis_device/time_stamp.h b/robotis_device/include/robotis_device/time_stamp.h new file mode 100644 index 0000000..b0de1e5 --- /dev/null +++ b/robotis_device/include/robotis_device/time_stamp.h @@ -0,0 +1,59 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of ROBOTIS nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *******************************************************************************/ + +/* + * time_stamp.h + * + * Created on: 2016. 5. 16. + * Author: zerom + */ + +#ifndef ROBOTIS_DEVICE_TIME_STAMP_H_ +#define ROBOTIS_DEVICE_TIME_STAMP_H_ + + +namespace robotis_framework +{ + +class TimeStamp { +public: + long sec_; + long nsec_; + + TimeStamp(long sec, long nsec) + : sec_(sec), + nsec_(nsec) + { } +}; + +} + + +#endif /* ROBOTIS_DEVICE_TIME_STAMP_H_ */ diff --git a/robotis_device/package.xml b/robotis_device/package.xml index be72665..6f7294a 100644 --- a/robotis_device/package.xml +++ b/robotis_device/package.xml @@ -6,29 +6,19 @@ robotis - BSD - - robotis - catkin roscpp rospy dynamixel_sdk - robotis_framework_common roscpp rospy dynamixel_sdk - - - - - \ No newline at end of file diff --git a/robotis_device/src/robotis_device/Dynamixel.cpp b/robotis_device/src/robotis_device/Dynamixel.cpp deleted file mode 100644 index daf454d..0000000 --- a/robotis_device/src/robotis_device/Dynamixel.cpp +++ /dev/null @@ -1,110 +0,0 @@ -/* - * Dynamixel.cpp - * - * Created on: 2015. 12. 8. - * Author: zerom - */ - -#include "robotis_device/Dynamixel.h" - -using namespace ROBOTIS; - -Dynamixel::Dynamixel(int id, std::string model_name, float protocol_version) - : ctrl_module_name("none"), - current_ratio(1.0), - velocity_ratio(1.0), - value_of_0_radian_position(0), - value_of_min_radian_position(0), - value_of_max_radian_position(0), - min_radian(-3.14159265), - max_radian(3.14159265), - torque_enable_item(0), - present_position_item(0), - present_velocity_item(0), - present_current_item(0), - goal_position_item(0), - goal_velocity_item(0), - goal_current_item(0) -{ - this->id = id; - this->model_name = model_name; - this->port_name = ""; - this->protocol_version = protocol_version; - - ctrl_table.clear(); - dxl_state = new DynamixelState(); - - bulk_read_items.clear(); -} - -double Dynamixel::ConvertValue2Radian(INT32_T value) -{ - double _radian = 0.0; - if(value > value_of_0_radian_position) - { - if(max_radian <= 0) - return max_radian; - _radian = (double)(value - value_of_0_radian_position) * max_radian / (double)(value_of_max_radian_position - value_of_0_radian_position); - } - else if(value < value_of_0_radian_position) - { - if(min_radian >= 0) - return min_radian; - _radian = (double)(value - value_of_0_radian_position) * min_radian / (double)(value_of_min_radian_position - value_of_0_radian_position); - } - - if(_radian > max_radian) - return max_radian; - else if(_radian < min_radian) - return min_radian; - - return _radian; -} - -INT32_T Dynamixel::ConvertRadian2Value(double radian) -{ - //return radian * value_of_max_radian_position / max_radian; - - INT32_T _value = 0; - if(radian > 0) - { - if(value_of_max_radian_position <= value_of_0_radian_position) - return value_of_max_radian_position; - _value = (radian * (value_of_max_radian_position - value_of_0_radian_position) / max_radian) + value_of_0_radian_position; - } - else if(radian < 0) - { - if(value_of_min_radian_position >= value_of_0_radian_position) - return value_of_min_radian_position; - _value = (radian * (value_of_min_radian_position - value_of_0_radian_position) / min_radian) + value_of_0_radian_position; - } - else - _value = value_of_0_radian_position; - - if(_value > value_of_max_radian_position) - return value_of_max_radian_position; - else if(_value < value_of_min_radian_position) - return value_of_min_radian_position; - - return _value; -} - -double Dynamixel::ConvertValue2Velocity(INT32_T value) -{ - return (double)value * velocity_ratio; -} - -INT32_T Dynamixel::ConvertVelocity2Value(double velocity) -{ - return (INT32_T)(velocity / velocity_ratio);; -} - -double Dynamixel::ConvertValue2Current(INT16_T value) -{ - return (double)value * current_ratio; -} - -INT16_T Dynamixel::ConvertCurrent2Value(double torque) -{ - return (INT16_T)(torque / current_ratio); -} diff --git a/robotis_device/src/robotis_device/Robot.cpp b/robotis_device/src/robotis_device/Robot.cpp deleted file mode 100644 index dc1b50e..0000000 --- a/robotis_device/src/robotis_device/Robot.cpp +++ /dev/null @@ -1,408 +0,0 @@ -/* - * Robot.cpp - * - * Created on: 2015. 12. 11. - * Author: zerom - */ - -#include -#include -#include -#include "../include/robotis_device/Robot.h" - -using namespace ROBOTIS; - -static inline std::string <rim(std::string &s) { - s.erase(s.begin(), std::find_if(s.begin(), s.end(), std::not1(std::ptr_fun(std::isspace)))); - return s; -} -static inline std::string &rtrim(std::string &s) { - s.erase(std::find_if(s.rbegin(), s.rend(), std::not1(std::ptr_fun(std::isspace))).base(), s.end()); - return s; -} -static inline std::string &trim(std::string &s) { - return ltrim(rtrim(s)); -} - -static inline std::vector split(const std::string &text, char sep) { - std::vector tokens; - std::size_t start = 0, end = 0; - while((end = text.find(sep, start)) != (std::string::npos)) { - tokens.push_back(text.substr(start, end - start)); - trim(tokens.back()); - start = end + 1; - } - tokens.push_back(text.substr(start)); - trim(tokens.back()); - return tokens; -} - -Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path) -{ - if(dev_desc_dir_path.compare(dev_desc_dir_path.size()-1, 1, "/") != 0) - dev_desc_dir_path += "/"; - - std::ifstream file(robot_file_path.c_str()); - if(file.is_open()) - { - std::string session = ""; - std::string input_str; - while(!file.eof()) - { - std::getline(file, input_str); - - // remove comment ( # ) - std::size_t pos = input_str.find("#"); - if(pos != std::string::npos) - input_str = input_str.substr(0, pos); - - // trim - input_str = trim(input_str); - - // find session - if(!input_str.compare(0, 1, "[") && !input_str.compare(input_str.size()-1, 1, "]")) - { - input_str = input_str.substr(1, input_str.size()-2); - std::transform(input_str.begin(), input_str.end(), input_str.begin(), ::tolower); - session = trim(input_str); - continue; - } - - if(session == "port info") - { - std::vector tokens = split(input_str, '|'); - if(tokens.size() != 3) - continue; - - std::cout << tokens[0] << " added. (baudrate: " << tokens[1] << ")" << std::endl; - - ports[tokens[0]] = (PortHandler*)PortHandler::GetPortHandler(tokens[0].c_str()); - ports[tokens[0]]->SetBaudRate(std::atoi(tokens[1].c_str())); - port_default_device[tokens[0]] = tokens[2]; - } - else if(session == "device info") - { - std::vector tokens = split(input_str, '|'); - if(tokens.size() != 7) - continue; - - if(tokens[0] == "dynamixel") - { - std::string _file_path = dev_desc_dir_path + tokens[0] + "/" + tokens[3] + ".device"; - int _id = std::atoi(tokens[2].c_str()); - std::string _port = tokens[1]; - float _protocol = std::atof(tokens[4].c_str()); - std::string _dev_name = tokens[5]; - - dxls[_dev_name] = getDynamixel(_file_path, _id, _port, _protocol); - - Dynamixel *_dxl = dxls[_dev_name]; - std::vector sub_tokens = split(tokens[6], ','); - if(sub_tokens.size() > 0) - { - std::map::iterator _indirect_it = _dxl->ctrl_table.find(INDIRECT_ADDRESS_1); - if(_indirect_it != _dxl->ctrl_table.end()) // INDIRECT_ADDRESS_1 exist - { - UINT16_T _indirect_data_addr = _dxl->ctrl_table[INDIRECT_DATA_1]->address; - for(int _i = 0; _i < sub_tokens.size(); _i++) - { - _dxl->bulk_read_items.push_back(new ControlTableItem()); - ControlTableItem *_dest_item = _dxl->bulk_read_items[_i]; - ControlTableItem *_src_item = _dxl->ctrl_table[sub_tokens[_i]]; - - _dest_item->item_name = _src_item->item_name; - _dest_item->address = _indirect_data_addr; - _dest_item->access_type = _src_item->access_type; - _dest_item->memory_type = _src_item->memory_type; - _dest_item->data_length = _src_item->data_length; - _dest_item->data_min_value = _src_item->data_min_value; - _dest_item->data_max_value = _src_item->data_max_value; - _dest_item->is_signed = _src_item->is_signed; - - _indirect_data_addr += _dest_item->data_length; - } - } - else // INDIRECT_ADDRESS_1 exist - { - for(int _i = 0; _i < sub_tokens.size(); _i++) - _dxl->bulk_read_items.push_back(_dxl->ctrl_table[sub_tokens[_i]]); - } - } - } - else if(tokens[0] == "sensor") - { - std::string _file_path = dev_desc_dir_path + tokens[0] + "/" + tokens[3] + ".device"; - int _id = std::atoi(tokens[2].c_str()); - std::string _port = tokens[1]; - float _protocol = std::atof(tokens[4].c_str()); - std::string _dev_name = tokens[5]; - - sensors[_dev_name] = getSensor(_file_path, _id, _port, _protocol); - - Sensor *_sensor = sensors[_dev_name]; - std::vector sub_tokens = split(tokens[6], ','); - if(sub_tokens.size() > 0) - { - std::map::iterator _indirect_it = _sensor->ctrl_table.find(INDIRECT_ADDRESS_1); - if(_indirect_it != _sensor->ctrl_table.end()) // INDIRECT_ADDRESS_1 exist - { - UINT16_T _indirect_data_addr = _sensor->ctrl_table[INDIRECT_DATA_1]->address; - for(int _i = 0; _i < sub_tokens.size(); _i++) - { - _sensor->bulk_read_items.push_back(new ControlTableItem()); - ControlTableItem *_dest_item = _sensor->bulk_read_items[_i]; - ControlTableItem *_src_item = _sensor->ctrl_table[sub_tokens[_i]]; - - _dest_item->item_name = _src_item->item_name; - _dest_item->address = _indirect_data_addr; - _dest_item->access_type = _src_item->access_type; - _dest_item->memory_type = _src_item->memory_type; - _dest_item->data_length = _src_item->data_length; - _dest_item->data_min_value = _src_item->data_min_value; - _dest_item->data_max_value = _src_item->data_max_value; - _dest_item->is_signed = _src_item->is_signed; - - _indirect_data_addr += _dest_item->data_length; - } - } - else // INDIRECT_ADDRESS_1 exist - { - for(int _i = 0; _i < sub_tokens.size(); _i++) - _sensor->bulk_read_items.push_back(_sensor->ctrl_table[sub_tokens[_i]]); - } - } - } - } - } - file.close(); - } - else - { - std::cout << "Unable to open file : " + robot_file_path << std::endl; - } -} - -Sensor *Robot::getSensor(std::string path, int id, std::string port, float protocol_version) -{ - Sensor *_sensor; - - // load file model_name.device - std::ifstream file(path.c_str()); - if(file.is_open()) - { - std::string _session = ""; - std::string _input_str; - - while(!file.eof()) - { - std::getline(file, _input_str); - - // remove comment ( # ) - std::size_t pos = _input_str.find("#"); - if(pos != std::string::npos) - _input_str = _input_str.substr(0, pos); - - // trim - _input_str = trim(_input_str); - if(_input_str == "") - continue; - - // find _session - if(!_input_str.compare(0, 1, "[") && !_input_str.compare(_input_str.size()-1, 1, "]")) - { - _input_str = _input_str.substr(1, _input_str.size()-2); - std::transform(_input_str.begin(), _input_str.end(), _input_str.begin(), ::tolower); - _session = trim(_input_str); - continue; - } - - if(_session == "device info") - { - std::vector tokens = split(_input_str, '='); - if(tokens.size() != 2) - continue; - - if(tokens[0] == "model_name") - _sensor = new Sensor(id, tokens[1], protocol_version); - } - else if(_session == "control table") - { - std::vector tokens = split(_input_str, '|'); - if(tokens.size() != 8) - continue; - - ControlTableItem *item = new ControlTableItem(); - item->item_name = tokens[1]; - item->address = std::atoi(tokens[0].c_str()); - item->data_length = std::atoi(tokens[2].c_str()); - if(tokens[3] == "R") - item->access_type = READ; - else if(tokens[3] == "RW") - item->access_type = READ_WRITE; - if(tokens[4] == "EEPROM") - item->memory_type = EEPROM; - else if(tokens[4] == "RAM") - item->memory_type = RAM; - item->data_min_value = std::atol(tokens[5].c_str()); - item->data_max_value = std::atol(tokens[6].c_str()); - if(tokens[7] == "Y") - item->is_signed = true; - else if(tokens[7] == "N") - item->is_signed = false; - _sensor->ctrl_table[tokens[1]] = item; - } - } - _sensor->port_name = port; - - fprintf(stderr, "(%s) [ID:%3d] %14s added. \n", port.c_str(), _sensor->id, _sensor->model_name.c_str()); - //std::cout << "[ID:" << (int)(_sensor->id) << "] " << _sensor->model_name << " added. (" << port << ")" << std::endl; - file.close(); - } - else - std::cout << "Unable to open file : " + path << std::endl; - - return _sensor; -} - -Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, float protocol_version) -{ - Dynamixel *_dxl; - - // load file model_name.device - std::ifstream file(path.c_str()); - if(file.is_open()) - { - std::string _session = ""; - std::string _input_str; - - std::string _torque_enable_item_name = ""; - std::string _present_position_item_name = ""; - std::string _present_velocity_item_name = ""; - std::string _present_current_item_name = ""; - std::string _goal_position_item_name = ""; - std::string _goal_velocity_item_name = ""; - std::string _goal_current_item_name = ""; - - while(!file.eof()) - { - std::getline(file, _input_str); - - // remove comment ( # ) - std::size_t pos = _input_str.find("#"); - if(pos != std::string::npos) - _input_str = _input_str.substr(0, pos); - - // trim - _input_str = trim(_input_str); - if(_input_str == "") - continue; - - // find _session - if(!_input_str.compare(0, 1, "[") && !_input_str.compare(_input_str.size()-1, 1, "]")) - { - _input_str = _input_str.substr(1, _input_str.size()-2); - std::transform(_input_str.begin(), _input_str.end(), _input_str.begin(), ::tolower); - _session = trim(_input_str); - continue; - } - - if(_session == "device info") - { - std::vector tokens = split(_input_str, '='); - if(tokens.size() != 2) - continue; - - if(tokens[0] == "model_name") - _dxl = new Dynamixel(id, tokens[1], protocol_version); - } - else if(_session == "type info") - { - std::vector tokens = split(_input_str, '='); - if(tokens.size() != 2) - continue; - - if(tokens[0] == "current_ratio") - _dxl->current_ratio = std::atof(tokens[1].c_str()); - else if(tokens[0] == "velocity_ratio") - _dxl->velocity_ratio = std::atof(tokens[1].c_str()); - - else if(tokens[0] == "value_of_0_radian_position") - _dxl->value_of_0_radian_position = std::atoi(tokens[1].c_str()); - else if(tokens[0] == "value_of_min_radian_position") - _dxl->value_of_min_radian_position = std::atoi(tokens[1].c_str()); - else if(tokens[0] == "value_of_max_radian_position") - _dxl->value_of_max_radian_position = std::atoi(tokens[1].c_str()); - else if(tokens[0] == "min_radian") - _dxl->min_radian = std::atof(tokens[1].c_str()); - else if(tokens[0] == "max_radian") - _dxl->max_radian = std::atof(tokens[1].c_str()); - - else if(tokens[0] == "torque_enable_item_name") - _torque_enable_item_name = tokens[1]; - else if(tokens[0] == "present_position_item_name") - _present_position_item_name = tokens[1]; - else if(tokens[0] == "present_velocity_item_name") - _present_velocity_item_name = tokens[1]; - else if(tokens[0] == "present_current_item_name") - _present_current_item_name = tokens[1]; - else if(tokens[0] == "goal_position_item_name") - _goal_position_item_name = tokens[1]; - else if(tokens[0] == "goal_velocity_item_name") - _goal_velocity_item_name = tokens[1]; - else if(tokens[0] == "goal_current_item_name") - _goal_current_item_name = tokens[1]; - } - else if(_session == "control table") - { - std::vector tokens = split(_input_str, '|'); - if(tokens.size() != 8) - continue; - - ControlTableItem *item = new ControlTableItem(); - item->item_name = tokens[1]; - item->address = std::atoi(tokens[0].c_str()); - item->data_length = std::atoi(tokens[2].c_str()); - if(tokens[3] == "R") - item->access_type = READ; - else if(tokens[3] == "RW") - item->access_type = READ_WRITE; - if(tokens[4] == "EEPROM") - item->memory_type = EEPROM; - else if(tokens[4] == "RAM") - item->memory_type = RAM; - item->data_min_value = std::atol(tokens[5].c_str()); - item->data_max_value = std::atol(tokens[6].c_str()); - if(tokens[7] == "Y") - item->is_signed = true; - else if(tokens[7] == "N") - item->is_signed = false; - _dxl->ctrl_table[tokens[1]] = item; - } - } - _dxl->port_name = port; - - if(_dxl->ctrl_table[_torque_enable_item_name] != NULL) - _dxl->torque_enable_item = _dxl->ctrl_table[_torque_enable_item_name]; - if(_dxl->ctrl_table[_present_position_item_name] != NULL) - _dxl->present_position_item = _dxl->ctrl_table[_present_position_item_name]; - if(_dxl->ctrl_table[_present_velocity_item_name] != NULL) - _dxl->present_velocity_item = _dxl->ctrl_table[_present_velocity_item_name]; - if(_dxl->ctrl_table[_present_current_item_name] != NULL) - _dxl->present_current_item = _dxl->ctrl_table[_present_current_item_name]; - if(_dxl->ctrl_table[_goal_position_item_name] != NULL) - _dxl->goal_position_item = _dxl->ctrl_table[_goal_position_item_name]; - if(_dxl->ctrl_table[_goal_velocity_item_name] != NULL) - _dxl->goal_velocity_item = _dxl->ctrl_table[_goal_velocity_item_name]; - if(_dxl->ctrl_table[_goal_current_item_name] != NULL) - _dxl->goal_current_item = _dxl->ctrl_table[_goal_current_item_name]; - - fprintf(stderr, "(%s) [ID:%3d] %14s added. \n", port.c_str(), _dxl->id, _dxl->model_name.c_str()); - //std::cout << "[ID:" << (int)(_dxl->id) << "] " << _dxl->model_name << " added. (" << port << ")" << std::endl; - file.close(); - } - else - std::cout << "Unable to open file : " + path << std::endl; - - return _dxl; -} - diff --git a/robotis_device/src/robotis_device/Sensor.cpp b/robotis_device/src/robotis_device/Sensor.cpp deleted file mode 100644 index db95494..0000000 --- a/robotis_device/src/robotis_device/Sensor.cpp +++ /dev/null @@ -1,23 +0,0 @@ -/* - * Sensor.cpp - * - * Created on: 2016. 5. 11. - * Author: zerom - */ - -#include "robotis_device/Sensor.h" - -using namespace ROBOTIS; - -Sensor::Sensor(int id, std::string model_name, float protocol_version) -{ - this->id = id; - this->model_name = model_name; - this->port_name = ""; - this->protocol_version = protocol_version; - ctrl_table.clear(); - - sensor_state = new SensorState(); - - bulk_read_items.clear(); -} diff --git a/robotis_device/src/robotis_device/dynamixel.cpp b/robotis_device/src/robotis_device/dynamixel.cpp new file mode 100644 index 0000000..cdd5470 --- /dev/null +++ b/robotis_device/src/robotis_device/dynamixel.cpp @@ -0,0 +1,152 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of ROBOTIS nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *******************************************************************************/ + +/* + * dynamixel.cpp + * + * Created on: 2015. 12. 8. + * Author: zerom + */ + +#include "robotis_device/dynamixel.h" + +using namespace robotis_framework; + +Dynamixel::Dynamixel(int id, std::string model_name, float protocol_version) + : ctrl_module_name_("none"), + torque_to_current_value_ratio_(1.0), + velocity_to_value_ratio_(1.0), + value_of_0_radian_position_(0), + value_of_min_radian_position_(0), + value_of_max_radian_position_(0), + min_radian_(-3.14159265), + max_radian_(3.14159265), + torque_enable_item_(0), + present_position_item_(0), + present_velocity_item_(0), + present_current_item_(0), + goal_position_item_(0), + goal_velocity_item_(0), + goal_current_item_(0), + position_p_gain_item_(0), + position_i_gain_item_(0), + position_d_gain_item_(0), + velocity_p_gain_item_(0), + velocity_i_gain_item_(0), + velocity_d_gain_item_(0) +{ + this->id_ = id; + this->model_name_ = model_name; + this->port_name_ = ""; + this->protocol_version_ = protocol_version; + + ctrl_table_.clear(); + dxl_state_ = new DynamixelState(); + + bulk_read_items_.clear(); +} + +double Dynamixel::convertValue2Radian(int32_t value) +{ + double radian = 0.0; + if (value > value_of_0_radian_position_) + { + if (max_radian_ <= 0) + return max_radian_; + + radian = (double) (value - value_of_0_radian_position_) * max_radian_ + / (double) (value_of_max_radian_position_ - value_of_0_radian_position_); + } + else if (value < value_of_0_radian_position_) + { + if (min_radian_ >= 0) + return min_radian_; + + radian = (double) (value - value_of_0_radian_position_) * min_radian_ + / (double) (value_of_min_radian_position_ - value_of_0_radian_position_); + } + + if (radian > max_radian_) + return max_radian_; + else if (radian < min_radian_) + return min_radian_; + + return radian; +} + +int32_t Dynamixel::convertRadian2Value(double radian) +{ + int32_t value = 0; + if (radian > 0) + { + if (value_of_max_radian_position_ <= value_of_0_radian_position_) + return value_of_max_radian_position_; + + value = (radian * (value_of_max_radian_position_ - value_of_0_radian_position_) / max_radian_) + + value_of_0_radian_position_; + } + else if (radian < 0) + { + if (value_of_min_radian_position_ >= value_of_0_radian_position_) + return value_of_min_radian_position_; + + value = (radian * (value_of_min_radian_position_ - value_of_0_radian_position_) / min_radian_) + + value_of_0_radian_position_; + } + else + value = value_of_0_radian_position_; + + if (value > value_of_max_radian_position_) + return value_of_max_radian_position_; + else if (value < value_of_min_radian_position_) + return value_of_min_radian_position_; + + return value; +} + +double Dynamixel::convertValue2Velocity(int32_t value) +{ + return (double) value / velocity_to_value_ratio_; +} + +int32_t Dynamixel::convertVelocity2Value(double velocity) +{ + return (int32_t) (velocity * velocity_to_value_ratio_);; +} + +double Dynamixel::convertValue2Torque(int16_t value) +{ + return (double) value / torque_to_current_value_ratio_; +} + +int16_t Dynamixel::convertTorque2Value(double torque) +{ + return (int16_t) (torque * torque_to_current_value_ratio_); +} diff --git a/robotis_device/src/robotis_device/robot.cpp b/robotis_device/src/robotis_device/robot.cpp new file mode 100644 index 0000000..900a9b9 --- /dev/null +++ b/robotis_device/src/robotis_device/robot.cpp @@ -0,0 +1,457 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of ROBOTIS nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *******************************************************************************/ + +/* + * robot.cpp + * + * Created on: 2015. 12. 11. + * Author: zerom + */ + +#include +#include +#include + +#include "robotis_device/robot.h" + +using namespace robotis_framework; + +static inline std::string <rim(std::string &s) +{ + s.erase(s.begin(), std::find_if(s.begin(), s.end(), std::not1(std::ptr_fun(std::isspace)))); + return s; +} +static inline std::string &rtrim(std::string &s) +{ + s.erase(std::find_if(s.rbegin(), s.rend(), std::not1(std::ptr_fun(std::isspace))).base(), s.end()); + return s; +} +static inline std::string &trim(std::string &s) +{ + return ltrim(rtrim(s)); +} + +static inline std::vector split(const std::string &text, char sep) +{ + std::vector tokens; + std::size_t start = 0, end = 0; + + while ((end = text.find(sep, start)) != (std::string::npos)) + { + tokens.push_back(text.substr(start, end - start)); + trim(tokens.back()); + start = end + 1; + } + tokens.push_back(text.substr(start)); + trim(tokens.back()); + + return tokens; +} + +Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path) +{ + if (dev_desc_dir_path.compare(dev_desc_dir_path.size() - 1, 1, "/") != 0) + dev_desc_dir_path += "/"; + + std::ifstream file(robot_file_path.c_str()); + if (file.is_open()) + { + std::string session = ""; + std::string input_str; + while (!file.eof()) + { + std::getline(file, input_str); + + // remove comment ( # ) + std::size_t pos = input_str.find("#"); + if (pos != std::string::npos) + input_str = input_str.substr(0, pos); + + // trim + input_str = trim(input_str); + + // find session + if (!input_str.compare(0, 1, "[") && !input_str.compare(input_str.size() - 1, 1, "]")) + { + input_str = input_str.substr(1, input_str.size() - 2); + std::transform(input_str.begin(), input_str.end(), input_str.begin(), ::tolower); + session = trim(input_str); + continue; + } + + if (session == SESSION_PORT_INFO) + { + std::vector tokens = split(input_str, '|'); + if (tokens.size() != 3) + continue; + + std::cout << tokens[0] << " added. (baudrate: " << tokens[1] << ")" << std::endl; + + ports_[tokens[0]] = dynamixel::PortHandler::getPortHandler(tokens[0].c_str()); + ports_[tokens[0]]->setBaudRate(std::atoi(tokens[1].c_str())); + port_default_device_[tokens[0]] = tokens[2]; + } + else if (session == SESSION_DEVICE_INFO) + { + std::vector tokens = split(input_str, '|'); + if (tokens.size() != 7) + continue; + + if (tokens[0] == DYNAMIXEL) + { + std::string file_path = dev_desc_dir_path + tokens[0] + "/" + tokens[3] + ".device"; + int id = std::atoi(tokens[2].c_str()); + std::string port = tokens[1]; + float protocol = std::atof(tokens[4].c_str()); + std::string dev_name = tokens[5]; + + dxls_[dev_name] = getDynamixel(file_path, id, port, protocol); + + Dynamixel *dxl = dxls_[dev_name]; + std::vector sub_tokens = split(tokens[6], ','); + if (sub_tokens.size() > 0) + { + std::map::iterator indirect_it = dxl->ctrl_table_.find(INDIRECT_ADDRESS_1); + if (indirect_it != dxl->ctrl_table_.end()) // INDIRECT_ADDRESS_1 exist + { + uint16_t indirect_data_addr = dxl->ctrl_table_[INDIRECT_DATA_1]->address_; + for (int _i = 0; _i < sub_tokens.size(); _i++) + { + dxl->bulk_read_items_.push_back(new ControlTableItem()); + ControlTableItem *dest_item = dxl->bulk_read_items_[_i]; + ControlTableItem *src_item = dxl->ctrl_table_[sub_tokens[_i]]; + + dest_item->item_name_ = src_item->item_name_; + dest_item->address_ = indirect_data_addr; + dest_item->access_type_ = src_item->access_type_; + dest_item->memory_type_ = src_item->memory_type_; + dest_item->data_length_ = src_item->data_length_; + dest_item->data_min_value_ = src_item->data_min_value_; + dest_item->data_max_value_ = src_item->data_max_value_; + dest_item->is_signed_ = src_item->is_signed_; + + indirect_data_addr += dest_item->data_length_; + } + } + else // INDIRECT_ADDRESS_1 not exist + { + for (int i = 0; i < sub_tokens.size(); i++) + { + if (dxl->ctrl_table_[sub_tokens[i]] != NULL) + dxl->bulk_read_items_.push_back(dxl->ctrl_table_[sub_tokens[i]]); + } + } + } + } + else if (tokens[0] == SENSOR) + { + std::string file_path = dev_desc_dir_path + tokens[0] + "/" + tokens[3] + ".device"; + int id = std::atoi(tokens[2].c_str()); + std::string port = tokens[1]; + float protocol = std::atof(tokens[4].c_str()); + std::string dev_name = tokens[5]; + + sensors_[dev_name] = getSensor(file_path, id, port, protocol); + + Sensor *sensor = sensors_[dev_name]; + std::vector sub_tokens = split(tokens[6], ','); + if (sub_tokens.size() > 0) + { + std::map::iterator indirect_it = sensor->ctrl_table_.find(INDIRECT_ADDRESS_1); + if (indirect_it != sensor->ctrl_table_.end()) // INDIRECT_ADDRESS_1 exist + { + uint16_t indirect_data_addr = sensor->ctrl_table_[INDIRECT_DATA_1]->address_; + for (int i = 0; i < sub_tokens.size(); i++) + { + sensor->bulk_read_items_.push_back(new ControlTableItem()); + ControlTableItem *dest_item = sensor->bulk_read_items_[i]; + ControlTableItem *src_item = sensor->ctrl_table_[sub_tokens[i]]; + + dest_item->item_name_ = src_item->item_name_; + dest_item->address_ = indirect_data_addr; + dest_item->access_type_ = src_item->access_type_; + dest_item->memory_type_ = src_item->memory_type_; + dest_item->data_length_ = src_item->data_length_; + dest_item->data_min_value_ = src_item->data_min_value_; + dest_item->data_max_value_ = src_item->data_max_value_; + dest_item->is_signed_ = src_item->is_signed_; + + indirect_data_addr += dest_item->data_length_; + } + } + else // INDIRECT_ADDRESS_1 exist + { + for (int i = 0; i < sub_tokens.size(); i++) + sensor->bulk_read_items_.push_back(sensor->ctrl_table_[sub_tokens[i]]); + } + } + } + } + } + file.close(); + } + else + { + std::cout << "Unable to open file : " + robot_file_path << std::endl; + } +} + +Sensor *Robot::getSensor(std::string path, int id, std::string port, float protocol_version) +{ + Sensor *sensor; + + // load file model_name.device + std::ifstream file(path.c_str()); + if (file.is_open()) + { + std::string session = ""; + std::string input_str; + + while (!file.eof()) + { + std::getline(file, input_str); + + // remove comment ( # ) + std::size_t pos = input_str.find("#"); + if (pos != std::string::npos) + input_str = input_str.substr(0, pos); + + // trim + input_str = trim(input_str); + if (input_str == "") + continue; + + // find _session + if (!input_str.compare(0, 1, "[") && !input_str.compare(input_str.size() - 1, 1, "]")) + { + input_str = input_str.substr(1, input_str.size() - 2); + std::transform(input_str.begin(), input_str.end(), input_str.begin(), ::tolower); + session = trim(input_str); + continue; + } + + if (session == SESSION_DEVICE_INFO) + { + std::vector tokens = split(input_str, '='); + if (tokens.size() != 2) + continue; + + if (tokens[0] == "model_name") + sensor = new Sensor(id, tokens[1], protocol_version); + } + else if (session == SESSION_CONTROL_TABLE) + { + std::vector tokens = split(input_str, '|'); + if (tokens.size() != 8) + continue; + + ControlTableItem *item = new ControlTableItem(); + item->item_name_ = tokens[1]; + item->address_ = std::atoi(tokens[0].c_str()); + item->data_length_ = std::atoi(tokens[2].c_str()); + + if (tokens[3] == "R") + item->access_type_ = Read; + else if (tokens[3] == "RW") + item->access_type_ = ReadWrite; + + if (tokens[4] == "EEPROM") + item->memory_type_ = EEPROM; + else if (tokens[4] == "RAM") + item->memory_type_ = RAM; + + item->data_min_value_ = std::atol(tokens[5].c_str()); + item->data_max_value_ = std::atol(tokens[6].c_str()); + + if (tokens[7] == "Y") + item->is_signed_ = true; + else if (tokens[7] == "N") + item->is_signed_ = false; + sensor->ctrl_table_[tokens[1]] = item; + } + } + sensor->port_name_ = port; + + fprintf(stderr, "(%s) [ID:%3d] %14s added. \n", port.c_str(), sensor->id_, sensor->model_name_.c_str()); + //std::cout << "[ID:" << (int)(_sensor->id) << "] " << _sensor->model_name << " added. (" << port << ")" << std::endl; + file.close(); + } + else + std::cout << "Unable to open file : " + path << std::endl; + + return sensor; +} + +Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, float protocol_version) +{ + Dynamixel *dxl; + + // load file model_name.device + std::ifstream file(path.c_str()); + if (file.is_open()) + { + std::string session = ""; + std::string input_str; + + std::string torque_enable_item_name = ""; + std::string present_position_item_name = ""; + std::string present_velocity_item_name = ""; + std::string present_current_item_name = ""; + std::string goal_position_item_name = ""; + std::string goal_velocity_item_name = ""; + std::string goal_current_item_name = ""; + + while (!file.eof()) + { + std::getline(file, input_str); + + // remove comment ( # ) + std::size_t pos = input_str.find("#"); + if (pos != std::string::npos) + input_str = input_str.substr(0, pos); + + // trim + input_str = trim(input_str); + if (input_str == "") + continue; + + // find session + if (!input_str.compare(0, 1, "[") && !input_str.compare(input_str.size() - 1, 1, "]")) + { + input_str = input_str.substr(1, input_str.size() - 2); + std::transform(input_str.begin(), input_str.end(), input_str.begin(), ::tolower); + session = trim(input_str); + continue; + } + + if (session == SESSION_DEVICE_INFO) + { + std::vector tokens = split(input_str, '='); + if (tokens.size() != 2) + continue; + + if (tokens[0] == "model_name") + dxl = new Dynamixel(id, tokens[1], protocol_version); + } + else if (session == SESSION_TYPE_INFO) + { + std::vector tokens = split(input_str, '='); + if (tokens.size() != 2) + continue; + + if (tokens[0] == "torque_to_current_value_ratio") + dxl->torque_to_current_value_ratio_ = std::atof(tokens[1].c_str()); + else if (tokens[0] == "velocity_to_value_ratio") + dxl->velocity_to_value_ratio_ = std::atof(tokens[1].c_str()); + + else if (tokens[0] == "value_of_0_radian_position") + dxl->value_of_0_radian_position_ = std::atoi(tokens[1].c_str()); + else if (tokens[0] == "value_of_min_radian_position") + dxl->value_of_min_radian_position_ = std::atoi(tokens[1].c_str()); + else if (tokens[0] == "value_of_max_radian_position") + dxl->value_of_max_radian_position_ = std::atoi(tokens[1].c_str()); + else if (tokens[0] == "min_radian") + dxl->min_radian_ = std::atof(tokens[1].c_str()); + else if (tokens[0] == "max_radian") + dxl->max_radian_ = std::atof(tokens[1].c_str()); + + else if (tokens[0] == "torque_enable_item_name") + torque_enable_item_name = tokens[1]; + else if (tokens[0] == "present_position_item_name") + present_position_item_name = tokens[1]; + else if (tokens[0] == "present_velocity_item_name") + present_velocity_item_name = tokens[1]; + else if (tokens[0] == "present_current_item_name") + present_current_item_name = tokens[1]; + else if (tokens[0] == "goal_position_item_name") + goal_position_item_name = tokens[1]; + else if (tokens[0] == "goal_velocity_item_name") + goal_velocity_item_name = tokens[1]; + else if (tokens[0] == "goal_current_item_name") + goal_current_item_name = tokens[1]; + } + else if (session == SESSION_CONTROL_TABLE) + { + std::vector tokens = split(input_str, '|'); + if (tokens.size() != 8) + continue; + + ControlTableItem *item = new ControlTableItem(); + item->item_name_ = tokens[1]; + item->address_ = std::atoi(tokens[0].c_str()); + item->data_length_ = std::atoi(tokens[2].c_str()); + + if (tokens[3] == "R") + item->access_type_ = Read; + else if (tokens[3] == "RW") + item->access_type_ = ReadWrite; + + if (tokens[4] == "EEPROM") + item->memory_type_ = EEPROM; + else if (tokens[4] == "RAM") + item->memory_type_ = RAM; + + item->data_min_value_ = std::atol(tokens[5].c_str()); + item->data_max_value_ = std::atol(tokens[6].c_str()); + + if (tokens[7] == "Y") + item->is_signed_ = true; + else if (tokens[7] == "N") + item->is_signed_ = false; + dxl->ctrl_table_[tokens[1]] = item; + } + } + dxl->port_name_ = port; + + if (dxl->ctrl_table_[torque_enable_item_name] != NULL) + dxl->torque_enable_item_ = dxl->ctrl_table_[torque_enable_item_name]; + if (dxl->ctrl_table_[present_position_item_name] != NULL) + dxl->present_position_item_ = dxl->ctrl_table_[present_position_item_name]; + if (dxl->ctrl_table_[present_velocity_item_name] != NULL) + dxl->present_velocity_item_ = dxl->ctrl_table_[present_velocity_item_name]; + if (dxl->ctrl_table_[present_current_item_name] != NULL) + dxl->present_current_item_ = dxl->ctrl_table_[present_current_item_name]; + if (dxl->ctrl_table_[goal_position_item_name] != NULL) + dxl->goal_position_item_ = dxl->ctrl_table_[goal_position_item_name]; + if (dxl->ctrl_table_[goal_velocity_item_name] != NULL) + dxl->goal_velocity_item_ = dxl->ctrl_table_[goal_velocity_item_name]; + if (dxl->ctrl_table_[goal_current_item_name] != NULL) + dxl->goal_current_item_ = dxl->ctrl_table_[goal_current_item_name]; + + fprintf(stderr, "(%s) [ID:%3d] %14s added. \n", port.c_str(), dxl->id_, dxl->model_name_.c_str()); + //std::cout << "[ID:" << (int)(_dxl->id) << "] " << _dxl->model_name << " added. (" << port << ")" << std::endl; + file.close(); + } + else + std::cout << "Unable to open file : " + path << std::endl; + + return dxl; +} + diff --git a/robotis_device/src/robotis_device/sensor.cpp b/robotis_device/src/robotis_device/sensor.cpp new file mode 100644 index 0000000..537989d --- /dev/null +++ b/robotis_device/src/robotis_device/sensor.cpp @@ -0,0 +1,53 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of ROBOTIS nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *******************************************************************************/ + +/* + * sensor.cpp + * + * Created on: 2016. 5. 11. + * Author: zerom + */ + +#include "robotis_device/sensor.h" + +using namespace robotis_framework; + +Sensor::Sensor(int id, std::string model_name, float protocol_version) +{ + this->id_ = id; + this->model_name_ = model_name; + this->port_name_ = ""; + this->protocol_version_ = protocol_version; + ctrl_table_.clear(); + + sensor_state_ = new SensorState(); + + bulk_read_items_.clear(); +} diff --git a/robotis_framework_common/CMakeLists.txt b/robotis_framework_common/CMakeLists.txt index 17cd2cc..51f0f3f 100644 --- a/robotis_framework_common/CMakeLists.txt +++ b/robotis_framework_common/CMakeLists.txt @@ -5,15 +5,6 @@ find_package(catkin REQUIRED COMPONENTS roscpp ) -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if you package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( INCLUDE_DIRS include # LIBRARIES robotis_framework_common @@ -21,36 +12,7 @@ catkin_package( # DEPENDS system_lib ) -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -# include_directories(include) include_directories( ${catkin_INCLUDE_DIRS} ) -## Declare a C++ library -# add_library(robotis_framework_common -# src/${PROJECT_NAME}/robotis_framework_common.cpp -# ) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -# add_dependencies(robotis_framework_common ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -# add_executable(robotis_framework_common_node src/robotis_framework_common_node.cpp) - -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(robotis_framework_common_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -# target_link_libraries(robotis_framework_common_node -# ${catkin_LIBRARIES} -# ) - diff --git a/robotis_framework_common/include/robotis_framework_common/MotionModule.h b/robotis_framework_common/include/robotis_framework_common/MotionModule.h deleted file mode 100644 index a305a6a..0000000 --- a/robotis_framework_common/include/robotis_framework_common/MotionModule.h +++ /dev/null @@ -1,51 +0,0 @@ -/* - * MotionModule.h - * - * Created on: 2016. 1. 15. - * Author: zerom - */ - -#ifndef ROBOTIS_FRAMEWORK_COMMON_INCLUDE_ROBOTIS_FRAMEWORK_COMMON_MOTIONMODULE_H_ -#define ROBOTIS_FRAMEWORK_COMMON_INCLUDE_ROBOTIS_FRAMEWORK_COMMON_MOTIONMODULE_H_ - - -#include -#include - -#include "robotis_device/Robot.h" -#include "robotis_device/Dynamixel.h" -#include "robotis_framework_common/Singleton.h" - -namespace ROBOTIS -{ - -enum CONTROL_MODE -{ - POSITION_CONTROL, - VELOCITY_CONTROL, - CURRENT_CONTROL -}; - -class MotionModule -{ -public: - bool enable; - std::string module_name; - CONTROL_MODE control_mode; - - std::map result; - - virtual ~MotionModule() { } - - virtual void Initialize(const int control_cycle_msec, Robot *robot) = 0; - virtual void Process(std::map dxls, std::map sensors) = 0; - - virtual void Stop() = 0; - virtual bool IsRunning() = 0; -}; - - -} - - -#endif /* ROBOTIS_FRAMEWORK_COMMON_INCLUDE_ROBOTIS_FRAMEWORK_COMMON_MOTIONMODULE_H_ */ diff --git a/robotis_framework_common/include/robotis_framework_common/RobotisDef.h b/robotis_framework_common/include/robotis_framework_common/RobotisDef.h deleted file mode 100644 index a860e9d..0000000 --- a/robotis_framework_common/include/robotis_framework_common/RobotisDef.h +++ /dev/null @@ -1,21 +0,0 @@ -/* - * RobotisDef.h - * - * Created on: 2016. 1. 27. - * Author: zerom - */ - -#ifndef ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_ROBOTISDEF_H_ -#define ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_ROBOTISDEF_H_ - - -typedef char INT8_T; -typedef short int INT16_T; -typedef int INT32_T; - -typedef unsigned char UINT8_T; -typedef unsigned short int UINT16_T; -typedef unsigned int UINT32_T; - - -#endif /* ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_ROBOTISDEF_H_ */ diff --git a/robotis_framework_common/include/robotis_framework_common/SensorModule.h b/robotis_framework_common/include/robotis_framework_common/SensorModule.h deleted file mode 100644 index 6111391..0000000 --- a/robotis_framework_common/include/robotis_framework_common/SensorModule.h +++ /dev/null @@ -1,38 +0,0 @@ -/* - * SensorModule.h - * - * Created on: 2016. 3. 30. - * Author: zerom - */ - -#ifndef ROBOTIS_FRAMEWORK_COMMON_INCLUDE_ROBOTIS_FRAMEWORK_COMMON_SENSORMODULE_H_ -#define ROBOTIS_FRAMEWORK_COMMON_INCLUDE_ROBOTIS_FRAMEWORK_COMMON_SENSORMODULE_H_ - - -#include -#include - -#include "robotis_device/Robot.h" -#include "robotis_device/Dynamixel.h" -#include "robotis_framework_common/Singleton.h" - -namespace ROBOTIS -{ - -class SensorModule -{ -public: - std::string module_name; - - std::map result; - - virtual ~SensorModule() { } - - virtual void Initialize(const int control_cycle_msec, Robot *robot) = 0; - virtual void Process(std::map dxls, std::map sensors) = 0; -}; - -} - - -#endif /* ROBOTIS_FRAMEWORK_COMMON_INCLUDE_ROBOTIS_FRAMEWORK_COMMON_SENSORMODULE_H_ */ diff --git a/robotis_framework_common/include/robotis_framework_common/Singleton.h b/robotis_framework_common/include/robotis_framework_common/Singleton.h deleted file mode 100644 index 2b4c7d8..0000000 --- a/robotis_framework_common/include/robotis_framework_common/Singleton.h +++ /dev/null @@ -1,49 +0,0 @@ -/* - * Singleton.h - * - * Created on: 2016. 5. 17. - * Author: zerom - */ - -#ifndef ROBOTIS_FRAMEWORK_COMMON_INCLUDE_ROBOTIS_FRAMEWORK_COMMON_SINGLETON_H_ -#define ROBOTIS_FRAMEWORK_COMMON_INCLUDE_ROBOTIS_FRAMEWORK_COMMON_SINGLETON_H_ - - -namespace ROBOTIS -{ - -template -class Singleton -{ -private: - static T *unique_instance_; - -protected: - Singleton() { } - Singleton(Singleton const&) { } - Singleton& operator=(Singleton const&) { return *this; } - -public: - static T* GetInstance() - { - if(unique_instance_ == NULL) - unique_instance_ = new T; - return unique_instance_; - } - - static void DestroyInstance() - { - if(unique_instance_) - { - delete unique_instance_; - unique_instance_ = NULL; - } - } -}; - -template T* Singleton::unique_instance_ = NULL; - -} - - -#endif /* ROBOTIS_FRAMEWORK_COMMON_INCLUDE_ROBOTIS_FRAMEWORK_COMMON_SINGLETON_H_ */ diff --git a/robotis_framework_common/include/robotis_framework_common/motion_module.h b/robotis_framework_common/include/robotis_framework_common/motion_module.h new file mode 100644 index 0000000..e85d938 --- /dev/null +++ b/robotis_framework_common/include/robotis_framework_common/motion_module.h @@ -0,0 +1,101 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of ROBOTIS nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *******************************************************************************/ + +/* + * motion_module.h + * + * Created on: 2016. 1. 15. + * Author: zerom + */ + +#ifndef ROBOTIS_FRAMEWORK_COMMON_MOTION_MODULE_H_ +#define ROBOTIS_FRAMEWORK_COMMON_MOTION_MODULE_H_ + + +#include +#include + +#include "singleton.h" +#include "robotis_device/robot.h" +#include "robotis_device/dynamixel.h" + +namespace robotis_framework +{ + +enum ControlMode +{ + PositionControl, + VelocityControl, + TorqueControl +}; + +class MotionModule +{ +protected: + bool enable_; + std::string module_name_; + ControlMode control_mode_; + +public: + std::map result_; + + virtual ~MotionModule() { } + + std::string getModuleName() { return module_name_; } + ControlMode getControlMode() { return control_mode_; } + + void setModuleEnable(bool enable) + { + if(this->enable_ == enable) + return; + + this->enable_ = enable; + if(enable) + onModuleEnable(); + else + onModuleDisable(); + } + bool getModuleEnable() { return enable_; } + + virtual void onModuleEnable() { } + virtual void onModuleDisable() { } + + virtual void initialize(const int control_cycle_msec, Robot *robot) = 0; + virtual void process(std::map dxls, std::map sensors) = 0; + + virtual void stop() = 0; + virtual bool isRunning() = 0; +}; + + +} + + +#endif /* ROBOTIS_FRAMEWORK_COMMON_MOTION_MODULE_H_ */ diff --git a/robotis_framework_common/include/robotis_framework_common/sensor_module.h b/robotis_framework_common/include/robotis_framework_common/sensor_module.h new file mode 100644 index 0000000..81699e2 --- /dev/null +++ b/robotis_framework_common/include/robotis_framework_common/sensor_module.h @@ -0,0 +1,71 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of ROBOTIS nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *******************************************************************************/ + +/* + * sensor_module.h + * + * Created on: 2016. 3. 30. + * Author: zerom + */ + +#ifndef ROBOTIS_FRAMEWORK_COMMON_SENSOR_MODULE_H_ +#define ROBOTIS_FRAMEWORK_COMMON_SENSOR_MODULE_H_ + + +#include +#include + +#include "singleton.h" +#include "robotis_device/robot.h" +#include "robotis_device/dynamixel.h" + +namespace robotis_framework +{ + +class SensorModule +{ +protected: + std::string module_name_; + +public: + std::map result_; + + virtual ~SensorModule() { } + + std::string getModuleName() { return module_name_; } + + virtual void initialize(const int control_cycle_msec, Robot *robot) = 0; + virtual void process(std::map dxls, std::map sensors) = 0; +}; + +} + + +#endif /* ROBOTIS_FRAMEWORK_COMMON_SENSOR_MODULE_H_ */ diff --git a/robotis_framework_common/include/robotis_framework_common/singleton.h b/robotis_framework_common/include/robotis_framework_common/singleton.h new file mode 100644 index 0000000..837b80b --- /dev/null +++ b/robotis_framework_common/include/robotis_framework_common/singleton.h @@ -0,0 +1,79 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of ROBOTIS nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *******************************************************************************/ + +/* + * singleton.h + * + * Created on: 2016. 5. 17. + * Author: zerom + */ + +#ifndef ROBOTIS_FRAMEWORK_COMMON_SINGLETON_H_ +#define ROBOTIS_FRAMEWORK_COMMON_SINGLETON_H_ + + +namespace robotis_framework +{ + +template +class Singleton +{ +private: + static T *unique_instance_; + +protected: + Singleton() { } + Singleton(Singleton const&) { } + Singleton& operator=(Singleton const&) { return *this; } + +public: + static T* getInstance() + { + if(unique_instance_ == NULL) + unique_instance_ = new T; + return unique_instance_; + } + + static void destroyInstance() + { + if(unique_instance_) + { + delete unique_instance_; + unique_instance_ = NULL; + } + } +}; + +template T* Singleton::unique_instance_ = NULL; + +} + + +#endif /* ROBOTIS_FRAMEWORK_COMMON_SINGLETON_H_ */ diff --git a/robotis_framework_common/package.xml b/robotis_framework_common/package.xml index feec7f8..820f4a2 100644 --- a/robotis_framework_common/package.xml +++ b/robotis_framework_common/package.xml @@ -1,52 +1,15 @@ robotis_framework_common - 0.0.0 + 0.1.0 The robotis_framework_common package + robotis - - - - robotis - - - - - BSD - - - - - - - - - - - - - - - - - - - - - - - - + ROBOTIS + catkin roscpp roscpp - - - - - - - \ No newline at end of file diff --git a/robotis_math/CMakeLists.txt b/robotis_math/CMakeLists.txt deleted file mode 100644 index 147809f..0000000 --- a/robotis_math/CMakeLists.txt +++ /dev/null @@ -1,65 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(robotis_math) - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - roscpp - cmake_modules -) - -find_package(Eigen REQUIRED) - - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if you package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( - INCLUDE_DIRS include - LIBRARIES robotis_math - CATKIN_DEPENDS roscpp -# DEPENDS system_lib -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( - include - ${catkin_INCLUDE_DIRS} - ${Eigen_INCLUDE_DIRS} -) - -## Declare a C++ library -add_library(robotis_math - src/RobotisMathBase.cpp - src/RobotisTrajectoryCalculator.cpp - src/RobotisLinearAlgebra.cpp -) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -add_dependencies(robotis_math ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -# add_executable(robotis_math_node src/robotis_math_node.cpp) - -## Add cmake target dependencies of the executable -## same as for the library above -#add_dependencies(robotis_math_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -target_link_libraries(robotis_math - ${catkin_LIBRARIES} -) diff --git a/robotis_math/include/robotis_math/RobotisLinearAlgebra.h b/robotis_math/include/robotis_math/RobotisLinearAlgebra.h deleted file mode 100644 index 6188c50..0000000 --- a/robotis_math/include/robotis_math/RobotisLinearAlgebra.h +++ /dev/null @@ -1,52 +0,0 @@ -/* - * RobotisLinearAlgebra.h - * - * Created on: Mar 18, 2016 - * Author: jay - */ - -#ifndef ROBOTIS_LINEAR_ALGEBRA_H_ -#define ROBOTIS_LINEAR_ALGEBRA_H_ - -#include - -#define EIGEN_NO_DEBUG -#define EIGEN_NO_STATIC_ASSERT - -#include - -namespace ROBOTIS -{ - -Eigen::MatrixXd transitionXYZ ( double position_x, double position_y, double position_z ); -Eigen::MatrixXd transformationXYZRPY ( double position_x, double position_y, double position_z , double roll , double pitch , double yaw ); -Eigen::MatrixXd InverseTransformation(Eigen::MatrixXd transform); - -Eigen::MatrixXd inertiaXYZ( double ixx, double ixy, double ixz , double iyy , double iyz, double izz ); - -Eigen::MatrixXd rotationX( double angle ); -Eigen::MatrixXd rotationY( double angle ); -Eigen::MatrixXd rotationZ( double angle ); - -Eigen::MatrixXd rotation2rpy( Eigen::MatrixXd rotation ); -Eigen::MatrixXd rpy2rotation( double roll, double pitch, double yaw ); - -Eigen::Quaterniond rpy2quaternion( double roll, double pitch, double yaw ); -Eigen::Quaterniond rotation2quaternion( Eigen::MatrixXd rotation ); - -Eigen::MatrixXd quaternion2rpy( Eigen::Quaterniond quaternion ); -Eigen::MatrixXd quaternion2rotation( Eigen::Quaterniond quaternion ); - -Eigen::MatrixXd rotation4d( double roll, double pitch, double yaw ); - -Eigen::MatrixXd hatto( Eigen::MatrixXd matrix3d ); -Eigen::MatrixXd Rodrigues( Eigen::MatrixXd hat_matrix , double angle ); -Eigen::MatrixXd rot2omega(Eigen::MatrixXd rotation ); -Eigen::MatrixXd cross(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b ); -double dot(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b ); - -} - - - -#endif /* ROBOTIS_LINEAR_ALGEBRA_H_ */ diff --git a/robotis_math/include/robotis_math/RobotisMath.h b/robotis_math/include/robotis_math/RobotisMath.h deleted file mode 100644 index 752038d..0000000 --- a/robotis_math/include/robotis_math/RobotisMath.h +++ /dev/null @@ -1,13 +0,0 @@ -/* - * robotis_math.h - * - * Created on: Mar 18, 2016 - * Author: jay - */ - -#ifndef ROBOTIS_MATH_H_ -#define ROBOTIS_MATH_H_ - -#include "RobotisTrajectoryCalculator.h" - -#endif /* ROBOTIS_MATH_H_ */ diff --git a/robotis_math/include/robotis_math/RobotisMathBase.h b/robotis_math/include/robotis_math/RobotisMathBase.h deleted file mode 100644 index 7b7c713..0000000 --- a/robotis_math/include/robotis_math/RobotisMathBase.h +++ /dev/null @@ -1,33 +0,0 @@ -/* - * RobotisMathBase.h - * - * Created on: 2016. 3. 28. - * Author: JaySong - */ - -#ifndef ROBOTIS_MATH_BASE_H_ -#define ROBOTIS_MATH_BASE_H_ - -#include - -namespace ROBOTIS -{ - -#define PRINT_VAR(X) std::cout << #X << " : " << X << std::endl -#define PRINT_MAT(X) std::cout << #X << ":\n" << X << std::endl << std::endl - -#define deg2rad (M_PI / 180.0) -#define rad2deg (180.0 / M_PI) - -inline double powDI(double a, int b) -{ - return (b == 0 ? 1 : (b > 0 ? a * powDI(a, b - 1) : 1 / powDI(a, -b))); -} - -double sign( double x ); - -} - - - -#endif /* ROBOTIS_MATH_BASE_H_ */ diff --git a/robotis_math/include/robotis_math/RobotisTrajectoryCalculator.h b/robotis_math/include/robotis_math/RobotisTrajectoryCalculator.h deleted file mode 100644 index 3d1ff70..0000000 --- a/robotis_math/include/robotis_math/RobotisTrajectoryCalculator.h +++ /dev/null @@ -1,37 +0,0 @@ -/* - * RobotisTrajectoryCalculator.h - * - * Created on: Mar 18, 2016 - * Author: jay - */ - -#ifndef ROBOTIS_TRAJECTORY_CALCULATOR_H_ -#define ROBOTIS_TRAJECTORY_CALCULATOR_H_ - - -#include "RobotisMathBase.h" -#include "RobotisLinearAlgebra.h" - -// minimum jerk trajectory - -namespace ROBOTIS -{ - -Eigen::MatrixXd minimum_jerk_tra( double pos_start , double vel_start , double accel_start, - double pos_end , double vel_end , double accel_end, - double smp_time , double mov_time ); - -Eigen::MatrixXd minimum_jerk_tra_via_n_qdqddq( int via_num, - double pos_start , double vel_start , double accel_start , - Eigen::MatrixXd pos_via, Eigen::MatrixXd vel_via, Eigen::MatrixXd accel_via, - double pos_end, double vel_end, double accel_end, - double smp_time, Eigen::MatrixXd via_time, double mov_time ); - -Eigen::MatrixXd arc3d_tra( double smp_time, double mov_time, - Eigen::MatrixXd center_point, Eigen::MatrixXd normal_vector, Eigen::MatrixXd start_point, - double rotation_angle, double cross_ratio ); - -} - - -#endif /* ROBOTIS_TRAJECTORY_CALCULATOR_H_ */ diff --git a/robotis_math/package.xml b/robotis_math/package.xml deleted file mode 100644 index 506f261..0000000 --- a/robotis_math/package.xml +++ /dev/null @@ -1,37 +0,0 @@ - - - robotis_math - 0.0.0 - The robotis_math package - - - - - jay - - - - - - BSD - - - - - - - - - - - - - - catkin - roscpp - cmake_modules - - roscpp - cmake_modules - - \ No newline at end of file diff --git a/robotis_math/src/RobotisLinearAlgebra.cpp b/robotis_math/src/RobotisLinearAlgebra.cpp deleted file mode 100644 index 30bb5ce..0000000 --- a/robotis_math/src/RobotisLinearAlgebra.cpp +++ /dev/null @@ -1,291 +0,0 @@ -/* - * RobotisLinearAlgebra.cpp - * - * Created on: Mar 18, 2016 - * Author: jay - */ - - -#include "robotis_math/RobotisLinearAlgebra.h" - -namespace ROBOTIS -{ - -Eigen::MatrixXd transitionXYZ ( double position_x, double position_y, double position_z ) -{ - Eigen::MatrixXd _position(3,1); - - _position << position_x, - position_y, - position_z; - - return _position; -} - -Eigen::MatrixXd transformationXYZRPY ( double position_x, double position_y, double position_z , double roll , double pitch , double yaw ) -{ -// Eigen::MatrixXd _position(3,1); -// -// _position << position_x, -// position_y, -// position_z; -// -// Eigen::MatrixXd _rotation = rpy2rotation( roll , pitch , yaw ); -// -// Eigen::MatrixXd _transformation(4,4); -// -// _transformation << _rotation , _position, -// 0 , 0 , 0 , 1; - - Eigen::MatrixXd _transformation = rotation4d(roll, pitch, yaw); - _transformation.coeffRef(0,3) = position_x; - _transformation.coeffRef(1,3) = position_y; - _transformation.coeffRef(2,3) = position_z; - - return _transformation; -} - -Eigen::MatrixXd InverseTransformation(Eigen::MatrixXd transform) -{ - Eigen::Vector3d vecBOA; //If T is Transform Matrix A from B, the BOA is translation component coordi. B to coordi. A - Eigen::Vector3d vec_x, vec_y, vec_z; - Eigen::MatrixXd _invT(4,4); - - vecBOA.coeffRef(0) = -transform(0,3); vecBOA(1) = -transform(1,3); vecBOA(2) = -transform(2,3); - vec_x(0) = transform(0,0); vec_x(1) = transform(1,0); vec_x(2) = transform(2,0); - vec_y(0) = transform(0,1); vec_y(1) = transform(1,1); vec_y(2) = transform(2,1); - vec_z(0) = transform(0,2); vec_z(1) = transform(1,2); vec_z(2) = transform(2,2); -// -// -// // inv = [ x' | -AtoB??x ] -// // [ y' | -AtoB??y ] -// // [ z' | -AtoB??z ] -// // [ 0 0 0 | 1 ] -// - _invT << vec_x(0), vec_x(1), vec_x(2), vecBOA.dot(vec_x), - vec_y(0), vec_y(1), vec_y(2), vecBOA.dot(vec_y), - vec_z(0), vec_z(1), vec_z(2), vecBOA.dot(vec_z), - 0, 0, 0, 1; - - return _invT; -} - -Eigen::MatrixXd inertiaXYZ( double ixx, double ixy, double ixz , double iyy , double iyz, double izz ) -{ - Eigen::MatrixXd _inertia(3,3); - - _inertia << ixx , ixy , ixz, - ixy , iyy , iyz, - ixz , iyz , izz ; - - return _inertia; -} - -Eigen::MatrixXd rotationX( double angle ) -{ - Eigen::MatrixXd _rotation( 3 , 3 ); - - _rotation << 1.0, 0.0, 0.0, - 0.0, cos( angle ), -sin( angle ), - 0.0, sin( angle ), cos( angle ); - - return _rotation; -} - -Eigen::MatrixXd rotationY( double angle ) -{ - Eigen::MatrixXd _rotation( 3 , 3 ); - - _rotation << cos( angle ), 0.0, sin( angle ), - 0.0, 1.0, 0.0, - -sin( angle ), 0.0, cos( angle ); - - return _rotation; -} - -Eigen::MatrixXd rotationZ( double angle ) -{ - Eigen::MatrixXd _rotation(3,3); - - _rotation << cos( angle ), -sin( angle ), 0.0, - sin( angle ), cos( angle ), 0.0, - 0.0, 0.0, 1.0; - - return _rotation; -} - -Eigen::MatrixXd rotation2rpy( Eigen::MatrixXd rotation ) -{ - Eigen::MatrixXd _rpy = Eigen::MatrixXd::Zero( 3 , 1 ); - - _rpy.coeffRef( 0 , 0 ) = atan2( rotation.coeff( 2 , 1 ), rotation.coeff( 2 , 2 ) ); - _rpy.coeffRef( 1 , 0 ) = atan2( -rotation.coeff( 2 , 0 ), sqrt( pow( rotation.coeff( 2 , 1 ) , 2 ) + pow( rotation.coeff( 2 , 2 ) , 2 ) ) ); - _rpy.coeffRef( 2 , 0 ) = atan2 ( rotation.coeff( 1 , 0 ) , rotation.coeff( 0 , 0 ) ); - - return _rpy; -} - -Eigen::MatrixXd rpy2rotation( double roll, double pitch, double yaw ) -{ - Eigen::MatrixXd _rotation = rotationZ( yaw ) * rotationY( pitch ) * rotationX( roll ); - - return _rotation; -} - -Eigen::Quaterniond rpy2quaternion( double roll, double pitch, double yaw ) -{ - Eigen::MatrixXd _rotation = rpy2rotation( roll, pitch, yaw ); - - Eigen::Matrix3d _rotation3d; - _rotation3d = _rotation.block<3,3>( 0 , 0); - - Eigen::Quaterniond _quaternion; - - _quaternion = _rotation3d; - - return _quaternion; -} - -Eigen::Quaterniond rotation2quaternion( Eigen::MatrixXd rotation ) -{ - Eigen::Matrix3d _rotation3d; - - _rotation3d = rotation.block<3,3>( 0 , 0); - - Eigen::Quaterniond _quaternion; - _quaternion = _rotation3d; - - return _quaternion; -} - -Eigen::MatrixXd quaternion2rpy( Eigen::Quaterniond quaternion ) -{ - Eigen::MatrixXd _rpy = rotation2rpy( quaternion.toRotationMatrix() ); - - return _rpy; -} - -Eigen::MatrixXd quaternion2rotation( Eigen::Quaterniond quaternion ) -{ - Eigen::MatrixXd _rotation = quaternion.toRotationMatrix(); - - return _rotation; -} - -Eigen::MatrixXd rotation4d( double roll, double pitch, double yaw ) -{ -// Eigen::MatrixXd _rotation4d = Eigen::MatrixXd::Zero( 4 , 4 ); -// _rotation4d.coeffRef( 3 , 3 ) = 1.0; -// -// Eigen::MatrixXd _rotation = rotationZ( yaw ) * rotationY( pitch ) * rotationX( roll ); -// -//// _rotation4d.block<3,3>(0,0) = _rotation; -// _rotation4d.coeffRef(0,0) = _rotation.coeff(0,0); -// _rotation4d.coeffRef(0,1) = _rotation.coeff(0,1); -// _rotation4d.coeffRef(0,2) = _rotation.coeff(0,2); -// _rotation4d.coeffRef(1,0) = _rotation.coeff(1,0); -// _rotation4d.coeffRef(1,1) = _rotation.coeff(1,1); -// _rotation4d.coeffRef(1,2) = _rotation.coeff(1,2); -// _rotation4d.coeffRef(2,0) = _rotation.coeff(2,0); -// _rotation4d.coeffRef(2,1) = _rotation.coeff(2,1); -// _rotation4d.coeffRef(2,2) = _rotation.coeff(2,2); - -// return _rotation4d; - double sr = sin(roll), cr = cos(roll); - double sp = sin(pitch), cp = cos(pitch); - double sy = sin(yaw), cy = cos(yaw); - - Eigen::MatrixXd matRoll(4,4); - Eigen::MatrixXd matPitch(4,4); - Eigen::MatrixXd matYaw(4,4); - - matRoll << 1, 0, 0, 0, - 0, cr, -sr, 0, - 0, sr, cr, 0, - 0, 0, 0, 1; - - matPitch << cp, 0, sp, 0, - 0, 1, 0, 0, - -sp, 0, cp, 0, - 0, 0, 0, 1; - - matYaw << cy, -sy, 0, 0, - sy, cy, 0, 0, - 0, 0, 1, 0, - 0, 0, 0, 1; - - - return (matYaw*matPitch)*matRoll; -} - - - -Eigen::MatrixXd hatto( Eigen::MatrixXd matrix3d ) -{ - Eigen::MatrixXd _hatto(3,3); - - _hatto << 0.0, -matrix3d.coeff(2,0), matrix3d.coeff(1,0), - matrix3d.coeff(2,0), 0.0, -matrix3d.coeff(0,0), - -matrix3d.coeff(1,0), matrix3d.coeff(0,0), 0.0; - - return _hatto; -} - -Eigen::MatrixXd Rodrigues( Eigen::MatrixXd hat_matrix , double angle ) -{ - Eigen::MatrixXd _E = Eigen::MatrixXd::Identity( 3 , 3 ); - Eigen::MatrixXd _Rodrigues = _E + hat_matrix * sin( angle ) + hat_matrix * hat_matrix * ( 1 - cos( angle ) ); - - return _Rodrigues; -} - -Eigen::MatrixXd rot2omega( Eigen::MatrixXd rotation ) -{ - double _eps = 1e-12; - -// Eigen::MatrixXd _E = Eigen::MatrixXd::Identity( 3 , 3 ); - - double _alpha = ( rotation.coeff( 0 , 0 ) + rotation.coeff( 1 , 1 ) + rotation.coeff( 2 , 2 ) - 1.0 ) / 2.0 ; - - double _alpha_plus = fabs( _alpha - 1.0 ); - - Eigen::MatrixXd _rot2omega( 3 , 1 ); - - if( _alpha_plus < _eps ) - { - _rot2omega << 0.0, - 0.0, - 0.0; - } - else - { - double _theta = acos( _alpha ); - - _rot2omega << rotation.coeff( 2 , 1 ) - rotation.coeff( 1 , 2 ), - rotation.coeff( 0 , 2 ) - rotation.coeff( 2 , 0 ), - rotation.coeff( 1 , 0 ) - rotation.coeff( 0 , 1 ); - - _rot2omega = 0.5 * ( _theta / sin( _theta ) ) * _rot2omega; - } - - return _rot2omega; -} - -Eigen::MatrixXd cross(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b ) -{ - Eigen::MatrixXd _cross( 3 , 1 ); - - _cross << vector3d_a.coeff( 1 , 0 ) * vector3d_b.coeff( 2 , 0 ) - vector3d_a.coeff( 2 , 0 ) * vector3d_b.coeff( 1 , 0 ), - vector3d_a.coeff( 2 , 0 ) * vector3d_b.coeff( 0 , 0 ) - vector3d_a.coeff( 0 , 0 ) * vector3d_b.coeff( 2 , 0 ), - vector3d_a.coeff( 0 , 0 ) * vector3d_b.coeff( 1 , 0 ) - vector3d_a.coeff( 1 , 0 ) * vector3d_b.coeff( 0 , 0 ); - - return _cross; -} - -double dot(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b ) -{ - return vector3d_a.dot(vector3d_b); - //return vector3d_a.coeff(0,0)*vector3d_b.coeff(0,0) + vector3d_a.coeff(1,0)*vector3d_b.coeff(1,0) + vector3d_a.coeff(2,0)*vector3d_b.coeff(2,0); -} - -} diff --git a/robotis_math/src/RobotisMathBase.cpp b/robotis_math/src/RobotisMathBase.cpp deleted file mode 100644 index 0afb845..0000000 --- a/robotis_math/src/RobotisMathBase.cpp +++ /dev/null @@ -1,26 +0,0 @@ -/* - * RobotisMathBase.cpp - * - * Created on: Mar 18, 2016 - * Author: jay - */ - -#include "robotis_math/RobotisMathBase.h" - - - - -namespace ROBOTIS -{ - -double sign( double x ) -{ - if ( x < 0.0 ) - return -1.0; - else if ( x > 0.0) - return 1.0; - else - return 0.0; -} - -} diff --git a/robotis_math/src/RobotisTrajectoryCalculator.cpp b/robotis_math/src/RobotisTrajectoryCalculator.cpp deleted file mode 100644 index 665015b..0000000 --- a/robotis_math/src/RobotisTrajectoryCalculator.cpp +++ /dev/null @@ -1,325 +0,0 @@ -/* - * RobotisTrajectoryCalculator.cpp - * - * Created on: Mar 18, 2016 - * Author: jay - */ - - - -#include "robotis_math/RobotisTrajectoryCalculator.h" - - -/* - * trajectory.cpp - * - * Created on: Jul 13, 2015 - * Author: sch - */ - - - -namespace ROBOTIS -{ - -Eigen::MatrixXd minimum_jerk_tra( double pos_start , double vel_start , double accel_start, - double pos_end , double vel_end , double accel_end, - double smp_time , double mov_time ) -/* - simple minimum jerk trajectory - - pos_start : position at initial state - vel_start : velocity at initial state - accel_start : acceleration at initial state - - pos_end : position at final state - vel_end : velocity at final state - accel_end : acceleration at final state - - smp_time : sampling time - - mov_time : movement time -*/ - -{ - Eigen::MatrixXd ___C( 3 , 3 ); - Eigen::MatrixXd __C( 3 , 1 ); - - ___C << pow( mov_time , 3 ) , pow( mov_time , 4 ) , pow( mov_time , 5 ), - 3 * pow( mov_time , 2 ) , 4 * pow( mov_time , 3 ) , 5 * pow( mov_time , 4 ), - 6 * mov_time , 12 * pow( mov_time , 2 ) , 20 * pow( mov_time , 3 ); - - __C << pos_end - pos_start - vel_start * mov_time - accel_start * pow( mov_time , 2 ) / 2, - vel_end - vel_start - accel_start * mov_time, - accel_end - accel_start ; - - Eigen::Matrix _A = ___C.inverse() * __C; - - double _time_steps; - - _time_steps = mov_time / smp_time; - int __time_steps = round( _time_steps + 1 ); - - Eigen::MatrixXd _time = Eigen::MatrixXd::Zero( __time_steps , 1 ); - Eigen::MatrixXd _tra = Eigen::MatrixXd::Zero( __time_steps , 1 ); - - for ( int step = 0; step < __time_steps; step++ ) - _time.coeffRef( step , 0 ) = step * smp_time; - - for ( int step = 0; step < __time_steps; step++ ) - { - _tra.coeffRef( step , 0 ) = - pos_start + - vel_start * _time.coeff( step , 0 ) + - 0.5 * accel_start * pow( _time.coeff( step , 0 ) , 2 ) + - _A.coeff( 0 , 0 ) * pow( _time.coeff( step , 0 ) , 3 ) + - _A.coeff( 1 , 0 ) * pow( _time.coeff( step , 0 ) , 4 ) + - _A.coeff( 2 , 0 ) * pow( _time.coeff( step , 0 ) , 5 ); - } - - return _tra; -} - -Eigen::MatrixXd minimum_jerk_tra_via_n_qdqddq( int via_num, - double pos_start , double vel_start , double accel_start , - Eigen::MatrixXd pos_via, Eigen::MatrixXd vel_via, Eigen::MatrixXd accel_via, - double pos_end, double vel_end, double accel_end, - double smp_time, Eigen::MatrixXd via_time, double mov_time ) -/* - minimum jerk trajectory with via-points - (via-point constraints: position and velocity at each point) - - n : the number of via-points - - x0 : position at initial state - v0 : velocity at initial state - a0 : acceleration at initial state - - x : position matrix at via-points state ( size : n x 1 ) - dx : velocity matrix at via-points state ( size : n x 1 ) - ddx : acceleration matrix at via-points state ( size : n x 1 ) - - xf : position at final state - vf : velocity at final state - af : acceleration at final state - - smp : sampling time - - t : time matrix passing through via-points state ( size : n x 1 ) - tf : movement time -*/ - -{ - int i,j,k ; - - /* Calculation Matrix B */ - - Eigen::MatrixXd B = Eigen::MatrixXd::Zero(3*via_num+3,1); - - for (i=1; i<=via_num; i++){ - B.coeffRef(3*i-3,0) = - pos_via.coeff(i-1,0) - - pos_start - - vel_start*via_time.coeff(i-1,0) - - (accel_start/2)*pow(via_time.coeff(i-1,0),2) ; - - B.coeffRef(3*i-2,0) = - vel_via.coeff(i-1,0) - - vel_start - - accel_start*via_time.coeff(i-1,0) ; - - B.coeffRef(3*i-1,0) = - accel_via.coeff(i-1,0) - - accel_start ; - } - - B.coeffRef(3*via_num,0) = - pos_end - - pos_start - - vel_start*mov_time - - (accel_start/2)*pow(mov_time,2) ; - - B.coeffRef(3*via_num+1,0) = - vel_end - - vel_start - - accel_start*mov_time ; - - B.coeffRef(3*via_num+2,0) = - accel_end - - accel_start ; - - - /* Calculation Matrix A */ - - Eigen::MatrixXd A1 = Eigen::MatrixXd::Zero(3*via_num,3); - - for (i=1; i<=via_num; i++){ - A1.coeffRef(3*i-3,0) = pow(via_time.coeff(i-1,0),3) ; - A1.coeffRef(3*i-3,1) = pow(via_time.coeff(i-1,0),4) ; - A1.coeffRef(3*i-3,2) = pow(via_time.coeff(i-1,0),5) ; - - A1.coeffRef(3*i-2,0) = 3*pow(via_time.coeff(i-1,0),2) ; - A1.coeffRef(3*i-2,1) = 4*pow(via_time.coeff(i-1,0),3) ; - A1.coeffRef(3*i-2,2) = 5*pow(via_time.coeff(i-1,0),4) ; - - A1.coeffRef(3*i-1,0) = 6*via_time.coeff(i-1,0) ; - A1.coeffRef(3*i-1,1) = 12*pow(via_time.coeff(i-1,0),2) ; - A1.coeffRef(3*i-1,2) = 20*pow(via_time.coeff(i-1,0),3) ; - } - - - Eigen::MatrixXd A2 = Eigen::MatrixXd::Zero(3*via_num,3*via_num); - - for (i=1; i<=via_num; i++){ - for (j=1; j<=via_num; j++){ - if (i > j){ - k = i ; - }else{ - k = j ; - } - A2.coeffRef(3*j-3,3*i-3) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),3)/6 ; - A2.coeffRef(3*j-3,3*i-2) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),4)/24 ; - A2.coeffRef(3*j-3,3*i-1) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),5)/120 ; - - A2.coeffRef(3*j-2,3*i-3) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),2)/2 ; - A2.coeffRef(3*j-2,3*i-2) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),3)/6 ; - A2.coeffRef(3*j-2,3*i-1) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),4)/24 ; - - A2.coeffRef(3*j-1,3*i-3) = via_time.coeff(k-1,0)-via_time.coeff(i-1,0) ; - A2.coeffRef(3*j-1,3*i-2) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),2)/2 ; - A2.coeffRef(3*j-1,3*i-1) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),3)/6 ; - } - } - - - Eigen::MatrixXd A3 = Eigen::MatrixXd::Zero(3,3*via_num+3); - - A3.coeffRef(0,0) = pow(mov_time,3); - A3.coeffRef(0,1) = pow(mov_time,4); - A3.coeffRef(0,2) = pow(mov_time,5); - - A3.coeffRef(1,0) = 3*pow(mov_time,2); - A3.coeffRef(1,1) = 4*pow(mov_time,3); - A3.coeffRef(1,2) = 5*pow(mov_time,4); - - A3.coeffRef(2,0) = 6*mov_time; - A3.coeffRef(2,1) = 12*pow(mov_time,2); - A3.coeffRef(2,2) = 20*pow(mov_time,3); - - for (i=1; i<=via_num; i++){ - A3.coeffRef(0,3*i) = pow(mov_time-via_time.coeff(i-1,0),3)/6 ; - A3.coeffRef(1,3*i) = pow(mov_time-via_time.coeff(i-1,0),2)/2 ; - A3.coeffRef(2,3*i) = mov_time-via_time.coeff(i-1,0) ; - - A3.coeffRef(0,3*i+1) = pow(mov_time-via_time.coeff(i-1,0),4)/24 ; - A3.coeffRef(1,3*i+1) = pow(mov_time-via_time.coeff(i-1,0),3)/6 ; - A3.coeffRef(2,3*i+1) = pow(mov_time-via_time.coeff(i-1,0),2)/2 ; - - A3.coeffRef(0,3*i+2) = pow(mov_time-via_time.coeff(i-1,0),5)/120 ; - A3.coeffRef(1,3*i+2) = pow(mov_time-via_time.coeff(i-1,0),4)/24 ; - A3.coeffRef(2,3*i+2) = pow(mov_time-via_time.coeff(i-1,0),3)/6 ; - } - - Eigen::MatrixXd A = Eigen::MatrixXd::Zero(3*via_num+3,3*via_num+3); - - A.block(0,0,3*via_num,3) = A1 ; - A.block(0,3,3*via_num,3*via_num) = A2 ; - A.block(3*via_num,0,3,3*via_num+3) = A3 ; - - /* Calculation Matrix C (coefficient of polynomial function) */ - - Eigen::MatrixXd C(3*via_num+3,1); - //C = A.inverse()*B; - C = A.colPivHouseholderQr().solve(B); - - /* Time */ - - int NN; - double N; - - N = mov_time/smp_time ; - NN = round(N) ; - - Eigen::MatrixXd Time = Eigen::MatrixXd::Zero(NN+1,1); - - for (i=1; i<=NN+1; i++){ - Time.coeffRef(i-1,0) = (i-1)*smp_time; - } - - /* Time_via */ - - Eigen::MatrixXd Time_via = Eigen::MatrixXd::Zero(via_num,1); - - for (i=1; i<=via_num; i++){ - Time_via.coeffRef(i-1,0) = round(via_time.coeff(i-1,0)/smp_time)+2; - } - - /* Minimum Jerk Trajectory with Via-points */ - - Eigen::MatrixXd _tra_jerk_via = Eigen::MatrixXd::Zero(NN+1,1); - - for (i=1; i<=NN+1; i++){ - _tra_jerk_via.coeffRef(i-1,0) = - pos_start + - vel_start*Time.coeff(i-1,0) + - 0.5*accel_start*pow(Time.coeff(i-1,0),2) + - C.coeff(0,0)*pow(Time.coeff(i-1,0),3) + - C.coeff(1,0)*pow(Time.coeff(i-1,0),4) + - C.coeff(2,0)*pow(Time.coeff(i-1,0),5) ; - } - - for (i=1; i<=via_num; i++){ - for (j=Time_via.coeff(i-1,0); j<=NN+1; j++){ - _tra_jerk_via.coeffRef(j-1,0) = - _tra_jerk_via.coeff(j-1,0) + - C.coeff(3*i,0)*pow((Time.coeff(j-1,0)-via_time.coeff(i-1,0)),3)/6 + - C.coeff(3*i+1,0)*pow((Time.coeff(j-1,0)-via_time.coeff(i-1,0)),4)/24 + - C.coeff(3*i+2,0)*pow((Time.coeff(j-1,0)-via_time.coeff(i-1,0)),5)/120 ; - - } - } - - return _tra_jerk_via; - -} - -Eigen::MatrixXd arc3d_tra( double smp_time, double mov_time, - Eigen::MatrixXd center_point, Eigen::MatrixXd normal_vector, Eigen::MatrixXd start_point, - double rotation_angle, double cross_ratio ) -{ - int _all_time_steps = int ( round( mov_time / smp_time ) ) + 1 ; - - Eigen::MatrixXd _angle_tra = minimum_jerk_tra( 0.0 , 0.0 , 0.0 , - rotation_angle , 0.0 , 0.0 , - smp_time , mov_time ); - - Eigen::MatrixXd _pt = Eigen::MatrixXd::Zero( 3 , _all_time_steps ); - - for (int i = 0; i < _all_time_steps; i++ ) - { - double _t = ( ( double ) i ) * smp_time ; - - double _th = _angle_tra.coeff( i , 0 );//( _t / mov_time ) * rotation_angle; - - Eigen::MatrixXd _w_wedge( 3 , 3 ); - - _w_wedge << 0.0 , -normal_vector.coeff(2,0), normal_vector.coeff(1,0), - normal_vector.coeff(2,0), 0.0 , -normal_vector.coeff(0,0), - -normal_vector.coeff(1,0), normal_vector.coeff(0,0), 0.0 ; - - Eigen::MatrixXd _E = Eigen::MatrixXd::Identity( 3 , 3 ); - - Eigen::MatrixXd _R = _E + _w_wedge * sin( _th ) + _w_wedge * _w_wedge * ( 1 - cos( _th ) ); - - double _cross = cross_ratio * ( 1.0 - 2.0 * ( abs ( 0.5 - _t/mov_time ) ) ); - - _pt.block( 0 , i , 3 , 1 ) = ( 1 + _cross ) * ( _R * ( start_point - center_point ) ) + center_point; - } - - Eigen::MatrixXd _pt_trans = _pt.transpose(); - - return _pt_trans; -} - - -} From 6f188fc1ffa86c643b24435159e3e8ac4e081b94 Mon Sep 17 00:00:00 2001 From: pyo Date: Fri, 12 Aug 2016 23:21:40 +0900 Subject: [PATCH 04/13] added a meta-package --- robotis_framework/CMakeLists.txt | 4 ++++ robotis_framework/package.xml | 17 +++++++++++++++++ 2 files changed, 21 insertions(+) create mode 100644 robotis_framework/CMakeLists.txt create mode 100644 robotis_framework/package.xml diff --git a/robotis_framework/CMakeLists.txt b/robotis_framework/CMakeLists.txt new file mode 100644 index 0000000..d336fe9 --- /dev/null +++ b/robotis_framework/CMakeLists.txt @@ -0,0 +1,4 @@ +cmake_minimum_required(VERSION 2.8.3) +project(robotis_framework) +find_package(catkin REQUIRED) +catkin_metapackage() diff --git a/robotis_framework/package.xml b/robotis_framework/package.xml new file mode 100644 index 0000000..170f107 --- /dev/null +++ b/robotis_framework/package.xml @@ -0,0 +1,17 @@ + + + robotis_framework + 0.1.0 + ROS packages for the robotis_framework (meta package) + BSD + zerom + pyo + https://github.com/ROBOTIS-GIT/ROBOTIS-Framework/issues + https://github.com/ROBOTIS-GIT/ROBOTIS-Framework + http://wiki.ros.org/robotis_framework + catkin + robotis_framework_common + robotis_device + robotis_controller + + From caf4884f0517d41db2189b089fa70032c9045e0e Mon Sep 17 00:00:00 2001 From: pyo Date: Fri, 12 Aug 2016 23:31:04 +0900 Subject: [PATCH 05/13] modified the package information for release --- robotis_controller/CMakeLists.txt | 38 ++++++++++++++++++++- robotis_controller/package.xml | 17 ++++------ robotis_device/CMakeLists.txt | 44 ++++++++++++++++++++++--- robotis_framework_common/CMakeLists.txt | 33 +++++++++++++++++-- robotis_framework_common/package.xml | 9 ++--- 5 files changed, 116 insertions(+), 25 deletions(-) diff --git a/robotis_controller/CMakeLists.txt b/robotis_controller/CMakeLists.txt index 97c4a6d..1d94330 100644 --- a/robotis_controller/CMakeLists.txt +++ b/robotis_controller/CMakeLists.txt @@ -1,8 +1,14 @@ +################################################################################ +# CMake +################################################################################ cmake_minimum_required(VERSION 2.8.3) project(robotis_controller) set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}") +################################################################################ +# Packages +################################################################################ find_package(catkin REQUIRED COMPONENTS roscpp roslib @@ -15,13 +21,26 @@ find_package(catkin REQUIRED COMPONENTS dynamixel_sdk ) +################################################################################ +# Declare ROS messages, services and actions +################################################################################ + +################################################################################ +# Declare ROS dynamic reconfigure parameters +################################################################################ + +################################################################################ +# Catkin specific configuration +################################################################################ catkin_package( INCLUDE_DIRS include LIBRARIES robotis_controller CATKIN_DEPENDS roscpp roslib sensor_msgs std_msgs -# DEPENDS system_lib ) +################################################################################ +# Build +################################################################################ include_directories( include ${catkin_INCLUDE_DIRS} @@ -35,3 +54,20 @@ target_link_libraries(robotis_controller yaml-cpp ${catkin_LIBRARIES} ) + +################################################################################ +# Install +################################################################################ +install(TARGETS robotis_controller + 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} +) + +################################################################################ +# Test +################################################################################ diff --git a/robotis_controller/package.xml b/robotis_controller/package.xml index 6dcf6d2..1320cf6 100644 --- a/robotis_controller/package.xml +++ b/robotis_controller/package.xml @@ -3,17 +3,13 @@ robotis_controller 0.1.1 The robotis_controller package - - ROBOTIS - BSD - - - - ROBOTIS - + zerom + pyo + https://github.com/ROBOTIS-GIT/ROBOTIS-Framework/issues + https://github.com/ROBOTIS-GIT/ROBOTIS-Framework + http://wiki.ros.org/robotis_controller catkin - roscpp roslib std_msgs @@ -22,7 +18,6 @@ robotis_device robotis_controller_msgs robotis_framework_common - roscpp roslib std_msgs @@ -30,5 +25,5 @@ dynamixel_sdk robotis_device robotis_controller_msgs - + diff --git a/robotis_device/CMakeLists.txt b/robotis_device/CMakeLists.txt index e492450..abc1965 100644 --- a/robotis_device/CMakeLists.txt +++ b/robotis_device/CMakeLists.txt @@ -1,19 +1,38 @@ +################################################################################ +# CMake +################################################################################ cmake_minimum_required(VERSION 2.8.3) project(robotis_device) +################################################################################ +# Packages +################################################################################ find_package(catkin REQUIRED COMPONENTS roscpp rospy dynamixel_sdk ) +################################################################################ +# Declare ROS messages, services and actions +################################################################################ + +################################################################################ +# Declare ROS dynamic reconfigure parameters +################################################################################ + +################################################################################ +# Catkin specific configuration +################################################################################ catkin_package( INCLUDE_DIRS include LIBRARIES robotis_device CATKIN_DEPENDS roscpp rospy -# DEPENDS system_lib ) +################################################################################ +# Build +################################################################################ include_directories( include ${catkin_INCLUDE_DIRS} @@ -24,9 +43,26 @@ add_library(robotis_device src/robotis_device/sensor.cpp src/robotis_device/dynamixel.cpp ) - add_dependencies(robotis_device ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(robotis_device ${catkin_LIBRARIES}) -target_link_libraries(robotis_device - ${catkin_LIBRARIES} +################################################################################ +# Install +################################################################################ +install(TARGETS robotis_device + 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} +) + +install(DIRECTORY devices/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +################################################################################ +# Test +################################################################################ diff --git a/robotis_framework_common/CMakeLists.txt b/robotis_framework_common/CMakeLists.txt index 51f0f3f..20c330f 100644 --- a/robotis_framework_common/CMakeLists.txt +++ b/robotis_framework_common/CMakeLists.txt @@ -1,18 +1,45 @@ +################################################################################ +# CMake +################################################################################ cmake_minimum_required(VERSION 2.8.3) project(robotis_framework_common) +################################################################################ +# Packages +################################################################################ find_package(catkin REQUIRED COMPONENTS roscpp ) +################################################################################ +# Declare ROS messages, services and actions +################################################################################ + +################################################################################ +# Declare ROS dynamic reconfigure parameters +################################################################################ + +################################################################################ +# Catkin specific configuration +################################################################################ catkin_package( INCLUDE_DIRS include -# LIBRARIES robotis_framework_common -# CATKIN_DEPENDS roscpp -# DEPENDS system_lib ) +################################################################################ +# Build +################################################################################ include_directories( ${catkin_INCLUDE_DIRS} ) +################################################################################ +# Install +################################################################################ +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +################################################################################ +# Test +################################################################################ diff --git a/robotis_framework_common/package.xml b/robotis_framework_common/package.xml index 820f4a2..aa42d57 100644 --- a/robotis_framework_common/package.xml +++ b/robotis_framework_common/package.xml @@ -3,13 +3,10 @@ robotis_framework_common 0.1.0 The robotis_framework_common package - robotis - BSD - - ROBOTIS - + zerom + pyo catkin roscpp roscpp - \ No newline at end of file + From b6fd3fc96ab820932a33bd4ba5e2d421c033500c Mon Sep 17 00:00:00 2001 From: pyo Date: Fri, 12 Aug 2016 23:37:57 +0900 Subject: [PATCH 06/13] made and modified the CHANGELOG.rst for release --- robotis_controller/CHANGELOG.rst | 21 +++++++++++++++++++++ robotis_device/CHANGELOG.rst | 20 ++++++++++++++++++++ robotis_framework/CHANGELOG.rst | 8 ++++++++ robotis_framework_common/CHANGELOG.rst | 12 ++++++++++++ 4 files changed, 61 insertions(+) create mode 100644 robotis_controller/CHANGELOG.rst create mode 100644 robotis_device/CHANGELOG.rst create mode 100644 robotis_framework/CHANGELOG.rst create mode 100644 robotis_framework_common/CHANGELOG.rst diff --git a/robotis_controller/CHANGELOG.rst b/robotis_controller/CHANGELOG.rst new file mode 100644 index 0000000..74299e5 --- /dev/null +++ b/robotis_controller/CHANGELOG.rst @@ -0,0 +1,21 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package robotis_controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.1.1 (2016-08-12) +----------- +* first public release for Kinetic +* modified the package information for release +* develop branch -> master branch +* Merge pull request `#4 `_ from ROBOTIS-GIT/add_sensor_device + Add sensor device +* function name changed : DeviceInit() -> InitDevice() +* Fixed high CPU consumption due to busy waits +* add SensorState + add Singleton template +* XM-430 / CM-740 device file added. + Sensor device added. +* added code to support the gazebo simulator +* added first bulk read failure protection code +* renewal +* Contributors: Alexander Stumpf, ROBOTIS, ROBOTIS-zerom, pyo diff --git a/robotis_device/CHANGELOG.rst b/robotis_device/CHANGELOG.rst new file mode 100644 index 0000000..50c29fd --- /dev/null +++ b/robotis_device/CHANGELOG.rst @@ -0,0 +1,20 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package robotis_device +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.1.0 (2016-08-12) +----------- +* first public release for Kinetic +* modified the package information for release +* develop branch -> master branch +* Setting the license to BSD. +* add SensorState + add Singleton template +* XM-430 / CM-740 device file added. + Sensor device added. +* modified. +* variable name changed. + ConvertRadian2Value / ConvertValue2Radian function bug fixed. +* added code to support the gazebo simulator +* renewal +* Contributors: ROBOTIS, ROBOTIS-zerom, pyo diff --git a/robotis_framework/CHANGELOG.rst b/robotis_framework/CHANGELOG.rst new file mode 100644 index 0000000..353baf6 --- /dev/null +++ b/robotis_framework/CHANGELOG.rst @@ -0,0 +1,8 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package robotis_framework +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.1.0 (2016-08-12) +----------- +* make a meta-package +* Contributors: pyo diff --git a/robotis_framework_common/CHANGELOG.rst b/robotis_framework_common/CHANGELOG.rst new file mode 100644 index 0000000..7aacc38 --- /dev/null +++ b/robotis_framework_common/CHANGELOG.rst @@ -0,0 +1,12 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package robotis_framework_common +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.1.0 (2016-08-12) +----------- +* modified the package information for release +* develop branch -> master branch +* Setting the license to BSD. +* add SensorState + add Singleton template +* Contributors: ROBOTIS, ROBOTIS-zerom, pyo From a6c71b147905bea806273a5bea6fe60e3fd8b910 Mon Sep 17 00:00:00 2001 From: pyo Date: Fri, 12 Aug 2016 23:46:41 +0900 Subject: [PATCH 07/13] made and modified the CHANGELOG.rst for release --- robotis_controller/CHANGELOG.rst | 2 +- robotis_controller/package.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/robotis_controller/CHANGELOG.rst b/robotis_controller/CHANGELOG.rst index 74299e5..c81a9f1 100644 --- a/robotis_controller/CHANGELOG.rst +++ b/robotis_controller/CHANGELOG.rst @@ -2,7 +2,7 @@ Changelog for package robotis_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -0.1.1 (2016-08-12) +0.1.0 (2016-08-12) ----------- * first public release for Kinetic * modified the package information for release diff --git a/robotis_controller/package.xml b/robotis_controller/package.xml index 1320cf6..b80a81f 100644 --- a/robotis_controller/package.xml +++ b/robotis_controller/package.xml @@ -1,7 +1,7 @@ robotis_controller - 0.1.1 + 0.1.0 The robotis_controller package BSD zerom From 79126bfdc4f35a91cd481194228f438c534d2933 Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Tue, 16 Aug 2016 13:28:06 +0900 Subject: [PATCH 08/13] - package description modified. --- robotis_controller/package.xml | 4 +++- robotis_device/package.xml | 6 +++++- robotis_framework_common/package.xml | 4 +++- 3 files changed, 11 insertions(+), 3 deletions(-) diff --git a/robotis_controller/package.xml b/robotis_controller/package.xml index b80a81f..9fb1356 100644 --- a/robotis_controller/package.xml +++ b/robotis_controller/package.xml @@ -2,7 +2,9 @@ robotis_controller 0.1.0 - The robotis_controller package + + The main package that controls THORMANG3. + BSD zerom pyo diff --git a/robotis_device/package.xml b/robotis_device/package.xml index 6f7294a..3ea40fe 100644 --- a/robotis_device/package.xml +++ b/robotis_device/package.xml @@ -2,7 +2,11 @@ robotis_device 0.1.0 - The robotis_device package + + The package that manages device information of ROBOTIS robots. + This package is used when reading device information with the robot information file + from the robotis_controller package. + robotis diff --git a/robotis_framework_common/package.xml b/robotis_framework_common/package.xml index aa42d57..30c4bbb 100644 --- a/robotis_framework_common/package.xml +++ b/robotis_framework_common/package.xml @@ -2,7 +2,9 @@ robotis_framework_common 0.1.0 - The robotis_framework_common package + + The package contains commonly used Headers for the ROBOTIS Framework. + BSD zerom pyo From 71aa71ec187cc0ed0ca8294b8d194fd06d83afe7 Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Tue, 16 Aug 2016 15:57:47 +0900 Subject: [PATCH 09/13] - added direct_sync_write to MotionModuleMode - moved indirect address setting code from initialize() to initializeDevice() function. --- .../robotis_controller/robotis_controller.cpp | 229 ++++++++++-------- 1 file changed, 122 insertions(+), 107 deletions(-) diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index 35c81b1..1bea26e 100644 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -348,7 +348,119 @@ bool RobotisController::initialize(const std::string robot_file_path, const std: initializeDevice(init_file_path); + queue_thread_ = boost::thread(boost::bind(&RobotisController::msgQueueThread, this)); + return true; +} + +void RobotisController::initializeDevice(const std::string init_file_path) +{ + // device initialize + if (DEBUG_PRINT) + ROS_WARN("INIT FILE LOAD"); + + YAML::Node doc; + try + { + doc = YAML::LoadFile(init_file_path.c_str()); + + for (YAML::const_iterator it_doc = doc.begin(); it_doc != doc.end(); it_doc++) + { + std::string joint_name = it_doc->first.as(); + + YAML::Node joint_node = doc[joint_name]; + if (joint_node.size() == 0) + continue; + + Dynamixel *dxl = NULL; + auto dxl_it = robot_->dxls_.find(joint_name); + if (dxl_it != robot_->dxls_.end()) + dxl = dxl_it->second; + + if (dxl == NULL) + { + ROS_WARN("Joint [%s] was not found.", joint_name.c_str()); + continue; + } + if (DEBUG_PRINT) + ROS_INFO("JOINT_NAME: %s", joint_name.c_str()); + + for (YAML::const_iterator it_joint = joint_node.begin(); it_joint != joint_node.end(); it_joint++) + { + std::string item_name = it_joint->first.as(); + + if (DEBUG_PRINT) + ROS_INFO(" ITEM_NAME: %s", item_name.c_str()); + + uint32_t value = it_joint->second.as(); + + ControlTableItem *item = dxl->ctrl_table_[item_name]; + if (item == NULL) + { + ROS_WARN("Control Item [%s] was not found.", item_name.c_str()); + continue; + } + + if (item->memory_type_ == EEPROM) + { + uint8_t data8 = 0; + uint16_t data16 = 0; + uint32_t data32 = 0; + + switch (item->data_length_) + { + case 1: + read1Byte(joint_name, item->address_, &data8); + if (data8 == value) + continue; + break; + case 2: + read2Byte(joint_name, item->address_, &data16); + if (data16 == value) + continue; + break; + case 4: + read4Byte(joint_name, item->address_, &data32); + if (data32 == value) + continue; + break; + default: + break; + } + } + + switch (item->data_length_) + { + case 1: + write1Byte(joint_name, item->address_, (uint8_t) value); + break; + case 2: + write2Byte(joint_name, item->address_, (uint16_t) value); + break; + case 4: + write4Byte(joint_name, item->address_, value); + break; + default: + break; + } + + if (item->memory_type_ == EEPROM) + { + // Write to EEPROM -> delay is required (max delay: 55 msec per byte) + usleep(item->data_length_ * 55 * 1000); + } + } + } + } catch (const std::exception& e) + { + ROS_INFO("Dynamixel Init file not found."); + } + // [ BulkRead ] StartAddress : Present Position , Length : 10 ( Position/Velocity/Current ) + for (auto& it : robot_->ports_) + { + if (port_to_bulk_read_[it.first] != 0) + port_to_bulk_read_[it.first]->clearParam(); + } for (auto& it : robot_->dxls_) { std::string joint_name = it.first; @@ -487,113 +599,6 @@ bool RobotisController::initialize(const std::string robot_file_path, const std: if (bulkread_start_addr != 0) port_to_bulk_read_[sensor->port_name_]->addParam(sensor->id_, bulkread_start_addr, bulkread_data_length); } - - queue_thread_ = boost::thread(boost::bind(&RobotisController::msgQueueThread, this)); - return true; -} - -void RobotisController::initializeDevice(const std::string init_file_path) -{ - // device initialize - if (DEBUG_PRINT) - ROS_WARN("INIT FILE LOAD"); - - YAML::Node doc; - try - { - doc = YAML::LoadFile(init_file_path.c_str()); - - for (YAML::const_iterator it_doc = doc.begin(); it_doc != doc.end(); it_doc++) - { - std::string joint_name = it_doc->first.as(); - - YAML::Node joint_node = doc[joint_name]; - if (joint_node.size() == 0) - continue; - - Dynamixel *dxl = NULL; - auto dxl_it = robot_->dxls_.find(joint_name); - if (dxl_it != robot_->dxls_.end()) - dxl = dxl_it->second; - - if (dxl == NULL) - { - ROS_WARN("Joint [%s] was not found.", joint_name.c_str()); - continue; - } - if (DEBUG_PRINT) - ROS_INFO("JOINT_NAME: %s", joint_name.c_str()); - - for (YAML::const_iterator it_joint = joint_node.begin(); it_joint != joint_node.end(); it_joint++) - { - std::string item_name = it_joint->first.as(); - - if (DEBUG_PRINT) - ROS_INFO(" ITEM_NAME: %s", item_name.c_str()); - - uint32_t value = it_joint->second.as(); - - ControlTableItem *item = dxl->ctrl_table_[item_name]; - if (item == NULL) - { - ROS_WARN("Control Item [%s] was not found.", item_name.c_str()); - continue; - } - - if (item->memory_type_ == EEPROM) - { - uint8_t data8 = 0; - uint16_t data16 = 0; - uint32_t data32 = 0; - - switch (item->data_length_) - { - case 1: - read1Byte(joint_name, item->address_, &data8); - if (data8 == value) - continue; - break; - case 2: - read2Byte(joint_name, item->address_, &data16); - if (data16 == value) - continue; - break; - case 4: - read4Byte(joint_name, item->address_, &data32); - if (data32 == value) - continue; - break; - default: - break; - } - } - - switch (item->data_length_) - { - case 1: - write1Byte(joint_name, item->address_, (uint8_t) value); - break; - case 2: - write2Byte(joint_name, item->address_, (uint16_t) value); - break; - case 4: - write4Byte(joint_name, item->address_, value); - break; - default: - break; - } - - if (item->memory_type_ == EEPROM) - { - // Write to EEPROM -> delay is required (max delay: 55 msec per byte) - usleep(item->data_length_ * 55 * 1000); - } - } - } - } catch (const std::exception& e) - { - ROS_INFO("Dynamixel Init file not found."); - } } void RobotisController::gazeboTimerThread() @@ -1186,6 +1191,16 @@ void RobotisController::process() // SyncWrite if (gazebo_mode_ == false && do_sync_write) { + if (direct_sync_write_.size() > 0) + { + for (int i = 0; i < direct_sync_write_.size(); i++) + { + direct_sync_write_[i]->txPacket(); + direct_sync_write_[i]->clearParam(); + } + direct_sync_write_.clear(); + } + if (port_to_sync_write_position_p_gain_.size() > 0) { for (auto& it : port_to_sync_write_position_p_gain_) From ede458ace61208ecd56856721cfe05b3c982fab2 Mon Sep 17 00:00:00 2001 From: pyo Date: Wed, 17 Aug 2016 07:39:23 +0900 Subject: [PATCH 10/13] added dependency option --- robotis_controller/CMakeLists.txt | 11 +++-------- 1 file changed, 3 insertions(+), 8 deletions(-) diff --git a/robotis_controller/CMakeLists.txt b/robotis_controller/CMakeLists.txt index 1d94330..8f36296 100644 --- a/robotis_controller/CMakeLists.txt +++ b/robotis_controller/CMakeLists.txt @@ -46,14 +46,9 @@ include_directories( ${catkin_INCLUDE_DIRS} ) -add_library(robotis_controller - src/robotis_controller/robotis_controller.cpp -) - -target_link_libraries(robotis_controller - yaml-cpp - ${catkin_LIBRARIES} -) +add_library(robotis_controller src/robotis_controller/robotis_controller.cpp) +add_dependencies(robotis_controller ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(robotis_controller yaml-cpp ${catkin_LIBRARIES}) ################################################################################ # Install From 2a41c315c87cff158d7d21096eaebfa1745ad33a Mon Sep 17 00:00:00 2001 From: pyo Date: Thu, 18 Aug 2016 08:10:20 +0900 Subject: [PATCH 11/13] updated the package information --- robotis_controller/package.xml | 4 ++-- robotis_device/package.xml | 17 ++++++----------- robotis_framework/package.xml | 4 ++-- robotis_framework_common/package.xml | 7 +++++-- 4 files changed, 15 insertions(+), 17 deletions(-) diff --git a/robotis_controller/package.xml b/robotis_controller/package.xml index 9fb1356..fd27552 100644 --- a/robotis_controller/package.xml +++ b/robotis_controller/package.xml @@ -6,8 +6,8 @@ The main package that controls THORMANG3. BSD - zerom - pyo + Zerom + Pyo https://github.com/ROBOTIS-GIT/ROBOTIS-Framework/issues https://github.com/ROBOTIS-GIT/ROBOTIS-Framework http://wiki.ros.org/robotis_controller diff --git a/robotis_device/package.xml b/robotis_device/package.xml index 3ea40fe..a7f8605 100644 --- a/robotis_device/package.xml +++ b/robotis_device/package.xml @@ -7,22 +7,17 @@ This package is used when reading device information with the robot information file from the robotis_controller package. - - robotis - BSD - - - - robotis - + Zerom + Pyo + https://github.com/ROBOTIS-GIT/ROBOTIS-Framework/issues + https://github.com/ROBOTIS-GIT/ROBOTIS-Framework + http://wiki.ros.org/robotis_device catkin - roscpp rospy dynamixel_sdk - roscpp rospy dynamixel_sdk - \ No newline at end of file + diff --git a/robotis_framework/package.xml b/robotis_framework/package.xml index 170f107..f6bb99f 100644 --- a/robotis_framework/package.xml +++ b/robotis_framework/package.xml @@ -4,8 +4,8 @@ 0.1.0 ROS packages for the robotis_framework (meta package) BSD - zerom - pyo + Zerom + Pyo https://github.com/ROBOTIS-GIT/ROBOTIS-Framework/issues https://github.com/ROBOTIS-GIT/ROBOTIS-Framework http://wiki.ros.org/robotis_framework diff --git a/robotis_framework_common/package.xml b/robotis_framework_common/package.xml index 30c4bbb..b3db87b 100644 --- a/robotis_framework_common/package.xml +++ b/robotis_framework_common/package.xml @@ -6,8 +6,11 @@ The package contains commonly used Headers for the ROBOTIS Framework. BSD - zerom - pyo + Zerom + Pyo + https://github.com/ROBOTIS-GIT/ROBOTIS-Framework/issues + https://github.com/ROBOTIS-GIT/ROBOTIS-Framework + http://wiki.ros.org/robotis_framework_common catkin roscpp roscpp From 079aa4e993f88491a02bd8bc16d44242ac163771 Mon Sep 17 00:00:00 2001 From: pyo Date: Thu, 18 Aug 2016 08:10:51 +0900 Subject: [PATCH 12/13] 0.1.1 --- robotis_controller/package.xml | 2 +- robotis_device/package.xml | 2 +- robotis_framework/package.xml | 2 +- robotis_framework_common/package.xml | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/robotis_controller/package.xml b/robotis_controller/package.xml index fd27552..b86d7bf 100644 --- a/robotis_controller/package.xml +++ b/robotis_controller/package.xml @@ -1,7 +1,7 @@ robotis_controller - 0.1.0 + 0.1.1 The main package that controls THORMANG3. diff --git a/robotis_device/package.xml b/robotis_device/package.xml index a7f8605..e5caa92 100644 --- a/robotis_device/package.xml +++ b/robotis_device/package.xml @@ -1,7 +1,7 @@ robotis_device - 0.1.0 + 0.1.1 The package that manages device information of ROBOTIS robots. This package is used when reading device information with the robot information file diff --git a/robotis_framework/package.xml b/robotis_framework/package.xml index f6bb99f..809e65e 100644 --- a/robotis_framework/package.xml +++ b/robotis_framework/package.xml @@ -1,7 +1,7 @@ robotis_framework - 0.1.0 + 0.1.1 ROS packages for the robotis_framework (meta package) BSD Zerom diff --git a/robotis_framework_common/package.xml b/robotis_framework_common/package.xml index b3db87b..f01e4d5 100644 --- a/robotis_framework_common/package.xml +++ b/robotis_framework_common/package.xml @@ -1,7 +1,7 @@ robotis_framework_common - 0.1.0 + 0.1.1 The package contains commonly used Headers for the ROBOTIS Framework. From 532e746302f255439503991b159ebd3a28ae02d3 Mon Sep 17 00:00:00 2001 From: pyo Date: Thu, 18 Aug 2016 08:15:19 +0900 Subject: [PATCH 13/13] updated CHANGLOG.rst for minor release --- robotis_controller/CHANGELOG.rst | 8 +++++--- robotis_device/CHANGELOG.rst | 4 ++++ robotis_framework/CHANGELOG.rst | 6 +++++- robotis_framework_common/CHANGELOG.rst | 7 +++++-- 4 files changed, 19 insertions(+), 6 deletions(-) diff --git a/robotis_controller/CHANGELOG.rst b/robotis_controller/CHANGELOG.rst index c81a9f1..2a476ea 100644 --- a/robotis_controller/CHANGELOG.rst +++ b/robotis_controller/CHANGELOG.rst @@ -2,13 +2,15 @@ Changelog for package robotis_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.1.1 (2016-08-18) +----------- +* updated the package information + 0.1.0 (2016-08-12) ----------- * first public release for Kinetic * modified the package information for release * develop branch -> master branch -* Merge pull request `#4 `_ from ROBOTIS-GIT/add_sensor_device - Add sensor device * function name changed : DeviceInit() -> InitDevice() * Fixed high CPU consumption due to busy waits * add SensorState @@ -18,4 +20,4 @@ Changelog for package robotis_controller * added code to support the gazebo simulator * added first bulk read failure protection code * renewal -* Contributors: Alexander Stumpf, ROBOTIS, ROBOTIS-zerom, pyo +* Contributors: Alexander Stumpf, Jay Song, Zerom, Pyo diff --git a/robotis_device/CHANGELOG.rst b/robotis_device/CHANGELOG.rst index 50c29fd..9a5117c 100644 --- a/robotis_device/CHANGELOG.rst +++ b/robotis_device/CHANGELOG.rst @@ -2,6 +2,10 @@ Changelog for package robotis_device ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.1.1 (2016-08-18) +----------- +* updated the package information + 0.1.0 (2016-08-12) ----------- * first public release for Kinetic diff --git a/robotis_framework/CHANGELOG.rst b/robotis_framework/CHANGELOG.rst index 353baf6..8f61ce3 100644 --- a/robotis_framework/CHANGELOG.rst +++ b/robotis_framework/CHANGELOG.rst @@ -2,7 +2,11 @@ Changelog for package robotis_framework ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.1.1 (2016-08-18) +----------- +* updated the package information + 0.1.0 (2016-08-12) ----------- * make a meta-package -* Contributors: pyo +* Contributors: Zerom, Pyo diff --git a/robotis_framework_common/CHANGELOG.rst b/robotis_framework_common/CHANGELOG.rst index 7aacc38..bc63720 100644 --- a/robotis_framework_common/CHANGELOG.rst +++ b/robotis_framework_common/CHANGELOG.rst @@ -2,11 +2,14 @@ Changelog for package robotis_framework_common ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.1.1 (2016-08-18) +----------- +* updated the package information + 0.1.0 (2016-08-12) ----------- * modified the package information for release -* develop branch -> master branch * Setting the license to BSD. * add SensorState add Singleton template -* Contributors: ROBOTIS, ROBOTIS-zerom, pyo +* Contributors: Jay Song, Zerom, Pyo