From 940d918df7cec89d3ac6844d510a4ef95fc3ae74 Mon Sep 17 00:00:00 2001 From: fmessmer Date: Wed, 20 Mar 2024 16:42:23 +0100 Subject: [PATCH 01/14] remove cob_phidget_em_state --- cob_phidget_em_state/CHANGELOG.rst | 137 ------------------ cob_phidget_em_state/CMakeLists.txt | 10 -- cob_phidget_em_state/package.xml | 17 --- .../scripts/em_state_phidget.py | 108 -------------- 4 files changed, 272 deletions(-) delete mode 100644 cob_phidget_em_state/CHANGELOG.rst delete mode 100644 cob_phidget_em_state/CMakeLists.txt delete mode 100644 cob_phidget_em_state/package.xml delete mode 100755 cob_phidget_em_state/scripts/em_state_phidget.py diff --git a/cob_phidget_em_state/CHANGELOG.rst b/cob_phidget_em_state/CHANGELOG.rst deleted file mode 100644 index 8c9909742..000000000 --- a/cob_phidget_em_state/CHANGELOG.rst +++ /dev/null @@ -1,137 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package cob_phidget_em_state -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.7.16 (2024-02-20) -------------------- - -0.7.15 (2023-11-06) -------------------- - -0.7.14 (2022-11-17) -------------------- - -0.7.13 (2022-07-29) -------------------- - -0.7.12 (2022-03-15) -------------------- - -0.7.11 (2022-01-12) -------------------- - -0.7.10 (2021-12-23) -------------------- - -0.7.9 (2021-11-26) ------------------- - -0.7.8 (2021-10-19) ------------------- - -0.7.7 (2021-08-02) ------------------- - -0.7.6 (2021-05-10) ------------------- - -0.7.5 (2021-04-06) ------------------- - -0.7.4 (2020-10-14) ------------------- -* Merge pull request `#417 `_ from fmessmer/test_noetic - test noetic -* Bump CMake version to avoid CMP0048 warning -* Contributors: Felix Messmer, fmessmer - -0.7.3 (2020-03-18) ------------------- - -0.7.2 (2020-03-18) ------------------- -* Merge pull request `#408 `_ from fmessmer/ci_updates - [travis] ci updates -* catkin_lint fixes -* Contributors: Felix Messmer, fmessmer - -0.7.1 (2019-11-07) ------------------- - -0.7.0 (2019-08-06) ------------------- -* Merge pull request `#396 `_ from HannesBachter/indigo_dev - 0.6.15 -* Contributors: Felix Messmer - -0.6.15 (2019-07-17) -------------------- - -0.6.14 (2019-06-07) -------------------- - -0.6.13 (2019-03-14) -------------------- - -0.6.12 (2018-07-21) -------------------- -* update maintainer -* Contributors: fmessmer - -0.6.11 (2018-01-07) -------------------- -* Merge remote-tracking branch 'origin/indigo_release_candidate' into indigo_dev -* Merge pull request `#341 `_ from ipa-fxm/APACHE_license - use license apache 2.0 -* use license apache 2.0 -* Contributors: Felix Messmer, ipa-fxm, ipa-uhr-mk - -0.6.10 (2017-07-24) -------------------- - -0.6.9 (2017-07-18) ------------------- -* send em_state only if data from phidget received -* Contributors: Benjamin Maidel - -0.6.8 (2016-10-10) ------------------- -* fix -* use values from message -* fixed syntax error -* fixed conditions -* new package to calculate em state from phidgets digital input -* Contributors: Benjamin Maidel - -0.6.3 (2015-06-17) ------------------- - -0.6.2 (2014-12-15) ------------------- - -0.6.1 (2014-09-17) ------------------- - -0.6.0 (2014-09-09) ------------------- - -0.5.7 (2014-08-26 09:47) ------------------------- - -0.5.6 (2014-08-26 09:42) ------------------------- - -0.5.5 (2014-08-26 08:33) ------------------------- - -0.5.4 (2014-08-25) ------------------- - -0.5.3 (2014-03-31) ------------------- - -0.5.2 (2014-03-21) ------------------- - -0.5.1 (2014-03-20 10:54) ------------------------- diff --git a/cob_phidget_em_state/CMakeLists.txt b/cob_phidget_em_state/CMakeLists.txt deleted file mode 100644 index 620ba5735..000000000 --- a/cob_phidget_em_state/CMakeLists.txt +++ /dev/null @@ -1,10 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(cob_phidget_em_state) - -find_package(catkin REQUIRED COMPONENTS) - -catkin_package() - -catkin_install_python(PROGRAMS scripts/em_state_phidget.py - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}/src -) diff --git a/cob_phidget_em_state/package.xml b/cob_phidget_em_state/package.xml deleted file mode 100644 index b0a257352..000000000 --- a/cob_phidget_em_state/package.xml +++ /dev/null @@ -1,17 +0,0 @@ - - - cob_phidget_em_state - 0.7.16 - The cob_phidget_em_state package publishes emergency state based on phidgets signals. - - Benjamin Maidel - Benjamin Maidel - - Apache 2.0 - - catkin - cob_msgs - cob_phidgets - rospy - - diff --git a/cob_phidget_em_state/scripts/em_state_phidget.py b/cob_phidget_em_state/scripts/em_state_phidget.py deleted file mode 100755 index 1028b26d9..000000000 --- a/cob_phidget_em_state/scripts/em_state_phidget.py +++ /dev/null @@ -1,108 +0,0 @@ -#!/usr/bin/env python -# -# Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - - -import rospy -from cob_phidgets.msg import AnalogSensor -from cob_phidgets.msg import DigitalSensor -from cob_msgs.msg import PowerState -from cob_msgs.msg import EmergencyStopState - -class EmState(): - ST_EM_FREE = 0 - ST_EM_ACTIVE = 1 - ST_EM_CONFIRMED = 2 - -class EMStatePhidget(): - def __init__(self): - self.last_update = None - self.em_caused_by_button = False - - self.last_rear_em_state = False - self.last_front_em_state = False - - self.em_stop_status = EmState.ST_EM_ACTIVE - self.em_msg = EmergencyStopState() - - self.sub_digital_sensors = rospy.Subscriber("digital_sensors", DigitalSensor, self.phidget_cb) - self.pub_em_state = rospy.Publisher('em_stop_state', EmergencyStopState, queue_size=1) - - def phidget_cb(self, msg): - front_em_active = False - rear_em_active = False - got_message = False - - for i in range(0, len(msg.uri)): - if msg.uri[i] == "em_stop_laser_rear": - rear_em_active = not msg.state[i] - got_message = True - if msg.uri[i] == "em_stop_laser_front": - front_em_active = not msg.state[i] - got_message = True - - if got_message: - self.last_update = rospy.Time.now() - - if (front_em_active and rear_em_active) and (not self.last_front_em_state and not self.last_rear_em_state): - self.em_msg.emergency_button_stop = True - self.em_caused_by_button = True - elif (not front_em_active and not rear_em_active) and (self.last_front_em_state and self.last_rear_em_state): - self.em_msg.emergency_button_stop = False - self.em_caused_by_button = False - elif (front_em_active is not rear_em_active) and self.em_caused_by_button : - self.em_msg.emergency_button_stop = False - self.em_caused_by_button = False - self.em_msg.scanner_stop = (front_em_active or rear_em_active) - else: - self.em_msg.scanner_stop = (front_em_active or rear_em_active); - - em_signal = self.em_msg.scanner_stop or self.em_msg.emergency_button_stop - - if self.em_stop_status == EmState.ST_EM_FREE: - if em_signal: - rospy.logdebug("Emergency stop was issued") - self.em_stop_status = EmergencyStopState.EMSTOP - - elif self.em_stop_status == EmState.ST_EM_ACTIVE: - if not em_signal: - rospy.logdebug("Emergency stop was confirmed") - self.em_stop_status = EmergencyStopState.EMCONFIRMED - elif self.em_stop_status == EmState.ST_EM_CONFIRMED: - if em_signal: - rospy.logdebug("Emergency stop issued") - self.em_stop_status = EmergencyStopState.EMSTOP - else: - rospy.logdebug("Emergency stop released") - self.em_stop_status = EmergencyStopState.EMFREE - - self.em_msg.emergency_state = self.em_stop_status - - self.last_rear_em_state = rear_em_active - self.last_front_em_state = front_em_active - - def publish(self): - if self.last_update is not None: - self.pub_em_state.publish(self.em_msg) - - -if __name__ == "__main__": - rospy.init_node("em_state_phidget") - emsp = EMStatePhidget() - rospy.loginfo("em_state_phidget running") - rate = rospy.Rate(10) - while not rospy.is_shutdown(): - emsp.publish() - rate.sleep() From d39ab37fa126c276066f835ef2ce2274cc3f619d Mon Sep 17 00:00:00 2001 From: fmessmer Date: Wed, 20 Mar 2024 16:43:52 +0100 Subject: [PATCH 02/14] remove cob_phidget_power_state --- cob_phidget_power_state/CHANGELOG.rst | 153 ------------------ cob_phidget_power_state/CMakeLists.txt | 10 -- cob_phidget_power_state/package.xml | 17 -- .../scripts/power_state_phidget.py | 150 ----------------- 4 files changed, 330 deletions(-) delete mode 100644 cob_phidget_power_state/CHANGELOG.rst delete mode 100644 cob_phidget_power_state/CMakeLists.txt delete mode 100644 cob_phidget_power_state/package.xml delete mode 100755 cob_phidget_power_state/scripts/power_state_phidget.py diff --git a/cob_phidget_power_state/CHANGELOG.rst b/cob_phidget_power_state/CHANGELOG.rst deleted file mode 100644 index 93a25220b..000000000 --- a/cob_phidget_power_state/CHANGELOG.rst +++ /dev/null @@ -1,153 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package cob_phidget_power_state -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.7.16 (2024-02-20) -------------------- - -0.7.15 (2023-11-06) -------------------- - -0.7.14 (2022-11-17) -------------------- - -0.7.13 (2022-07-29) -------------------- - -0.7.12 (2022-03-15) -------------------- - -0.7.11 (2022-01-12) -------------------- - -0.7.10 (2021-12-23) -------------------- - -0.7.9 (2021-11-26) ------------------- - -0.7.8 (2021-10-19) ------------------- - -0.7.7 (2021-08-02) ------------------- - -0.7.6 (2021-05-10) ------------------- - -0.7.5 (2021-04-06) ------------------- - -0.7.4 (2020-10-14) ------------------- -* Merge pull request `#417 `_ from fmessmer/test_noetic - test noetic -* Bump CMake version to avoid CMP0048 warning -* Contributors: Felix Messmer, fmessmer - -0.7.3 (2020-03-18) ------------------- - -0.7.2 (2020-03-18) ------------------- -* Merge pull request `#409 `_ from LoyVanBeek/feature/python3_compatibility - [ci_updates] pylint + Python3 compatibility -* fix pylint errors -* Merge pull request `#408 `_ from fmessmer/ci_updates - [travis] ci updates -* catkin_lint fixes -* Contributors: Felix Messmer, fmessmer - -0.7.1 (2019-11-07) ------------------- - -0.7.0 (2019-08-06) ------------------- -* Merge pull request `#396 `_ from HannesBachter/indigo_dev - 0.6.15 -* Contributors: Felix Messmer - -0.6.15 (2019-07-17) -------------------- - -0.6.14 (2019-06-07) -------------------- - -0.6.13 (2019-03-14) -------------------- - -0.6.12 (2018-07-21) -------------------- -* update maintainer -* Contributors: fmessmer - -0.6.11 (2018-01-07) -------------------- -* Merge remote-tracking branch 'origin/indigo_release_candidate' into indigo_dev -* Merge pull request `#341 `_ from ipa-fxm/APACHE_license - use license apache 2.0 -* use license apache 2.0 -* Contributors: Felix Messmer, ipa-fxm, ipa-uhr-mk - -0.6.10 (2017-07-24) -------------------- - -0.6.9 (2017-07-18) ------------------- -* shut down if the voltage divider factor is undefined -* renamed max_voltage to more descriptive voltage_divider_factor -* smooth calculated voltage from phidget board by adapting average over windows of measurements -* Contributors: Benjamin Maidel, Jannik Abbenseth - -0.6.8 (2016-10-10) ------------------- -* fix current sign -* fix current sign -* remove print -* fix namespace and syntax for phidget power state -* Merge branch 'indigo_dev' into feature/phidget_power_state_params - Conflicts: - cob_phidget_power_state/scripts/power_state_phidget.py -* read power state params from param server -* increase voltage limit for empty battery -* cut percentage between 0 and 100 -* updates for changed hardware -* fixed typo -* remove find_package entries as this package only as python nodes -* Update package.xml -* updated license and version -* move power_state_phidget node to new package -* Contributors: Benjamin Maidel, Florian Weisshardt, msh - -0.6.3 (2015-06-17) ------------------- - -0.6.2 (2014-12-15) ------------------- - -0.6.1 (2014-09-17) ------------------- - -0.6.0 (2014-09-09) ------------------- - -0.5.7 (2014-08-26 09:47) ------------------------- - -0.5.6 (2014-08-26 09:42) ------------------------- - -0.5.5 (2014-08-26 08:33) ------------------------- - -0.5.4 (2014-08-25) ------------------- - -0.5.3 (2014-03-31) ------------------- - -0.5.2 (2014-03-21) ------------------- - -0.5.1 (2014-03-20 10:54) ------------------------- diff --git a/cob_phidget_power_state/CMakeLists.txt b/cob_phidget_power_state/CMakeLists.txt deleted file mode 100644 index 593f7d2d5..000000000 --- a/cob_phidget_power_state/CMakeLists.txt +++ /dev/null @@ -1,10 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(cob_phidget_power_state) - -find_package(catkin REQUIRED COMPONENTS) - -catkin_package() - -catkin_install_python(PROGRAMS scripts/power_state_phidget.py - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}/src -) diff --git a/cob_phidget_power_state/package.xml b/cob_phidget_power_state/package.xml deleted file mode 100644 index 213c021e3..000000000 --- a/cob_phidget_power_state/package.xml +++ /dev/null @@ -1,17 +0,0 @@ - - - cob_phidget_power_state - 0.7.16 - The cob_phidget_power_state package publishes power state based on phidgets signals. - - Benjamin Maidel - Benjamin Maidel - - Apache 2.0 - - catkin - cob_msgs - cob_phidgets - rospy - - diff --git a/cob_phidget_power_state/scripts/power_state_phidget.py b/cob_phidget_power_state/scripts/power_state_phidget.py deleted file mode 100755 index d134d4a1e..000000000 --- a/cob_phidget_power_state/scripts/power_state_phidget.py +++ /dev/null @@ -1,150 +0,0 @@ -#!/usr/bin/env python -# -# Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - - -import numpy as np -import rospy -from cob_msgs.msg import PowerState -from cob_phidgets.msg import AnalogSensor - - -class PowerStatePhidget(): - PHIDGET_MAX_VALUE = 999 - PHIDGET_MIN_VALUE = 0 - PERIOD_RECORD_SIZE = 6 - VOLTAGE_COLLECTION_TIME = 6.0 # sec - - def __init__(self): - self.voltage = None - self.current = None - self.last_update = rospy.Time(0) - self.charging = False - try: - self.voltage_divider_factor = rospy.get_param("~voltage_divider_factor") - except KeyError: - raise KeyError("Parameter \"~voltage_divider_factor\" not found on parameter server.") - self.voltage_full = rospy.get_param("~voltage_full", 52.0) - self.voltage_empty = rospy.get_param("~voltage_empty", 38.0) - self.current_max = rospy.get_param("~current_max", 30.0) - self.current_min = rospy.get_param("~current_min", -30.0) - - self.pub_power_state = rospy.Publisher('power_state', PowerState, queue_size=1) - self.sub_analog_sensors = rospy.Subscriber("analog_sensors", AnalogSensor, self.phidget_cb) - - self.pr_next = 0 - self.period_record = [] - self.cb_avg_time = 0.1 - self.voltage_bag_maxlen = 100 - self.voltage_bag = [] - - def append_voltage_bag(self, num): - while len(self.voltage_bag) >= self.voltage_bag_maxlen: - self.voltage_bag.pop(0) - self.voltage_bag.append(num) - - def calculate_voltage(self): - if len(self.voltage_bag) > 0: - self.voltage = np.mean(self.voltage_bag) - - def phidget_cb(self, msg): - # Estimate commands frequency; we do continuously as it can be very different depending on the - # publisher type, and we don't want to impose extra constraints to keep this package flexible - if len(self.period_record) < self.PERIOD_RECORD_SIZE: - self.period_record.append((rospy.Time.now() - self.last_update).to_sec()) - else: - self.period_record[self.pr_next] = (rospy.Time.now() - self.last_update).to_sec() - - self.pr_next += 1 - self.pr_next %= len(self.period_record) - self.last_update = rospy.Time.now() - - if len(self.period_record) <= self.PERIOD_RECORD_SIZE / 2: - # wait until we have some values; make a reasonable assumption (10 Hz) meanwhile - self.cb_avg_time = 0.1 - else: - # enough; recalculate with the latest input - self.cb_avg_time = np.median(self.period_record) - - # now set the max voltage bag size - self.voltage_bag_maxlen = int(self.VOLTAGE_COLLECTION_TIME / self.cb_avg_time) - - voltage_raw = None - current_raw = None - - for i in range(0, len(msg.uri)): - if msg.uri[i] == "voltage": - voltage_raw = msg.value[i] - if msg.uri[i] == "current": - current_raw = msg.value[i] - - if voltage_raw != None: - # Calculation of real voltage - voltage = self.voltage_divider_factor * voltage_raw / self.PHIDGET_MAX_VALUE - voltage = round(voltage, 3) - self.append_voltage_bag(voltage) - - if current_raw != None: - # Calculation of real current - self.current = self.current_min + (self.current_max - self.current_min) * (current_raw - - self.PHIDGET_MIN_VALUE) / (self.PHIDGET_MAX_VALUE - self.PHIDGET_MIN_VALUE) - self.current = round(self.current, 3) - - if self.current > 0: - self.charging = True - else: - self.charging = False - - def calculate_power_consumption(self): - if not self.charging and self.voltage != None and self.current != None: - return round(self.voltage * abs(self.current), 3) - else: - return 0.0 - - def calculate_relative_remaining_capacity(self): - percentage = None - if self.voltage != None: - percentage = round((self.voltage - self.voltage_empty) * 100 / (self.voltage_full - self.voltage_empty), 3) - percentage = min(percentage, 100) - percentage = max(percentage, 0) - return percentage - else: - return 0.0 - - def publish(self): - self.calculate_voltage() - if self.voltage != None and self.current != None and (rospy.Time.now() - self.last_update) < rospy.Duration(1): - ps = PowerState() - ps.header.stamp = self.last_update - ps.voltage = self.voltage - ps.current = self.current - ps.power_consumption = self.calculate_power_consumption() - ps.relative_remaining_capacity = self.calculate_relative_remaining_capacity() - ps.charging = self.charging - self.pub_power_state.publish(ps) - -if __name__ == "__main__": - rospy.init_node("power_state_phidget") - try: - psp = PowerStatePhidget() - except KeyError as e: - rospy.logerr("Shutting down: {}".format(e)) - exit(1) - - rospy.loginfo("power state phidget running") - rate = rospy.Rate(10) - while not rospy.is_shutdown(): - psp.publish() - rate.sleep() From 0f83e86f78a9036add07b4bcbb378b110294f34d Mon Sep 17 00:00:00 2001 From: fmessmer Date: Wed, 20 Mar 2024 16:48:00 +0100 Subject: [PATCH 03/14] remove cob_sick_lms1xx --- cob_driver/package.xml | 1 - cob_sick_lms1xx/CHANGELOG.rst | 188 --- cob_sick_lms1xx/CMakeLists.txt | 32 - cob_sick_lms1xx/common/include/lms1xx.h | 299 ----- cob_sick_lms1xx/common/src/lms1xx.cpp | 469 ------- cob_sick_lms1xx/common/src/test.cpp | 95 -- cob_sick_lms1xx/doc/Doxyfile | 1587 ----------------------- cob_sick_lms1xx/package.xml | 24 - cob_sick_lms1xx/ros/src/lms1xx_node.cpp | 313 ----- cob_sick_lms1xx/ros/src/set_config.cpp | 65 - 10 files changed, 3073 deletions(-) delete mode 100644 cob_sick_lms1xx/CHANGELOG.rst delete mode 100644 cob_sick_lms1xx/CMakeLists.txt delete mode 100644 cob_sick_lms1xx/common/include/lms1xx.h delete mode 100644 cob_sick_lms1xx/common/src/lms1xx.cpp delete mode 100644 cob_sick_lms1xx/common/src/test.cpp delete mode 100644 cob_sick_lms1xx/doc/Doxyfile delete mode 100644 cob_sick_lms1xx/package.xml delete mode 100644 cob_sick_lms1xx/ros/src/lms1xx_node.cpp delete mode 100644 cob_sick_lms1xx/ros/src/set_config.cpp diff --git a/cob_driver/package.xml b/cob_driver/package.xml index d8fd422af..8d779e7e2 100644 --- a/cob_driver/package.xml +++ b/cob_driver/package.xml @@ -23,7 +23,6 @@ cob_phidgets cob_relayboard cob_scan_unifier - cob_sick_lms1xx cob_sick_s300 cob_sound cob_undercarriage_ctrl diff --git a/cob_sick_lms1xx/CHANGELOG.rst b/cob_sick_lms1xx/CHANGELOG.rst deleted file mode 100644 index cfdbe7493..000000000 --- a/cob_sick_lms1xx/CHANGELOG.rst +++ /dev/null @@ -1,188 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package cob_sick_lms1xx -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.7.16 (2024-02-20) -------------------- - -0.7.15 (2023-11-06) -------------------- - -0.7.14 (2022-11-17) -------------------- - -0.7.13 (2022-07-29) -------------------- - -0.7.12 (2022-03-15) -------------------- - -0.7.11 (2022-01-12) -------------------- - -0.7.10 (2021-12-23) -------------------- - -0.7.9 (2021-11-26) ------------------- - -0.7.8 (2021-10-19) ------------------- - -0.7.7 (2021-08-02) ------------------- - -0.7.6 (2021-05-10) ------------------- - -0.7.5 (2021-04-06) ------------------- -* Merge pull request `#418 `_ from fmessmer/fix_catkin_lint - fix catkin_lint -* fix catkin_lint -* Contributors: Felix Messmer, fmessmer - -0.7.4 (2020-10-14) ------------------- -* Merge pull request `#417 `_ from fmessmer/test_noetic - test noetic -* Bump CMake version to avoid CMP0048 warning -* Contributors: Felix Messmer, fmessmer - -0.7.3 (2020-03-18) ------------------- - -0.7.2 (2020-03-18) ------------------- -* Merge pull request `#408 `_ from fmessmer/ci_updates - [travis] ci updates -* catkin_lint fixes -* Merge pull request `#405 `_ from fmessmer/fix_warnings - fix compile warnings -* fix -Wunused-result in cob_sick_lms1xx -* Contributors: Felix Messmer, fmessmer - -0.7.1 (2019-11-07) ------------------- - -0.7.0 (2019-08-06) ------------------- -* Merge pull request `#396 `_ from HannesBachter/indigo_dev - 0.6.15 -* Contributors: Felix Messmer - -0.6.15 (2019-07-17) -------------------- - -0.6.14 (2019-06-07) -------------------- - -0.6.13 (2019-03-14) -------------------- - -0.6.12 (2018-07-21) -------------------- - -0.6.11 (2018-01-07) -------------------- -* Merge remote-tracking branch 'origin/indigo_release_candidate' into indigo_dev -* Merge pull request `#341 `_ from ipa-fxm/APACHE_license - use license apache 2.0 -* use license apache 2.0 -* Contributors: Felix Messmer, ipa-fxm, ipa-uhr-mk - -0.6.10 (2017-07-24) -------------------- - -0.6.9 (2017-07-18) ------------------- -* manually fix changelog -* Contributors: ipa-fxm - -0.6.8 (2016-10-10) ------------------- -* cob_sick_lms1xx: add range configuration -* Contributors: Shin - -0.6.7 (2016-04-02) ------------------- -* add missing dependencies -* Contributors: ipa-fxm - -0.6.6 (2016-04-01) ------------------- -* Sick LMS1xx node now uses global NodeHandle -* Resolved problem with tabs and spaces. -* Clean trailing spaces. Convert default frame name to tf2 format (remove "/") -* Revert indentation style as it was -* Correction. -* Corrected revert. -* Revert indentation style as it was -* Node rewritten and publishing on diagnostics topic added. -* Contributors: Denis Štogl - -0.6.5 (2015-08-31) ------------------- - -0.6.4 (2015-08-25) ------------------- -* install tags for libraries -* boost revision -* do not install headers in executable-only packages -* explicit dependency to boost -* catkin_package according to install tags -* remove trailing whitespaces -* add_dependencies EXPORTED_TARGETS -* migrate to package format 2 -* sort dependencies -* critically review dependencies -* Contributors: ipa-fxm - -0.6.3 (2015-06-17) ------------------- - -0.6.2 (2014-12-15) ------------------- - -0.6.1 (2014-09-17) ------------------- - -0.6.0 (2014-09-09) ------------------- - -0.5.7 (2014-08-26) ------------------- -* 0.5.6 -* update changelog -* merge -* fix python3 ascii error while parsing "S" -* Corrected inversion for lms1xx -* Merge pull request `#136 `_ from ipa-fmw/hydro_dev - change maintainer and add missing dependency -* Update package.xml -* Contributors: Denis Štogl, Florian Weisshardt, Nadia Hammoudeh García, ipa-fxm - -0.5.6 (2014-08-26) ------------------- -* merge -* fix python3 ascii error while parsing "S" -* Corrected inversion for lms1xx -* Merge pull request `#136 `_ from ipa-fmw/hydro_dev - change maintainer and add missing dependency -* Update package.xml -* Contributors: Denis Štogl, Florian Weisshardt, Nadia Hammoudeh García, ipa-fxm - -0.5.3 (2014-03-31) ------------------- -* install tags -* Contributors: ipa-fxm - -0.5.2 (2014-03-20) ------------------- - -0.5.1 (2014-03-20) ------------------- -* fixed missing include allowing sleep() -* Changed node name. -* New package with driver for Sick LMS1xx. Driver is taken from https://github.com/ipa320/RCPRG_laser_drivers.git. -* Contributors: Alexander Hagg, Denis Štogl, IPR-SR2 diff --git a/cob_sick_lms1xx/CMakeLists.txt b/cob_sick_lms1xx/CMakeLists.txt deleted file mode 100644 index 3b407a80b..000000000 --- a/cob_sick_lms1xx/CMakeLists.txt +++ /dev/null @@ -1,32 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(cob_sick_lms1xx) - -find_package(catkin REQUIRED COMPONENTS diagnostic_msgs roscpp sensor_msgs) - -find_package(Boost REQUIRED) - -catkin_package( - CATKIN_DEPENDS diagnostic_msgs roscpp sensor_msgs -) - -### BUILD ### -include_directories(common/include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) - -add_library(lms1xx common/src/lms1xx.cpp) - -add_executable(lms1xx_test common/src/test.cpp) -add_executable(lms100 ros/src/lms1xx_node.cpp) -add_executable(set_config ros/src/set_config.cpp) - -add_dependencies(lms100 ${catkin_EXPORTED_TARGETS}) - -target_link_libraries(lms1xx_test lms1xx ${catkin_LIBRARIES}) -target_link_libraries(lms100 lms1xx ${catkin_LIBRARIES}) -target_link_libraries(set_config lms1xx ${catkin_LIBRARIES}) - -### INSTALL ### -install(TARGETS lms100 lms1xx lms1xx_test set_config - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) diff --git a/cob_sick_lms1xx/common/include/lms1xx.h b/cob_sick_lms1xx/common/include/lms1xx.h deleted file mode 100644 index d5668e845..000000000 --- a/cob_sick_lms1xx/common/include/lms1xx.h +++ /dev/null @@ -1,299 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -#ifndef LMS1XX_H_ -#define LMS1XX_H_ - -#include -#include - -/*! -* @class scanCfg -* @brief Structure containing scan configuration. -* -* @author Konrad Banachowicz -*/ -typedef struct _scanCfg { - /*! - * @brief Scanning frequency. - * 1/100 Hz - */ - int scaningFrequency; - - /*! - * @brief Scanning resolution. - * 1/10000 degree - */ - int angleResolution; - - /*! - * @brief Start angle. - * 1/10000 degree - */ - int startAngle; - - /*! - * @brief Stop angle. - * 1/10000 degree - */ - int stopAngle; -} scanCfg; - -/*! -* @class scanDataCfg -* @brief Structure containing scan data configuration. -* -* @author Konrad Banachowicz -*/ -typedef struct _scanDataCfg { - - /*! - * @brief Output channels. - * Defines which output channel is activated. - */ - int outputChannel; - - /*! - * @brief Remission. - * Defines whether remission values are output. - */ - bool remission; - - /*! - * @brief Remission resolution. - * Defines whether the remission values are output with 8-bit or 16bit resolution. - */ - int resolution; - - /*! - * @brief Encoders channels. - * Defines which output channel is activated. - */ - int encoder; - - /*! - * @brief Position. - * Defines whether position values are output. - */ - bool position; - - /*! - * @brief Device name. - * Determines whether the device name is to be output. - */ - bool deviceName; - - bool timestamp; - - /*! - * @brief Output interval. - * Defines which scan is output. - * - * 01 every scan\n - * 02 every 2nd scan\n - * ...\n - * 50000 every 50000th scan - */ - int outputInterval; -} scanDataCfg; - -/*! -* @class scanData -* @brief Structure containing single scan message. -* -* @author Konrad Banachowicz -*/ -typedef struct _scanData { - - /*! - * @brief Number of samples in dist1. - * - */ - int dist_len1; - - /*! - * @brief Radial distance for the first reflected pulse - * - */ - uint16_t dist1[1082]; - - /*! - * @brief Number of samples in dist2. - * - */ - int dist_len2; - - /*! - * @brief Radial distance for the second reflected pulse - * - */ - uint16_t dist2[1082]; - - /*! - * @brief Number of samples in rssi1. - * - */ - int rssi_len1; - - /*! - * @brief Remission values for the first reflected pulse - * - */ - uint16_t rssi1[1082]; - - /*! - * @brief Number of samples in rssi2. - * - */ - int rssi_len2; - - /*! - * @brief Remission values for the second reflected pulse - * - */ - uint16_t rssi2[1082]; -} scanData; - -typedef enum { - undefined = 0, - initialisation = 1, - configuration = 2, - idle = 3, - rotated = 4, - in_preparation = 5, - ready = 6, - ready_for_measurement = 7 -} status_t; - -/*! -* @class LMS1xx -* @brief Class responsible for communicating with LMS1xx device. -* -* @author Konrad Banachowicz -*/ - -class LMS1xx { -public: - LMS1xx(); - virtual ~LMS1xx(); - - /*! - * @brief Connect to LMS1xx. - * @param host LMS1xx host name or ip address. - * @param port LMS1xx port number. - */ - void connect(std::string host, int port = 2111); - - /*! - * @brief Disconnect from LMS1xx device. - */ - void disconnect(); - - /*! - * @brief Get status of connection. - * @returns connected or not. - */ - bool isConnected(); - - /*! - * @brief Start measurements. - * After receiving this command LMS1xx unit starts spinning laser and measuring. - */ - void startMeas(); - - /*! - * @brief Stop measurements. - * After receiving this command LMS1xx unit stop spinning laser and measuring. - */ - void stopMeas(); - - /*! - * @brief Get current status of LMS1xx device. - * @returns status of LMS1xx device. - */ - status_t queryStatus(); - - /*! - * @brief Log into LMS1xx unit. - * Increase privilege level, giving ability to change device configuration. - */ - void login(); - - /*! - * @brief Get current scan configuration. - * Get scan configuration : - * - scanning frequency. - * - scanning resolution. - * - start angle. - * - stop angle. - * @returns scanCfg structure. - */ - scanCfg getScanCfg() const; - - /*! - * @brief Set scan configuration. - * Get scan configuration : - * - scanning frequency. - * - scanning resolution. - * - start angle. - * - stop angle. - * @param cfg structure containing scan configuration. - */ - void setScanCfg(const scanCfg &cfg); - - /*! - * @brief Set scan data configuration. - * Set format of scan message returned by device. - * @param cfg structure containing scan data configuration. - */ - void setScanDataCfg(const scanDataCfg &cfg); - - /*! - * @brief Start or stop continuous data acquisition. - * After reception of this command device start or stop continuous data stream containing scan messages. - * @param start 1 : start 0 : stop - */ - void scanContinous(int start); - - /*! - * @brief Receive single scan message. - * - * @param data pointer to scanData buffer structure. - */ - bool getData(scanData& data); - - /*! - * @brief Save data permanently. - * Parameters are saved in the EEPROM of the LMS and will also be available after the device is switched off and on again. - * - */ - void saveConfig(); - - /*! - * @brief The device is returned to the measurement mode after configuration. - * - */ - void startDevice(); - -private: - bool connected; - bool debug; - - int sockDesc; -}; - -#endif /* LMS1XX_H_ */ diff --git a/cob_sick_lms1xx/common/src/lms1xx.cpp b/cob_sick_lms1xx/common/src/lms1xx.cpp deleted file mode 100644 index 91004401d..000000000 --- a/cob_sick_lms1xx/common/src/lms1xx.cpp +++ /dev/null @@ -1,469 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "lms1xx.h" - -LMS1xx::LMS1xx() : - connected(false) { - debug = false; -} - -LMS1xx::~LMS1xx() { - -} - -void LMS1xx::connect(std::string host, int port) { - if (!connected) { - sockDesc = socket(PF_INET, SOCK_STREAM, IPPROTO_TCP); - if (sockDesc) { - struct sockaddr_in stSockAddr; - int Res; - stSockAddr.sin_family = PF_INET; - stSockAddr.sin_port = htons(port); - Res = inet_pton(AF_INET, host.c_str(), &stSockAddr.sin_addr); - - int ret = ::connect(sockDesc, (struct sockaddr *) &stSockAddr, - sizeof stSockAddr); - if (ret == 0) { - connected = true; - } - } - } -} - -void LMS1xx::disconnect() { - if (connected) { - close(sockDesc); - connected = false; - } -} - -bool LMS1xx::isConnected() { - return connected; -} - -void LMS1xx::startMeas() { - char buf[100]; - sprintf(buf, "%c%s%c", 0x02, "sMN LMCstartmeas", 0x03); - - ssize_t res = write(sockDesc, buf, strlen(buf)); - - int len = read(sockDesc, buf, 100); - // if (buf[0] != 0x02) - // std::cout << "invalid packet recieved" << std::endl; - // if (debug) { - // buf[len] = 0; - // std::cout << buf << std::endl; - // } -} - -void LMS1xx::stopMeas() { - char buf[100]; - sprintf(buf, "%c%s%c", 0x02, "sMN LMCstopmeas", 0x03); - - ssize_t res = write(sockDesc, buf, strlen(buf)); - - int len = read(sockDesc, buf, 100); - // if (buf[0] != 0x02) - // std::cout << "invalid packet recieved" << std::endl; - // if (debug) { - // buf[len] = 0; - // std::cout << buf << std::endl; - // } -} - -status_t LMS1xx::queryStatus() { - char buf[100]; - sprintf(buf, "%c%s%c", 0x02, "sRN STlms", 0x03); - - ssize_t res = write(sockDesc, buf, strlen(buf)); - - int len = read(sockDesc, buf, 100); - // if (buf[0] != 0x02) - // std::cout << "invalid packet recieved" << std::endl; - // if (debug) { - // buf[len] = 0; - // std::cout << buf << std::endl; - // } - int ret; - sscanf((buf + 10), "%d", &ret); - - return (status_t) ret; -} - -void LMS1xx::login() { - char buf[100]; - sprintf(buf, "%c%s%c", 0x02, "sMN SetAccessMode 03 F4724744", 0x03); - - ssize_t res = write(sockDesc, buf, strlen(buf)); - - int len = read(sockDesc, buf, 100); - // if (buf[0] != 0x02) - // std::cout << "invalid packet recieved" << std::endl; - // if (debug) { - // buf[len] = 0; - // std::cout << buf << std::endl; - // } -} - -scanCfg LMS1xx::getScanCfg() const { - scanCfg cfg; - char buf[100]; - sprintf(buf, "%c%s%c", 0x02, "sRN LMPscancfg", 0x03); - - ssize_t res = write(sockDesc, buf, strlen(buf)); - - int len = read(sockDesc, buf, 100); - // if (buf[0] != 0x02) - // std::cout << "invalid packet recieved" << std::endl; - // if (debug) { - // buf[len] = 0; - // std::cout << buf << std::endl; - // } - - sscanf(buf + 1, "%*s %*s %X %*d %X %X %X", &cfg.scaningFrequency, - &cfg.angleResolution, &cfg.startAngle, &cfg.stopAngle); - return cfg; -} - -void LMS1xx::setScanCfg(const scanCfg &cfg) { - char buf[100]; - sprintf(buf, "%c%s %X +1 %X %X %X%c", 0x02, "sMN mLMPsetscancfg", - cfg.scaningFrequency, cfg.angleResolution, cfg.startAngle, - cfg.stopAngle, 0x03); - - ssize_t res = write(sockDesc, buf, strlen(buf)); - - int len = read(sockDesc, buf, 100); - - buf[len - 1] = 0; -} - -void LMS1xx::setScanDataCfg(const scanDataCfg &cfg) { - char buf[100]; - sprintf(buf, "%c%s %02X 00 %d %d 0 %02X 00 %d %d 0 %d +%d%c", 0x02, - "sWN LMDscandatacfg", cfg.outputChannel, cfg.remission ? 1 : 0, - cfg.resolution, cfg.encoder, cfg.position ? 1 : 0, - cfg.deviceName ? 1 : 0, cfg.timestamp ? 1 : 0, cfg.outputInterval, 0x03); - if(debug) - printf("%s\n", buf); - ssize_t res = write(sockDesc, buf, strlen(buf)); - - int len = read(sockDesc, buf, 100); - buf[len - 1] = 0; -} - -void LMS1xx::scanContinous(int start) { - char buf[100]; - sprintf(buf, "%c%s %d%c", 0x02, "sEN LMDscandata", start, 0x03); - - ssize_t res = write(sockDesc, buf, strlen(buf)); - - int len = read(sockDesc, buf, 100); - - if (buf[0] != 0x02) - printf("invalid packet recieved\n"); - - if (debug) { - buf[len] = 0; - printf("%s\n", buf); - } - - if (start = 0) { - for (int i = 0; i < 10; i++) - ssize_t res = read(sockDesc, buf, 100); - } -} - -bool LMS1xx::getData(scanData& data) { - char buf[20000]; - fd_set rfds; - struct timeval tv; - int retval, len = 0; - int bytes_read = 0; - int inc = 0; - int r_off = 0; - - do{ - FD_ZERO(&rfds); - FD_SET(sockDesc, &rfds); - tv.tv_sec = 1; - tv.tv_usec = 0; - bytes_read = 0; inc = 0; - retval = select(sockDesc + 1, &rfds, NULL, NULL, &tv); - if(retval) - { - bytes_read = read(sockDesc, buf, 1); - if(bytes_read > 1) - { - //std::cerr<<"Error in getData: received "<< bytes_read <<" bytes"<= 20000) - { - int i; - for(i = 0; i < len; i++) - { - if(buf[i] == 0x03) - { - len = i+1; - bFoundEnd = true; - //std::cerr<<"Found end!\n"; - } - if(bFoundEnd && buf[i] == 0x02) - { - //std::cerr<<"Found start!\n"; - bFoundStart = true; - } - } - if(!bFoundEnd) - { - char flushBuffer[100000]; - int bytesFlushed = recv(sockDesc, flushBuffer, 100000, 0); - //std::cerr<<"Flushed read buffer"< -#include - -int main() -{ - LMS1xx laser; - scanData data; - - laser.connect("192.168.1.2"); - if(!laser.isConnected()) - { - std::cout << "connection failend" << std::endl; - return 0; - } - - std::cout << "Connected to laser" << std::endl; - - std::cout << "Loging in ..." << std::endl; - laser.login(); - - laser.stopMeas(); - - std::cout << "Geting scan configuration ..." << ::std::endl; - scanCfg c = laser.getScanCfg(); - - //std::cout << "Scanning Frequency : " << c.scaningFrequency/100.0 << "Hz AngleResolution : " << c.angleResolution/10000.0 << "deg " << std::endl; - - c.angleResolution = 5000; - c.scaningFrequency = 5000; - - laser.setScanCfg(c); - - scanDataCfg cc; - cc.deviceName = false; - cc.encoder = 0; - cc.outputChannel = 3; - cc.remission = true; - cc.resolution = 0; - cc.position = false; - cc.outputInterval = 1; - - laser.setScanDataCfg(cc); - - int ret = 0; - std::cout << "Start measurements ..." << std::endl; - laser.startMeas(); - - std::cout << "Wait for ready status ..." << std::endl; - ret = 0; - while (ret != 7) - { - ret = laser.queryStatus(); - std::cout << "status : " << ret << std::endl; - sleep(1); - } - std::cout << "Laser ready" << std::endl; - - std::cout << "Start continuous data transmission ..." << std::endl; - laser.scanContinous(1); - - for(int i =0; i < 3; i++) - { - std::cout << "Receive data sample ..." << std::endl; - laser.getData(data); - } - - std::cout << "Stop continuous data transmission ..." << std::endl; - laser.scanContinous(0); - - laser.stopMeas(); - - std::cout << "Disconnect from laser" << std::endl; - laser.disconnect(); - - return 0; -} diff --git a/cob_sick_lms1xx/doc/Doxyfile b/cob_sick_lms1xx/doc/Doxyfile deleted file mode 100644 index dc80032ee..000000000 --- a/cob_sick_lms1xx/doc/Doxyfile +++ /dev/null @@ -1,1587 +0,0 @@ -#!/usr/bin/env python -# -# Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - - -# Doxyfile 1.6.1 - -# This file describes the settings to be used by the documentation system -# doxygen (www.doxygen.org) for a project -# -# All text after a hash (#) is considered a comment and will be ignored -# The format is: -# TAG = value [value, ...] -# For lists items can also be appended using: -# TAG += value [value, ...] -# Values that contain spaces should be placed between quotes (" ") - -#--------------------------------------------------------------------------- -# Project related configuration options -#--------------------------------------------------------------------------- - -# This tag specifies the encoding used for all characters in the config file -# that follow. The default is UTF-8 which is also the encoding used for all -# text before the first occurrence of this tag. Doxygen uses libiconv (or the -# iconv built into libc) for the transcoding. See -# http://www.gnu.org/software/libiconv for the list of possible encodings. - -DOXYFILE_ENCODING = UTF-8 - -# The PROJECT_NAME tag is a single word (or a sequence of words surrounded -# by quotes) that should identify the project. - -PROJECT_NAME = "libLMS1xx" - -# The PROJECT_NUMBER tag can be used to enter a project or revision number. -# This could be handy for archiving the generated documentation or -# if some version control system is used. - -PROJECT_NUMBER = - -# The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute) -# base path where the generated documentation will be put. -# If a relative path is entered, it will be relative to the location -# where doxygen was started. If left blank the current directory will be used. - -OUTPUT_DIRECTORY = doc - -# If the CREATE_SUBDIRS tag is set to YES, then doxygen will create -# 4096 sub-directories (in 2 levels) under the output directory of each output -# format and will distribute the generated files over these directories. -# Enabling this option can be useful when feeding doxygen a huge amount of -# source files, where putting all generated files in the same directory would -# otherwise cause performance problems for the file system. - -CREATE_SUBDIRS = NO - -# The OUTPUT_LANGUAGE tag is used to specify the language in which all -# documentation generated by doxygen is written. Doxygen will use this -# information to generate all constant output in the proper language. -# The default language is English, other supported languages are: -# Afrikaans, Arabic, Brazilian, Catalan, Chinese, Chinese-Traditional, -# Croatian, Czech, Danish, Dutch, Esperanto, Farsi, Finnish, French, German, -# Greek, Hungarian, Italian, Japanese, Japanese-en (Japanese with English -# messages), Korean, Korean-en, Lithuanian, Norwegian, Macedonian, Persian, -# Polish, Portuguese, Romanian, Russian, Serbian, Serbian-Cyrilic, Slovak, -# Slovene, Spanish, Swedish, Ukrainian, and Vietnamese. - -OUTPUT_LANGUAGE = English - -# If the BRIEF_MEMBER_DESC tag is set to YES (the default) Doxygen will -# include brief member descriptions after the members that are listed in -# the file and class documentation (similar to JavaDoc). -# Set to NO to disable this. - -BRIEF_MEMBER_DESC = YES - -# If the REPEAT_BRIEF tag is set to YES (the default) Doxygen will prepend -# the brief description of a member or function before the detailed description. -# Note: if both HIDE_UNDOC_MEMBERS and BRIEF_MEMBER_DESC are set to NO, the -# brief descriptions will be completely suppressed. - -REPEAT_BRIEF = YES - -# This tag implements a quasi-intelligent brief description abbreviator -# that is used to form the text in various listings. Each string -# in this list, if found as the leading text of the brief description, will be -# stripped from the text and the result after processing the whole list, is -# used as the annotated text. Otherwise, the brief description is used as-is. -# If left blank, the following values are used ("$name" is automatically -# replaced with the name of the entity): "The $name class" "The $name widget" -# "The $name file" "is" "provides" "specifies" "contains" -# "represents" "a" "an" "the" - -ABBREVIATE_BRIEF = "The $name class " \ - "The $name widget " \ - "The $name file " \ - is \ - provides \ - specifies \ - contains \ - represents \ - a \ - an \ - the - -# If the ALWAYS_DETAILED_SEC and REPEAT_BRIEF tags are both set to YES then -# Doxygen will generate a detailed section even if there is only a brief -# description. - -ALWAYS_DETAILED_SEC = NO - -# If the INLINE_INHERITED_MEMB tag is set to YES, doxygen will show all -# inherited members of a class in the documentation of that class as if those -# members were ordinary class members. Constructors, destructors and assignment -# operators of the base classes will not be shown. - -INLINE_INHERITED_MEMB = NO - -# If the FULL_PATH_NAMES tag is set to YES then Doxygen will prepend the full -# path before files name in the file list and in the header files. If set -# to NO the shortest path that makes the file name unique will be used. - -FULL_PATH_NAMES = YES - -# If the FULL_PATH_NAMES tag is set to YES then the STRIP_FROM_PATH tag -# can be used to strip a user-defined part of the path. Stripping is -# only done if one of the specified strings matches the left-hand part of -# the path. The tag can be used to show relative paths in the file list. -# If left blank the directory from which doxygen is run is used as the -# path to strip. - -STRIP_FROM_PATH = . - -# The STRIP_FROM_INC_PATH tag can be used to strip a user-defined part of -# the path mentioned in the documentation of a class, which tells -# the reader which header file to include in order to use a class. -# If left blank only the name of the header file containing the class -# definition is used. Otherwise one should specify the include paths that -# are normally passed to the compiler using the -I flag. - -STRIP_FROM_INC_PATH = - -# If the SHORT_NAMES tag is set to YES, doxygen will generate much shorter -# (but less readable) file names. This can be useful is your file systems -# doesn't support long names like on DOS, Mac, or CD-ROM. - -SHORT_NAMES = NO - -# If the JAVADOC_AUTOBRIEF tag is set to YES then Doxygen -# will interpret the first line (until the first dot) of a JavaDoc-style -# comment as the brief description. If set to NO, the JavaDoc -# comments will behave just like regular Qt-style comments -# (thus requiring an explicit @brief command for a brief description.) - -JAVADOC_AUTOBRIEF = NO - -# If the QT_AUTOBRIEF tag is set to YES then Doxygen will -# interpret the first line (until the first dot) of a Qt-style -# comment as the brief description. If set to NO, the comments -# will behave just like regular Qt-style comments (thus requiring -# an explicit \brief command for a brief description.) - -QT_AUTOBRIEF = NO - -# The MULTILINE_CPP_IS_BRIEF tag can be set to YES to make Doxygen -# treat a multi-line C++ special comment block (i.e. a block of //! or /// -# comments) as a brief description. This used to be the default behaviour. -# The new default is to treat a multi-line C++ comment block as a detailed -# description. Set this tag to YES if you prefer the old behaviour instead. - -MULTILINE_CPP_IS_BRIEF = NO - -# If the INHERIT_DOCS tag is set to YES (the default) then an undocumented -# member inherits the documentation from any documented member that it -# re-implements. - -INHERIT_DOCS = YES - -# If the SEPARATE_MEMBER_PAGES tag is set to YES, then doxygen will produce -# a new page for each member. If set to NO, the documentation of a member will -# be part of the file/class/namespace that contains it. - -SEPARATE_MEMBER_PAGES = NO - -# The TAB_SIZE tag can be used to set the number of spaces in a tab. -# Doxygen uses this value to replace tabs by spaces in code fragments. - -TAB_SIZE = 8 - -# This tag can be used to specify a number of aliases that acts -# as commands in the documentation. An alias has the form "name=value". -# For example adding "sideeffect=\par Side Effects:\n" will allow you to -# put the command \sideeffect (or @sideeffect) in the documentation, which -# will result in a user-defined paragraph with heading "Side Effects:". -# You can put \n's in the value part of an alias to insert newlines. - -ALIASES = - -# Set the OPTIMIZE_OUTPUT_FOR_C tag to YES if your project consists of C -# sources only. Doxygen will then generate output that is more tailored for C. -# For instance, some of the names that are used will be different. The list -# of all members will be omitted, etc. - -OPTIMIZE_OUTPUT_FOR_C = NO - -# Set the OPTIMIZE_OUTPUT_JAVA tag to YES if your project consists of Java -# sources only. Doxygen will then generate output that is more tailored for -# Java. For instance, namespaces will be presented as packages, qualified -# scopes will look different, etc. - -OPTIMIZE_OUTPUT_JAVA = NO - -# Set the OPTIMIZE_FOR_FORTRAN tag to YES if your project consists of Fortran -# sources only. Doxygen will then generate output that is more tailored for -# Fortran. - -OPTIMIZE_FOR_FORTRAN = NO - -# Set the OPTIMIZE_OUTPUT_VHDL tag to YES if your project consists of VHDL -# sources. Doxygen will then generate output that is tailored for -# VHDL. - -OPTIMIZE_OUTPUT_VHDL = NO - -# Doxygen selects the parser to use depending on the extension of the files it parses. -# With this tag you can assign which parser to use for a given extension. -# Doxygen has a built-in mapping, but you can override or extend it using this tag. -# The format is ext=language, where ext is a file extension, and language is one of -# the parsers supported by doxygen: IDL, Java, Javascript, C#, C, C++, D, PHP, -# Objective-C, Python, Fortran, VHDL, C, C++. For instance to make doxygen treat -# .inc files as Fortran files (default is PHP), and .f files as C (default is Fortran), -# use: inc=Fortran f=C. Note that for custom extensions you also need to set -# FILE_PATTERNS otherwise the files are not read by doxygen. - -EXTENSION_MAPPING = - -# If you use STL classes (i.e. std::string, std::vector, etc.) but do not want -# to include (a tag file for) the STL sources as input, then you should -# set this tag to YES in order to let doxygen match functions declarations and -# definitions whose arguments contain STL classes (e.g. func(std::string); v.s. -# func(std::string) {}). This also make the inheritance and collaboration -# diagrams that involve STL classes more complete and accurate. - -BUILTIN_STL_SUPPORT = NO - -# If you use Microsoft's C++/CLI language, you should set this option to YES to -# enable parsing support. - -CPP_CLI_SUPPORT = NO - -# Set the SIP_SUPPORT tag to YES if your project consists of sip sources only. -# Doxygen will parse them like normal C++ but will assume all classes use public -# instead of private inheritance when no explicit protection keyword is present. - -SIP_SUPPORT = NO - -# For Microsoft's IDL there are propget and propput attributes to indicate getter -# and setter methods for a property. Setting this option to YES (the default) -# will make doxygen to replace the get and set methods by a property in the -# documentation. This will only work if the methods are indeed getting or -# setting a simple type. If this is not the case, or you want to show the -# methods anyway, you should set this option to NO. - -IDL_PROPERTY_SUPPORT = YES - -# If member grouping is used in the documentation and the DISTRIBUTE_GROUP_DOC -# tag is set to YES, then doxygen will reuse the documentation of the first -# member in the group (if any) for the other members of the group. By default -# all members of a group must be documented explicitly. - -DISTRIBUTE_GROUP_DOC = NO - -# Set the SUBGROUPING tag to YES (the default) to allow class member groups of -# the same type (for instance a group of public functions) to be put as a -# subgroup of that type (e.g. under the Public Functions section). Set it to -# NO to prevent subgrouping. Alternatively, this can be done per class using -# the \nosubgrouping command. - -SUBGROUPING = YES - -# When TYPEDEF_HIDES_STRUCT is enabled, a typedef of a struct, union, or enum -# is documented as struct, union, or enum with the name of the typedef. So -# typedef struct TypeS {} TypeT, will appear in the documentation as a struct -# with name TypeT. When disabled the typedef will appear as a member of a file, -# namespace, or class. And the struct will be named TypeS. This can typically -# be useful for C code in case the coding convention dictates that all compound -# types are typedef'ed and only the typedef is referenced, never the tag name. - -TYPEDEF_HIDES_STRUCT = YES - -# The SYMBOL_CACHE_SIZE determines the size of the internal cache use to -# determine which symbols to keep in memory and which to flush to disk. -# When the cache is full, less often used symbols will be written to disk. -# For small to medium size projects (<1000 input files) the default value is -# probably good enough. For larger projects a too small cache size can cause -# doxygen to be busy swapping symbols to and from disk most of the time -# causing a significant performance penality. -# If the system has enough physical memory increasing the cache will improve the -# performance by keeping more symbols in memory. Note that the value works on -# a logarithmic scale so increasing the size by one will rougly double the -# memory usage. The cache size is given by this formula: -# 2^(16+SYMBOL_CACHE_SIZE). The valid range is 0..9, the default is 0, -# corresponding to a cache size of 2^16 = 65536 symbols - -SYMBOL_CACHE_SIZE = 0 - -#--------------------------------------------------------------------------- -# Build related configuration options -#--------------------------------------------------------------------------- - -# If the EXTRACT_ALL tag is set to YES doxygen will assume all entities in -# documentation are documented, even if no documentation was available. -# Private class members and static file members will be hidden unless -# the EXTRACT_PRIVATE and EXTRACT_STATIC tags are set to YES - -EXTRACT_ALL = YES - -# If the EXTRACT_PRIVATE tag is set to YES all private members of a class -# will be included in the documentation. - -EXTRACT_PRIVATE = YES - -# If the EXTRACT_STATIC tag is set to YES all static members of a file -# will be included in the documentation. - -EXTRACT_STATIC = YES - -# If the EXTRACT_LOCAL_CLASSES tag is set to YES classes (and structs) -# defined locally in source files will be included in the documentation. -# If set to NO only classes defined in header files are included. - -EXTRACT_LOCAL_CLASSES = YES - -# This flag is only useful for Objective-C code. When set to YES local -# methods, which are defined in the implementation section but not in -# the interface are included in the documentation. -# If set to NO (the default) only methods in the interface are included. - -EXTRACT_LOCAL_METHODS = NO - -# If this flag is set to YES, the members of anonymous namespaces will be -# extracted and appear in the documentation as a namespace called -# 'anonymous_namespace{file}', where file will be replaced with the base -# name of the file that contains the anonymous namespace. By default -# anonymous namespace are hidden. - -EXTRACT_ANON_NSPACES = NO - -# If the HIDE_UNDOC_MEMBERS tag is set to YES, Doxygen will hide all -# undocumented members of documented classes, files or namespaces. -# If set to NO (the default) these members will be included in the -# various overviews, but no documentation section is generated. -# This option has no effect if EXTRACT_ALL is enabled. - -HIDE_UNDOC_MEMBERS = NO - -# If the HIDE_UNDOC_CLASSES tag is set to YES, Doxygen will hide all -# undocumented classes that are normally visible in the class hierarchy. -# If set to NO (the default) these classes will be included in the various -# overviews. This option has no effect if EXTRACT_ALL is enabled. - -HIDE_UNDOC_CLASSES = NO - -# If the HIDE_FRIEND_COMPOUNDS tag is set to YES, Doxygen will hide all -# friend (class|struct|union) declarations. -# If set to NO (the default) these declarations will be included in the -# documentation. - -HIDE_FRIEND_COMPOUNDS = NO - -# If the HIDE_IN_BODY_DOCS tag is set to YES, Doxygen will hide any -# documentation blocks found inside the body of a function. -# If set to NO (the default) these blocks will be appended to the -# function's detailed documentation block. - -HIDE_IN_BODY_DOCS = NO - -# The INTERNAL_DOCS tag determines if documentation -# that is typed after a \internal command is included. If the tag is set -# to NO (the default) then the documentation will be excluded. -# Set it to YES to include the internal documentation. - -INTERNAL_DOCS = NO - -# If the CASE_SENSE_NAMES tag is set to NO then Doxygen will only generate -# file names in lower-case letters. If set to YES upper-case letters are also -# allowed. This is useful if you have classes or files whose names only differ -# in case and if your file system supports case sensitive file names. Windows -# and Mac users are advised to set this option to NO. - -CASE_SENSE_NAMES = YES - -# If the HIDE_SCOPE_NAMES tag is set to NO (the default) then Doxygen -# will show members with their full class and namespace scopes in the -# documentation. If set to YES the scope will be hidden. - -HIDE_SCOPE_NAMES = NO - -# If the SHOW_INCLUDE_FILES tag is set to YES (the default) then Doxygen -# will put a list of the files that are included by a file in the documentation -# of that file. - -SHOW_INCLUDE_FILES = YES - -# If the INLINE_INFO tag is set to YES (the default) then a tag [inline] -# is inserted in the documentation for inline members. - -INLINE_INFO = YES - -# If the SORT_MEMBER_DOCS tag is set to YES (the default) then doxygen -# will sort the (detailed) documentation of file and class members -# alphabetically by member name. If set to NO the members will appear in -# declaration order. - -SORT_MEMBER_DOCS = YES - -# If the SORT_BRIEF_DOCS tag is set to YES then doxygen will sort the -# brief documentation of file, namespace and class members alphabetically -# by member name. If set to NO (the default) the members will appear in -# declaration order. - -SORT_BRIEF_DOCS = NO - -# If the SORT_MEMBERS_CTORS_1ST tag is set to YES then doxygen -# will sort the (brief and detailed) documentation of class members so that -# constructors and destructors are listed first. If set to NO (the default) -# the constructors will appear in the respective orders defined by -# SORT_MEMBER_DOCS and SORT_BRIEF_DOCS. -# This tag will be ignored for brief docs if SORT_BRIEF_DOCS is set to NO -# and ignored for detailed docs if SORT_MEMBER_DOCS is set to NO. - -SORT_MEMBERS_CTORS_1ST = NO - -# If the SORT_GROUP_NAMES tag is set to YES then doxygen will sort the -# hierarchy of group names into alphabetical order. If set to NO (the default) -# the group names will appear in their defined order. - -SORT_GROUP_NAMES = NO - -# If the SORT_BY_SCOPE_NAME tag is set to YES, the class list will be -# sorted by fully-qualified names, including namespaces. If set to -# NO (the default), the class list will be sorted only by class name, -# not including the namespace part. -# Note: This option is not very useful if HIDE_SCOPE_NAMES is set to YES. -# Note: This option applies only to the class list, not to the -# alphabetical list. - -SORT_BY_SCOPE_NAME = NO - -# The GENERATE_TODOLIST tag can be used to enable (YES) or -# disable (NO) the todo list. This list is created by putting \todo -# commands in the documentation. - -GENERATE_TODOLIST = YES - -# The GENERATE_TESTLIST tag can be used to enable (YES) or -# disable (NO) the test list. This list is created by putting \test -# commands in the documentation. - -GENERATE_TESTLIST = YES - -# The GENERATE_BUGLIST tag can be used to enable (YES) or -# disable (NO) the bug list. This list is created by putting \bug -# commands in the documentation. - -GENERATE_BUGLIST = YES - -# The GENERATE_DEPRECATEDLIST tag can be used to enable (YES) or -# disable (NO) the deprecated list. This list is created by putting -# \deprecated commands in the documentation. - -GENERATE_DEPRECATEDLIST= YES - -# The ENABLED_SECTIONS tag can be used to enable conditional -# documentation sections, marked by \if sectionname ... \endif. - -ENABLED_SECTIONS = - -# The MAX_INITIALIZER_LINES tag determines the maximum number of lines -# the initial value of a variable or define consists of for it to appear in -# the documentation. If the initializer consists of more lines than specified -# here it will be hidden. Use a value of 0 to hide initializers completely. -# The appearance of the initializer of individual variables and defines in the -# documentation can be controlled using \showinitializer or \hideinitializer -# command in the documentation regardless of this setting. - -MAX_INITIALIZER_LINES = 30 - -# Set the SHOW_USED_FILES tag to NO to disable the list of files generated -# at the bottom of the documentation of classes and structs. If set to YES the -# list will mention the files that were used to generate the documentation. - -SHOW_USED_FILES = YES - -# If the sources in your project are distributed over multiple directories -# then setting the SHOW_DIRECTORIES tag to YES will show the directory hierarchy -# in the documentation. The default is NO. - -SHOW_DIRECTORIES = NO - -# Set the SHOW_FILES tag to NO to disable the generation of the Files page. -# This will remove the Files entry from the Quick Index and from the -# Folder Tree View (if specified). The default is YES. - -SHOW_FILES = YES - -# Set the SHOW_NAMESPACES tag to NO to disable the generation of the -# Namespaces page. This will remove the Namespaces entry from the Quick Index -# and from the Folder Tree View (if specified). The default is YES. - -SHOW_NAMESPACES = YES - -# The FILE_VERSION_FILTER tag can be used to specify a program or script that -# doxygen should invoke to get the current version for each file (typically from -# the version control system). Doxygen will invoke the program by executing (via -# popen()) the command , where is the value of -# the FILE_VERSION_FILTER tag, and is the name of an input file -# provided by doxygen. Whatever the program writes to standard output -# is used as the file version. See the manual for examples. - -FILE_VERSION_FILTER = - -# The LAYOUT_FILE tag can be used to specify a layout file which will be parsed by -# doxygen. The layout file controls the global structure of the generated output files -# in an output format independent way. The create the layout file that represents -# doxygen's defaults, run doxygen with the -l option. You can optionally specify a -# file name after the option, if omitted DoxygenLayout.xml will be used as the name -# of the layout file. - -LAYOUT_FILE = - -#--------------------------------------------------------------------------- -# configuration options related to warning and progress messages -#--------------------------------------------------------------------------- - -# The QUIET tag can be used to turn on/off the messages that are generated -# by doxygen. Possible values are YES and NO. If left blank NO is used. - -QUIET = NO - -# The WARNINGS tag can be used to turn on/off the warning messages that are -# generated by doxygen. Possible values are YES and NO. If left blank -# NO is used. - -WARNINGS = YES - -# If WARN_IF_UNDOCUMENTED is set to YES, then doxygen will generate warnings -# for undocumented members. If EXTRACT_ALL is set to YES then this flag will -# automatically be disabled. - -WARN_IF_UNDOCUMENTED = YES - -# If WARN_IF_DOC_ERROR is set to YES, doxygen will generate warnings for -# potential errors in the documentation, such as not documenting some -# parameters in a documented function, or documenting parameters that -# don't exist or using markup commands wrongly. - -WARN_IF_DOC_ERROR = YES - -# This WARN_NO_PARAMDOC option can be abled to get warnings for -# functions that are documented, but have no documentation for their parameters -# or return value. If set to NO (the default) doxygen will only warn about -# wrong or incomplete parameter documentation, but not about the absence of -# documentation. - -WARN_NO_PARAMDOC = NO - -# The WARN_FORMAT tag determines the format of the warning messages that -# doxygen can produce. The string should contain the $file, $line, and $text -# tags, which will be replaced by the file and line number from which the -# warning originated and the warning text. Optionally the format may contain -# $version, which will be replaced by the version of the file (if it could -# be obtained via FILE_VERSION_FILTER) - -WARN_FORMAT = "$file:$line: $text" - -# The WARN_LOGFILE tag can be used to specify a file to which warning -# and error messages should be written. If left blank the output is written -# to stderr. - -WARN_LOGFILE = - -#--------------------------------------------------------------------------- -# configuration options related to the input files -#--------------------------------------------------------------------------- - -# The INPUT tag can be used to specify the files and/or directories that contain -# documented source files. You may enter file names like "myfile.cpp" or -# directories like "/usr/src/myproject". Separate the files or directories -# with spaces. - -INPUT = ./ - - -# This tag can be used to specify the character encoding of the source files -# that doxygen parses. Internally doxygen uses the UTF-8 encoding, which is -# also the default input encoding. Doxygen uses libiconv (or the iconv built -# into libc) for the transcoding. See http://www.gnu.org/software/libiconv for -# the list of possible encodings. - -INPUT_ENCODING = UTF-8 - -# If the value of the INPUT tag contains directories, you can use the -# FILE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp -# and *.h) to filter out the source-files in the directories. If left -# blank the following patterns are tested: -# *.c *.cc *.cxx *.cpp *.c++ *.java *.ii *.ixx *.ipp *.i++ *.inl *.h *.hh *.hxx -# *.hpp *.h++ *.idl *.odl *.cs *.php *.php3 *.inc *.m *.mm *.py *.f90 - -FILE_PATTERNS = *.c \ - *.cc \ - *.cxx \ - *.cpp \ - *.c++ \ - *.d \ - *.java \ - *.ii \ - *.ixx \ - *.ipp \ - *.i++ \ - *.inl \ - *.h \ - *.hh \ - *.hxx \ - *.hpp \ - *.h++ \ - *.idl \ - *.odl \ - *.cs \ - *.php \ - *.php3 \ - *.inc \ - *.m \ - *.mm \ - *.dox \ - *.py \ - *.f90 \ - *.C \ - *.CC \ - *.C++ \ - *.II \ - *.I++ \ - *.H \ - *.HH \ - *.H++ \ - *.CS \ - *.PHP \ - *.PHP3 \ - *.M \ - *.MM \ - *.PY \ - *.F90 - -# The RECURSIVE tag can be used to turn specify whether or not subdirectories -# should be searched for input files as well. Possible values are YES and NO. -# If left blank NO is used. - -RECURSIVE = NO - -# The EXCLUDE tag can be used to specify files and/or directories that should -# excluded from the INPUT source files. This way you can easily exclude a -# subdirectory from a directory tree whose root is specified with the INPUT tag. - -EXCLUDE = - -# The EXCLUDE_SYMLINKS tag can be used select whether or not files or -# directories that are symbolic links (a Unix filesystem feature) are excluded -# from the input. - -EXCLUDE_SYMLINKS = NO - -# If the value of the INPUT tag contains directories, you can use the -# EXCLUDE_PATTERNS tag to specify one or more wildcard patterns to exclude -# certain files from those directories. Note that the wildcards are matched -# against the file with absolute path, so to exclude all test directories -# for example use the pattern */test/* - -EXCLUDE_PATTERNS = - -# The EXCLUDE_SYMBOLS tag can be used to specify one or more symbol names -# (namespaces, classes, functions, etc.) that should be excluded from the -# output. The symbol name can be a fully qualified name, a word, or if the -# wildcard * is used, a substring. Examples: ANamespace, AClass, -# AClass::ANamespace, ANamespace::*Test - -EXCLUDE_SYMBOLS = - -# The EXAMPLE_PATH tag can be used to specify one or more files or -# directories that contain example code fragments that are included (see -# the \include command). - -EXAMPLE_PATH = - -# If the value of the EXAMPLE_PATH tag contains directories, you can use the -# EXAMPLE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp -# and *.h) to filter out the source-files in the directories. If left -# blank all files are included. - -EXAMPLE_PATTERNS = * - -# If the EXAMPLE_RECURSIVE tag is set to YES then subdirectories will be -# searched for input files to be used with the \include or \dontinclude -# commands irrespective of the value of the RECURSIVE tag. -# Possible values are YES and NO. If left blank NO is used. - -EXAMPLE_RECURSIVE = NO - -# The IMAGE_PATH tag can be used to specify one or more files or -# directories that contain image that are included in the documentation (see -# the \image command). - -IMAGE_PATH = - -# The INPUT_FILTER tag can be used to specify a program that doxygen should -# invoke to filter for each input file. Doxygen will invoke the filter program -# by executing (via popen()) the command , where -# is the value of the INPUT_FILTER tag, and is the name of an -# input file. Doxygen will then use the output that the filter program writes -# to standard output. If FILTER_PATTERNS is specified, this tag will be -# ignored. - -INPUT_FILTER = - -# The FILTER_PATTERNS tag can be used to specify filters on a per file pattern -# basis. Doxygen will compare the file name with each pattern and apply the -# filter if there is a match. The filters are a list of the form: -# pattern=filter (like *.cpp=my_cpp_filter). See INPUT_FILTER for further -# info on how filters are used. If FILTER_PATTERNS is empty, INPUT_FILTER -# is applied to all files. - -FILTER_PATTERNS = - -# If the FILTER_SOURCE_FILES tag is set to YES, the input filter (if set using -# INPUT_FILTER) will be used to filter the input files when producing source -# files to browse (i.e. when SOURCE_BROWSER is set to YES). - -FILTER_SOURCE_FILES = NO - -#--------------------------------------------------------------------------- -# configuration options related to source browsing -#--------------------------------------------------------------------------- - -# If the SOURCE_BROWSER tag is set to YES then a list of source files will -# be generated. Documented entities will be cross-referenced with these sources. -# Note: To get rid of all source code in the generated output, make sure also -# VERBATIM_HEADERS is set to NO. - -SOURCE_BROWSER = NO - -# Setting the INLINE_SOURCES tag to YES will include the body -# of functions and classes directly in the documentation. - -INLINE_SOURCES = NO - -# Setting the STRIP_CODE_COMMENTS tag to YES (the default) will instruct -# doxygen to hide any special comment blocks from generated source code -# fragments. Normal C and C++ comments will always remain visible. - -STRIP_CODE_COMMENTS = YES - -# If the REFERENCED_BY_RELATION tag is set to YES -# then for each documented function all documented -# functions referencing it will be listed. - -REFERENCED_BY_RELATION = NO - -# If the REFERENCES_RELATION tag is set to YES -# then for each documented function all documented entities -# called/used by that function will be listed. - -REFERENCES_RELATION = NO - -# If the REFERENCES_LINK_SOURCE tag is set to YES (the default) -# and SOURCE_BROWSER tag is set to YES, then the hyperlinks from -# functions in REFERENCES_RELATION and REFERENCED_BY_RELATION lists will -# link to the source code. Otherwise they will link to the documentation. - -REFERENCES_LINK_SOURCE = YES - -# If the USE_HTAGS tag is set to YES then the references to source code -# will point to the HTML generated by the htags(1) tool instead of doxygen -# built-in source browser. The htags tool is part of GNU's global source -# tagging system (see http://www.gnu.org/software/global/global.html). You -# will need version 4.8.6 or higher. - -USE_HTAGS = NO - -# If the VERBATIM_HEADERS tag is set to YES (the default) then Doxygen -# will generate a verbatim copy of the header file for each class for -# which an include is specified. Set to NO to disable this. - -VERBATIM_HEADERS = NO - -#--------------------------------------------------------------------------- -# configuration options related to the alphabetical class index -#--------------------------------------------------------------------------- - -# If the ALPHABETICAL_INDEX tag is set to YES, an alphabetical index -# of all compounds will be generated. Enable this if the project -# contains a lot of classes, structs, unions or interfaces. - -ALPHABETICAL_INDEX = NO - -# If the alphabetical index is enabled (see ALPHABETICAL_INDEX) then -# the COLS_IN_ALPHA_INDEX tag can be used to specify the number of columns -# in which this list will be split (can be a number in the range [1..20]) - -COLS_IN_ALPHA_INDEX = 5 - -# In case all classes in a project start with a common prefix, all -# classes will be put under the same header in the alphabetical index. -# The IGNORE_PREFIX tag can be used to specify one or more prefixes that -# should be ignored while generating the index headers. - -IGNORE_PREFIX = - -#--------------------------------------------------------------------------- -# configuration options related to the HTML output -#--------------------------------------------------------------------------- - -# If the GENERATE_HTML tag is set to YES (the default) Doxygen will -# generate HTML output. - -GENERATE_HTML = YES - -# The HTML_OUTPUT tag is used to specify where the HTML docs will be put. -# If a relative path is entered the value of OUTPUT_DIRECTORY will be -# put in front of it. If left blank `html' will be used as the default path. - -HTML_OUTPUT = html - -# The HTML_FILE_EXTENSION tag can be used to specify the file extension for -# each generated HTML page (for example: .htm,.php,.asp). If it is left blank -# doxygen will generate files with .html extension. - -HTML_FILE_EXTENSION = .html - -# The HTML_HEADER tag can be used to specify a personal HTML header for -# each generated HTML page. If it is left blank doxygen will generate a -# standard header. - -HTML_HEADER = - -# The HTML_FOOTER tag can be used to specify a personal HTML footer for -# each generated HTML page. If it is left blank doxygen will generate a -# standard footer. - -HTML_FOOTER = - -# The HTML_STYLESHEET tag can be used to specify a user-defined cascading -# style sheet that is used by each HTML page. It can be used to -# fine-tune the look of the HTML output. If the tag is left blank doxygen -# will generate a default style sheet. Note that doxygen will try to copy -# the style sheet file to the HTML output directory, so don't put your own -# stylesheet in the HTML output directory as well, or it will be erased! - -HTML_STYLESHEET = - -# If the HTML_ALIGN_MEMBERS tag is set to YES, the members of classes, -# files or namespaces will be aligned in HTML using tables. If set to -# NO a bullet list will be used. - -HTML_ALIGN_MEMBERS = YES - -# If the HTML_DYNAMIC_SECTIONS tag is set to YES then the generated HTML -# documentation will contain sections that can be hidden and shown after the -# page has loaded. For this to work a browser that supports -# JavaScript and DHTML is required (for instance Mozilla 1.0+, Firefox -# Netscape 6.0+, Internet explorer 5.0+, Konqueror, or Safari). - -HTML_DYNAMIC_SECTIONS = NO - -# If the GENERATE_DOCSET tag is set to YES, additional index files -# will be generated that can be used as input for Apple's Xcode 3 -# integrated development environment, introduced with OSX 10.5 (Leopard). -# To create a documentation set, doxygen will generate a Makefile in the -# HTML output directory. Running make will produce the docset in that -# directory and running "make install" will install the docset in -# ~/Library/Developer/Shared/Documentation/DocSets so that Xcode will find -# it at startup. -# See http://developer.apple.com/tools/creatingdocsetswithdoxygen.html for more information. - -GENERATE_DOCSET = NO - -# When GENERATE_DOCSET tag is set to YES, this tag determines the name of the -# feed. A documentation feed provides an umbrella under which multiple -# documentation sets from a single provider (such as a company or product suite) -# can be grouped. - -DOCSET_FEEDNAME = "Doxygen generated docs" - -# When GENERATE_DOCSET tag is set to YES, this tag specifies a string that -# should uniquely identify the documentation set bundle. This should be a -# reverse domain-name style string, e.g. com.mycompany.MyDocSet. Doxygen -# will append .docset to the name. - -DOCSET_BUNDLE_ID = org.doxygen.Project - -# If the GENERATE_HTMLHELP tag is set to YES, additional index files -# will be generated that can be used as input for tools like the -# Microsoft HTML help workshop to generate a compiled HTML help file (.chm) -# of the generated HTML documentation. - -GENERATE_HTMLHELP = NO - -# If the GENERATE_HTMLHELP tag is set to YES, the CHM_FILE tag can -# be used to specify the file name of the resulting .chm file. You -# can add a path in front of the file if the result should not be -# written to the html output directory. - -CHM_FILE = - -# If the GENERATE_HTMLHELP tag is set to YES, the HHC_LOCATION tag can -# be used to specify the location (absolute path including file name) of -# the HTML help compiler (hhc.exe). If non-empty doxygen will try to run -# the HTML help compiler on the generated index.hhp. - -HHC_LOCATION = - -# If the GENERATE_HTMLHELP tag is set to YES, the GENERATE_CHI flag -# controls if a separate .chi index file is generated (YES) or that -# it should be included in the master .chm file (NO). - -GENERATE_CHI = NO - -# If the GENERATE_HTMLHELP tag is set to YES, the CHM_INDEX_ENCODING -# is used to encode HtmlHelp index (hhk), content (hhc) and project file -# content. - -CHM_INDEX_ENCODING = - -# If the GENERATE_HTMLHELP tag is set to YES, the BINARY_TOC flag -# controls whether a binary table of contents is generated (YES) or a -# normal table of contents (NO) in the .chm file. - -BINARY_TOC = NO - -# The TOC_EXPAND flag can be set to YES to add extra items for group members -# to the contents of the HTML help documentation and to the tree view. - -TOC_EXPAND = NO - -# If the GENERATE_QHP tag is set to YES and both QHP_NAMESPACE and QHP_VIRTUAL_FOLDER -# are set, an additional index file will be generated that can be used as input for -# Qt's qhelpgenerator to generate a Qt Compressed Help (.qch) of the generated -# HTML documentation. - -GENERATE_QHP = NO - -# If the QHG_LOCATION tag is specified, the QCH_FILE tag can -# be used to specify the file name of the resulting .qch file. -# The path specified is relative to the HTML output folder. - -QCH_FILE = - -# The QHP_NAMESPACE tag specifies the namespace to use when generating -# Qt Help Project output. For more information please see -# http://doc.trolltech.com/qthelpproject.html#namespace - -QHP_NAMESPACE = - -# The QHP_VIRTUAL_FOLDER tag specifies the namespace to use when generating -# Qt Help Project output. For more information please see -# http://doc.trolltech.com/qthelpproject.html#virtual-folders - -QHP_VIRTUAL_FOLDER = doc - -# If QHP_CUST_FILTER_NAME is set, it specifies the name of a custom filter to add. -# For more information please see -# http://doc.trolltech.com/qthelpproject.html#custom-filters - -QHP_CUST_FILTER_NAME = - -# The QHP_CUST_FILT_ATTRS tag specifies the list of the attributes of the custom filter to add.For more information please see -# Qt Help Project / Custom Filters. - -QHP_CUST_FILTER_ATTRS = - -# The QHP_SECT_FILTER_ATTRS tag specifies the list of the attributes this project's -# filter section matches. -# Qt Help Project / Filter Attributes. - -QHP_SECT_FILTER_ATTRS = - -# If the GENERATE_QHP tag is set to YES, the QHG_LOCATION tag can -# be used to specify the location of Qt's qhelpgenerator. -# If non-empty doxygen will try to run qhelpgenerator on the generated -# .qhp file. - -QHG_LOCATION = - -# The DISABLE_INDEX tag can be used to turn on/off the condensed index at -# top of each HTML page. The value NO (the default) enables the index and -# the value YES disables it. - -DISABLE_INDEX = NO - -# This tag can be used to set the number of enum values (range [1..20]) -# that doxygen will group on one line in the generated HTML documentation. - -ENUM_VALUES_PER_LINE = 4 - -# The GENERATE_TREEVIEW tag is used to specify whether a tree-like index -# structure should be generated to display hierarchical information. -# If the tag value is set to YES, a side panel will be generated -# containing a tree-like index structure (just like the one that -# is generated for HTML Help). For this to work a browser that supports -# JavaScript, DHTML, CSS and frames is required (i.e. any modern browser). -# Windows users are probably better off using the HTML help feature. - -GENERATE_TREEVIEW = NO - -# By enabling USE_INLINE_TREES, doxygen will generate the Groups, Directories, -# and Class Hierarchy pages using a tree view instead of an ordered list. - -USE_INLINE_TREES = NO - -# If the treeview is enabled (see GENERATE_TREEVIEW) then this tag can be -# used to set the initial width (in pixels) of the frame in which the tree -# is shown. - -TREEVIEW_WIDTH = 250 - -# Use this tag to change the font size of Latex formulas included -# as images in the HTML documentation. The default is 10. Note that -# when you change the font size after a successful doxygen run you need -# to manually remove any form_*.png images from the HTML output directory -# to force them to be regenerated. - -FORMULA_FONTSIZE = 10 - -# When the SEARCHENGINE tag is enable doxygen will generate a search box -# for the HTML output. The underlying search engine uses javascript -# and DHTML and should work on any modern browser. Note that when using -# HTML help (GENERATE_HTMLHELP) or Qt help (GENERATE_QHP) -# there is already a search function so this one should typically -# be disabled. - -SEARCHENGINE = NO - -#--------------------------------------------------------------------------- -# configuration options related to the LaTeX output -#--------------------------------------------------------------------------- - -# If the GENERATE_LATEX tag is set to YES (the default) Doxygen will -# generate Latex output. - -GENERATE_LATEX = NO - -# The LATEX_OUTPUT tag is used to specify where the LaTeX docs will be put. -# If a relative path is entered the value of OUTPUT_DIRECTORY will be -# put in front of it. If left blank `latex' will be used as the default path. - -LATEX_OUTPUT = latex - -# The LATEX_CMD_NAME tag can be used to specify the LaTeX command name to be -# invoked. If left blank `latex' will be used as the default command name. - -LATEX_CMD_NAME = latex - -# The MAKEINDEX_CMD_NAME tag can be used to specify the command name to -# generate index for LaTeX. If left blank `makeindex' will be used as the -# default command name. - -MAKEINDEX_CMD_NAME = makeindex - -# If the COMPACT_LATEX tag is set to YES Doxygen generates more compact -# LaTeX documents. This may be useful for small projects and may help to -# save some trees in general. - -COMPACT_LATEX = NO - -# The PAPER_TYPE tag can be used to set the paper type that is used -# by the printer. Possible values are: a4, a4wide, letter, legal and -# executive. If left blank a4wide will be used. - -PAPER_TYPE = a4wide - -# The EXTRA_PACKAGES tag can be to specify one or more names of LaTeX -# packages that should be included in the LaTeX output. - -EXTRA_PACKAGES = - -# The LATEX_HEADER tag can be used to specify a personal LaTeX header for -# the generated latex document. The header should contain everything until -# the first chapter. If it is left blank doxygen will generate a -# standard header. Notice: only use this tag if you know what you are doing! - -LATEX_HEADER = - -# If the PDF_HYPERLINKS tag is set to YES, the LaTeX that is generated -# is prepared for conversion to pdf (using ps2pdf). The pdf file will -# contain links (just like the HTML output) instead of page references -# This makes the output suitable for online browsing using a pdf viewer. - -PDF_HYPERLINKS = YES - -# If the USE_PDFLATEX tag is set to YES, pdflatex will be used instead of -# plain latex in the generated Makefile. Set this option to YES to get a -# higher quality PDF documentation. - -USE_PDFLATEX = YES - -# If the LATEX_BATCHMODE tag is set to YES, doxygen will add the \\batchmode. -# command to the generated LaTeX files. This will instruct LaTeX to keep -# running if errors occur, instead of asking the user for help. -# This option is also used when generating formulas in HTML. - -LATEX_BATCHMODE = NO - -# If LATEX_HIDE_INDICES is set to YES then doxygen will not -# include the index chapters (such as File Index, Compound Index, etc.) -# in the output. - -LATEX_HIDE_INDICES = NO - -# If LATEX_SOURCE_CODE is set to YES then doxygen will include -# source code with syntax highlighting in the LaTeX output. -# Note that which sources are shown also depends on other settings -# such as SOURCE_BROWSER. - -LATEX_SOURCE_CODE = NO - -#--------------------------------------------------------------------------- -# configuration options related to the RTF output -#--------------------------------------------------------------------------- - -# If the GENERATE_RTF tag is set to YES Doxygen will generate RTF output -# The RTF output is optimized for Word 97 and may not look very pretty with -# other RTF readers or editors. - -GENERATE_RTF = NO - -# The RTF_OUTPUT tag is used to specify where the RTF docs will be put. -# If a relative path is entered the value of OUTPUT_DIRECTORY will be -# put in front of it. If left blank `rtf' will be used as the default path. - -RTF_OUTPUT = rtf - -# If the COMPACT_RTF tag is set to YES Doxygen generates more compact -# RTF documents. This may be useful for small projects and may help to -# save some trees in general. - -COMPACT_RTF = NO - -# If the RTF_HYPERLINKS tag is set to YES, the RTF that is generated -# will contain hyperlink fields. The RTF file will -# contain links (just like the HTML output) instead of page references. -# This makes the output suitable for online browsing using WORD or other -# programs which support those fields. -# Note: wordpad (write) and others do not support links. - -RTF_HYPERLINKS = NO - -# Load stylesheet definitions from file. Syntax is similar to doxygen's -# config file, i.e. a series of assignments. You only have to provide -# replacements, missing definitions are set to their default value. - -RTF_STYLESHEET_FILE = - -# Set optional variables used in the generation of an rtf document. -# Syntax is similar to doxygen's config file. - -RTF_EXTENSIONS_FILE = - -#--------------------------------------------------------------------------- -# configuration options related to the man page output -#--------------------------------------------------------------------------- - -# If the GENERATE_MAN tag is set to YES (the default) Doxygen will -# generate man pages - -GENERATE_MAN = NO - -# The MAN_OUTPUT tag is used to specify where the man pages will be put. -# If a relative path is entered the value of OUTPUT_DIRECTORY will be -# put in front of it. If left blank `man' will be used as the default path. - -MAN_OUTPUT = man - -# The MAN_EXTENSION tag determines the extension that is added to -# the generated man pages (default is the subroutine's section .3) - -MAN_EXTENSION = .3 - -# If the MAN_LINKS tag is set to YES and Doxygen generates man output, -# then it will generate one additional man file for each entity -# documented in the real man page(s). These additional files -# only source the real man page, but without them the man command -# would be unable to find the correct page. The default is NO. - -MAN_LINKS = NO - -#--------------------------------------------------------------------------- -# configuration options related to the XML output -#--------------------------------------------------------------------------- - -# If the GENERATE_XML tag is set to YES Doxygen will -# generate an XML file that captures the structure of -# the code including all documentation. - -GENERATE_XML = NO - -# The XML_OUTPUT tag is used to specify where the XML pages will be put. -# If a relative path is entered the value of OUTPUT_DIRECTORY will be -# put in front of it. If left blank `xml' will be used as the default path. - -XML_OUTPUT = xml - -# The XML_SCHEMA tag can be used to specify an XML schema, -# which can be used by a validating XML parser to check the -# syntax of the XML files. - -XML_SCHEMA = - -# The XML_DTD tag can be used to specify an XML DTD, -# which can be used by a validating XML parser to check the -# syntax of the XML files. - -XML_DTD = - -# If the XML_PROGRAMLISTING tag is set to YES Doxygen will -# dump the program listings (including syntax highlighting -# and cross-referencing information) to the XML output. Note that -# enabling this will significantly increase the size of the XML output. - -XML_PROGRAMLISTING = YES - -#--------------------------------------------------------------------------- -# configuration options for the AutoGen Definitions output -#--------------------------------------------------------------------------- - -# If the GENERATE_AUTOGEN_DEF tag is set to YES Doxygen will -# generate an AutoGen Definitions (see autogen.sf.net) file -# that captures the structure of the code including all -# documentation. Note that this feature is still experimental -# and incomplete at the moment. - -GENERATE_AUTOGEN_DEF = NO - -#--------------------------------------------------------------------------- -# configuration options related to the Perl module output -#--------------------------------------------------------------------------- - -# If the GENERATE_PERLMOD tag is set to YES Doxygen will -# generate a Perl module file that captures the structure of -# the code including all documentation. Note that this -# feature is still experimental and incomplete at the -# moment. - -GENERATE_PERLMOD = NO - -# If the PERLMOD_LATEX tag is set to YES Doxygen will generate -# the necessary Makefile rules, Perl scripts and LaTeX code to be able -# to generate PDF and DVI output from the Perl module output. - -PERLMOD_LATEX = NO - -# If the PERLMOD_PRETTY tag is set to YES the Perl module output will be -# nicely formatted so it can be parsed by a human reader. This is useful -# if you want to understand what is going on. On the other hand, if this -# tag is set to NO the size of the Perl module output will be much smaller -# and Perl will parse it just the same. - -PERLMOD_PRETTY = YES - -# The names of the make variables in the generated doxyrules.make file -# are prefixed with the string contained in PERLMOD_MAKEVAR_PREFIX. -# This is useful so different doxyrules.make files included by the same -# Makefile don't overwrite each other's variables. - -PERLMOD_MAKEVAR_PREFIX = - -#--------------------------------------------------------------------------- -# Configuration options related to the preprocessor -#--------------------------------------------------------------------------- - -# If the ENABLE_PREPROCESSING tag is set to YES (the default) Doxygen will -# evaluate all C-preprocessor directives found in the sources and include -# files. - -ENABLE_PREPROCESSING = YES - -# If the MACRO_EXPANSION tag is set to YES Doxygen will expand all macro -# names in the source code. If set to NO (the default) only conditional -# compilation will be performed. Macro expansion can be done in a controlled -# way by setting EXPAND_ONLY_PREDEF to YES. - -MACRO_EXPANSION = NO - -# If the EXPAND_ONLY_PREDEF and MACRO_EXPANSION tags are both set to YES -# then the macro expansion is limited to the macros specified with the -# PREDEFINED and EXPAND_AS_DEFINED tags. - -EXPAND_ONLY_PREDEF = NO - -# If the SEARCH_INCLUDES tag is set to YES (the default) the includes files -# in the INCLUDE_PATH (see below) will be search if a #include is found. - -SEARCH_INCLUDES = YES - -# The INCLUDE_PATH tag can be used to specify one or more directories that -# contain include files that are not input files but should be processed by -# the preprocessor. - -INCLUDE_PATH = - -# You can use the INCLUDE_FILE_PATTERNS tag to specify one or more wildcard -# patterns (like *.h and *.hpp) to filter out the header-files in the -# directories. If left blank, the patterns specified with FILE_PATTERNS will -# be used. - -INCLUDE_FILE_PATTERNS = - -# The PREDEFINED tag can be used to specify one or more macro names that -# are defined before the preprocessor is started (similar to the -D option of -# gcc). The argument of the tag is a list of macros of the form: name -# or name=definition (no spaces). If the definition and the = are -# omitted =1 is assumed. To prevent a macro definition from being -# undefined via #undef or recursively expanded use the := operator -# instead of the = operator. - -PREDEFINED = - -# If the MACRO_EXPANSION and EXPAND_ONLY_PREDEF tags are set to YES then -# this tag can be used to specify a list of macro names that should be expanded. -# The macro definition that is found in the sources will be used. -# Use the PREDEFINED tag if you want to use a different macro definition. - -EXPAND_AS_DEFINED = - -# If the SKIP_FUNCTION_MACROS tag is set to YES (the default) then -# doxygen's preprocessor will remove all function-like macros that are alone -# on a line, have an all uppercase name, and do not end with a semicolon. Such -# function macros are typically used for boiler-plate code, and will confuse -# the parser if not removed. - -SKIP_FUNCTION_MACROS = YES - -#--------------------------------------------------------------------------- -# Configuration::additions related to external references -#--------------------------------------------------------------------------- - -# The TAGFILES option can be used to specify one or more tagfiles. -# Optionally an initial location of the external documentation -# can be added for each tagfile. The format of a tag file without -# this location is as follows: -# TAGFILES = file1 file2 ... -# Adding location for the tag files is done as follows: -# TAGFILES = file1=loc1 "file2 = loc2" ... -# where "loc1" and "loc2" can be relative or absolute paths or -# URLs. If a location is present for each tag, the installdox tool -# does not have to be run to correct the links. -# Note that each tag file must have a unique name -# (where the name does NOT include the path) -# If a tag file is not located in the directory in which doxygen -# is run, you must also specify the path to the tagfile here. - -TAGFILES = - -# When a file name is specified after GENERATE_TAGFILE, doxygen will create -# a tag file that is based on the input files it reads. - -GENERATE_TAGFILE = - -# If the ALLEXTERNALS tag is set to YES all external classes will be listed -# in the class index. If set to NO only the inherited external classes -# will be listed. - -ALLEXTERNALS = NO - -# If the EXTERNAL_GROUPS tag is set to YES all external groups will be listed -# in the modules index. If set to NO, only the current project's groups will -# be listed. - -EXTERNAL_GROUPS = YES - -# The PERL_PATH should be the absolute path and name of the perl script -# interpreter (i.e. the result of `which perl'). - -PERL_PATH = /usr/bin/perl - -#--------------------------------------------------------------------------- -# Configuration options related to the dot tool -#--------------------------------------------------------------------------- - -# If the CLASS_DIAGRAMS tag is set to YES (the default) Doxygen will -# generate a inheritance diagram (in HTML, RTF and LaTeX) for classes with base -# or super classes. Setting the tag to NO turns the diagrams off. Note that -# this option is superseded by the HAVE_DOT option below. This is only a -# fallback. It is recommended to install and use dot, since it yields more -# powerful graphs. - -CLASS_DIAGRAMS = YES - -# You can define message sequence charts within doxygen comments using the \msc -# command. Doxygen will then run the mscgen tool (see -# http://www.mcternan.me.uk/mscgen/) to produce the chart and insert it in the -# documentation. The MSCGEN_PATH tag allows you to specify the directory where -# the mscgen tool resides. If left empty the tool is assumed to be found in the -# default search path. - -MSCGEN_PATH = - -# If set to YES, the inheritance and collaboration graphs will hide -# inheritance and usage relations if the target is undocumented -# or is not a class. - -HIDE_UNDOC_RELATIONS = YES - -# If you set the HAVE_DOT tag to YES then doxygen will assume the dot tool is -# available from the path. This tool is part of Graphviz, a graph visualization -# toolkit from AT&T and Lucent Bell Labs. The other options in this section -# have no effect if this option is set to NO (the default) - -HAVE_DOT = YES - -# By default doxygen will write a font called FreeSans.ttf to the output -# directory and reference it in all dot files that doxygen generates. This -# font does not include all possible unicode characters however, so when you need -# these (or just want a differently looking font) you can specify the font name -# using DOT_FONTNAME. You need need to make sure dot is able to find the font, -# which can be done by putting it in a standard location or by setting the -# DOTFONTPATH environment variable or by setting DOT_FONTPATH to the directory -# containing the font. - -DOT_FONTNAME = FreeSans - -# The DOT_FONTSIZE tag can be used to set the size of the font of dot graphs. -# The default size is 10pt. - -DOT_FONTSIZE = 10 - -# By default doxygen will tell dot to use the output directory to look for the -# FreeSans.ttf font (which doxygen will put there itself). If you specify a -# different font using DOT_FONTNAME you can set the path where dot -# can find it using this tag. - -DOT_FONTPATH = - -# If the CLASS_GRAPH and HAVE_DOT tags are set to YES then doxygen -# will generate a graph for each documented class showing the direct and -# indirect inheritance relations. Setting this tag to YES will force the -# the CLASS_DIAGRAMS tag to NO. - -CLASS_GRAPH = YES - -# If the COLLABORATION_GRAPH and HAVE_DOT tags are set to YES then doxygen -# will generate a graph for each documented class showing the direct and -# indirect implementation dependencies (inheritance, containment, and -# class references variables) of the class with other documented classes. - -COLLABORATION_GRAPH = NO - -# If the GROUP_GRAPHS and HAVE_DOT tags are set to YES then doxygen -# will generate a graph for groups, showing the direct groups dependencies - -GROUP_GRAPHS = YES - -# If the UML_LOOK tag is set to YES doxygen will generate inheritance and -# collaboration diagrams in a style similar to the OMG's Unified Modeling -# Language. - -UML_LOOK = NO - -# If set to YES, the inheritance and collaboration graphs will show the -# relations between templates and their instances. - -TEMPLATE_RELATIONS = NO - -# If the ENABLE_PREPROCESSING, SEARCH_INCLUDES, INCLUDE_GRAPH, and HAVE_DOT -# tags are set to YES then doxygen will generate a graph for each documented -# file showing the direct and indirect include dependencies of the file with -# other documented files. - -INCLUDE_GRAPH = YES - -# If the ENABLE_PREPROCESSING, SEARCH_INCLUDES, INCLUDED_BY_GRAPH, and -# HAVE_DOT tags are set to YES then doxygen will generate a graph for each -# documented header file showing the documented files that directly or -# indirectly include this file. - -INCLUDED_BY_GRAPH = YES - -# If the CALL_GRAPH and HAVE_DOT options are set to YES then -# doxygen will generate a call dependency graph for every global function -# or class method. Note that enabling this option will significantly increase -# the time of a run. So in most cases it will be better to enable call graphs -# for selected functions only using the \callgraph command. - -CALL_GRAPH = YES - -# If the CALLER_GRAPH and HAVE_DOT tags are set to YES then -# doxygen will generate a caller dependency graph for every global function -# or class method. Note that enabling this option will significantly increase -# the time of a run. So in most cases it will be better to enable caller -# graphs for selected functions only using the \callergraph command. - -CALLER_GRAPH = NO - -# If the GRAPHICAL_HIERARCHY and HAVE_DOT tags are set to YES then doxygen -# will graphical hierarchy of all classes instead of a textual one. - -GRAPHICAL_HIERARCHY = YES - -# If the DIRECTORY_GRAPH, SHOW_DIRECTORIES and HAVE_DOT tags are set to YES -# then doxygen will show the dependencies a directory has on other directories -# in a graphical way. The dependency relations are determined by the #include -# relations between the files in the directories. - -DIRECTORY_GRAPH = YES - -# The DOT_IMAGE_FORMAT tag can be used to set the image format of the images -# generated by dot. Possible values are png, jpg, or gif -# If left blank png will be used. - -DOT_IMAGE_FORMAT = png - -# The tag DOT_PATH can be used to specify the path where the dot tool can be -# found. If left blank, it is assumed the dot tool can be found in the path. - -DOT_PATH = - -# The DOTFILE_DIRS tag can be used to specify one or more directories that -# contain dot files that are included in the documentation (see the -# \dotfile command). - -DOTFILE_DIRS = - -# The DOT_GRAPH_MAX_NODES tag can be used to set the maximum number of -# nodes that will be shown in the graph. If the number of nodes in a graph -# becomes larger than this value, doxygen will truncate the graph, which is -# visualized by representing a node as a red box. Note that doxygen if the -# number of direct children of the root node in a graph is already larger than -# DOT_GRAPH_MAX_NODES then the graph will not be shown at all. Also note -# that the size of a graph can be further restricted by MAX_DOT_GRAPH_DEPTH. - -DOT_GRAPH_MAX_NODES = 50 - -# The MAX_DOT_GRAPH_DEPTH tag can be used to set the maximum depth of the -# graphs generated by dot. A depth value of 3 means that only nodes reachable -# from the root by following a path via at most 3 edges will be shown. Nodes -# that lay further from the root node will be omitted. Note that setting this -# option to 1 or 2 may greatly reduce the computation time needed for large -# code bases. Also note that the size of a graph can be further restricted by -# DOT_GRAPH_MAX_NODES. Using a depth of 0 means no depth restriction. - -MAX_DOT_GRAPH_DEPTH = 1000 - -# Set the DOT_TRANSPARENT tag to YES to generate images with a transparent -# background. This is disabled by default, because dot on Windows does not -# seem to support this out of the box. Warning: Depending on the platform used, -# enabling this option may lead to badly anti-aliased labels on the edges of -# a graph (i.e. they become hard to read). - -DOT_TRANSPARENT = YES - -# Set the DOT_MULTI_TARGETS tag to YES allow dot to generate multiple output -# files in one run (i.e. multiple -o and -T options on the command line). This -# makes dot run faster, but since only newer versions of dot (>1.8.10) -# support this, this feature is disabled by default. - -DOT_MULTI_TARGETS = NO - -# If the GENERATE_LEGEND tag is set to YES (the default) Doxygen will -# generate a legend page explaining the meaning of the various boxes and -# arrows in the dot generated graphs. - -GENERATE_LEGEND = YES - -# If the DOT_CLEANUP tag is set to YES (the default) Doxygen will -# remove the intermediate dot files that are used to generate -# the various graphs. - -DOT_CLEANUP = YES diff --git a/cob_sick_lms1xx/package.xml b/cob_sick_lms1xx/package.xml deleted file mode 100644 index 5c48da3e9..000000000 --- a/cob_sick_lms1xx/package.xml +++ /dev/null @@ -1,24 +0,0 @@ - - cob_sick_lms1xx - 0.7.16 - This package published a laser scan message out of a Sick LMS1xx laser scanner. - This version is made by fusion of ipa320/RCPRG_laser_drivers and ipa320/libLMS1xx repository. This package shuld have clearer structure and be easier to install. - - - Joshua Hampp - Denis Stogl - Konrad Banachowicz - - Apache 2.0 - http://ros.org/wiki/cob_sick_lms1xx - - catkin - - boost - diagnostic_msgs - roscpp - sensor_msgs - - - - diff --git a/cob_sick_lms1xx/ros/src/lms1xx_node.cpp b/cob_sick_lms1xx/ros/src/lms1xx_node.cpp deleted file mode 100644 index fd44c21c7..000000000 --- a/cob_sick_lms1xx/ros/src/lms1xx_node.cpp +++ /dev/null @@ -1,313 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -// standard includes -#include -#include - -// ROS includes -#include "ros/ros.h" - -// ROS message includes -#include -#include - -// external includes -#include - -#define DEG2RAD M_PI/180.0 - - -class SickLMS1xxNode -{ -public: - - SickLMS1xxNode(); - - bool initalize(); - - void startScanner(); - void publish(); - void stopScanner(); - - ros::NodeHandle nh; - -private: - - bool initalizeLaser(); - bool initalizeMessage(); - void setScanDataConfig(); - void publishError(std::string error_str); - - ros::Publisher scan_pub; - ros::Publisher diagnostic_pub; - - // laser data - LMS1xx laser; - scanCfg cfg; - scanDataCfg dataCfg; - scanData data; - // published data - sensor_msgs::LaserScan scan_msg; - // parameters - std::string host; - std::string frame_id; - bool inverted; - double resolution; - double frequency; - bool set_config; - double min_range; - double max_range; -}; - -SickLMS1xxNode::SickLMS1xxNode() -{ - ros::NodeHandle nh; - - scan_pub = nh.advertise("scan", 1); - diagnostic_pub = nh.advertise("/diagnostics", 1); - - if(!nh.hasParam("host")) ROS_WARN("Used default parameter for host"); - nh.param("host", host, "192.168.1.2"); - if(!nh.hasParam("frame_id")) ROS_WARN("Used default parameter for frame_id"); - nh.param("frame_id", frame_id, "base_laser_link"); - if(!nh.hasParam("inverted")) ROS_WARN("Used default parameter for inverted"); - nh.param("inverted", inverted, false); - if(!nh.hasParam("angle_resolution")) ROS_WARN("Used default parameter for resolution"); - nh.param("angle_resolution", resolution, 0.5); - if(!nh.hasParam("scan_frequency")) ROS_WARN("Used default parameter for frequency"); - nh.param("scan_frequency", frequency, 25); - if(!nh.hasParam("set_config")) ROS_WARN("Used default parameter for set_config"); - nh.param("set_config", set_config, false); - if(!nh.hasParam("min_range")) ROS_WARN("Used default parameter for min_range"); - nh.param("min_range", min_range, 0.01); - if(!nh.hasParam("max_range")) ROS_WARN("Used default parameter for max_range"); - nh.param("max_range", max_range, 20.0); - - ROS_INFO("connecting to laser at : %s", host.c_str()); - ROS_INFO("using frame_id : %s", frame_id.c_str()); - ROS_INFO("inverted : %s", (inverted)?"true":"false"); - ROS_INFO("using res : %f", resolution); - ROS_INFO("using freq : %f", frequency); -} - -bool SickLMS1xxNode::initalize() -{ - bool ret = false; - - if (initalizeLaser() && initalizeMessage()) { - - setScanDataConfig(); - ret = true; - } - - return ret; -} - -bool SickLMS1xxNode::initalizeLaser() -{ - bool ret = false; - - laser.connect(host); - - if (laser.isConnected()) { - - ROS_INFO("Connected to laser."); - ret = true; - - //setup laserscanner config - laser.login(); - cfg = laser.getScanCfg(); - - if(set_config) - { - ROS_DEBUG("Set angle resolution to %f deg",resolution); - cfg.angleResolution = (int)(resolution * 10000); - ROS_DEBUG("Set scan frequency to %f hz",frequency); - cfg.scaningFrequency = (int)(frequency * 100); - - laser.setScanCfg(cfg); - laser.saveConfig(); - } - - cfg = laser.getScanCfg(); - - if(cfg.angleResolution != (int)(resolution * 10000)) - ROS_ERROR("Setting angle resolution failed: Current angle resolution is %f.", cfg.angleResolution/10000.0); - if(cfg.scaningFrequency != (int)(frequency * 100)) - ROS_ERROR("Setting scan frequency failed: Current scan frequency is %f.", cfg.scaningFrequency/100.0); - - } else { - ROS_ERROR("Connection to device failed"); - publishError("Connection to device failed"); - } - return ret; -} - -bool SickLMS1xxNode::initalizeMessage() -{ - bool ret = true; - - //init scan msg - scan_msg.header.frame_id = frame_id; - - scan_msg.range_min = min_range; - scan_msg.range_max = max_range; - - scan_msg.scan_time = 100.0/cfg.scaningFrequency; - - scan_msg.angle_increment = (double)cfg.angleResolution/10000.0 * DEG2RAD; - scan_msg.angle_min = (double)cfg.startAngle/10000.0 * DEG2RAD - M_PI/2; - scan_msg.angle_max = (double)cfg.stopAngle/10000.0 * DEG2RAD - M_PI/2; - - int num_values; - if (cfg.angleResolution == 2500) - { - num_values = 1081; - } - else if (cfg.angleResolution == 5000) - { - num_values = 541; - } - else - { - ROS_ERROR("Unsupported resolution"); - publishError("Unsupported resolution"); - ret = false; - } - - scan_msg.time_increment = scan_msg.scan_time/num_values; - - scan_msg.ranges.resize(num_values); - scan_msg.intensities.resize(num_values); - - if(not inverted) - scan_msg.time_increment *= -1.; - - return ret; -} - -void SickLMS1xxNode::setScanDataConfig() -{ - //set scandata config - dataCfg.outputChannel = 1; - dataCfg.remission = true; - dataCfg.resolution = 1; - dataCfg.encoder = 0; - dataCfg.position = false; - dataCfg.deviceName = false; - dataCfg.outputInterval = 1; - - laser.setScanDataCfg(dataCfg); - ROS_DEBUG("setScanDataCfg"); - - laser.startMeas(); - ROS_DEBUG("startMeas"); -} - -void SickLMS1xxNode::startScanner() -{ - status_t stat; - do // wait for ready status - { - stat = laser.queryStatus(); - ros::Duration(1.0).sleep(); - } - while (stat != ready_for_measurement); - - laser.startDevice(); // Log out to properly re-enable system after config - ROS_DEBUG("startDevice"); - - laser.scanContinous(1); - ROS_DEBUG("scanContinous true"); -} - -void SickLMS1xxNode::publish() -{ - scan_msg.header.stamp = ros::Time::now(); - ++scan_msg.header.seq; - - if(laser.getData(data)) - { - for (int i = 0; i < data.dist_len1; i++) - { - if(not inverted) { - scan_msg.ranges[i] = data.dist1[data.dist_len1-1-i] * 0.001; - scan_msg.intensities[i] = data.rssi1[data.rssi_len1-1-i]; - } else { - scan_msg.ranges[i] = data.dist1[i] * 0.001; - scan_msg.intensities[i] = data.rssi1[i]; - } - } - scan_pub.publish(scan_msg); - - //Diagnostics - diagnostic_msgs::DiagnosticArray diagnostics; - diagnostics.status.resize(1); - diagnostics.status[0].level = 0; - diagnostics.status[0].name = nh.getNamespace(); - diagnostics.status[0].message = "sick scanner running"; - diagnostic_pub.publish(diagnostics); - } -} - -void SickLMS1xxNode::stopScanner() -{ - laser.scanContinous(0); - ROS_DEBUG("scanContinous false"); - laser.stopMeas(); - ROS_DEBUG("stopMeas"); - laser.disconnect(); - ROS_DEBUG("disconnect"); -} - -void SickLMS1xxNode::publishError(std::string error_str) -{ - diagnostic_msgs::DiagnosticArray diagnostics; - diagnostics.status.resize(1); - diagnostics.status[0].level = 2; - diagnostics.status[0].name = nh.getNamespace(); - diagnostics.status[0].message = error_str; - diagnostic_pub.publish(diagnostics); -} - - -//####################### -//#### main programm #### -int main(int argc, char** argv) -{ - ros::init(argc, argv, "sick_lms1xx_node"); - - SickLMS1xxNode node; - - if (!node.initalize()) { - return 1; - } - - node.startScanner(); - - while(ros::ok()) - { - node.publish(); - - ros::spinOnce(); - } - - node.stopScanner(); - - return 0; -} diff --git a/cob_sick_lms1xx/ros/src/set_config.cpp b/cob_sick_lms1xx/ros/src/set_config.cpp deleted file mode 100644 index 62dd210ab..000000000 --- a/cob_sick_lms1xx/ros/src/set_config.cpp +++ /dev/null @@ -1,65 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -#include - -#include - -#include - -void print_usage() { - - std::cout << " Usage : " << std::endl; - std::cout << " set_config ip resolution rate " << std::endl; - std::cout << " Exqample : " << std::endl; - std::cout << " set_config 192.168.1.2 0.25 50 " << std::endl; -} - -int main(int argc, char** argv) { - - LMS1xx laser; - scanCfg sCfg; - - if(argc < 4) { - print_usage(); - return 0; - } - - laser.connect(argv[1]); - - if(!laser.isConnected()) { - std::cout << "Unable to connect to device at address : " << argv[1] << std::endl; - return 0; - } - - sCfg.angleResolution = (int)(boost::lexical_cast(std::string(argv[2])) * 10000); - sCfg.scaningFrequency = boost::lexical_cast(std::string(argv[3])) * 100; - - laser.login(); - laser.setScanCfg(sCfg); - laser.saveConfig(); - - sCfg = laser.getScanCfg(); - - std::cout << "Configuration set to : " << std::endl; - std::cout << "resolution : " << (double)sCfg.angleResolution/10000.0 << std::endl; - std::cout << "frequency : " << (double)sCfg.scaningFrequency/100.0 << std::endl; - - laser.disconnect(); - - return 0; -} From 938d608bfb1a41a475fdd57204a4cf53bb25ae07 Mon Sep 17 00:00:00 2001 From: fmessmer Date: Wed, 20 Mar 2024 16:49:27 +0100 Subject: [PATCH 04/14] remove cob_voltage_control --- cob_driver/package.xml | 1 - cob_voltage_control/CHANGELOG.rst | 287 --------------- cob_voltage_control/CMakeLists.txt | 32 -- .../cfg/cob_voltage_control.cfg | 11 - .../common/src/cob_voltage_control_common.cpp | 123 ------- .../model/voltage_control.ros_package | 11 - .../model/voltage_control.ros_package_diagram | 43 --- cob_voltage_control/package.xml | 32 -- .../ros/config/voltage_filter.yaml | 0 .../ros/launch/csv_processing.launch | 13 - .../launch/record_current_and_voltage.launch | 13 - .../ros/src/cob_voltage_control_ros.cpp | 225 ------------ .../ros/src/record_current_and_voltage.py | 49 --- cob_voltage_control/ros/src/savitzky.py | 119 ------ cob_voltage_control/ros/src/savitzky_golay.py | 87 ----- cob_voltage_control/ros/src/time_volt.py | 344 ------------------ 16 files changed, 1390 deletions(-) delete mode 100644 cob_voltage_control/CHANGELOG.rst delete mode 100644 cob_voltage_control/CMakeLists.txt delete mode 100755 cob_voltage_control/cfg/cob_voltage_control.cfg delete mode 100644 cob_voltage_control/common/src/cob_voltage_control_common.cpp delete mode 100644 cob_voltage_control/model/voltage_control.ros_package delete mode 100644 cob_voltage_control/model/voltage_control.ros_package_diagram delete mode 100644 cob_voltage_control/package.xml delete mode 100644 cob_voltage_control/ros/config/voltage_filter.yaml delete mode 100644 cob_voltage_control/ros/launch/csv_processing.launch delete mode 100644 cob_voltage_control/ros/launch/record_current_and_voltage.launch delete mode 100644 cob_voltage_control/ros/src/cob_voltage_control_ros.cpp delete mode 100755 cob_voltage_control/ros/src/record_current_and_voltage.py delete mode 100755 cob_voltage_control/ros/src/savitzky.py delete mode 100755 cob_voltage_control/ros/src/savitzky_golay.py delete mode 100755 cob_voltage_control/ros/src/time_volt.py diff --git a/cob_driver/package.xml b/cob_driver/package.xml index 8d779e7e2..d6a6001e2 100644 --- a/cob_driver/package.xml +++ b/cob_driver/package.xml @@ -27,7 +27,6 @@ cob_sound cob_undercarriage_ctrl cob_utilities - cob_voltage_control diff --git a/cob_voltage_control/CHANGELOG.rst b/cob_voltage_control/CHANGELOG.rst deleted file mode 100644 index 06e3c3a1e..000000000 --- a/cob_voltage_control/CHANGELOG.rst +++ /dev/null @@ -1,287 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package cob_voltage_control -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.7.16 (2024-02-20) -------------------- - -0.7.15 (2023-11-06) -------------------- - -0.7.14 (2022-11-17) -------------------- - -0.7.13 (2022-07-29) -------------------- - -0.7.12 (2022-03-15) -------------------- - -0.7.11 (2022-01-12) -------------------- - -0.7.10 (2021-12-23) -------------------- - -0.7.9 (2021-11-26) ------------------- - -0.7.8 (2021-10-19) ------------------- - -0.7.7 (2021-08-02) ------------------- - -0.7.6 (2021-05-10) ------------------- -* Merge pull request `#423 `_ from mikaelarguedas/python2-deps - ROS_PYTHON_VERSION conditional dependency for `python-tk` and `python-numpy` -* ROS_PYTHON_VERSION conditional dependency for python-tk -* Contributors: Felix Messmer, Mikael Arguedas - -0.7.5 (2021-04-06) ------------------- -* Merge pull request `#418 `_ from fmessmer/fix_catkin_lint - fix catkin_lint -* fix catkin_lint -* Contributors: Felix Messmer, fmessmer - -0.7.4 (2020-10-14) ------------------- -* Merge pull request `#417 `_ from fmessmer/test_noetic - test noetic -* ROS_PYTHON_VERSION conditional dependency for matplotlib -* Bump CMake version to avoid CMP0048 warning -* Contributors: Felix Messmer, fmessmer - -0.7.3 (2020-03-18) ------------------- - -0.7.2 (2020-03-18) ------------------- -* Merge pull request `#409 `_ from LoyVanBeek/feature/python3_compatibility - [ci_updates] pylint + Python3 compatibility -* fix pylint errors -* Merge pull request `#408 `_ from fmessmer/ci_updates - [travis] ci updates -* catkin_lint fixes -* Contributors: Felix Messmer, fmessmer - -0.7.1 (2019-11-07) ------------------- - -0.7.0 (2019-08-06) ------------------- -* Merge pull request `#396 `_ from HannesBachter/indigo_dev - 0.6.15 -* Contributors: Felix Messmer - -0.6.15 (2019-07-17) -------------------- - -0.6.14 (2019-06-07) -------------------- - -0.6.13 (2019-03-14) -------------------- - -0.6.12 (2018-07-21) -------------------- - -0.6.11 (2018-01-07) -------------------- -* Merge remote-tracking branch 'origin/indigo_release_candidate' into indigo_dev -* Merge pull request `#341 `_ from ipa-fxm/APACHE_license - use license apache 2.0 -* use license apache 2.0 -* Contributors: Felix Messmer, ipa-fxm, ipa-uhr-mk - -0.6.10 (2017-07-24) -------------------- - -0.6.9 (2017-07-18) ------------------- -* Update csv_processing.launch -* manually fix changelog -* Contributors: Felix Messmer, ipa-fxm - -0.6.8 (2016-10-10) ------------------- - -0.6.7 (2016-04-02) ------------------- - -0.6.6 (2016-04-01) ------------------- -* beautify -* Merge branch 'aggregated_power_message' into feature/raw_batterie_state - Conflicts: - cob_voltage_control/common/src/cob_voltage_control_common.cpp - cob_voltage_control/ros/src/cob_voltage_control_ros.cpp -* voltage and current measurements for raw3-3 -* use aggregated power state message -* changed name relayboard to powerboard -* removed debug outputs -* fixed topic names -* Contributors: Benjamin Maidel, ipa-bnm, ipa-fmw - -0.6.5 (2015-08-31) ------------------- - -0.6.4 (2015-08-25) ------------------- -* explicit dependency to boost -* remove trailing whitespaces -* add_dependencies EXPORTED_TARGETS -* migrate to package format 2 -* sort dependencies -* critically review dependencies -* Contributors: ipa-fxm - -0.6.3 (2015-06-17) ------------------- - -0.6.2 (2014-12-15) ------------------- -* fix install tag -* Tested on cob4-2 -* make record current and voltage generic -* Contributors: ipa-cob4-2 - -0.6.1 (2014-09-17) ------------------- -* merge conflict -* Deleted CurrentMeasurement.msg -* Re-add time_volt -* reocord current script and launch file -* fix install tags -* Current measurement -* Removed emergency model and custom Relayboard -* Merge branch 'groovy_dev' of https://github.com/ipa320/cob_driver into groovy_dev - Conflicts: - cob_relayboard/ros/src/new_method.py - cob_relayboard/ros/src/relayboard_sim.py -* Voltage filter now on the config folder -* Changes -* Adjusting path for the required files -* More thrash -* Removing some thrash -* License -* Modifications for the battery characterization on the robot -* Contributors: Thiago de Freitas, ipa-cob4-2, ipa-nhg, thiagodefreitas, thiagodefreitas@gmail.com - -0.6.0 (2014-09-09) ------------------- -* trying to fix cob_voltage_control buildfarm error -* missing dependency -* Contributors: Florian Weisshardt, ipa-fxm - -0.5.7 (2014-08-26) ------------------- -* Merge pull request `#163 `_ from ipa320/hydro_dev - updates from hydro_dev -* 0.5.6 -* update changelog -* merge -* move EmergencyStopState.msg to cob_msgs + PowerBoardState works again -* changes due to introduction of cob_msgs -* added message to submit voltage data -* Cleaned up cob_driver with reduced deps to compile on indigo -* increased receive buffer -* Merge pull request `#136 `_ from ipa-fmw/hydro_dev - change maintainer and add missing dependency -* Update package.xml -* fix -* fix -* voltage ctrl over phidgets -* voltage info from phidgets -* work in progress voltagectrl with cob_phidget - Merge branch 'groovy_dev' into feature/voltagectrl_newphidget - Conflicts: - cob_voltage_control/ros/src/cob_voltage_control_ros.cpp -* voltagectrl work in progress -* Contributors: Alexander Bubeck, Felix Messmer, Florian Weisshardt, Nadia Hammoudeh García, ipa-bnm, ipa-fxm - -0.5.6 (2014-08-26) ------------------- -* Merge pull request `#163 `_ from ipa320/hydro_dev - updates from hydro_dev -* merge -* move EmergencyStopState.msg to cob_msgs + PowerBoardState works again -* changes due to introduction of cob_msgs -* added message to submit voltage data -* Cleaned up cob_driver with reduced deps to compile on indigo -* increased receive buffer -* Merge pull request `#136 `_ from ipa-fmw/hydro_dev - change maintainer and add missing dependency -* Update package.xml -* fix -* fix -* voltage ctrl over phidgets -* voltage info from phidgets -* work in progress voltagectrl with cob_phidget - Merge branch 'groovy_dev' into feature/voltagectrl_newphidget - Conflicts: - cob_voltage_control/ros/src/cob_voltage_control_ros.cpp -* voltagectrl work in progress -* Contributors: Alexander Bubeck, Felix Messmer, Florian Weisshardt, Nadia Hammoudeh García, ipa-bnm, ipa-fxm - -0.5.3 (2014-03-31) ------------------- -* install tags -* Contributors: ipa-fxm - -0.5.2 (2014-03-20) ------------------- - -0.5.1 (2014-03-20) ------------------- -* some install tag updates -* removed phidget21.h include inside main source file due to missing header guard -* bug fix in voltage_control -* remove old rosbuild import -* merged with groovy_dev upstream -* No need for the stds, thrash from the voltage checks on robot-at-work -* Modification from RAW3-1 -* Installation stuff -* fix for battery dashboard -* V instead of mV -* use v not mV -* Set parameter nedded for battery monitor -* cleaned up CMakeLists and added install directives -* further modifications for catkin, now everything is compiling and linking -* Removed csv files from the cob_voltage_control -* Changed launch file to be related to each robot -* compiling but still some linker errors -* Second catkinization push -* First catkinization, still need to update some CMakeLists.txt -* debugging voltage filter -* fix launch file -* nasty fix for rob3-6 -* Changes the modes nomenclatures -* More organization to the voltage commit -* Organizing the voltage filter commit -* Reverting new method -* Method modification on the robot -* Reverting new method -* Merge branch 'groovy_dev' of github.com:thiagodefreitas/cob_driver into groovy_dev - Conflicts: - cob_voltage_control/ros/src/new_method.py -* Method -* Modifications to the modes -* Mods -* Launch files -* IPA PC -* Starting real time implementation -* Saved at home computer -* Mods -* More mods on the battery statistics -* Mods on the plots -* Starting the statistical analysis of the battery -* added topic for relayboard message -* changed mapping of em state topics to make sense for gui -* added parameters -* added simple voltage calculation -* changes with sensor attached -* initial version of cob_voltage_control -* Contributors: Alexander Bubeck, Frederik Hegger, Thiago de Freitas, Thiago de Freitas Oliveira Araujo, abubeck, ipa-bnm, ipa-cob3-4, ipa-cob3-6, ipa-cob3-7, ipa-fmw, robot, thiagodefreitas, thiagodefreitas@gmail.com diff --git a/cob_voltage_control/CMakeLists.txt b/cob_voltage_control/CMakeLists.txt deleted file mode 100644 index 061ff1776..000000000 --- a/cob_voltage_control/CMakeLists.txt +++ /dev/null @@ -1,32 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(cob_voltage_control) - -find_package(catkin REQUIRED COMPONENTS cob_msgs cob_phidgets dynamic_reconfigure roscpp std_msgs) - -generate_dynamic_reconfigure_options(cfg/${PROJECT_NAME}.cfg) - -catkin_package( - CATKIN_DEPENDS cob_msgs cob_phidgets dynamic_reconfigure roscpp std_msgs -) - -### BUILD ### -include_directories(common/src ${catkin_INCLUDE_DIRS}) - -add_executable(${PROJECT_NAME} ros/src/${PROJECT_NAME}_ros.cpp) -add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) - -### INSTALL ### -install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -install(DIRECTORY ros/launch/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/ros/launch -) - -catkin_install_python(PROGRAMS ros/src/record_current_and_voltage.py ros/src/savitzky.py ros/src/savitzky_golay.py ros/src/time_volt.py - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) diff --git a/cob_voltage_control/cfg/cob_voltage_control.cfg b/cob_voltage_control/cfg/cob_voltage_control.cfg deleted file mode 100755 index cffa38c96..000000000 --- a/cob_voltage_control/cfg/cob_voltage_control.cfg +++ /dev/null @@ -1,11 +0,0 @@ -#!/usr/bin/env python - -from dynamic_reconfigure.parameter_generator_catkin import * - -gen = ParameterGenerator() - -gen.add("scanner_em_input", int_t, 0, "Autogenerated parameter based on model.", 0, 0, 100) -gen.add("voltage_level_input", int_t, 0, "Autogenerated parameter based on model.", 0, 0, 100) -gen.add("global_em_input", int_t, 0, "Autogenerated parameter based on model.", 1, 0, 100) - -exit(gen.generate("cob_voltage_control", "cob_voltage_control", "cob_voltage_control")) \ No newline at end of file diff --git a/cob_voltage_control/common/src/cob_voltage_control_common.cpp b/cob_voltage_control/common/src/cob_voltage_control_common.cpp deleted file mode 100644 index 2d1a8163a..000000000 --- a/cob_voltage_control/common/src/cob_voltage_control_common.cpp +++ /dev/null @@ -1,123 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -//ROS typedefs -#include "ros/ros.h" -#include -#include -#include -#include - -class cob_voltage_control_config -{ -public: - double min_voltage; - double max_voltage; - double max_voltage_res; - int num_voltage_port; - int num_em_stop_port; - int num_scanner_em_port; -}; - -class cob_voltage_control_data -{ -// autogenerated: don't touch this class -public: - //configuration data - - //input data - int in_phidget_voltage; - int in_phidget_current; - - //output data - cob_msgs::PowerState out_pub_power_state_; - cob_msgs::EmergencyStopState out_pub_em_stop_state_; - std_msgs::Float64 out_pub_voltage_; - std_msgs::Float64 out_pub_current_; -}; - -//document how this class has to look -//never change after first generation -class cob_voltage_control_impl -{ - -public: - - //CPhidgetInterfaceKitHandle IFK; - cob_voltage_control_impl() - { - //user specific code - } - void configure() - { - } - void update(cob_voltage_control_data &data, cob_voltage_control_config config) - { - //user specific code - //Get Battery Voltage - double voltage_raw = 0; - voltage_raw = data.in_phidget_voltage; - //Get Battery Current - double current_raw = 0; - current_raw = data.in_phidget_current; - - ROS_DEBUG("voltage_raw: %f", voltage_raw); - ROS_DEBUG("current_raw: %f", current_raw); - - //Calculation of real voltage - //max_voltage = 50V ; max_counts = 999 - //from measurements: 49.1 V => 486 counts - double max_counts = 693.0; // 3v => max - double voltage = voltage_raw * config.max_voltage_res/max_counts; - data.out_pub_voltage_.data = voltage; - ROS_DEBUG("voltage %f", voltage); - - //current calculation - //500 counts => 0 A - //999 counts => 2.5 A - //000 counts => -2.5 A - double count_min = 611; - double count_max = 450; - double v_min = -3.0; - double v_max = 1.6; - - double current = v_min+(v_max - v_min)*(current_raw-count_min)/(count_max - count_min); - data.out_pub_current_.data = current; - ROS_DEBUG("current %f", current); - - bool charging; - if (current > 0){charging = true;} - else {charging = false;} - - //Linear calculation for percentage - double percentage = (voltage - config.min_voltage) * 100/(config.max_voltage - config.min_voltage); - percentage = std::min(percentage, 100.0); - - data.out_pub_power_state_.header.stamp = ros::Time::now(); - data.out_pub_power_state_.voltage = voltage; - data.out_pub_power_state_.current = current; - data.out_pub_power_state_.power_consumption = voltage * current; - data.out_pub_power_state_.relative_remaining_capacity = percentage; - data.out_pub_power_state_.charging = charging; - - } - - void exit() - { - } - -}; diff --git a/cob_voltage_control/model/voltage_control.ros_package b/cob_voltage_control/model/voltage_control.ros_package deleted file mode 100644 index 825052332..000000000 --- a/cob_voltage_control/model/voltage_control.ros_package +++ /dev/null @@ -1,11 +0,0 @@ - - - - - - - - - - - diff --git a/cob_voltage_control/model/voltage_control.ros_package_diagram b/cob_voltage_control/model/voltage_control.ros_package_diagram deleted file mode 100644 index 78b88a881..000000000 --- a/cob_voltage_control/model/voltage_control.ros_package_diagram +++ /dev/null @@ -1,43 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/cob_voltage_control/package.xml b/cob_voltage_control/package.xml deleted file mode 100644 index 63bbfe22d..000000000 --- a/cob_voltage_control/package.xml +++ /dev/null @@ -1,32 +0,0 @@ - - - - cob_voltage_control - 0.7.16 - Interface to IO board that manages emergency stop and battery voltage on rob@work 3 - - Apache 2.0 - - None - - - Alexander Bubeck - Alexander Bubeck - - catkin - - cob_msgs - cob_phidgets - dynamic_reconfigure - roscpp - std_msgs - - python-matplotlib - python3-matplotlib - python-tk - python3-tk - rospy - - diff --git a/cob_voltage_control/ros/config/voltage_filter.yaml b/cob_voltage_control/ros/config/voltage_filter.yaml deleted file mode 100644 index e69de29bb..000000000 diff --git a/cob_voltage_control/ros/launch/csv_processing.launch b/cob_voltage_control/ros/launch/csv_processing.launch deleted file mode 100644 index 1cc003eb8..000000000 --- a/cob_voltage_control/ros/launch/csv_processing.launch +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - - - - - diff --git a/cob_voltage_control/ros/launch/record_current_and_voltage.launch b/cob_voltage_control/ros/launch/record_current_and_voltage.launch deleted file mode 100644 index 7392a95dd..000000000 --- a/cob_voltage_control/ros/launch/record_current_and_voltage.launch +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - - - - - diff --git a/cob_voltage_control/ros/src/cob_voltage_control_ros.cpp b/cob_voltage_control/ros/src/cob_voltage_control_ros.cpp deleted file mode 100644 index ffff84353..000000000 --- a/cob_voltage_control/ros/src/cob_voltage_control_ros.cpp +++ /dev/null @@ -1,225 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -// ROS includes -#include - -// ROS message includes -#include -#include -#include -#include - -#include -#include -#include - -class cob_voltage_control_ros -{ - private: - int EM_stop_status_; - - public: - ros::NodeHandle n_; - - ros::Publisher topicPub_em_stop_state_; - ros::Publisher topicPub_power_state_; - - ros::Publisher topicPub_Current_; - ros::Publisher topicPub_Voltage_; - - ros::Subscriber topicSub_AnalogInputs; - ros::Subscriber topicSub_DigitalInputs; - - cob_voltage_control_data component_data_; - cob_voltage_control_config component_config_; - cob_voltage_control_impl component_implementation_; - - // - bool last_rear_em_state; - bool last_front_em_state; - - // possible states of emergency stop - enum - { - ST_EM_FREE = 0, - ST_EM_ACTIVE = 1, - ST_EM_CONFIRMED = 2 - }; - - cob_voltage_control_ros() - { - topicPub_power_state_ = n_.advertise("power_state", 1); - topicPub_em_stop_state_ = n_.advertise("em_stop_state", 1); - - topicPub_Current_ = n_.advertise("current", 10); - topicPub_Voltage_ = n_.advertise("voltage", 10); - - topicSub_AnalogInputs = n_.subscribe("input/analog_sensors", 10, &cob_voltage_control_ros::analogPhidgetSignalsCallback, this); - topicSub_DigitalInputs = n_.subscribe("input/digital_sensors", 10, &cob_voltage_control_ros::digitalPhidgetSignalsCallback, this); - - n_.param("battery_max_voltage", component_config_.max_voltage, 48.5); - n_.param("battery_min_voltage", component_config_.min_voltage, 44.0); - n_.param("robot_max_voltage", component_config_.max_voltage_res, 70.0); - n_.param("voltage_analog_port", component_config_.num_voltage_port, 1); - n_.param("em_stop_dio_port", component_config_.num_em_stop_port, 0); - n_.param("scanner_stop_dio_port", component_config_.num_scanner_em_port, 1); - - last_rear_em_state = false; - last_front_em_state = false; - - EM_stop_status_ = ST_EM_ACTIVE; - component_data_.out_pub_em_stop_state_.scanner_stop = false; - } - - void configure() - { - component_implementation_.configure(); - } - - void update() - { - component_implementation_.update(component_data_, component_config_); - topicPub_Voltage_.publish(component_data_.out_pub_voltage_); - topicPub_Current_.publish(component_data_.out_pub_current_); - topicPub_power_state_.publish(component_data_.out_pub_power_state_); - topicPub_em_stop_state_.publish(component_data_.out_pub_em_stop_state_); - } - - void analogPhidgetSignalsCallback(const cob_phidgets::AnalogSensorConstPtr &msg) - { - for(int i = 0; i < msg->uri.size(); i++) - { - if( msg->uri[i] == "voltage") - { - component_data_.in_phidget_voltage = msg->value[i]; - } - if( msg->uri[i] == "current") - { - component_data_.in_phidget_current = msg->value[i]; - } - } - } - - void digitalPhidgetSignalsCallback(const cob_phidgets::DigitalSensorConstPtr &msg) - { - bool front_em_active = false; - bool rear_em_active = false; - static bool em_caused_by_button = false; - cob_msgs::EmergencyStopState EM_msg; - bool EM_signal = false; - bool got_message = false; - - for(int i = 0; i < msg->uri.size(); i++) - { - if( msg->uri[i] == "em_stop_laser_rear") - { - rear_em_active = !((bool)msg->state[i]); - got_message = true; - } - else if( msg->uri[i] == "em_stop_laser_front") - { - front_em_active = !((bool)msg->state[i]); - got_message = true; - } - } - if(got_message) - { - if( (front_em_active && rear_em_active) && (!last_front_em_state && !last_rear_em_state)) - { - component_data_.out_pub_em_stop_state_.emergency_button_stop = true; - em_caused_by_button = true; - } - else if((!front_em_active && !rear_em_active) && (last_front_em_state && last_rear_em_state)) - { - component_data_.out_pub_em_stop_state_.emergency_button_stop = false; - em_caused_by_button = false; - } - else if((front_em_active != rear_em_active) && em_caused_by_button) - { - component_data_.out_pub_em_stop_state_.emergency_button_stop = false; - em_caused_by_button = false; - component_data_.out_pub_em_stop_state_.scanner_stop = (bool)(front_em_active | rear_em_active); - } - else - { - component_data_.out_pub_em_stop_state_.scanner_stop = (bool)(front_em_active | rear_em_active); - } - - EM_signal = component_data_.out_pub_em_stop_state_.scanner_stop | component_data_.out_pub_em_stop_state_.emergency_button_stop; - - switch (EM_stop_status_) - { - case ST_EM_FREE: - { - if (EM_signal == true) - { - ROS_INFO("Emergency stop was issued"); - EM_stop_status_ = EM_msg.EMSTOP; - } - break; - } - case ST_EM_ACTIVE: - { - if (EM_signal == false) - { - ROS_INFO("Emergency stop was confirmed"); - EM_stop_status_ = EM_msg.EMCONFIRMED; - } - break; - } - case ST_EM_CONFIRMED: - { - if (EM_signal == true) - { - ROS_INFO("Emergency stop was issued"); - EM_stop_status_ = EM_msg.EMSTOP; - } - else - { - ROS_INFO("Emergency stop released"); - EM_stop_status_ = EM_msg.EMFREE; - } - break; - } - }; - - component_data_.out_pub_em_stop_state_.emergency_state = EM_stop_status_; - - last_front_em_state = front_em_active; - last_rear_em_state = rear_em_active; - } - } -}; - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "cob_voltage_control"); - - cob_voltage_control_ros node; - node.configure(); - - ros::Rate loop_rate(20); // Hz // if cycle time == 0 do a spin() here without calling node.update() - - while(node.n_.ok()) - { - node.update(); - loop_rate.sleep(); - ros::spinOnce(); - } - return 0; -} diff --git a/cob_voltage_control/ros/src/record_current_and_voltage.py b/cob_voltage_control/ros/src/record_current_and_voltage.py deleted file mode 100755 index 582b1a377..000000000 --- a/cob_voltage_control/ros/src/record_current_and_voltage.py +++ /dev/null @@ -1,49 +0,0 @@ -#!/usr/bin/env python -# -# Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - - -import csv - -import rospy -from cob_phidgets.msg import AnalogSensor - -def callback(data): - #print "voltage=", data.value[1] # voltage sensor is on the second port of the phidgets board - #print "current=", data.value[0] # current sensor is on the first port of the phidgets board - writer.writerow( ( round((rospy.Time.now() - starttime).to_sec(),5), data.value[1], data.value[0]) ) - -def record(): - rospy.init_node('record_current_and_voltage') - global starttime - starttime = rospy.Time.now() - - global f - global writer - filename = rospy.get_param("~filename") - global port_voltage, port_current - port_voltage = rospy.get_param("~port_voltage") - port_current = rospy.get_param("~port_current") - f = open(filename, 'wt', 1) - writer = csv.writer(f) - writer.writerow(("time_from_start", "voltage", "current")) - - rospy.Subscriber("/analog_sensors", AnalogSensor, callback) - - rospy.spin() - -if __name__ == '__main__': - record() - diff --git a/cob_voltage_control/ros/src/savitzky.py b/cob_voltage_control/ros/src/savitzky.py deleted file mode 100755 index 30bacd161..000000000 --- a/cob_voltage_control/ros/src/savitzky.py +++ /dev/null @@ -1,119 +0,0 @@ -#!/usr/bin/env python -# -# Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - - -import numpy as np -from math import factorial -import pylab - -class savitzky_golay(): - r"""Smooth (and optionally differentiate) data with a Savitzky-Golay filter. - The Savitzky-Golay filter removes high frequency noise from data. - It has the advantage of preserving the original shape and - features of the signal better than other types of filtering - approaches, such as moving averages techniques. - Parameters - ---------- - y : array_like, shape (N,) - the values of the time history of the signal. - window_size : int - the length of the window. Must be an odd integer number. - order : int - the order of the polynomial used in the filtering. - Must be less then `window_size` - 1. - deriv: int - the order of the derivative to compute (default = 0 means only smoothing) - Returns - ------- - ys : ndarray, shape (N) - the smoothed signal (or it's n-th derivative). - Notes - ----- - The Savitzky-Golay is a type of low-pass filter, particularly - suited for smoothing noisy data. The main idea behind this - approach is to make for each point a least-square fit with a - polynomial of high order over a odd-sized window centered at - the point. - Examples - -------- - t = np.linspace(-4, 4, 500) - y = np.exp( -t**2 ) + np.random.normal(0, 0.05, t.shape) - ysg = savitzky_golay(y, window_size=31, order=4) - import matplotlib.pyplot as plt - plt.plot(t, y, label='Noisy signal') - plt.plot(t, np.exp(-t**2), 'k', lw=1.5, label='Original signal') - plt.plot(t, ysg, 'r', label='Filtered signal') - plt.legend() - plt.show() - References - ---------- - .. [1] A. Savitzky, M. J. E. Golay, Smoothing and Differentiation of - Data by Simplified Least Squares Procedures. Analytical - Chemistry, 1964, 36 (8), pp 1627-1639. - .. [2] Numerical Recipes 3rd Edition: The Art of Scientific Computing - W.H. Press, S.A. Teukolsky, W.T. Vetterling, B.P. Flannery - Cambridge University Press ISBN-13: 9780521880688 - """ - - def __init__(self, window_size, order, deriv=0, rate=1): - - self.window_size = window_size - self.order = order - self.deriv = deriv - self.rate = rate - - try: - self.window_size = np.abs(np.int(self.window_size)) - self.order = np.abs(np.int(self.order)) - except ValueError as msg: - raise ValueError("ValueError: window_size and order have to be of type int - {}".format(msg)) - if self.window_size % 2 != 1 or self.window_size < 1: - raise TypeError("window_size size must be a positive odd number") - if self.window_size < self.order + 2: - raise TypeError("window_size is too small for the polynomials order") - - def filter(self, y): - - self.order_range = list(range(self.order+1)) - half_window = (self.window_size -1) // 2 - # precompute coefficients - b = np.mat([[k**i for i in self.order_range] for k in range(-half_window, half_window+1)]) - m = np.linalg.pinv(b).A[self.deriv] * self.rate**self.deriv * factorial(self.deriv) - # pad the signal at the extremes with - # values taken from the signal itself - firstvals = y[0] - np.abs( y[1:half_window+1][::-1] - y[0] ) - lastvals = y[-1] + np.abs(y[-half_window-1:-1][::-1] - y[-1]) - y = np.concatenate((firstvals, y, lastvals)) - return np.convolve( m[::-1], y, mode='valid') - -if __name__ == "__main__": - - # create some sample twoD data - x = np.linspace(-3,3,100) - Z = np.exp( np.negative(x)) - - # add noise - Zn = Z + np.random.normal( 0, 0.2, Z.shape ) - - sg = savitzky_golay(window_size=29, order=4) - - l = sg.filter(Zn) - - pylab.plot(l) - - pylab.plot(Zn) - - pylab.show() diff --git a/cob_voltage_control/ros/src/savitzky_golay.py b/cob_voltage_control/ros/src/savitzky_golay.py deleted file mode 100755 index bb24694d1..000000000 --- a/cob_voltage_control/ros/src/savitzky_golay.py +++ /dev/null @@ -1,87 +0,0 @@ -#!/usr/bin/env python -# -# Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - - -from math import cos, sin -import numpy as np - -import rospy -import savitzky -from std_msgs.msg import Float64 -from cob_msgs.msg import EmergencyStopState, PowerState - -class volts_filter(): - - def __init__(self): - self.volts = 0. - self.wsize = 61 - self.filter_order = 3 - self.theta = rospy.get_param("~theta") - self.off_y = rospy.get_param("~off_y") - self.abcd = rospy.get_param("~abcd") - self.maximum_time = rospy.get_param("~maximum_time") - self.sg = savitzky.savitzky_golay(window_size=self.wsize, order=self.filter_order) - size = 2*self.wsize+1 - self.volt_filt = 48000*np.ones(size) - - rospy.Subscriber("/power_board/voltage", Float64, self.callback) - - self.pub_power = rospy.Publisher('/power_state', PowerState, queue_size=1) - self.msg_power = PowerState() - - def callback(self, data): - - self.volts = data.data - self.volts = self.volts*1000 - - if(self.volts <= 44000): - self.volts = 44000 - elif(self.volts >= 48000): - self.volts = 48000 - - self.process_voltage() - - def process_voltage(self): - - self.volt_filt = np.insert(self.volt_filt, 0, self.volts) - self.volt_filt = np.delete(self.volt_filt, -1) - - vfilt = self.sg.filter(self.volt_filt) - - old_settings = np.seterr(all='raise') - - self.t_est = np.polyval(self.abcd, vfilt[self.wsize]) - - self.t_est = vfilt[self.wsize]*sin(self.theta) + self.t_est*cos(self.theta) - self.t_est = self.t_est + self.off_y - - if(self.t_est <0): - self.t_est = 0 - - self.msg_power.header.stamp = rospy.Time.now() - self.msg_power.time_remaining.secs = self.t_est - #self.msg_power.prediction_method = '3rd_order_polynom' - #self.msg_power.relative_capacity = (self.t_est/self.maximum_time) * 100 - #self.msg_power.AC_present = 0 - - self.pub_power.publish(self.msg_power) - -if __name__ == '__main__': - rospy.init_node('volt_filt') - vf = volts_filter() - - while not rospy.is_shutdown(): - rospy.sleep(1.0) diff --git a/cob_voltage_control/ros/src/time_volt.py b/cob_voltage_control/ros/src/time_volt.py deleted file mode 100755 index fa863d231..000000000 --- a/cob_voltage_control/ros/src/time_volt.py +++ /dev/null @@ -1,344 +0,0 @@ -#!/usr/bin/env python -# -# Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - - -import pickle -import csv -import pylab -import sys -import getopt - -from math import cos, sin -import numpy as np - -import rospy -import rosparam -import yaml -import savitzky - -def main(argv): - rospy.init_node('csv_proc') - - volt_values=[] - time_values=[] - - sg = savitzky.savitzky_golay(window_size=901, order=3) - -#################### -# Parameters for the Python script -#################### - - filename = rosparam.get_param("/csv_proc/file_name") - robot_name = rosparam.get_param("/csv_proc/robot_name") - mode = rosparam.get_param("/csv_proc/mode") - yaml_file = open("voltage_filter.yaml", "w") - yl = {} - - if(mode=="update"): - abcd = rosparam.get_param("/csv_proc/abcd") - - try: - opts, args = getopt.getopt(argv,"hf:r:",["ifile=", "irobot="]) - except getopt.GetoptError: - print('time_volt.py -f -r ') - sys.exit(2) - for opt, arg in opts: - if opt == '-h': - print('time_volt.py -f -r ') - sys.exit() - elif opt in ("-f", "--ifile"): - filename = arg - elif opt in ("-r", "--irobot"): - robot_name = arg - - -#################### -# Opening the csv file and getting the values for time and voltage -#################### - - with open(filename, 'rb') as csvfile: - - csvreader = csv.reader(csvfile, delimiter=' ', quotechar='|') - for row in csvreader: - row = row[0].split(',') - volt_v = (float)(row[1]) * 1000.0 - if(volt_v < 48000 and volt_v > 44000): - time_values.append((float)(row[0])) - volt_values.append(volt_v) - - time_values[:] = [x - time_values[0] for x in time_values] - time_values = time_values[::-1] - -#################### -# Plotting graphics for the Voltage vs Time -#################### - - pylab.figure(1) - - pylab.plot(volt_values, time_values) - pylab.ylabel("Time elapsed(seconds)") - pylab.xlabel("Voltage(mV)") - pylab.title("Time x Volt,file="+ filename.replace('.csv','')) - pylab.grid(True) - - secArray = np.asarray(time_values) - voltArray = np.asarray(volt_values) - - # Polyfit for the voltage values - z1 = np.polyfit(voltArray, secArray,1) - z2 = np.polyfit(voltArray, secArray,2) - z3 = np.polyfit(voltArray, secArray,3) - - # Linear space for better visualization - xp = np.linspace(49000, 43000, 100) - - # Generating the polynomial function from the fit - p1 = np.poly1d(z1) - p2 = np.poly1d(z2) - p3 = np.poly1d(z3) - - pylab.plot(xp, p1(xp), 'r-', xp, p2(xp), 'g-', xp, p3(xp), 'm-') - - pylab.text(46000, 11900, 'p1=' + p1.__str__(), bbox=dict(facecolor='red', alpha=0.5)) - pylab.text(46000, 11600, 'p2=' + p2.__str__(), bbox=dict(facecolor='green', alpha=0.5)) - pylab.text(46000, 11200, 'p3=' + p3.__str__(), bbox=dict(facecolor='magenta', alpha=0.5)) - - pylab.savefig(filename.replace('.csv',''), format="pdf") - #pylab.show() - -#################### -# Residuals Analysis -#################### - - pylab.figure(2) - - pylab.ylabel("Residuals(s)") - pylab.xlabel("Voltage(mV)") - pylab.title("Residuals x Voltage,file="+ filename.replace('.csv','')) - - #Evaluating the polynomial through all the volt values - z1_val = np.polyval(z1, volt_values) - z2_val = np.polyval(z2, volt_values) - z3_val = np.polyval(z3, volt_values) - - # Getting the residuals from the fit functions compared to the real values - z1_res = time_values - z1_val - z2_res = time_values - z2_val - z3_res = time_values - z3_val - - pylab.plot(time_values, z1_res, time_values, z2_res, time_values, z3_res) - - pylab.grid() - - pylab.legend(('Residuals 1st order', 'Residuals 2nd order', 'Residuals 3rd order')) - - pylab.savefig(filename.replace('.csv','')+'_res', format="pdf") - -################### -# Savitzky Golay Filter Applied to the Battery Voltage -################### - - values_filt = sg.filter(voltArray) - - pylab.figure(3) - - pylab.subplot(211) - - pylab.plot(voltArray, time_values) - - pylab.grid(True) - pylab.title('Comparison between real and filtered data') - pylab.ylabel('Real Values(mV)') - - pylab.subplot(212) - - pylab.plot(values_filt, time_values) - - pylab.grid(True) - pylab.ylabel('Filtered Values(mV)') - pylab.xlabel('Time(s)') - -##### - -################### -# Filtered fits -################### - pylab.figure(4) - - pylab.plot(values_filt, time_values) - pylab.ylabel("Time elapsed(seconds)") - pylab.xlabel("Voltage(mV)") - pylab.title("Time x Volt,file="+ filename.replace('.csv','')) - pylab.grid(True) - - secArray = np.asarray(time_values) - - # Polyfit for the voltage values - z1 = np.polyfit(values_filt, secArray,1) - z2 = np.polyfit(values_filt, secArray,2) - z3 = np.polyfit(values_filt, secArray,3) - - if (mode=="initial"): - z3_l = [] - - for el in z3: - z3_l.append((float)(el)) - - abcd = z3_l - yl["abcd"] = z3_l - yl["theta"] = 0. - yl["off_y"] = 0. - - # Linear space for better visualization - xp = np.linspace(49000, 43000, 100) - - # Generating the polynomial function from the fit - p1 = np.poly1d(z1) - p2 = np.poly1d(z2) - p3 = np.poly1d(z3) - - pylab.plot(xp, p1(xp), 'r-', xp, p2(xp), 'g-', xp, p3(xp), 'm-') - - pylab.text(46000, 11900, 'p1=' + p1.__str__(), bbox=dict(facecolor='red', alpha=0.5)) - pylab.text(46000, 11600, 'p2=' + p2.__str__(), bbox=dict(facecolor='green', alpha=0.5)) - pylab.text(46000, 11200, 'p3=' + p3.__str__(), bbox=dict(facecolor='magenta', alpha=0.5)) - -#################### -# Residuals Analysis for the filtered Signal -#################### - - pylab.figure(5) - - pylab.ylabel("Residuals(s)") - pylab.xlabel("Voltage(mV)") - pylab.title("Residuals x Voltage,file="+ filename.replace('.csv','')) - - #Evaluating the polynomial through all the time values - z1_val = np.polyval(z1, values_filt) - z2_val = np.polyval(z2, values_filt) - z3_val = np.polyval(z3, values_filt) - - # Getting the residuals from the fit functions compared to the real values - z1_res = time_values - z1_val - z2_res = time_values - z2_val - z3_res = time_values - z3_val - - pylab.plot(time_values, z1_res, time_values, z2_res, time_values, z3_res) - - pylab.grid() - - pylab.legend(('Residuals 1st order', 'Residuals 2nd order', 'Residuals 3rd order')) - -#################### -# Polynomial Evaluation for the filtered signal and the function from the non-moving case -#################### - - if(mode == "update"): - pylab.figure(6) - - - poly_vals = np.polyval(abcd, values_filt) - - - pylab.plot(values_filt, poly_vals, values_filt, time_values) - - pylab.legend(('Polynomial', 'Real')) - - pylab.grid() - ##### - pylab.figure(7) - - pylab.ylabel("Time available(seconds)") - pylab.xlabel("Voltage(mV)") - pylab.title("Time x Volt,file="+ filename.replace('.csv','')) - pylab.grid(True) - - poly_vals = np.polyval(abcd, values_filt) - - ss = lambda y1, y2: ((y1-y2)**2).sum() - - #theta = -0.07 - #new_x = values_filt*cos(theta) - poly_vals*sin(theta) - #new_y = values_filt*sin(theta) + poly_vals*cos(theta) - - theta = -0.2 - theta_values = [] - cost_values = [] - off_values = [] - - while theta < 0.2: - theta +=0.01 - new_x = values_filt*cos(theta) - poly_vals*sin(theta) - new_y = values_filt*sin(theta) + poly_vals*cos(theta) - print("new_x: {}, new_y: {}".format(new_x, new_y)) - - - off_y = -6000 - - cost_values_i =[] - off_y_values_i=[] - - while off_y < 6000: - off_y +=200 - new_y_temp = new_y - new_y_temp = new_y_temp + off_y - - ss1=ss(time_values,new_y_temp) - print(ss1, off_y) - - cost_values_i.append(ss1) - off_y_values_i.append(off_y) - - #ss1=ss(time_values,new_y) - #print ss1, theta - #cost_values.append(ss1) - theta_values.append(theta) - cost_min = min(cost_values_i) - cost_min_index = cost_values_i.index(cost_min) - cost_values.append(cost_values_i[cost_min_index]) - off_values.append(off_y_values_i[cost_min_index]) - - cost_min = min(cost_values) - cost_min_index = cost_values.index(cost_min) - - theta = theta_values[cost_min_index] - off_y = off_values[cost_min_index] - - new_x = values_filt*cos(theta) - poly_vals*sin(theta) - new_y = values_filt*sin(theta) + poly_vals*cos(theta) - print("new_x: {}, new_y: {}".format(new_x, new_y)) - - new_y = new_y + off_y - - yl["abcd"] = abcd - yl["theta"] = theta - yl["off_y"] = off_y - yl["maximum_time"] = (float)(new_y[0]) - - pylab.plot(poly_vals, values_filt, time_values, values_filt, new_y, values_filt) - - pylab.legend(('Poly not moving', 'Real', 'Shifted Fit')) - - yaml.safe_dump(yl, yaml_file,default_flow_style=False) - - yaml_file.close() - - pylab.show() - - - -if __name__=="__main__": - main(sys.argv[1:]) From e8f8025b7ac21f8c1f382318d7f2b95d025bd36e Mon Sep 17 00:00:00 2001 From: fmessmer Date: Wed, 20 Mar 2024 16:50:34 +0100 Subject: [PATCH 05/14] remove cob_elmo_homing --- cob_driver/package.xml | 1 - cob_elmo_homing/CHANGELOG.rst | 157 ------------------------- cob_elmo_homing/CMakeLists.txt | 27 ----- cob_elmo_homing/canopen_402_plugin.xml | 5 - cob_elmo_homing/package.xml | 20 ---- cob_elmo_homing/src/plugin.cpp | 124 ------------------- 6 files changed, 334 deletions(-) delete mode 100644 cob_elmo_homing/CHANGELOG.rst delete mode 100644 cob_elmo_homing/CMakeLists.txt delete mode 100644 cob_elmo_homing/canopen_402_plugin.xml delete mode 100644 cob_elmo_homing/package.xml delete mode 100644 cob_elmo_homing/src/plugin.cpp diff --git a/cob_driver/package.xml b/cob_driver/package.xml index d6a6001e2..0e8af5efe 100644 --- a/cob_driver/package.xml +++ b/cob_driver/package.xml @@ -16,7 +16,6 @@ cob_base_drive_chain cob_bms_driver cob_canopen_motor - cob_elmo_homing cob_generic_can cob_light cob_mimic diff --git a/cob_elmo_homing/CHANGELOG.rst b/cob_elmo_homing/CHANGELOG.rst deleted file mode 100644 index 393fcafd0..000000000 --- a/cob_elmo_homing/CHANGELOG.rst +++ /dev/null @@ -1,157 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package cob_elmo_homing -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.7.16 (2024-02-20) -------------------- - -0.7.15 (2023-11-06) -------------------- - -0.7.14 (2022-11-17) -------------------- - -0.7.13 (2022-07-29) -------------------- - -0.7.12 (2022-03-15) -------------------- - -0.7.11 (2022-01-12) -------------------- - -0.7.10 (2021-12-23) -------------------- - -0.7.9 (2021-11-26) ------------------- - -0.7.8 (2021-10-19) ------------------- - -0.7.7 (2021-08-02) ------------------- - -0.7.6 (2021-05-10) ------------------- - -0.7.5 (2021-04-06) ------------------- - -0.7.4 (2020-10-14) ------------------- -* Merge pull request `#417 `_ from fmessmer/test_noetic - test noetic -* Bump CMake version to avoid CMP0048 warning -* Contributors: Felix Messmer, fmessmer - -0.7.3 (2020-03-18) ------------------- - -0.7.2 (2020-03-18) ------------------- -* Merge pull request `#408 `_ from fmessmer/ci_updates - [travis] ci updates -* catkin_lint fixes -* Contributors: Felix Messmer, fmessmer - -0.7.1 (2019-11-07) ------------------- -* Merge pull request `#400 `_ from fmessmer/fix_plugin_deprecation - fix class_loader deprecation -* fix class_loader deprecation -* Contributors: Felix Messmer, fmessmer - -0.7.0 (2019-08-06) ------------------- -* Merge pull request `#380 `_ from ipa-jba/fix/boost_shared_ptr - [Melodic] combined melodify pr -* use ROSCANOPEN_MAKE_SHARED -* use all the pointer names I could find -* fix forgotten ptr_name -* use pointer names -* enable c++ 11 -* replace boost::shared_ptr - with std::shared_ptr -* Merge pull request `#396 `_ from HannesBachter/indigo_dev - 0.6.15 -* Contributors: Felix Messmer, Jannik Abbenseth - -0.6.15 (2019-07-17) -------------------- - -0.6.14 (2019-06-07) -------------------- - -0.6.13 (2019-03-14) -------------------- - -0.6.12 (2018-07-21) -------------------- - -0.6.11 (2018-01-07) -------------------- -* Merge remote-tracking branch 'origin/indigo_release_candidate' into indigo_dev -* Merge pull request `#341 `_ from ipa-fxm/APACHE_license - use license apache 2.0 -* change maintainer -* use license apache 2.0 -* Contributors: Felix Messmer, ipa-fxm, ipa-uhr-mk - -0.6.10 (2017-07-24) -------------------- - -0.6.9 (2017-07-18) ------------------- -* manually fix changelog -* Contributors: ipa-fxm - -0.6.8 (2016-10-10) ------------------- - -0.6.7 (2016-04-02) ------------------- - -0.6.6 (2016-04-01) ------------------- -* added cob_elmo_homing -* Contributors: Mathias Lüdtke - -0.6.5 (2015-08-31) ------------------- - -0.6.4 (2015-08-25) ------------------- - -0.6.3 (2015-06-17) ------------------- - -0.6.2 (2014-12-15) ------------------- - -0.6.1 (2014-09-17) ------------------- - -0.6.0 (2014-09-09) ------------------- - -0.5.7 (2014-08-26 09:47) ------------------------- - -0.5.6 (2014-08-26 09:42) ------------------------- - -0.5.5 (2014-08-26 08:33) ------------------------- - -0.5.4 (2014-08-25) ------------------- - -0.5.3 (2014-03-31) ------------------- - -0.5.2 (2014-03-21) ------------------- - -0.5.1 (2014-03-20 10:54) ------------------------- diff --git a/cob_elmo_homing/CMakeLists.txt b/cob_elmo_homing/CMakeLists.txt deleted file mode 100644 index 5f3a97079..000000000 --- a/cob_elmo_homing/CMakeLists.txt +++ /dev/null @@ -1,27 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(cob_elmo_homing) - -find_package(catkin REQUIRED COMPONENTS - canopen_402 - pluginlib - socketcan_interface -) -catkin_package() - - -include_directories(${catkin_INCLUDE_DIRS}) - -add_library(${PROJECT_NAME}_plugin src/plugin.cpp) -target_link_libraries(${PROJECT_NAME}_plugin - ${catkin_LIBRARIES} -) - -install(TARGETS ${PROJECT_NAME}_plugin - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -install(FILES canopen_402_plugin.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) diff --git a/cob_elmo_homing/canopen_402_plugin.xml b/cob_elmo_homing/canopen_402_plugin.xml deleted file mode 100644 index e709e667b..000000000 --- a/cob_elmo_homing/canopen_402_plugin.xml +++ /dev/null @@ -1,5 +0,0 @@ - - - Allocator for ElmoMotor402 instances - - \ No newline at end of file diff --git a/cob_elmo_homing/package.xml b/cob_elmo_homing/package.xml deleted file mode 100644 index 2069edff7..000000000 --- a/cob_elmo_homing/package.xml +++ /dev/null @@ -1,20 +0,0 @@ - - - cob_elmo_homing - 0.7.16 - This packagae implements the special homing procedure that is needed for old cob4/raw bases - - Florian Weisshardt - Mathias Lüdtke - - Apache 2.0 - - catkin - canopen_402 - pluginlib - socketcan_interface - - - - - diff --git a/cob_elmo_homing/src/plugin.cpp b/cob_elmo_homing/src/plugin.cpp deleted file mode 100644 index 9fbe213d9..000000000 --- a/cob_elmo_homing/src/plugin.cpp +++ /dev/null @@ -1,124 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -#include -#include -#include - - -namespace cob_elmo_homing { - -class ElmoMotor402 : public canopen::Motor402 { - - canopen::ObjectStorage::Entry command_entry_; - canopen::ObjectStorage::Entry response_entry_; - - static const uint64_t byte_3_bit_6 = (1 << (3*8+6));; - static const uint64_t compare_mask = 0xFFFFFFFF ^ byte_3_bit_6; - - int32_t offset_; - int32_t event_; - int32_t speed_; - uint32_t timeout_; - - uint64_t readResponse(uint64_t command) { - boost::this_thread::sleep_for(boost::chrono::milliseconds(10)); - uint64_t response = response_entry_.get(); - return (response & compare_mask) == (command & compare_mask) ? response : 0; - } - - bool set(char c1, char c2, uint16_t index, uint32_t val){ - uint64_t response = 0; - uint64_t command = c1 | (c2 << (1*8)) | ((index & 0x3FFF) << (2*8)) | static_cast(val) << (4*8); - command_entry_.set(command); - - canopen::time_point timeout = canopen::get_abs_time(boost::chrono::seconds(1)); - do{ - response = readResponse(command); - }while(response != command && (response & byte_3_bit_6) == 0 && canopen::get_abs_time() < timeout); - - bool ok = response == command && (response & byte_3_bit_6) == 0; - return ok; - } - template bool test(char c1, char c2, uint16_t index, uint32_t val, T dur){ - uint64_t response = 0; - uint64_t command = c1 | (c2 << (1*8)) | ((index & 0x3FFF) << (2*8)) | byte_3_bit_6; - - canopen::time_point timeout = canopen::get_abs_time(dur); - do{ - command_entry_.set(command); - response = readResponse(command); - }while( (response >> (4*8)) != val && canopen::get_abs_time() < timeout); - - return (response & byte_3_bit_6) == 0 && (response >> (4*8)) == val; - } - -public: - ElmoMotor402(const std::string &name, canopen::ObjectStorageSharedPtr storage, const canopen::Settings &settings) - : Motor402(name, storage, settings) - { - storage->entry(command_entry_, 0x2012); - storage->entry(response_entry_, 0x2013); - event_ = settings.get_optional("homing_event", -1); - speed_ = settings.get_optional("homing_speed", 50000); - offset_ = settings.get_optional("homing_offset", 0); - timeout_= settings.get_optional("homing_timeout", 60); - } - - void handleInit(canopen::LayerStatus &status){ - Motor402::handleInit(status); - if(status.bounded() && event_ >= 0){ - if(!command_entry_.valid()){ - status.error("Command entry is not valid"); - }else if(!response_entry_.valid()){ - status.error("Response entry is not valid"); - }else if(!enterModeAndWait(Profiled_Velocity)){ - status.error("Could not switch mode"); - }else{ - if(!set('H','M', 1, 0) || // reset homing - !set('H','M', 2, offset_) || - !set('H','M', 3, event_) || - !set('H','M', 4, 2) || // do not stop after homing - !set('H','M', 5, 0) || // set PX to offset - !set('H','M', 1, 1) || // start homing - !setTarget(speed_)){ - status.error("could not initialize homing"); - return; - } - - if(!test('H','M', 1, 0, boost::chrono::seconds(timeout_))) status.error("homing timeout"); - - if(!setTarget(0)) status.error("could not stop motor"); - if(!set('H','M', 1, 0)) status.error("could not stop homing"); - } - } - } - - class Allocator : public canopen::MotorBase::Allocator{ - public: - virtual canopen::MotorBaseSharedPtr allocate(const std::string& name, - canopen::ObjectStorageSharedPtr storage, - const canopen::Settings& settings) - { - return ROSCANOPEN_MAKE_SHARED(name, storage, settings); - } - }; -}; - -} - -PLUGINLIB_EXPORT_CLASS(cob_elmo_homing::ElmoMotor402::Allocator, canopen::MotorBase::Allocator) From a90b1553f1e22e1d8085fb26ecdbc303e34e93d9 Mon Sep 17 00:00:00 2001 From: fmessmer Date: Wed, 20 Mar 2024 16:52:49 +0100 Subject: [PATCH 06/14] remove cob_undercarriage_ctrl --- cob_driver/package.xml | 1 - cob_undercarriage_ctrl/CHANGELOG.rst | 287 ------- cob_undercarriage_ctrl/CMakeLists.txt | 24 - .../UndercarriageCtrlGeom.h | 199 ----- .../common/src/UndercarriageCtrlGeom.cpp | 697 ----------------- cob_undercarriage_ctrl/package.xml | 26 - .../ros/src/cob_undercarriage_ctrl.cpp | 714 ------------------ 7 files changed, 1948 deletions(-) delete mode 100644 cob_undercarriage_ctrl/CHANGELOG.rst delete mode 100644 cob_undercarriage_ctrl/CMakeLists.txt delete mode 100644 cob_undercarriage_ctrl/common/include/cob_undercarriage_ctrl/UndercarriageCtrlGeom.h delete mode 100644 cob_undercarriage_ctrl/common/src/UndercarriageCtrlGeom.cpp delete mode 100644 cob_undercarriage_ctrl/package.xml delete mode 100644 cob_undercarriage_ctrl/ros/src/cob_undercarriage_ctrl.cpp diff --git a/cob_driver/package.xml b/cob_driver/package.xml index 0e8af5efe..cbd83795c 100644 --- a/cob_driver/package.xml +++ b/cob_driver/package.xml @@ -24,7 +24,6 @@ cob_scan_unifier cob_sick_s300 cob_sound - cob_undercarriage_ctrl cob_utilities diff --git a/cob_undercarriage_ctrl/CHANGELOG.rst b/cob_undercarriage_ctrl/CHANGELOG.rst deleted file mode 100644 index ae92f0c22..000000000 --- a/cob_undercarriage_ctrl/CHANGELOG.rst +++ /dev/null @@ -1,287 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package cob_undercarriage_ctrl -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.7.16 (2024-02-20) -------------------- - -0.7.15 (2023-11-06) -------------------- - -0.7.14 (2022-11-17) -------------------- - -0.7.13 (2022-07-29) -------------------- - -0.7.12 (2022-03-15) -------------------- - -0.7.11 (2022-01-12) -------------------- - -0.7.10 (2021-12-23) -------------------- - -0.7.9 (2021-11-26) ------------------- - -0.7.8 (2021-10-19) ------------------- - -0.7.7 (2021-08-02) ------------------- - -0.7.6 (2021-05-10) ------------------- - -0.7.5 (2021-04-06) ------------------- -* Merge pull request `#418 `_ from fmessmer/fix_catkin_lint - fix catkin_lint -* fix catkin_lint -* Contributors: Felix Messmer, fmessmer - -0.7.4 (2020-10-14) ------------------- -* Merge pull request `#417 `_ from fmessmer/test_noetic - test noetic -* Bump CMake version to avoid CMP0048 warning -* Merge pull request `#414 `_ from nlamprian/nlamprian/fix-frame-ids - Remove leading slashes from frame ids -* Remove leading slashes from frame ids -* Contributors: Felix Messmer, Nick Lamprianidis, fmessmer - -0.7.3 (2020-03-18) ------------------- - -0.7.2 (2020-03-18) ------------------- -* Merge pull request `#408 `_ from fmessmer/ci_updates - [travis] ci updates -* catkin_lint fixes -* Contributors: Felix Messmer, fmessmer - -0.7.1 (2019-11-07) ------------------- - -0.7.0 (2019-08-06) ------------------- -* Merge pull request `#396 `_ from HannesBachter/indigo_dev - 0.6.15 -* Contributors: Felix Messmer - -0.6.15 (2019-07-17) -------------------- - -0.6.14 (2019-06-07) -------------------- - -0.6.13 (2019-03-14) -------------------- - -0.6.12 (2018-07-21) -------------------- - -0.6.11 (2018-01-07) -------------------- -* Merge remote-tracking branch 'origin/indigo_release_candidate' into indigo_dev -* Merge pull request `#341 `_ from ipa-fxm/APACHE_license - use license apache 2.0 -* use license apache 2.0 -* Contributors: Felix Messmer, ipa-fxm, ipa-uhr-mk - -0.6.10 (2017-07-24) -------------------- - -0.6.9 (2017-07-18) ------------------- -* manually fix changelog -* Contributors: ipa-fxm - -0.6.8 (2016-10-10) ------------------- - -0.6.7 (2016-04-02) ------------------- - -0.6.6 (2016-04-01) ------------------- -* fix frame id for odometry -* Contributors: ipa-cob4-2 - -0.6.5 (2015-08-31) ------------------- - -0.6.4 (2015-08-25) ------------------- -* install tags for libraries -* do not install headers in executable-only packages -* explicit dependency to boost -* remove obsolete autogenerated mainpage.dox files -* remove trailing whitespaces -* add_dependencies EXPORTED_TARGETS -* migrate to package format 2 -* sort dependencies -* critically review dependencies -* Contributors: ipa-fxm - -0.6.3 (2015-06-17) ------------------- -* now checking for NaN-values in Twist message -* Contributors: Thorsten Kannacher - -0.6.2 (2014-12-15) ------------------- - -0.6.1 (2014-09-17) ------------------- - -0.6.0 (2014-09-09) ------------------- - -0.5.7 (2014-08-26) ------------------- -* Merge pull request `#163 `_ from ipa320/hydro_dev - updates from hydro_dev -* 0.5.6 -* update changelog -* move EmergencyStopState.msg to cob_msgs + PowerBoardState works again -* Merge branch 'hydro_dev' of github.com:ipa-fxm/cob_driver into indigo_dev -* cob_undercarriage_ctrl: add parameter to disable tf broadcast -* updated default values for maximal velocities in ucar_ctrl_watchdog -* fixed indentation cob_undercarriage_ctrl.cpp -* updated watchdog in ucar ctrl to stop in case we receive a really high command in at least one direction -* added missing absolute value functions to velocity watchdog in undercarriage_control -* beautification of some outputs in undercarriage control -* corrected some typos and minor bugs -* split maximal allowed velocity in undercarriage control in translational and rotaional part. set velocity to zero, if the maximal allowed velocity is exceeded. -* first draft for undercarriage_control velocity-watchdog. not tested yet. -* Cleaned up cob_driver with reduced deps to compile on indigo -* fix install tags -* remove deprecated launch files in cob_driver and add nodes to cob_robots -* Contributors: Alexander Bubeck, Felix Messmer, Florian Mirus, Florian Weisshardt, ipa-fxm, ipa-mig - -0.5.6 (2014-08-26) ------------------- -* Merge pull request `#163 `_ from ipa320/hydro_dev - updates from hydro_dev -* move EmergencyStopState.msg to cob_msgs + PowerBoardState works again -* Merge branch 'hydro_dev' of github.com:ipa-fxm/cob_driver into indigo_dev -* cob_undercarriage_ctrl: add parameter to disable tf broadcast -* updated default values for maximal velocities in ucar_ctrl_watchdog -* fixed indentation cob_undercarriage_ctrl.cpp -* updated watchdog in ucar ctrl to stop in case we receive a really high command in at least one direction -* added missing absolute value functions to velocity watchdog in undercarriage_control -* beautification of some outputs in undercarriage control -* corrected some typos and minor bugs -* split maximal allowed velocity in undercarriage control in translational and rotaional part. set velocity to zero, if the maximal allowed velocity is exceeded. -* first draft for undercarriage_control velocity-watchdog. not tested yet. -* Cleaned up cob_driver with reduced deps to compile on indigo -* fix install tags -* remove deprecated launch files in cob_driver and add nodes to cob_robots -* Contributors: Alexander Bubeck, Felix Messmer, Florian Mirus, Florian Weisshardt, ipa-fxm, ipa-mig - -0.5.3 (2014-03-31) ------------------- -* removed obsoledte OpenCV reference -* install tags -* Contributors: ipa-fxm - -0.5.2 (2014-03-20) ------------------- - -0.5.1 (2014-03-20) ------------------- -* some install tag updates -* merge with groovy_dev -* cherry-pick -* removed a lot of code related to packages not available in hydro anymore -* bugfix flexible odometry calculation based on number of wheels -* edited odometry calculation so that we are now flexible on how many wheels we use -* odometry calculation for 3 wheels -* upstream changes -* cob_undercarriage_ctrl: expose param for watchdog timeout -* Installation stuff -* Some small dependency tweaks. -* cleaned up CMakeLists and added install directives -* further modifications for catkin, now everything is compiling and linking -* futher include and linkpath modifications -* compiling but still some linker errors -* Second catkinization push -* First catkinization, still need to update some CMakeLists.txt -* cleanup in base_drive_chain and undercarriage_ctrl -* integration of cob_base_velocity_smoother, moved here from cob_navigation -* activated tf publishing out of undercarriagectrl -* cob_undercarriage_ctrl: cleaned and improved ucar_ctrl now working properly on real robot (including recover) -* cob_undercarriage: removed odom_tf that conflicts with robot-pose-ekf in simulation -* Merge remote branch 'origin-ipa320/master' into automerge -* fixed calculation error in transform -* changed odometry frames -* undercarriage adaptions -* cob_undercarriage: reverted changes that made recover impossible -> cpc-pk/ucar -* cob_undercarriage CMakeList fixed -* moved cob_undercarriage Trike ctrl to cob3_intern -* cob_undercarriage_ctrl: changed odometry frames -* cob_undercarriage_ctrl: odom in simulation looks great, in reality not -* cob_undercarriage: cleaned up, odom-improvements tested in simu with navigation -* cob_undercarriage_ctrl: corrected tf-name error -* cob_undercarriage_ctrl: now using timer callback instead of ros::Rate -* cob_undercarriage SIM: corrected wheel geometry parameters of PLatform.ini for simulation modell -> much improved odometry in simulation -* cob_undercarriage_ctrl: improved odometry, doubled odom-rate and doing midpoint integration now -* cob_undercarriage_ctrl: experiments on odometry -* merge -* undercarriage_nt: addings in ini-Files -* comment unused code -* removed compiler warnings -* removed dependency to cob_msgs -* rearranging cob_camera_sensors launch files -* cob_base: communication between controller and driver now directly using joint_command and state topics with pr2::JointTrajectoryControllerState msgs -* added is_moving service for undercarriage_ctrl -* Adaptions in base_drive_chain and undercarriage_ctrl for global /joint_states -* Adapted base_drive_chain to communicate with controller using joint names and not only numbers anymore -* camera settings added for head -* Some adaptions for version 2 of tricycle testplatform -* changed position of topic -* added state topic to base controller -* Merge branch 'master' of https://github.com/ipa-fmw/cob_driver into review-fmw -* additional undercarriage ctrl in simulation -* moved service -* moved Emergency stop message -* modified init_test -* changed trigger service -* cob_base_drive_chain DEBUG. GetJointStates Service replaced through cyclical publishing topic in cob_base_drive_chain -* cleanup in cob_driver -* Moved hard-coded lines for head_axis_homing from CanDriveHarmonica.cpp into ElmoCtrl.cpp. Removed debugger in base_drive_chain.launch and undercarriage_ctrl.launch -* added joint_state_combined to cob_bringup, small device modifications on cob3-1 -* Starting base_drive_chain and undercarriage_ctrl with GDB-debugger -* added testing and diag to sdh and base -* some fake covariance -* added watchdog to base controller -* restructured base_controller -* base_drive_chain now can be reverted after EMStop -* base_drive_chain: added main loop with evalCanBuffer to enable ElmoRecorderReadout. NEW: evalCanBuffer is only executed, when and until a readout is in process -* Modified launch files of cob_base_drive_chain, cob_relayboard, cob_undercaariage_ctrl and cob_teleop_ucar and made them hierarchic -* added indirect dependencies (relayboard node, base_drive_chain node) to manifest of under_carriage_ctrlr -* merged with cpc-pk: added ctrl for tricycle-kinematic; specification of limit in CanDriveHarmonica can now be specified via Inifile; base_drive_chain can be operated on variable numbers of motors (lesser or equal to eight); variable setting of path to inifile for UndercarriageCtrlGeom; debugged relaysboard - reads Bus now nonblocking -* removed hard coded entry of camera-axis limit switch in CanDriveHarmonica -* Direct Kinematics, publish effort option in base_drive_chain -* Running in teleop_joystick mode, need small adaptions to EncIncrementsOffset of steering motor -* Controller working for cob3_5 using standart ROS cob3 components -* Made interface of undercarriage_ctrl_geom common for cob3 and cob3_5, adapted some launch files -* Working on cob_undercarriage3_5 -* update documentation and deleted tf broadcaster -* modifications for navigation with ucar -* debugging odometry calc -* merging with cpc -* implemented, debugged and tested basic undercarriage controller - works on Descartes principal of rigid body motion -* Deployment of undercarriage controller debugged and finished: launch-script cob_ucar_joy starts up relayboard, base_drive_chain and controller; also remaps topics and services in correct namespaces. Debugging of controller itself is work in progress: simplified and removed old stuff - code compiles - controller runs but appaerently has some bugs -> may not yet be used -* Merge branch 'review-cpc' -* updated simulation files -* debugging undercarriage drivers (base_drive_chain + relayboard + ucar_ctrl) - work in progress -* cleanup in cob_driver -* renamed pltf_command topic in ucarctrl -* debugged ucar controller and base drive chain node - still not running -* Implemented base controller - cob_undercarriage_ctrl - based on principle of rigid body motion; controller is not yet tested on hardware; moreover, not yet used: parameterserver for initializing controller, urdf-file to associate joints; also removed some bugs from base_drive_chain -* added files for undercarriage controller -* Contributors: Alexander Bubeck, Christian, Christian Connette, Richard Bormann, abubeck, cob, cpc, cpc-pk, fmw-jk, ipa-cpc, ipa-fmw, ipa-frm, ipa-fxm, ipa-mig, ipa-srd diff --git a/cob_undercarriage_ctrl/CMakeLists.txt b/cob_undercarriage_ctrl/CMakeLists.txt deleted file mode 100644 index bcae78810..000000000 --- a/cob_undercarriage_ctrl/CMakeLists.txt +++ /dev/null @@ -1,24 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(cob_undercarriage_ctrl) - -find_package(catkin REQUIRED COMPONENTS cob_msgs cob_utilities control_msgs diagnostic_msgs diagnostic_updater geometry_msgs nav_msgs roscpp tf) - -catkin_package( - CATKIN_DEPENDS cob_msgs control_msgs diagnostic_msgs geometry_msgs nav_msgs roscpp tf -) - -### BUILD ### -include_directories(common/include ${catkin_INCLUDE_DIRS}) - -add_library(${PROJECT_NAME} common/src/UndercarriageCtrlGeom.cpp) - -add_executable(${PROJECT_NAME}_node ros/src/${PROJECT_NAME}.cpp) -add_dependencies(${PROJECT_NAME}_node ${catkin_EXPORTED_TARGETS}) -target_link_libraries(${PROJECT_NAME}_node ${PROJECT_NAME} ${catkin_LIBRARIES}) - -### INSTALL ### -install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) diff --git a/cob_undercarriage_ctrl/common/include/cob_undercarriage_ctrl/UndercarriageCtrlGeom.h b/cob_undercarriage_ctrl/common/include/cob_undercarriage_ctrl/UndercarriageCtrlGeom.h deleted file mode 100644 index 5110592e6..000000000 --- a/cob_undercarriage_ctrl/common/include/cob_undercarriage_ctrl/UndercarriageCtrlGeom.h +++ /dev/null @@ -1,199 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -#ifndef UndercarriageCtrlGeom_INCLUDEDEF_H -#define UndercarriageCtrlGeom_INCLUDEDEF_H - -//#include - -#include -#include -#include - -class UndercarriageCtrlGeom -{ -private: - - bool m_bEMStopActive; - - int m_iNumberOfDrives; - - std::string m_sIniDirectory; - - /*// Logging for debugging - // create Filepointers - FILE * m_pfileDesVel, * m_pfileMeasVel; - FILE * m_pfileSteerAngTarget1, * m_pfileSteerAngTarget2; - FILE * m_pfileSteerAngTarget, * m_pfileDriveVelTarget; - FILE * m_pfileSteerAngCmd, * m_pfileSteerVelCmd, * m_pfileDriveVelCmd; - // create TimeStamp - TimeStamp m_RawTime, m_StartTime; - double m_dNowTime;*/ - - - // Actual Values for PltfMovement (calculated from Actual Wheelspeeds) - double m_dVelLongMMS; - double m_dVelLatMMS; - double m_dRotRobRadS; - double m_dRotVelRadS; - - // Actual Wheelspeed (read from Motor-Ctrls) - std::vector m_vdVelGearDriveRadS; - std::vector m_vdVelGearSteerRadS; - std::vector m_vdDltAngGearDriveRad; - std::vector m_vdAngGearSteerRad; - - // Desired Pltf-Movement (set from PltfHwItf) - double m_dCmdVelLongMMS; - double m_dCmdVelLatMMS; - double m_dCmdRotRobRadS; - double m_dCmdRotVelRadS; - - /*// Interpolated Wheelspeeds (calculated from desired and current Pltf-Config) - std::vector m_vdVelGearDriveIntpRadS; - std::vector m_vdVelGearSteerIntpRadS; - std::vector m_vdAngGearSteerIntpRad; - - // Delta values for interpolating between two commands - std::vector m_vdDeltaAngIntpRad; - std::vector m_vdDeltaDriveIntpRadS;*/ - - // Desired Wheelspeeds set to ELMO-Ctrl's (calculated from desired Pltf-Movement) - std::vector m_vdVelGearDriveCmdRadS; - std::vector m_vdVelGearSteerCmdRadS; - std::vector m_vdAngGearSteerCmdRad; - - // Target Wheelspeed and -angle (calculated from desired Pltf-Movement with Inverse without controle!) - // This Values might not be valid (to high step response in steering rate, ...) for commanding the drives - std::vector m_vdAngGearSteerTarget1Rad; // alternativ 1 for steering angle - std::vector m_vdVelGearDriveTarget1RadS; - std::vector m_vdAngGearSteerTarget2Rad; // alternativ 2 for steering angle (+/- PI) - std::vector m_vdVelGearDriveTarget2RadS; - std::vector m_vdAngGearSteerTargetRad; // choosen alternativ for steering angle - std::vector m_vdVelGearDriveTargetRadS; - - /** Position of the Wheels' Steering Axis' - * in cartesian (X/Y) and polar (Dist/Ang) coordinates - * relative to robot coordinate System - */ - std::vector m_vdWheelXPosMM; - std::vector m_vdWheelYPosMM; - std::vector m_vdWheelDistMM; - std::vector m_vdWheelAngRad; - - /** Exact Position of the Wheels' itself - * in cartesian (X/Y) and polar (Dist/Ang) coordinates - * relative to robot coordinate System - */ - std::vector m_vdExWheelXPosMM; - std::vector m_vdExWheelYPosMM; - std::vector m_vdExWheelDistMM; - std::vector m_vdExWheelAngRad; - - struct ParamType - { - int iDistWheels; - int iRadiusWheelMM; - - int iDistSteerAxisToDriveWheelMM; - - double dMaxDriveRateRadpS; - double dMaxSteerRateRadpS; - double dCmdRateS; - std::vector WheelNeutralPos; - std::vector vdSteerDriveCoupling; - /** Factor between steering motion and steering induced motion of drive wheels - * subtract from Drive-Wheel Vel to get effective Drive Velocity (Direct Kinematics) - * add to Drive-Wheel Vel (Inverse Kinematics) to account for coupling when commanding velos - */ - std::vector vdFactorVel; - }; - - ParamType m_UnderCarriagePrms; - - /** ------- Position Controller Steer Wheels ------- - * Impedance-Ctrlr Prms - * -> model Stiffness via Spring-Damper-Modell - * -> only oriented at impedance-ctrl (no forces commanded) - * m_dSpring Spring-constant (elasticity) - * m_dDamp Damping coefficient (also prop. for Velocity Feedforward) - * m_dVirtM Virtual Mass of Spring-Damper System - * m_dDPhiMax maximum angular velocity (cut-off) - * m_dDDPhiMax maximum angular acceleration (cut-off) - */ - double m_dSpring, m_dDamp, m_dVirtM, m_dDPhiMax, m_dDDPhiMax; - /** storage for internal controller states - * m_vdCtrlVal is Vector with stored Controller-values of all wheels - * m_vdCtrlVal[iWheelNr][iVariableNr] - * iWheelNr: Index of Wheel Number (0..3) - * iVariableNr: 0: previous Commanded deltaPhi e(k-1) - * 1: pre-previous Commanded deltaPhi e(k-2) - * 2: previous Commanded Velocity u(k-1) - */ - std::vector< std::vector > m_vdCtrlVal; - - // Factor for thread cycle time of ThreadMotionPltfCtrl and ThreadUnderCarriageCtrl - //double m_dThreadCycleMultiplier; - - // calculate inverse kinematics - void CalcInverse(void); - - // calculate direct kinematics - void CalcDirect(void); - - // calculate Exact Wheel Position in robot coordinates - void CalcExWheelPos(void); - - // perform one discrete Controle Step - void CalcControlStep(void); - -public: - - // Constructor - UndercarriageCtrlGeom(std::string sIniDirectory); - - // Destructor - ~UndercarriageCtrlGeom(void); - - // Initialize Parameters for Controller and Kinematics - void InitUndercarriageCtrl(void); - - // Set desired value for Plattform Velocity to UndercarriageCtrl (Sollwertvorgabe) - void SetDesiredPltfVelocity(double dCmdVelLongMMS, double dCmdVelLatMMS, double dCmdRotRobRadS, double dCmdRotVelRadS); - - // Set actual values of wheels (steer/drive velocity/position) (Istwerte) - void SetActualWheelValues(std::vector vdVelGearDriveRadS, std::vector vdVelGearSteerRadS, std::vector vdDltAngGearDriveRad, std::vector vdAngGearSteerRad); - - // Get result of inverse kinematics (without controller) - void GetSteerDriveSetValues(std::vector & vdVelGearDriveRadS, std::vector & vdAngGearSteerRad); - - // Get set point values for the Wheels (including controller) from UndercarriangeCtrl - void GetNewCtrlStateSteerDriveSetValues(std::vector & vdVelGearDriveRadS, std::vector & vdVelGearSteerRadS, std::vector & vdAngGearSteerRad, - double & dVelLongMMS, double & dVelLatMMS, double & dRotRobRadS, double & dRotVelRadS); - - // Get result of direct kinematics - void GetActualPltfVelocity(double & dDeltaLongMM, double & dDeltaLatMM, double & dDeltaRotRobRad, double & dDeltaRotVelRad, - double & dVelLongMMS, double & dVelLatMMS, double & dRotRobRadS, double & dRotVelRadS); - - // Set EM flag and stop Ctrlr - void setEMStopActive(bool bEMStopActive); - - // operator overloading - void operator=(const UndercarriageCtrlGeom & GeomCtrl); -}; -#endif - diff --git a/cob_undercarriage_ctrl/common/src/UndercarriageCtrlGeom.cpp b/cob_undercarriage_ctrl/common/src/UndercarriageCtrlGeom.cpp deleted file mode 100644 index 444a4feba..000000000 --- a/cob_undercarriage_ctrl/common/src/UndercarriageCtrlGeom.cpp +++ /dev/null @@ -1,697 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -#include - -// Constructor -UndercarriageCtrlGeom::UndercarriageCtrlGeom(std::string sIniDirectory) -{ - m_sIniDirectory = sIniDirectory; - - // init EMStop flag - m_bEMStopActive = false; - - IniFile iniFile; - iniFile.SetFileName(m_sIniDirectory + "Platform.ini", "UnderCarriageCtrlGeom.cpp"); - iniFile.GetKeyInt("Config", "NumberOfWheels", &m_iNumberOfDrives, true); - - // init vectors - m_vdVelGearDriveRadS.assign(4,0); - m_vdVelGearSteerRadS.assign(4,0); - m_vdDltAngGearDriveRad.assign(4,0); - m_vdAngGearSteerRad.assign(4,0); - - //m_vdVelGearDriveIntpRadS.assign(4,0); - //m_vdVelGearSteerIntpRadS.assign(4,0); - //m_vdAngGearSteerIntpRad.assign(4,0); - - m_vdVelGearDriveCmdRadS.assign(4,0); - m_vdVelGearSteerCmdRadS.assign(4,0); - m_vdAngGearSteerCmdRad.assign(4,0); - - m_vdWheelXPosMM.assign(4,0); - m_vdWheelYPosMM.assign(4,0); - m_vdWheelDistMM.assign(4,0); - m_vdWheelAngRad.assign(4,0); - - m_vdExWheelXPosMM.assign(4,0); - m_vdExWheelYPosMM.assign(4,0); - m_vdExWheelDistMM.assign(4,0); - m_vdExWheelAngRad.assign(4,0); - - m_vdAngGearSteerTarget1Rad.assign(4,0); - m_vdVelGearDriveTarget1RadS.assign(4,0); - m_vdAngGearSteerTarget2Rad.assign(4,0); - m_vdVelGearDriveTarget2RadS.assign(4,0); - m_vdAngGearSteerTargetRad.assign(4,0); - m_vdVelGearDriveTargetRadS.assign(4,0); - - m_dCmdVelLongMMS = 0; - m_dCmdVelLatMMS = 0; - m_dCmdRotRobRadS = 0; - m_dCmdRotVelRadS = 0; - - m_UnderCarriagePrms.WheelNeutralPos.assign(4,0); - m_UnderCarriagePrms.vdSteerDriveCoupling.assign(4,0); - m_UnderCarriagePrms.vdFactorVel.assign(4,0); - - m_vdCtrlVal.assign( 4, std::vector (2,0.0) ); - - //m_vdDeltaAngIntpRad.assign(4,0); - //m_vdDeltaDriveIntpRadS.assign(4,0); - - // init Prms of Impedance-Ctrlr - m_dSpring = 10.0; - m_dDamp = 2.5; - m_dVirtM = 0.1; - m_dDPhiMax = 12.0; - m_dDDPhiMax = 100.0; - - /*// Logging for debugging - // Init timestamp for startup of the robot - m_StartTime.SetNow(); - // open Files for PltfVel - m_pfileDesVel = fopen("LogCtrl/DesPltfVel.txt","w"); - m_pfileMeasVel = fopen("LogCtrl/MeasPltfVel.txt","w"); - // open Files for corresponding Joint-Configuration - m_pfileSteerAngTarget1 = fopen("LogCtrl/SteerAngTarget1.txt","w"); - m_pfileSteerAngTarget2 = fopen("LogCtrl/SteerAngTarget2.txt","w"); - m_pfileSteerAngTarget = fopen("LogCtrl/SteerAngTarget.txt","w"); - m_pfileDriveVelTarget = fopen("LogCtrl/DriveVelTarget.txt","w"); - // open Files for resulting Joint-Commands - m_pfileSteerAngCmd = fopen("LogCtrl/SteerAngCmd.txt","w"); - m_pfileSteerVelCmd = fopen("LogCtrl/SteerVelCmd.txt","w"); - m_pfileDriveVelCmd = fopen("LogCtrl/DriveVelCmd.txt","w");*/ - -} - -// Destructor -UndercarriageCtrlGeom::~UndercarriageCtrlGeom(void) -{ - /*fclose(m_pfileDesVel); - fclose(m_pfileMeasVel); - - fclose(m_pfileSteerAngTarget1); - fclose(m_pfileSteerAngTarget2); - - fclose(m_pfileSteerAngTarget); - fclose(m_pfileDriveVelTarget); - - fclose(m_pfileSteerAngCmd); - fclose(m_pfileSteerVelCmd); - fclose(m_pfileDriveVelCmd);*/ -} - -// Initialize Parameters for Controller and Kinematics -void UndercarriageCtrlGeom::InitUndercarriageCtrl(void) -{ - //LOG_OUT("Initializing Undercarriage-Controller (Geom)"); - - IniFile iniFile; - - iniFile.SetFileName(m_sIniDirectory + "Platform.ini", "UnderCarriageCtrlGeom.cpp"); - iniFile.GetKeyInt("Geom", "DistWheels", &m_UnderCarriagePrms.iDistWheels, true); - iniFile.GetKeyInt("Geom", "RadiusWheel", &m_UnderCarriagePrms.iRadiusWheelMM, true); - iniFile.GetKeyInt("Geom", "DistSteerAxisToDriveWheelCenter", &m_UnderCarriagePrms.iDistSteerAxisToDriveWheelMM, true); - - iniFile.GetKeyDouble("Geom", "Wheel1XPos", &m_vdWheelXPosMM[0], true); - iniFile.GetKeyDouble("Geom", "Wheel1YPos", &m_vdWheelYPosMM[0], true); - iniFile.GetKeyDouble("Geom", "Wheel2XPos", &m_vdWheelXPosMM[1], true); - iniFile.GetKeyDouble("Geom", "Wheel2YPos", &m_vdWheelYPosMM[1], true); - iniFile.GetKeyDouble("Geom", "Wheel3XPos", &m_vdWheelXPosMM[2], true); - iniFile.GetKeyDouble("Geom", "Wheel3YPos", &m_vdWheelYPosMM[2], true); - iniFile.GetKeyDouble("Geom", "Wheel4XPos", &m_vdWheelXPosMM[3], true); - iniFile.GetKeyDouble("Geom", "Wheel4YPos", &m_vdWheelYPosMM[3], true); - - iniFile.GetKeyDouble("DrivePrms", "MaxDriveRate", &m_UnderCarriagePrms.dMaxDriveRateRadpS, true); - iniFile.GetKeyDouble("DrivePrms", "MaxSteerRate", &m_UnderCarriagePrms.dMaxSteerRateRadpS, true); - - iniFile.GetKeyDouble("DrivePrms", "Wheel1SteerDriveCoupling", &m_UnderCarriagePrms.vdSteerDriveCoupling[0], true); - iniFile.GetKeyDouble("DrivePrms", "Wheel2SteerDriveCoupling", &m_UnderCarriagePrms.vdSteerDriveCoupling[1], true); - iniFile.GetKeyDouble("DrivePrms", "Wheel3SteerDriveCoupling", &m_UnderCarriagePrms.vdSteerDriveCoupling[2], true); - iniFile.GetKeyDouble("DrivePrms", "Wheel4SteerDriveCoupling", &m_UnderCarriagePrms.vdSteerDriveCoupling[3], true); - - iniFile.GetKeyDouble("DrivePrms", "Wheel1NeutralPosition", &m_UnderCarriagePrms.WheelNeutralPos[0], true); - iniFile.GetKeyDouble("DrivePrms", "Wheel2NeutralPosition", &m_UnderCarriagePrms.WheelNeutralPos[1], true); - iniFile.GetKeyDouble("DrivePrms", "Wheel3NeutralPosition", &m_UnderCarriagePrms.WheelNeutralPos[2], true); - iniFile.GetKeyDouble("DrivePrms", "Wheel4NeutralPosition", &m_UnderCarriagePrms.WheelNeutralPos[3], true); - - for(int i = 0; i<4; i++) - { - m_UnderCarriagePrms.WheelNeutralPos[i] = MathSup::convDegToRad(m_UnderCarriagePrms.WheelNeutralPos[i]); - // provisorial --> skip interpolation - m_vdAngGearSteerCmdRad[i] = m_UnderCarriagePrms.WheelNeutralPos[i]; - //m_vdAngGearSteerIntpRad[i] = m_UnderCarriagePrms.WheelNeutralPos[i]; - - // also Init choosen Target angle - m_vdAngGearSteerTargetRad[i] = m_UnderCarriagePrms.WheelNeutralPos[i]; - } - - iniFile.GetKeyDouble("Thread", "ThrUCarrCycleTimeS", &m_UnderCarriagePrms.dCmdRateS, true); - - // Read Values for Steering Position Controller from IniFile - iniFile.SetFileName(m_sIniDirectory + "MotionCtrl.ini", "PltfHardwareCoB3.h"); - // Prms of Impedance-Ctrlr - iniFile.GetKeyDouble("SteerCtrl", "Spring", &m_dSpring, true); - iniFile.GetKeyDouble("SteerCtrl", "Damp", &m_dDamp, true); - iniFile.GetKeyDouble("SteerCtrl", "VirtMass", &m_dVirtM, true); - iniFile.GetKeyDouble("SteerCtrl", "DPhiMax", &m_dDPhiMax, true); - iniFile.GetKeyDouble("SteerCtrl", "DDPhiMax", &m_dDDPhiMax, true); - - // calculate polar coords of Wheel Axis in robot coordinate frame - for(int i=0; i<4; i++) - { - m_vdWheelDistMM[i] = sqrt( (m_vdWheelXPosMM[i] * m_vdWheelXPosMM[i]) + (m_vdWheelYPosMM[i] * m_vdWheelYPosMM[i]) ); - m_vdWheelAngRad[i] = MathSup::atan4quad(m_vdWheelXPosMM[i], m_vdWheelYPosMM[i]); - } - - // Calculate exact position of wheels in cart. and polar coords in robot coordinate frame - CalcExWheelPos(); - - // calculate compensation factor for velocity - for(int i = 0; i<4; i++) - { - m_UnderCarriagePrms.vdFactorVel[i] = - m_UnderCarriagePrms.vdSteerDriveCoupling[i] - +(double(m_UnderCarriagePrms.iDistSteerAxisToDriveWheelMM) / double(m_UnderCarriagePrms.iRadiusWheelMM)); - } - -} - -// Set desired value for Plattfrom Velocity to UndercarriageCtrl (Sollwertvorgabe) -void UndercarriageCtrlGeom::SetDesiredPltfVelocity(double dCmdVelLongMMS, double dCmdVelLatMMS, double dCmdRotRobRadS, double dCmdRotVelRadS) -{ - // declare auxiliary variables - double dCurrentPosWheelRAD; - double dtempDeltaPhi1RAD, dtempDeltaPhi2RAD; // difference between possible steering angels and current steering angle - double dtempDeltaPhiCmd1RAD, dtempDeltaPhiCmd2RAD; // difference between possible steering angels and last target steering angle - double dtempWeightedDelta1RAD, dtempWeightedDelta2RAD; // weighted Summ of the two distance values - - // copy function parameters to member variables - m_dCmdVelLongMMS = dCmdVelLongMMS; - m_dCmdVelLatMMS = dCmdVelLatMMS; - m_dCmdRotRobRadS = dCmdRotRobRadS; - m_dCmdRotVelRadS = dCmdRotVelRadS; - - CalcInverse(); - - // determine optimal Pltf-Configuration - for (int i = 0; i<4; i++) - { - // Normalize Actual Wheel Position before calculation - dCurrentPosWheelRAD = m_vdAngGearSteerRad[i]; - MathSup::normalizePi(dCurrentPosWheelRAD); - - // Calculate differences between current config to possible set-points - dtempDeltaPhi1RAD = m_vdAngGearSteerTarget1Rad[i] - dCurrentPosWheelRAD; - dtempDeltaPhi2RAD = m_vdAngGearSteerTarget2Rad[i] - dCurrentPosWheelRAD; - MathSup::normalizePi(dtempDeltaPhi1RAD); - MathSup::normalizePi(dtempDeltaPhi2RAD); - // Calculate differences between last steering target to possible set-points - dtempDeltaPhiCmd1RAD = m_vdAngGearSteerTarget1Rad[i] - m_vdAngGearSteerTargetRad[i]; - dtempDeltaPhiCmd2RAD = m_vdAngGearSteerTarget2Rad[i] - m_vdAngGearSteerTargetRad[i]; - MathSup::normalizePi(dtempDeltaPhiCmd1RAD); - MathSup::normalizePi(dtempDeltaPhiCmd2RAD); - - // determine optimal setpoint value - // 1st which set point is closest to current cinfog - // but: avoid permanent switching (if next target is about PI/2 from current config) - // 2nd which set point is closest to last set point - // "fitness criteria" to choose optimal set point: - // calculate accumulted (+ weighted) difference between targets, current config. and last command - dtempWeightedDelta1RAD = 0.6*fabs(dtempDeltaPhi1RAD) + 0.4*fabs(dtempDeltaPhiCmd1RAD); - dtempWeightedDelta2RAD = 0.6*fabs(dtempDeltaPhi2RAD) + 0.4*fabs(dtempDeltaPhiCmd2RAD); - - // check which set point "minimizes fitness criteria" - if (dtempWeightedDelta1RAD <= dtempWeightedDelta2RAD) - { - // Target1 is "optimal" - m_vdVelGearDriveTargetRadS[i] = m_vdVelGearDriveTarget1RadS[i]; - m_vdAngGearSteerTargetRad[i] = m_vdAngGearSteerTarget1Rad[i]; - } - else - { - // Target2 is "optimal" - m_vdVelGearDriveTargetRadS[i] = m_vdVelGearDriveTarget2RadS[i]; - m_vdAngGearSteerTargetRad[i] = m_vdAngGearSteerTarget2Rad[i]; - } - - // provisorial --> skip interpolation and always take Target1 - //m_vdVelGearDriveCmdRadS[i] = m_vdVelGearDriveTarget1RadS[i]; - //m_vdAngGearSteerCmdRad[i] = m_vdAngGearSteerTarget1Rad[i]; - - /*// interpolate between last setpoint and theone of the new setpoint, which is closest to the current configuration - if (fabs(dtempDeltaPhi1RAD) <= fabs(dtempDeltaPhi2RAD)) - { - // difference between new target orientation and last (interpolated) target orientation - dtempDeltaPhi1RAD = m_vdAngGearSteerTarget1Rad[i] - m_vdAngGearSteerIntpRad[i]; - MathSup::normalizePi(dtempDeltaPhi1RAD); - - // calculate interpolation step sizes, to reach target at end of the cycle - m_vdDeltaAngIntpRad[i] = dtempDeltaPhi1RAD; - m_vdDeltaDriveIntpRadS[i] = (m_vdVelGearDriveTarget1RadS[i] - m_vdVelGearDriveIntpRadS[i]); - - // additionally calculate meen change in angular config for feedforward cmd - //m_vdVelGearSteerIntpRadS[i] = dtempDeltaPhi1RAD/m_UnderCarriagePrms.dCmdRateS; - } - else - { - // difference between new target orientation and last (interpolated) target orientation - dtempDeltaPhi2RAD = m_vdAngGearSteerTarget2Rad[i] - m_vdAngGearSteerIntpRad[i]; - MathSup::normalizePi(dtempDeltaPhi2RAD); - - // calculate interpolation step sizes, to reach target at end of the cycle - m_vdDeltaAngIntpRad[i] = dtempDeltaPhi2RAD; - m_vdDeltaDriveIntpRadS[i] = (m_vdVelGearDriveTarget2RadS[i] - m_vdVelGearDriveIntpRadS[i]); - - // additionally calculate meen change in angular config for feedforward cmd - //m_vdVelGearSteerIntpRadS[i] = dtempDeltaPhi2RAD/m_UnderCarriagePrms.dCmdRateS; - }*/ - } - - /*// Logging for debugging - // get current time - m_RawTime.SetNow(); - m_dNowTime = m_RawTime - m_StartTime; - // Log out Pltf-Velocities - fprintf(m_pfileDesVel, "%f %f %f %f \n", m_dNowTime, dCmdVelLongMMS, dCmdVelLatMMS, dCmdRotRobRadS); - fprintf(m_pfileMeasVel, "%f %f %f %f \n", m_dNowTime, m_dVelLongMMS, m_dVelLatMMS, m_dRotRobRadS); - // Log out corresponding Joint-Configuration - fprintf(m_pfileSteerAngTarget1, "%f %f %f %f %f \n", m_dNowTime, m_vdAngGearSteerTarget1Rad[0], m_vdAngGearSteerTarget1Rad[1], m_vdAngGearSteerTarget1Rad[2], m_vdAngGearSteerTarget1Rad[3]); - fprintf(m_pfileSteerAngTarget2, "%f %f %f %f %f \n", m_dNowTime, m_vdAngGearSteerTarget2Rad[0], m_vdAngGearSteerTarget2Rad[1], m_vdAngGearSteerTarget2Rad[2], m_vdAngGearSteerTarget2Rad[3]); - fprintf(m_pfileSteerAngTarget, "%f %f %f %f %f \n", m_dNowTime, m_vdAngGearSteerTargetRad[0], m_vdAngGearSteerTargetRad[1], m_vdAngGearSteerTargetRad[2], m_vdAngGearSteerTargetRad[3]); - fprintf(m_pfileDriveVelTarget, "%f %f %f %f %f \n", m_dNowTime, m_vdVelGearDriveTargetRadS[0], m_vdVelGearDriveTargetRadS[1], m_vdVelGearDriveTargetRadS[2], m_vdVelGearDriveTargetRadS[3]);*/ -} - -// Set actual values of wheels (steer/drive velocity/position) (Istwerte) -void UndercarriageCtrlGeom::SetActualWheelValues(std::vector vdVelGearDriveRadS, std::vector vdVelGearSteerRadS, std::vector vdDltAngGearDriveRad, std::vector vdAngGearSteerRad) -{ - //LOG_OUT("Set Wheel Position to Controller"); - - m_vdVelGearDriveRadS = vdVelGearDriveRadS; - m_vdVelGearSteerRadS = vdVelGearSteerRadS; - m_vdDltAngGearDriveRad = vdDltAngGearDriveRad; - m_vdAngGearSteerRad = vdAngGearSteerRad; - - // calc exact Wheel Positions (taking into account lever arm) - CalcExWheelPos(); - - // Peform calculation of direct kinematics (approx.) based on corrected Wheel Positions - CalcDirect(); -} - -// Get result of inverse kinematics (without controller) -void UndercarriageCtrlGeom::GetSteerDriveSetValues(std::vector & vdVelGearDriveRadS, std::vector & vdAngGearSteerRad) -{ - //LOG_OUT("Calculate Inverse for given Velocity Command"); - - CalcInverse(); - - vdVelGearDriveRadS = m_vdVelGearDriveTarget1RadS; - vdAngGearSteerRad = m_vdAngGearSteerTarget1Rad; -} - -// Get set point values for the Wheels (including controller) from UndercarriangeCtrl -void UndercarriageCtrlGeom::GetNewCtrlStateSteerDriveSetValues(std::vector & vdVelGearDriveRadS, std::vector & vdVelGearSteerRadS, std::vector & vdAngGearSteerRad, - double & dVelLongMMS, double & dVelLatMMS, double & dRotRobRadS, double & dRotVelRadS) -{ - - if(m_bEMStopActive == false) - { - //Calculate next step - CalcControlStep(); - } - - vdVelGearDriveRadS = m_vdVelGearDriveCmdRadS; - vdVelGearSteerRadS = m_vdVelGearSteerCmdRadS; - vdAngGearSteerRad = m_vdAngGearSteerCmdRad; - - dVelLongMMS = m_dCmdVelLongMMS; - dVelLatMMS = m_dCmdVelLatMMS; - dRotRobRadS = m_dCmdRotRobRadS; - dRotVelRadS = m_dCmdRotVelRadS; - - /*// Logging for debugging - // get current time - m_RawTime.SetNow(); - m_dNowTime = m_RawTime - m_StartTime; - // Log out resulting joint-commands - fprintf(m_pfileSteerAngCmd, "%f %f %f %f %f \n", m_dNowTime, m_vdAngGearSteerCmdRad[0], m_vdAngGearSteerCmdRad[1], m_vdAngGearSteerCmdRad[2], m_vdAngGearSteerCmdRad[3]); - fprintf(m_pfileSteerVelCmd, "%f %f %f %f %f \n", m_dNowTime, m_vdVelGearSteerCmdRadS[0], m_vdVelGearSteerCmdRadS[1], m_vdVelGearSteerCmdRadS[2], m_vdVelGearSteerCmdRadS[3]); - fprintf(m_pfileDriveVelCmd, "%f %f %f %f %f \n", m_dNowTime, m_vdVelGearDriveCmdRadS[0], m_vdVelGearDriveCmdRadS[1], m_vdVelGearDriveCmdRadS[2], m_vdVelGearDriveCmdRadS[3]);*/ -} - -// Get result of direct kinematics -void UndercarriageCtrlGeom::GetActualPltfVelocity(double & dDeltaLongMM, double & dDeltaLatMM, double & dDeltaRotRobRad, double & dDeltaRotVelRad, - double & dVelLongMMS, double & dVelLatMMS, double & dRotRobRadS, double & dRotVelRadS) -{ - dVelLongMMS = m_dVelLongMMS; - dVelLatMMS = m_dVelLatMMS; - dRotRobRadS = m_dRotRobRadS; - dRotVelRadS = m_dRotVelRadS; - - // calculate travelled distance and angle (from velocity) for output - // ToDo: make sure this corresponds to cycle-freqnecy of calling node - // --> specify via config file - dDeltaLongMM = dVelLongMMS * m_UnderCarriagePrms.dCmdRateS; - dDeltaLatMM = dVelLatMMS * m_UnderCarriagePrms.dCmdRateS; - dDeltaRotRobRad = dRotRobRadS * m_UnderCarriagePrms.dCmdRateS; - dDeltaRotVelRad = dRotVelRadS * m_UnderCarriagePrms.dCmdRateS; -} - -// calculate inverse kinematics -void UndercarriageCtrlGeom::CalcInverse(void) -{ - // help variable to store velocities of the steering axis in mm/s - double dtempAxVelXRobMMS, dtempAxVelYRobMMS; - - // check if zero movement commanded -> keep orientation of wheels, set wheel velocity to zero - if((m_dCmdVelLongMMS == 0) && (m_dCmdVelLatMMS == 0) && (m_dCmdRotRobRadS == 0) && (m_dCmdRotVelRadS == 0)) - { - for(int i = 0; i<4; i++) - { - m_vdAngGearSteerTarget1Rad[i] = m_vdAngGearSteerRad[i]; - m_vdVelGearDriveTarget1RadS[i] = 0.0; - m_vdAngGearSteerTarget2Rad[i] = m_vdAngGearSteerRad[i]; - m_vdVelGearDriveTarget2RadS[i] = 0.0; - } - return; - } - - // calculate sets of possible Steering Angle // Drive-Velocity combinations - for (int i = 0; i<4; i++) - { - // calculate velocity and direction of single wheel motion - // Translational Portion - dtempAxVelXRobMMS = m_dCmdVelLongMMS; - dtempAxVelYRobMMS = m_dCmdVelLatMMS; - // Rotational Portion - dtempAxVelXRobMMS += m_dCmdRotRobRadS * m_vdExWheelDistMM[i] * -sin(m_vdExWheelAngRad[i]); - dtempAxVelYRobMMS += m_dCmdRotRobRadS * m_vdExWheelDistMM[i] * cos(m_vdExWheelAngRad[i]); - - // calculate resulting steering angle - // Wheel has to move in direction of resulting velocity vector of steering axis - m_vdAngGearSteerTarget1Rad[i] = MathSup::atan4quad(dtempAxVelYRobMMS, dtempAxVelXRobMMS); - // calculate corresponding angle in opposite direction (+180 degree) - m_vdAngGearSteerTarget2Rad[i] = m_vdAngGearSteerTarget1Rad[i] + MathSup::PI; - MathSup::normalizePi(m_vdAngGearSteerTarget2Rad[i]); - - // calculate absolute value of rotational rate of driving wheels in rad/s - m_vdVelGearDriveTarget1RadS[i] = sqrt( (dtempAxVelXRobMMS * dtempAxVelXRobMMS) + - (dtempAxVelYRobMMS * dtempAxVelYRobMMS) ) / (double)m_UnderCarriagePrms.iRadiusWheelMM; - // now adapt to direction (forward/backward) of wheel - m_vdVelGearDriveTarget2RadS[i] = - m_vdVelGearDriveTarget1RadS[i]; - } -} - -// calculate direct kinematics -void UndercarriageCtrlGeom::CalcDirect(void) -{ - // declare auxilliary variables - double dtempVelXRobMMS; // Robot-Velocity in x-Direction (longitudinal) in mm/s (in Robot-Coordinateframe) - double dtempVelYRobMMS; // Robot-Velocity in y-Direction (lateral) in mm/s (in Robot-Coordinateframe) - double dtempRotRobRADPS; // Robot-Rotation-Rate in rad/s (in Robot-Coordinateframe) - double dtempDiffXMM; // Difference in X-Coordinate of two wheels in mm - double dtempDiffYMM; // Difference in Y-Coordinate of two wheels in mm - double dtempRelPhiWheelsRAD; // Angle between axis of two wheels w.r.t the X-Axis of the Robot-Coordinate-System in rad - double dtempRelDistWheelsMM; // distance of two wheels in mm - double dtempRelPhiWheel1RAD; // Steering Angle of (im math. pos. direction) first Wheel w.r.t. the linking axis of the two wheels - double dtempRelPhiWheel2RAD; // Steering Angle of (im math. pos. direction) first Wheel w.r.t. the linking axis of the two wheels - std::vector vdtempVelWheelMMS(4); // Wheel-Velocities (all Wheels) in mm/s - - // initial values - dtempVelXRobMMS = 0; // Robot-Velocity in x-Direction (longitudinal) in mm/s (in Robot-Coordinateframe) - dtempVelYRobMMS = 0; // Robot-Velocity in y-Direction (lateral) in mm/s (in Robot-Coordinateframe) - dtempRotRobRADPS = 0; - - - - // calculate corrected wheel velocities - for(int i = 0; i subtract angles - dtempRelPhiWheel1RAD = m_vdAngGearSteerRad[i] - dtempRelPhiWheelsRAD; - dtempRelPhiWheel2RAD = m_vdAngGearSteerRad[i+1] - dtempRelPhiWheelsRAD; - - dtempRotRobRADPS += (vdtempVelWheelMMS[i+1] * sin(dtempRelPhiWheel2RAD) - vdtempVelWheelMMS[i] * sin(dtempRelPhiWheel1RAD))/dtempRelDistWheelsMM; - } - - // calculate last missing axis (between last wheel and 1.) - // calc. Parameters (Dist,Phi) of virtual linking axis of the two considered wheels - dtempDiffXMM = m_vdExWheelXPosMM[0] - m_vdExWheelXPosMM[m_iNumberOfDrives-1]; - dtempDiffYMM = m_vdExWheelYPosMM[0] - m_vdExWheelYPosMM[m_iNumberOfDrives-1]; - dtempRelDistWheelsMM = sqrt( dtempDiffXMM*dtempDiffXMM + dtempDiffYMM*dtempDiffYMM ); - dtempRelPhiWheelsRAD = MathSup::atan4quad( dtempDiffYMM, dtempDiffXMM ); - - // transform velocity of wheels into relative coordinate frame of linking axes -> subtract angles - dtempRelPhiWheel1RAD = m_vdAngGearSteerRad[m_iNumberOfDrives-1] - dtempRelPhiWheelsRAD; - dtempRelPhiWheel2RAD = m_vdAngGearSteerRad[0] - dtempRelPhiWheelsRAD; - - // close calculation of robots rotational velocity - dtempRotRobRADPS += (vdtempVelWheelMMS[0]*sin(dtempRelPhiWheel2RAD) - vdtempVelWheelMMS[m_iNumberOfDrives-1]*sin(dtempRelPhiWheel1RAD))/dtempRelDistWheelsMM; - - // calculate linear velocity of robot - for(int i = 0; i set to zero - - // assign linear velocity of robot for output - m_dVelLongMMS = dtempVelXRobMMS/m_iNumberOfDrives; - m_dVelLatMMS = dtempVelYRobMMS/m_iNumberOfDrives; - - - -} - -// calculate Exact Wheel Position in robot coordinates -void UndercarriageCtrlGeom::CalcExWheelPos(void) -{ - // calculate wheel position and velocity - for(int i = 0; i<4; i++) - { - // calculate current geometry of robot (exact wheel position, taking into account steering offset of wheels) - m_vdExWheelXPosMM[i] = m_vdWheelXPosMM[i] + m_UnderCarriagePrms.iDistSteerAxisToDriveWheelMM * sin(m_vdAngGearSteerRad[i]); - m_vdExWheelYPosMM[i] = m_vdWheelYPosMM[i] - m_UnderCarriagePrms.iDistSteerAxisToDriveWheelMM * cos(m_vdAngGearSteerRad[i]); - - // calculate distance from platform center to wheel center - m_vdExWheelDistMM[i] = sqrt( (m_vdExWheelXPosMM[i] * m_vdExWheelXPosMM[i]) + (m_vdExWheelYPosMM[i] * m_vdExWheelYPosMM[i]) ); - - // calculate direction of rotational vector - m_vdExWheelAngRad[i] = MathSup::atan4quad( m_vdExWheelYPosMM[i], m_vdExWheelXPosMM[i]); - } -} - -// perform one discrete Control Step (controls steering angle) -void UndercarriageCtrlGeom::CalcControlStep(void) -{ - // check if zero movement commanded -> keep orientation of wheels, set steer velocity to zero - if ((m_dCmdVelLongMMS == 0) && (m_dCmdVelLatMMS == 0) && (m_dCmdRotRobRadS == 0) && (m_dCmdRotVelRadS == 0)) - { - m_vdVelGearDriveCmdRadS.assign(4,0.0); // set velocity for drives to zero - m_vdVelGearSteerCmdRadS.assign(4,0.0); // set velocity for steers to zero - - // set internal states of controller to zero - for(int i=0; i<4; i++) - { - m_vdCtrlVal[i][0] = 0.0; - m_vdCtrlVal[i][1] = 0.0; - } - return; - } - - // declare auxilliary variables - double dCurrentPosWheelRAD; - double dDeltaPhi; - double dForceDamp, dForceProp, dAccCmd, dVelCmdInt; // PI- and Impedance-Ctrl - - for (int i=0; i<4; i++) - { - // provisorial --> skip interpolation and always take Target - m_vdVelGearDriveCmdRadS[i] = m_vdVelGearDriveTargetRadS[i]; - m_vdAngGearSteerCmdRad[i] = m_vdAngGearSteerTargetRad[i]; - - // provisorial --> skip interpolation and always take Target1 - //m_vdVelGearDriveCmdRadS[i] = m_vdVelGearDriveTarget1RadS[i]; - //m_vdAngGearSteerCmdRad[i] = m_vdAngGearSteerTarget1Rad[i]; - - /*m_vdAngGearSteerIntpRad[i] += m_vdDeltaAngIntpRad[i]; - MathSup::normalizePi(m_vdAngGearSteerIntpRad[i]); - m_vdVelGearDriveIntpRadS[i] += m_vdDeltaDriveIntpRadS[i]; - - m_vdVelGearDriveCmdRadS[i] = m_vdVelGearDriveIntpRadS[i]; - m_vdAngGearSteerCmdRad[i] = m_vdAngGearSteerIntpRad[i];*/ - } - - - for (int i = 0; i<4; i++) - { - // Normalize Actual Wheel Position before calculation - dCurrentPosWheelRAD = m_vdAngGearSteerRad[i]; - MathSup::normalizePi(dCurrentPosWheelRAD); - dDeltaPhi = m_vdAngGearSteerCmdRad[i] - dCurrentPosWheelRAD; - MathSup::normalizePi(dDeltaPhi); - - // Impedance-Ctrl - // Calculate resulting desired forces, velocities - // double dForceDamp, dForceProp, dAccCmd, dVelCmdInt; - dForceDamp = - m_dDamp * m_vdCtrlVal[i][1]; - dForceProp = m_dSpring * dDeltaPhi; - dAccCmd = (dForceDamp + dForceProp) / m_dVirtM; - if (dAccCmd > m_dDDPhiMax) - { - dAccCmd = m_dDDPhiMax; - } - else if (dAccCmd < -m_dDDPhiMax) - { - dAccCmd = -m_dDDPhiMax; - } - dVelCmdInt = m_vdCtrlVal[i][1] + m_UnderCarriagePrms.dCmdRateS * dAccCmd; - if (dVelCmdInt > m_dDPhiMax) - { - dVelCmdInt = m_dDPhiMax; - } - else if (dVelCmdInt < -m_dDPhiMax) - { - dVelCmdInt = -m_dDPhiMax; - } - // Store internal ctrlr-states - m_vdCtrlVal[i][0] = dDeltaPhi; - m_vdCtrlVal[i][1] = dVelCmdInt; - // set outputs - m_vdVelGearSteerCmdRadS[i] = dVelCmdInt; - - // Check if Steeringvelocity overgo maximum allowed rates. - if(fabs(m_vdVelGearSteerCmdRadS[i]) > m_UnderCarriagePrms.dMaxSteerRateRadpS) - { - if (m_vdVelGearSteerCmdRadS[i] > 0) - m_vdVelGearSteerCmdRadS[i] = m_UnderCarriagePrms.dMaxSteerRateRadpS; - else - m_vdVelGearSteerCmdRadS[i] = -m_UnderCarriagePrms.dMaxSteerRateRadpS; - } - } - - // Correct Driving-Wheel-Velocity, because of coupling and axis-offset - for (int i = 0; i<4; i++) - { - m_vdVelGearDriveCmdRadS[i] += m_vdVelGearSteerCmdRadS[i] * m_UnderCarriagePrms.vdFactorVel[i]; - } - -} - -// operator overloading -void UndercarriageCtrlGeom::operator=(const UndercarriageCtrlGeom & GeomCtrl) -{ - // Actual Values for PltfMovement (calculated from Actual Wheelspeeds) - m_dVelLongMMS = GeomCtrl.m_dVelLongMMS; - m_dVelLatMMS = GeomCtrl.m_dVelLatMMS; - m_dRotRobRadS = GeomCtrl.m_dRotRobRadS; - m_dRotVelRadS = GeomCtrl.m_dRotVelRadS; - - // Actual Wheelspeed (read from Motor-Ctrls) - m_vdVelGearDriveRadS = GeomCtrl.m_vdVelGearDriveRadS; - m_vdVelGearSteerRadS = GeomCtrl.m_vdVelGearSteerRadS; - m_vdDltAngGearDriveRad = GeomCtrl.m_vdDltAngGearDriveRad; - m_vdAngGearSteerRad = GeomCtrl.m_vdAngGearSteerRad; - - // Desired Pltf-Movement (set from PltfHwItf) - m_dCmdVelLongMMS = GeomCtrl.m_dCmdVelLongMMS; - m_dCmdVelLatMMS = GeomCtrl.m_dCmdVelLatMMS; - m_dCmdRotRobRadS = GeomCtrl.m_dCmdRotRobRadS; - m_dCmdRotVelRadS = GeomCtrl.m_dCmdRotVelRadS; - - // Desired Wheelspeeds (calculated from desired ICM-configuration) - m_vdVelGearDriveCmdRadS = GeomCtrl.m_vdVelGearDriveCmdRadS; - m_vdVelGearSteerCmdRadS = GeomCtrl.m_vdVelGearSteerCmdRadS; - m_vdAngGearSteerCmdRad = GeomCtrl.m_vdAngGearSteerCmdRad; - - // Target Wheelspeed and -angle (calculated from desired Pltf-Movement with Inverse without controle!) - // alternativ 1 for steering angle - m_vdAngGearSteerTarget1Rad = GeomCtrl.m_vdAngGearSteerTarget1Rad; - m_vdVelGearDriveTarget1RadS = GeomCtrl.m_vdVelGearDriveTarget1RadS; - // alternativ 2 for steering angle (+/- PI) - m_vdAngGearSteerTarget2Rad = GeomCtrl.m_vdAngGearSteerTarget2Rad; - m_vdVelGearDriveTarget2RadS = GeomCtrl.m_vdVelGearDriveTarget2RadS; - - // Position of the Wheels' Steering Axis' - m_vdWheelXPosMM = GeomCtrl.m_vdWheelXPosMM; - m_vdWheelYPosMM = GeomCtrl.m_vdWheelYPosMM; - m_vdWheelDistMM = GeomCtrl.m_vdWheelDistMM; - m_vdWheelAngRad = GeomCtrl.m_vdWheelAngRad; - - // Exact Position of the Wheels' itself - m_vdExWheelXPosMM = GeomCtrl.m_vdExWheelXPosMM; - m_vdExWheelYPosMM = GeomCtrl.m_vdExWheelYPosMM; - m_vdExWheelDistMM = GeomCtrl.m_vdExWheelDistMM; - m_vdExWheelAngRad = GeomCtrl.m_vdExWheelAngRad; - - // Prms - m_UnderCarriagePrms = GeomCtrl.m_UnderCarriagePrms; - - // Position Controller Steer Wheels - // Impedance-Ctrlr - m_dSpring = GeomCtrl.m_dSpring; - m_dDamp = GeomCtrl.m_dDamp; - m_dVirtM = GeomCtrl.m_dDPhiMax; - m_dDPhiMax = GeomCtrl.m_dDPhiMax; - m_dDDPhiMax = GeomCtrl.m_dDDPhiMax; - // Storage for internal controller states - m_vdCtrlVal = GeomCtrl.m_vdCtrlVal; -} - -// set EM Flag and stop ctrlr if active -void UndercarriageCtrlGeom::setEMStopActive(bool bEMStopActive) -{ - m_bEMStopActive = bEMStopActive; - - // if emergency stop reset ctrlr to zero - if(m_bEMStopActive) - { - // Steermodules - for(int i=0; i<4; i++) - { - for(int j=0; j< 2; j++) - { - m_vdCtrlVal[i][j] = 0.0; - } - } - // Outputs - for(int i=0; i<4; i++) - { - m_vdVelGearDriveCmdRadS[i] = 0.0; - m_vdVelGearSteerCmdRadS[i] = 0.0; - } - } - -} diff --git a/cob_undercarriage_ctrl/package.xml b/cob_undercarriage_ctrl/package.xml deleted file mode 100644 index c6bf7cc88..000000000 --- a/cob_undercarriage_ctrl/package.xml +++ /dev/null @@ -1,26 +0,0 @@ - - cob_undercarriage_ctrl - 0.7.16 - cob_undercarriage_ctrl implements a controller for the omnidirectional base of Care-O-bot 3 on joint level. For a given Pltf-Twist the according wheel steering angles and linear wheel velocities are calculated based on the principle of rigid body motion. Each joint is than controlled individually to achieve the computed position and velocity - - Apache 2.0 - - http://ros.org/wiki/cob_undercarriage_ctrl - - - Matthias Gruhler - Christian Connette - - catkin - - cob_msgs - cob_utilities - control_msgs - diagnostic_msgs - diagnostic_updater - geometry_msgs - nav_msgs - roscpp - tf - - diff --git a/cob_undercarriage_ctrl/ros/src/cob_undercarriage_ctrl.cpp b/cob_undercarriage_ctrl/ros/src/cob_undercarriage_ctrl.cpp deleted file mode 100644 index 2c03e5ca7..000000000 --- a/cob_undercarriage_ctrl/ros/src/cob_undercarriage_ctrl.cpp +++ /dev/null @@ -1,714 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -//################## -//#### includes #### - -// standard includes -#include - -// ROS includes -#include - -// ROS message includes -#include -#include -#include -#include -#include -#include -#include - -// external includes -#include -#include -//#include - -//#################### -//#### node class #### -class NodeClass -{ - // - public: - // create a handle for this node, initialize node - ros::NodeHandle n; - - // topics to publish - ros::Publisher topic_pub_joint_state_cmd_; // cmd issued for single joints of undercarriage - ros::Publisher topic_pub_controller_joint_command_; - ros::Publisher topic_pub_odometry_; // calculated (measured) velocity, rotation and pose (odometry-based) for the robot - tf::TransformBroadcaster tf_broadcast_odometry_; // according transformation for the tf broadcaster - - // topics to subscribe, callback is called for new messages arriving - ros::Subscriber topic_sub_CMD_pltf_twist_; // issued command to be achieved by the platform - ros::Subscriber topic_sub_EM_stop_state_; // current emergency stop state (free, active, confirmed) - ros::Subscriber topic_sub_drive_diagnostic_;// status of drive chain (initializing, error, normal) - - //subscribe to JointStates topic - //ros::Subscriber topic_sub_joint_states_; - ros::Subscriber topic_sub_joint_controller_states_; - - // diagnostic stuff - diagnostic_updater::Updater updater_; - - // controller Timer - ros::Timer timer_ctrl_step_; - - // member variables - UndercarriageCtrlGeom * ucar_ctrl_; // instantiate undercarriage controller - std::string sIniDirectory; - bool is_initialized_bool_; // flag wether node is already up and running - bool broadcast_tf_; // flag wether to broadcast the tf from odom to base_link - int drive_chain_diagnostic_; // flag whether base drive chain is operating normal - ros::Time last_time_; // time Stamp for last odometry measurement - ros::Time joint_state_odom_stamp_; // time stamp of joint states used for current odometry calc - double sample_time_, timeout_; - double x_rob_m_, y_rob_m_, theta_rob_rad_; // accumulated motion of robot since startup - int iwatchdog_; - double vel_x_rob_last_, vel_y_rob_last_, vel_theta_rob_last_; //save velocities for better odom calculation - double max_vel_trans_, max_vel_rot_; - - int m_iNumJoints; - - diagnostic_msgs::DiagnosticStatus diagnostic_status_lookup_; // used to access defines for warning levels - - // Constructor - NodeClass() - { - // initialization of variables - is_initialized_bool_ = false; - broadcast_tf_ = true; - iwatchdog_ = 0; - last_time_ = ros::Time::now(); - sample_time_ = 0.020; - x_rob_m_ = 0.0; - y_rob_m_ = 0.0; - theta_rob_rad_ = 0.0; - vel_x_rob_last_ = 0.0; - vel_y_rob_last_ = 0.0; - vel_theta_rob_last_ = 0.0; - // set status of drive chain to WARN by default - drive_chain_diagnostic_ = diagnostic_status_lookup_.OK; //WARN; <- THATS FOR DEBUGGING ONLY! - - // Parameters are set within the launch file - // read in timeout for watchdog stopping the controller. - if (n.hasParam("timeout")) - { - n.getParam("timeout", timeout_); - ROS_INFO("Timeout loaded from Parameter-Server is: %fs", timeout_); - } - else - { - ROS_WARN("No parameter timeout on Parameter-Server. Using default: 1.0s"); - timeout_ = 1.0; - } - if ( timeout_ < sample_time_ ) - { - ROS_WARN("Specified timeout < sample_time. Setting timeout to sample_time = %fs", sample_time_); - timeout_ = sample_time_; - } - - // Read number of drives from iniFile and pass IniDirectory to CobPlatfCtrl. - if (n.hasParam("IniDirectory")) - { - n.getParam("IniDirectory", sIniDirectory); - ROS_INFO("IniDirectory loaded from Parameter-Server is: %s", sIniDirectory.c_str()); - } - else - { - sIniDirectory = "Platform/IniFiles/"; - ROS_WARN("IniDirectory not found on Parameter-Server, using default value: %s", sIniDirectory.c_str()); - } - - if (n.hasParam("max_trans_velocity")) - { - n.getParam("max_trans_velocity", max_vel_trans_); - ROS_INFO("Max translational velocity loaded from Parameter-Server is: %fs", max_vel_trans_); - } - else - { - ROS_WARN("No parameter max_trans_velocity on Parameter-Server. Using default: 1.1 m/s"); - max_vel_trans_ = 1.1; - } - if (n.hasParam("max_rot_velocity")) - { - n.getParam("max_rot_velocity", max_vel_rot_); - ROS_INFO("Max rotational velocity loaded from Parameter-Server is: %fs", max_vel_rot_); - } - else - { - ROS_WARN("No parameter max_rot_velocity on Parameter-Server. Using default: 1.8 rad/s"); - max_vel_rot_ = 1.8; - } - if (n.hasParam("broadcast_tf")) - { - n.getParam("broadcast_tf", broadcast_tf_); - } - - IniFile iniFile; - iniFile.SetFileName(sIniDirectory + "Platform.ini", "PltfHardwareCoB3.h"); - iniFile.GetKeyInt("Config", "NumberOfMotors", &m_iNumJoints, true); - - ucar_ctrl_ = new UndercarriageCtrlGeom(sIniDirectory); - - - // implementation of topics - // published topics - //topic_pub_joint_state_cmd_ = n.advertise("joint_command", 1); - topic_pub_controller_joint_command_ = n.advertise ("joint_command", 1); - - topic_pub_odometry_ = n.advertise("odometry", 1); - - // subscribed topics - topic_sub_CMD_pltf_twist_ = n.subscribe("command", 1, &NodeClass::topicCallbackTwistCmd, this); - topic_sub_EM_stop_state_ = n.subscribe("/emergency_stop_state", 1, &NodeClass::topicCallbackEMStop, this); - topic_sub_drive_diagnostic_ = n.subscribe("diagnostic", 1, &NodeClass::topicCallbackDiagnostic, this); - - - - //topic_sub_joint_states_ = n.subscribe("/joint_states", 1, &NodeClass::topicCallbackJointStates, this); - topic_sub_joint_controller_states_ = n.subscribe("state", 1, &NodeClass::topicCallbackJointControllerStates, this); - - // diagnostics - updater_.setHardwareID(ros::this_node::getName()); - updater_.add("initialization", this, &NodeClass::diag_init); - - //set up timer to cyclically call controller-step - timer_ctrl_step_ = n.createTimer(ros::Duration(sample_time_), &NodeClass::timerCallbackCtrlStep, this); - - } - - // Destructor - ~NodeClass() - { - } - - void diag_init(diagnostic_updater::DiagnosticStatusWrapper &stat) - { - if(is_initialized_bool_) - stat.summary(diagnostic_msgs::DiagnosticStatus::OK, ""); - else - stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, ""); - stat.add("Initialized", is_initialized_bool_); - } - - // Listen for Pltf Cmds - void topicCallbackTwistCmd(const geometry_msgs::Twist::ConstPtr& msg) - { - double vx_cmd_mms, vy_cmd_mms, w_cmd_rads; - - // check for NaN value in Twist message - if(isnan(msg->linear.x) || isnan(msg->linear.y) || isnan(msg->angular.z)) { - iwatchdog_ = 0; - ROS_FATAL("Received NaN-value in Twist message. Stopping the robot."); - // force platform velocity commands to zero; - ucar_ctrl_->SetDesiredPltfVelocity(0.0, 0.0, 0.0, 0.0); - ROS_DEBUG("Forced platform velocity commands to zero"); - return; - } - - if( (fabs(msg->linear.x) > max_vel_trans_) || (fabs(msg->linear.y) > max_vel_trans_) || (fabs(msg->angular.z) > max_vel_rot_)) - { - if(fabs(msg->linear.x) > max_vel_trans_) - { - ROS_DEBUG_STREAM("Recevied cmdVelX: " << msg->linear.x << - ", which is bigger than the maximal allowed translational velocity: " << max_vel_trans_ << " so stop the robot"); - } - if(fabs(msg->linear.y) > max_vel_trans_) - { - ROS_DEBUG_STREAM("Recevied cmdVelY: " << msg->linear.x << - ", which is bigger than the maximal allowed translational velocity: " << max_vel_trans_ << " so stop the robot"); - } - - if(fabs(msg->angular.z) > max_vel_rot_) - { - ROS_DEBUG_STREAM("Recevied cmdVelTh: " << msg->angular.z << - ", which is bigger than the maximal allowed rotational velocity: " << max_vel_rot_ << " so stop the robot"); - } - vx_cmd_mms = 0.0; - vy_cmd_mms = 0.0; - w_cmd_rads = 0.0; - } - else - { - // controller expects velocities in mm/s, ROS works with SI-Units -> convert - // ToDo: rework Controller Class to work with SI-Units - vx_cmd_mms = msg->linear.x*1000.0; - vy_cmd_mms = msg->linear.y*1000.0; - w_cmd_rads = msg->angular.z; - } - - iwatchdog_ = 0; - - // only process if controller is already initialized - if (is_initialized_bool_ && drive_chain_diagnostic_==diagnostic_status_lookup_.OK) - { - ROS_DEBUG("received new velocity command [cmdVelX=%3.5f,cmdVelY=%3.5f,cmdVelTh=%3.5f]", - msg->linear.x, msg->linear.y, msg->angular.z); - - // Set desired value for Plattform Velocity to UndercarriageCtrl (setpoint setting) - ucar_ctrl_->SetDesiredPltfVelocity(vx_cmd_mms, vy_cmd_mms, w_cmd_rads, 0.0); - // ToDo: last value (0.0) is not used anymore --> remove from interface - } - else - { - // Set desired value for Plattform Velocity to zero (setpoint setting) - ucar_ctrl_->SetDesiredPltfVelocity( 0.0, 0.0, 0.0, 0.0); - // ToDo: last value (0.0) is not used anymore --> remove from interface - ROS_DEBUG("Forced platform-velocity cmds to zero"); - } - - } - - // Listen for Emergency Stop - void topicCallbackEMStop(const cob_msgs::EmergencyStopState::ConstPtr& msg) - { - int EM_state; - EM_state = msg->emergency_state; - - if (EM_state == msg->EMFREE) - { - // Reset EM flag in Ctrlr - if (is_initialized_bool_) - { - ucar_ctrl_->setEMStopActive(false); - ROS_DEBUG("Undercarriage Controller EM-Stop released"); - // reset only done, when system initialized - // -> allows to stop ctrlr during init, reset and shutdown - } - } - else - { - ROS_DEBUG("Undercarriage Controller stopped due to EM-Stop"); - - // Set desired value for Plattform Velocity to zero (setpoint setting) - ucar_ctrl_->SetDesiredPltfVelocity( 0.0, 0.0, 0.0, 0.0); - // ToDo: last value (0.0) is not used anymore --> remove from interface - ROS_DEBUG("Forced platform-velocity cmds to zero"); - - // Set EM flag and stop Ctrlr - ucar_ctrl_->setEMStopActive(true); - } - } - - // Listens for status of underlying hardware (base drive chain) - void topicCallbackDiagnostic(const diagnostic_msgs::DiagnosticStatus::ConstPtr& msg) - { - control_msgs::JointTrajectoryControllerState joint_state_cmd; - - // prepare joint_cmds for heartbeat (compose header) - joint_state_cmd.header.stamp = ros::Time::now(); - //joint_state_cmd.header.frame_id = frame_id; //Where to get this id from? - // ToDo: configure over Config-File (number of motors) and Msg - // assign right size to JointState data containers - //joint_state_cmd.set_name_size(m_iNumMotors); - joint_state_cmd.desired.positions.resize(m_iNumJoints); - joint_state_cmd.desired.velocities.resize(m_iNumJoints); - //joint_state_cmd.desired.effort.resize(m_iNumJoints); - joint_state_cmd.joint_names.push_back("fl_caster_r_wheel_joint"); - joint_state_cmd.joint_names.push_back("fl_caster_rotation_joint"); - joint_state_cmd.joint_names.push_back("bl_caster_r_wheel_joint"); - joint_state_cmd.joint_names.push_back("bl_caster_rotation_joint"); - joint_state_cmd.joint_names.push_back("br_caster_r_wheel_joint"); - joint_state_cmd.joint_names.push_back("br_caster_rotation_joint"); - joint_state_cmd.joint_names.push_back("fr_caster_r_wheel_joint"); - joint_state_cmd.joint_names.push_back("fr_caster_rotation_joint"); - joint_state_cmd.joint_names.resize(m_iNumJoints); - - // compose jointcmds - for(int i=0; ilevel; - - // if controller is already started up ... - if (is_initialized_bool_) - { - // ... but underlying drive chain is not yet operating normal - if (drive_chain_diagnostic_ != diagnostic_status_lookup_.OK) - { - // halt controller - ROS_DEBUG("drive chain not availlable: halt Controller"); - - // Set EM flag to Ctrlr (resets internal states) - ucar_ctrl_->setEMStopActive(true); - - // Set desired value for Plattform Velocity to zero (setpoint setting) - ucar_ctrl_->SetDesiredPltfVelocity( 0.0, 0.0, 0.0, 0.0); - // ToDo: last value (0.0) is not used anymore --> remove from interface - ROS_DEBUG("Forced platform-velocity cmds to zero"); - - // if is not Initializing - if (drive_chain_diagnostic_ != diagnostic_status_lookup_.WARN) - { - // publish zero-vel. jointcmds to avoid Watchdogs stopping ctrlr - // this is already done in CalcControlStep - } - } - } - // ... while controller is not initialized send heartbeats to keep motors alive - else - { - // ... as soon as base drive chain is initialized - if(drive_chain_diagnostic_ != diagnostic_status_lookup_.WARN) - { - // publish zero-vel. jointcmds to avoid Watchdogs stopping ctrlr - topic_pub_controller_joint_command_.publish(joint_state_cmd); - } - } - } - - void topicCallbackJointControllerStates(const control_msgs::JointTrajectoryControllerState::ConstPtr& msg) { - int num_joints; - int iter_k, iter_j; - std::vector drive_joint_ang_rad, drive_joint_vel_rads, drive_joint_effort_NM; - std::vector steer_joint_ang_rad, steer_joint_vel_rads, steer_joint_effort_NM; - - joint_state_odom_stamp_ = msg->header.stamp; - - // copy configuration into vector classes - num_joints = msg->joint_names.size(); - // drive joints - drive_joint_ang_rad.assign(m_iNumJoints, 0.0); - drive_joint_vel_rads.assign(m_iNumJoints, 0.0); - drive_joint_effort_NM.assign(m_iNumJoints, 0.0); - // steer joints - steer_joint_ang_rad.assign(m_iNumJoints, 0.0); - steer_joint_vel_rads.assign(m_iNumJoints, 0.0); - steer_joint_effort_NM.assign(m_iNumJoints, 0.0); - - // init iterators - iter_k = 0; - iter_j = 0; - - for(int i = 0; i < num_joints; i++) - { - // associate inputs to according steer and drive joints - // ToDo: specify this globally (Prms-File or config-File or via msg-def.) - if(msg->joint_names[i] == "fl_caster_r_wheel_joint") - { - drive_joint_ang_rad[0] = msg->actual.positions[i]; - drive_joint_vel_rads[0] = msg->actual.velocities[i]; - //drive_joint_effort_NM[0] = msg->effort[i]; - } - if(msg->joint_names[i] == "bl_caster_r_wheel_joint") - { - drive_joint_ang_rad[1] = msg->actual.positions[i]; - drive_joint_vel_rads[1] = msg->actual.velocities[i]; - //drive_joint_effort_NM[1] = msg->effort[i]; - } - if(msg->joint_names[i] == "br_caster_r_wheel_joint") - { - drive_joint_ang_rad[2] = msg->actual.positions[i]; - drive_joint_vel_rads[2] = msg->actual.velocities[i]; - //drive_joint_effort_NM[2] = msg->effort[i]; - } - if(msg->joint_names[i] == "fr_caster_r_wheel_joint") - { - drive_joint_ang_rad[3] = msg->actual.positions[i]; - drive_joint_vel_rads[3] = msg->actual.velocities[i]; - //drive_joint_effort_NM[3] = msg->effort[i]; - } - if(msg->joint_names[i] == "fl_caster_rotation_joint") - { - steer_joint_ang_rad[0] = msg->actual.positions[i]; - steer_joint_vel_rads[0] = msg->actual.velocities[i]; - //steer_joint_effort_NM[0] = msg->effort[i]; - } - if(msg->joint_names[i] == "bl_caster_rotation_joint") - { - steer_joint_ang_rad[1] = msg->actual.positions[i]; - steer_joint_vel_rads[1] = msg->actual.velocities[i]; - //steer_joint_effort_NM[1] = msg->effort[i]; - } - if(msg->joint_names[i] == "br_caster_rotation_joint") - { - steer_joint_ang_rad[2] = msg->actual.positions[i]; - steer_joint_vel_rads[2] = msg->actual.velocities[i]; - //steer_joint_effort_NM[2] = msg->effort[i]; - } - if(msg->joint_names[i] == "fr_caster_rotation_joint") - { - steer_joint_ang_rad[3] = msg->actual.positions[i]; - steer_joint_vel_rads[3] = msg->actual.velocities[i]; - //steer_joint_effort_NM[3] = msg->effort[i]; - } - } - - // Set measured Wheel Velocities and Angles to Controler Class (implements inverse kinematic) - ucar_ctrl_->SetActualWheelValues(drive_joint_vel_rads, steer_joint_vel_rads, - drive_joint_ang_rad, steer_joint_ang_rad); - - - // calculate odometry every time - UpdateOdometry(); - - } - - void timerCallbackCtrlStep(const ros::TimerEvent& e) { - CalcCtrlStep(); - } - - // other function declarations - // Initializes controller - bool InitCtrl(); - // perform one control step, calculate inverse kinematics and publish updated joint cmd's (if no EMStop occurred) - void CalcCtrlStep(); - // acquires the current undercarriage configuration from base_drive_chain - // calculates odometry from current measurement values and publishes it via an odometry topic and the tf broadcaster - void UpdateOdometry(); -}; - -//####################### -//#### main programm #### -int main(int argc, char** argv) -{ - // initialize ROS, spezify name of node - ros::init(argc, argv, "undercarriage_ctrl"); - - // construct nodeClass - NodeClass nodeClass; - - // automatically do initializing of controller, because it's not directly depending any hardware components - nodeClass.ucar_ctrl_->InitUndercarriageCtrl(); - nodeClass.is_initialized_bool_ = true; - - if( nodeClass.is_initialized_bool_ ) { - nodeClass.last_time_ = ros::Time::now(); - ROS_INFO("Undercarriage control successfully initialized."); - } else { - ROS_FATAL("Undercarriage control initialization failed!"); - throw std::runtime_error("Undercarriage control initialization failed, check ini-Files!"); - } - - /* - CALLBACKS being executed are: - - actual motor values -> calculating direct kinematics and doing odometry (topicCallbackJointControllerStates) - - timer callback -> calculate controller step at a rate of sample_time_ (timerCallbackCtrlStep) - - other topic callbacks (diagnostics, command, em_stop_state) - */ - ros::spin(); - - return 0; -} - -//################################## -//#### function implementations #### - -// perform one control step, calculate inverse kinematics and publish updated joint cmd's (if no EMStop occurred) -void NodeClass::CalcCtrlStep() -{ - double vx_cmd_ms, vy_cmd_ms, w_cmd_rads, dummy; - std::vector drive_jointvel_cmds_rads, steer_jointvel_cmds_rads, steer_jointang_cmds_rad; - control_msgs::JointTrajectoryControllerState joint_state_cmd; - int j, k; - iwatchdog_ += 1; - - // if controller is initialized and underlying hardware is operating normal - if (is_initialized_bool_) //&& (drive_chain_diagnostic_ != diagnostic_status_lookup_.OK)) - { - // as soon as (but only as soon as) platform drive chain is initialized start to send velocity commands - // Note: topicCallbackDiagnostic checks whether drives are operating nominal. - // -> if warning or errors are issued target velocity is set to zero - - // perform one control step, - // get the resulting cmd's for the wheel velocities and -angles from the controller class - // and output the achievable pltf velocity-cmds (if velocity limits where exceeded) - ucar_ctrl_->GetNewCtrlStateSteerDriveSetValues(drive_jointvel_cmds_rads, steer_jointvel_cmds_rads, steer_jointang_cmds_rad, vx_cmd_ms, vy_cmd_ms, w_cmd_rads, dummy); - // ToDo: adapt interface of controller class --> remove last values (not used anymore) - - // if drives not operating nominal -> force commands to zero - if(drive_chain_diagnostic_ != diagnostic_status_lookup_.OK) - { - steer_jointang_cmds_rad.assign(m_iNumJoints, 0.0); - steer_jointvel_cmds_rads.assign(m_iNumJoints, 0.0); - } - - // convert variables to SI-Units - vx_cmd_ms = vx_cmd_ms/1000.0; - vy_cmd_ms = vy_cmd_ms/1000.0; - - // compose jointcmds - // compose header - joint_state_cmd.header.stamp = ros::Time::now(); - //joint_state_cmd.header.frame_id = frame_id; //Where to get this id from? - // ToDo: configure over Config-File (number of motors) and Msg - // assign right size to JointState data containers - //joint_state_cmd.set_name_size(m_iNumMotors); - joint_state_cmd.desired.positions.resize(m_iNumJoints); - joint_state_cmd.desired.velocities.resize(m_iNumJoints); - //joint_state_cmd.effort.resize(m_iNumJoints); - joint_state_cmd.joint_names.push_back("fl_caster_r_wheel_joint"); - joint_state_cmd.joint_names.push_back("fl_caster_rotation_joint"); - joint_state_cmd.joint_names.push_back("bl_caster_r_wheel_joint"); - joint_state_cmd.joint_names.push_back("bl_caster_rotation_joint"); - joint_state_cmd.joint_names.push_back("br_caster_r_wheel_joint"); - joint_state_cmd.joint_names.push_back("br_caster_rotation_joint"); - joint_state_cmd.joint_names.push_back("fr_caster_r_wheel_joint"); - joint_state_cmd.joint_names.push_back("fr_caster_rotation_joint"); - joint_state_cmd.joint_names.resize(m_iNumJoints); - - // compose data body - j = 0; - k = 0; - for(int i = 0; i remove from interface - ucar_ctrl_->GetActualPltfVelocity(delta_x_rob_m, delta_y_rob_m, delta_theta_rob_rad, dummy1, - vel_x_rob_ms, vel_y_rob_ms, rot_rob_rads, dummy2); - - // convert variables to SI-Units - vel_x_rob_ms = vel_x_rob_ms/1000.0; - vel_y_rob_ms = vel_y_rob_ms/1000.0; - delta_x_rob_m = delta_x_rob_m/1000.0; - delta_y_rob_m = delta_y_rob_m/1000.0; - - ROS_DEBUG("Odmonetry delta is: x=%f, y=%f, th=%f", delta_x_rob_m, delta_y_rob_m, rot_rob_rads); - } - else - { - // otherwise set data (velocity and pose-delta) to zero - vel_x_rob_ms = 0.0; - vel_y_rob_ms = 0.0; - delta_x_rob_m = 0.0; - delta_y_rob_m = 0.0; - } - - // calc odometry (from startup) - // get time since last odometry-measurement - current_time = ros::Time::now(); - dt = current_time.toSec() - last_time_.toSec(); - last_time_ = current_time; - vel_rob_ms = sqrt(vel_x_rob_ms*vel_x_rob_ms + vel_y_rob_ms*vel_y_rob_ms); - - // calculation from ROS odom publisher tutorial http://www.ros.org/wiki/navigation/Tutorials/RobotSetup/Odom, using now midpoint integration - x_rob_m_ = x_rob_m_ + ((vel_x_rob_ms+vel_x_rob_last_)/2.0 * cos(theta_rob_rad_) - (vel_y_rob_ms+vel_y_rob_last_)/2.0 * sin(theta_rob_rad_)) * dt; - y_rob_m_ = y_rob_m_ + ((vel_x_rob_ms+vel_x_rob_last_)/2.0 * sin(theta_rob_rad_) + (vel_y_rob_ms+vel_y_rob_last_)/2.0 * cos(theta_rob_rad_)) * dt; - theta_rob_rad_ = theta_rob_rad_ + rot_rob_rads * dt; - //theta_rob_rad_ = theta_rob_rad_ + (rot_rob_rads+vel_theta_rob_last_)/2.0 * dt; - - vel_x_rob_last_ = vel_x_rob_ms; - vel_y_rob_last_ = vel_y_rob_ms; - vel_theta_rob_last_ = rot_rob_rads; - - - // format data for compatibility with tf-package and standard odometry msg - // generate quaternion for rotation - geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(theta_rob_rad_); - - if (broadcast_tf_ == true) - { - // compose and publish transform for tf package - geometry_msgs::TransformStamped odom_tf; - // compose header - odom_tf.header.stamp = joint_state_odom_stamp_; - odom_tf.header.frame_id = "odom_combined"; - odom_tf.child_frame_id = "base_footprint"; - // compose data container - odom_tf.transform.translation.x = x_rob_m_; - odom_tf.transform.translation.y = y_rob_m_; - odom_tf.transform.translation.z = 0.0; - odom_tf.transform.rotation = odom_quat; - - // publish the transform (for debugging, conflicts with robot-pose-ekf) - tf_broadcast_odometry_.sendTransform(odom_tf); - } - - // compose and publish odometry message as topic - nav_msgs::Odometry odom_top; - // compose header - odom_top.header.stamp = joint_state_odom_stamp_; - odom_top.header.frame_id = "odom_combined"; - odom_top.child_frame_id = "base_footprint"; - // compose pose of robot - odom_top.pose.pose.position.x = x_rob_m_; - odom_top.pose.pose.position.y = y_rob_m_; - odom_top.pose.pose.position.z = 0.0; - odom_top.pose.pose.orientation = odom_quat; - for(int i = 0; i < 6; i++) - odom_top.pose.covariance[i*6+i] = 0.1; - - // compose twist of robot - odom_top.twist.twist.linear.x = vel_x_rob_ms; - odom_top.twist.twist.linear.y = vel_y_rob_ms; - odom_top.twist.twist.linear.z = 0.0; - odom_top.twist.twist.angular.x = 0.0; - odom_top.twist.twist.angular.y = 0.0; - odom_top.twist.twist.angular.z = rot_rob_rads; - for(int i = 0; i < 6; i++) - odom_top.twist.covariance[6*i+i] = 0.1; - - // publish odometry msg - topic_pub_odometry_.publish(odom_top); -} - - - - From a4951822ee21f7bbea5778b093bb0af6ed47397c Mon Sep 17 00:00:00 2001 From: fmessmer Date: Wed, 20 Mar 2024 16:54:45 +0100 Subject: [PATCH 07/14] remove cob_base_drive_chain --- cob_base_drive_chain/CHANGELOG.rst | 301 ---- cob_base_drive_chain/CMakeLists.txt | 40 - .../cob_base_drive_chain/CanCtrlPltfCOb3.h | 435 ----- .../common/src/CanCtrlPltfCOb3.cpp | 1490 ----------------- cob_base_drive_chain/package.xml | 29 - .../ros/src/cob_base_drive_chain.cpp | 879 ---------- .../srv/ElmoRecorderConfig.srv | 9 - .../srv/ElmoRecorderReadout.srv | 24 - cob_driver/package.xml | 1 - 9 files changed, 3208 deletions(-) delete mode 100644 cob_base_drive_chain/CHANGELOG.rst delete mode 100644 cob_base_drive_chain/CMakeLists.txt delete mode 100644 cob_base_drive_chain/common/include/cob_base_drive_chain/CanCtrlPltfCOb3.h delete mode 100644 cob_base_drive_chain/common/src/CanCtrlPltfCOb3.cpp delete mode 100644 cob_base_drive_chain/package.xml delete mode 100644 cob_base_drive_chain/ros/src/cob_base_drive_chain.cpp delete mode 100644 cob_base_drive_chain/srv/ElmoRecorderConfig.srv delete mode 100644 cob_base_drive_chain/srv/ElmoRecorderReadout.srv diff --git a/cob_base_drive_chain/CHANGELOG.rst b/cob_base_drive_chain/CHANGELOG.rst deleted file mode 100644 index ebb653e8d..000000000 --- a/cob_base_drive_chain/CHANGELOG.rst +++ /dev/null @@ -1,301 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package cob_base_drive_chain -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.7.16 (2024-02-20) -------------------- - -0.7.15 (2023-11-06) -------------------- - -0.7.14 (2022-11-17) -------------------- - -0.7.13 (2022-07-29) -------------------- - -0.7.12 (2022-03-15) -------------------- - -0.7.11 (2022-01-12) -------------------- - -0.7.10 (2021-12-23) -------------------- - -0.7.9 (2021-11-26) ------------------- - -0.7.8 (2021-10-19) ------------------- - -0.7.7 (2021-08-02) ------------------- - -0.7.6 (2021-05-10) ------------------- - -0.7.5 (2021-04-06) ------------------- -* Merge pull request `#418 `_ from fmessmer/fix_catkin_lint - fix catkin_lint -* fix catkin_lint -* Contributors: Felix Messmer, fmessmer - -0.7.4 (2020-10-14) ------------------- -* Merge pull request `#417 `_ from fmessmer/test_noetic - test noetic -* Bump CMake version to avoid CMP0048 warning -* Contributors: Felix Messmer, fmessmer - -0.7.3 (2020-03-18) ------------------- - -0.7.2 (2020-03-18) ------------------- -* Merge pull request `#408 `_ from fmessmer/ci_updates - [travis] ci updates -* catkin_lint fixes -* Contributors: Felix Messmer, fmessmer - -0.7.1 (2019-11-07) ------------------- - -0.7.0 (2019-08-06) ------------------- -* Merge pull request `#396 `_ from HannesBachter/indigo_dev - 0.6.15 -* Contributors: Felix Messmer - -0.6.15 (2019-07-17) -------------------- - -0.6.14 (2019-06-07) -------------------- - -0.6.13 (2019-03-14) -------------------- - -0.6.12 (2018-07-21) -------------------- - -0.6.11 (2018-01-07) -------------------- -* Merge remote-tracking branch 'origin/indigo_release_candidate' into indigo_dev -* Merge pull request `#341 `_ from ipa-fxm/APACHE_license - use license apache 2.0 -* use license apache 2.0 -* Contributors: Felix Messmer, ipa-fxm, ipa-uhr-mk - -0.6.10 (2017-07-24) -------------------- - -0.6.9 (2017-07-18) ------------------- -* Fix/diagnostic rate (`#329 `_) - * Turned down global diagnostic rate to 1Hz - * Fixed comments - * Intermediate - * Fixed -* manually fix changelog -* Contributors: ipa-fxm, mig-em - -0.6.8 (2016-10-10) ------------------- -* introduced param to set homing velocity -* Contributors: Benjamin Maidel - -0.6.7 (2016-04-02) ------------------- - -0.6.6 (2016-04-01) ------------------- -* changed drive_chains diag name from //base_controller to /base_controller/base_drive_chain_node -* Contributors: bnm - -0.6.5 (2015-08-31) ------------------- - -0.6.4 (2015-08-25) ------------------- -* install tags for libraries -* do not install headers in executable-only packages -* explicit dependency to boost -* remove obsolete autogenerated mainpage.dox files -* remove trailing whitespaces -* migrate to package format 2 -* sort dependencies -* critically review dependencies -* Contributors: ipa-fxm - -0.6.3 (2015-06-17) ------------------- -* add_dependencies to generate_messages_cpp -* use new Trigger from std_srvs -* changed hardcoded namespace -* changed wrong ROS output -* Contributors: Thorsten Kannacher, ipa-fxm - -0.6.2 (2014-12-15) ------------------- -* remove joint_states publisher for simulation - using JointStateController anyway -* Contributors: ipa-fxm - -0.6.1 (2014-09-17) ------------------- - -0.6.0 (2014-09-09) ------------------- - -0.5.7 (2014-08-26) ------------------- -* Merge pull request `#163 `_ from ipa320/hydro_dev - updates from hydro_dev -* 0.5.6 -* update changelog -* adding timestamp to diagnostic message -* Cleaned up cob_driver with reduced deps to compile on indigo -* fix install tags -* remove deprecated launch files in cob_driver and add nodes to cob_robots -* Contributors: Alexander Bubeck, Florian Weisshardt, ipa-fxm - -0.5.6 (2014-08-26) ------------------- -* Merge pull request `#163 `_ from ipa320/hydro_dev - updates from hydro_dev -* adding timestamp to diagnostic message -* Cleaned up cob_driver with reduced deps to compile on indigo -* fix install tags -* remove deprecated launch files in cob_driver and add nodes to cob_robots -* Contributors: Alexander Bubeck, Florian Weisshardt, ipa-fxm - -0.5.3 (2014-03-31) ------------------- -* install tags -* Contributors: ipa-fxm - -0.5.2 (2014-03-20) ------------------- - -0.5.1 (2014-03-20) ------------------- -* some install tag updates -* merge with groovy_dev -* cherry-pick -* removed a lot of code related to packages not available in hydro anymore -* bugfix flexible odometry calculation based on number of wheels -* reverted changes -* new phidget driver -* Merge branch 'groovy_dev' of git://github.com/ipa320/cob_driver into groovy_dev -* fixed build errors for gcc version >= 4.7 -* fix compiler error for quantal -* Installation stuff -* cleaned up CMakeLists and added install directives -* further modifications for catkin, now everything is compiling and linking -* futher include and linkpath modifications -* compiling but still some linker errors -* Second catkinization push -* First catkinization, still need to update some CMakeLists.txt -* cleanup in base_drive_chain and undercarriage_ctrl -* cob_undercarriage_ctrl: cleaned and improved ucar_ctrl now working properly on real robot (including recover) -* cob_base_drive_chain: bugfixed for less than 4 wheels, doing initDrives() in sim-mode later: avoid node crash -* cob_base_drive_chain: cleaned unused code -* merge with ipa320 -* added joint name checking -* added global diagnostic messages to base_drive_chain -* cob_undercarriage_ctrl: odom in simulation looks great, in reality not -* cob_undercarriage: cleaned up, odom-improvements tested in simu with navigation -* merge -* add header time stamp -* remove compiler warnings -* fix base_drive chain -* moved services to base drive chain -* base_sim: in simulation, now redirecting base joint infos from /joint_states to base_controller -* worked on base_drive_chain for sim -* using arg sim in launch files for base_controller -* removed debug output -* worked on base controller for simulation -* recover and init services return true -* fix for simulation -* rearranging cob_camera_sensors launch files -* cob_base: communication between controller and driver now directly using joint_command and state topics with pr2::JointTrajectoryControllerState msgs -* modifications for icob and bugfix in base drive chain -* config for cob3-3 -* Adaptions in base_drive_chain and undercarriage_ctrl for global /joint_states -* Adapted base_drive_chain to communicate with controller using joint names and not only numbers anymore -* Merge branch 'master' of https://github.com/ipa-fmw/cob_driver into review-fmw -* additional undercarriage ctrl in simulation -* undercarriage_ctrl in simulation -* added missing file -* moved GetJointState message ro base_drive_chain -* changed trigger service -* cob_base_drive_chain DEBUG. GetJointStates Service replaced through cyclical publishing topic in cob_base_drive_chain -* cleanup in cob_driver -* Moved hard-coded lines for head_axis_homing from CanDriveHarmonica.cpp into ElmoCtrl.cpp. Removed debugger in base_drive_chain.launch and undercarriage_ctrl.launch -* added joint_state_combined to cob_bringup, small device modifications on cob3-1 -* Starting base_drive_chain and undercarriage_ctrl with GDB-debugger -* Added cob_bringup _nt for ICM ctrl and added shutdown of drives in base_drive_chain to securily stop drives on Ctrl-C -* restructured base_controller -* base_drive_chain now can be reverted after EMStop -* Now also with ElmoRecorderReadout feature low CPU costs in base_drive_chain -* base_drive_chain: added main loop with evalCanBuffer to enable ElmoRecorderReadout. NEW: evalCanBuffer is only executed, when and until a readout is in process -* debugged base_drive_chain: removed (empty) while-loop in main of node -> no more comp. power spoiled -* system cleaned - missing launch files added -* Modified launch files of cob_base_drive_chain, cob_relayboard, cob_undercaariage_ctrl and cob_teleop_ucar and made them hierarchic -* merged with cpc-pk: added ctrl for tricycle-kinematic; specification of limit in CanDriveHarmonica can now be specified via Inifile; base_drive_chain can be operated on variable numbers of motors (lesser or equal to eight); variable setting of path to inifile for UndercarriageCtrlGeom; debugged relaysboard - reads Bus now nonblocking -* Direct Kinematics, publish effort option in base_drive_chain -* Made interface of undercarriage_ctrl_geom common for cob3 and cob3_5, adapted some launch files -* Added HomingDigIn in CanCtrl.ini to specify which digital input gives homing signal. It's read out and passed to the CanNode via DriveParam.h -* Successfully adapted multi-motor support on the level of base_drive_chain -* Added NumMotors in Platform.ini, read this out in constructors of base_drive_chain and CanCtrlPltfCob3 -* Merged cob_base_drive_chain from cpc-ck for support of a variable number of motors -* Merged in CanCtrlPltfCob3_5 to according CanCtrlPltfCob3. The new version by cpc-ck allows a variable number of motors. -* temp commit of only CanCtrlPltfCob3.5 merged -* update documentation and deleted tf broadcaster -* Merge branch 'cpc-pk' of git@github.com:ipa-cpc/care-o-bot into review-cpc-pk -* Renamed and worked on cob_drive_identification, moved Elmo Recorder services to cob_srvs -* Started generating a cob_drive_identification package -* Corrected one mis-merge, successfully built merge. -* merged in master and manually solved conflicts in base_drive_chain.cpp -* Replaced some spaces with tabs -* cleanup in stacks -* debugging odometry calc -* merging with cpc -* Merge branch 'cpc-pk' of git@github.com:ipa-cpc/care-o-bot into cpc-pk -* fixed loop error in base_drive_chain -* Merge branch 'review' into cpc-pk -* xml description updated -* Cahnged cob_base_drive_chain -> watchdogs activated again, evalCanBuffer at rate of 50Hz, services continous, a lot Doxygen documentation in all ElmoRecorder related files -* Deployment of undercarriage controller debugged and finished: launch-script cob_ucar_joy starts up relayboard, base_drive_chain and controller; also remaps topics and services in correct namespaces. Debugging of controller itself is work in progress: simplified and removed old stuff - code compiles - controller runs but appaerently has some bugs -> may not yet be used -* Merge branch 'review-cpc' -* services added -* Interface polishing, added srvs for base_drive_chain -* Merge branch 'review' into cpc-pk -* Working ElmoRecorder Eadout, multiple motors, different objects with StatusRegister check -* Improved interface for Readout control, added readoutRecorderTry using StatusRegister -* Successfully uploaded Recorder Data, Watchdogs deactivated -* Successfully uploaded Recorder Data, Watchdogs deactivated -* Trying to get Readout running. Working system state (with debug outputs) -* updated simulation files -* debugging undercarriage drivers (base_drive_chain + relayboard + ucar_ctrl) - work in progress -* cleanup in cob_driver -* After merging in review branch -* Added EvalCanBuffer to main loop of base_drive_chain. -* Added some testing ElmoRecorder Service in base_drive_chain -* Introduced a statusFlag in segData instead of FinishedTransmission and locked. -* Frontend in base_drive_chain added, filenames can be passed now -* ElmoRecorder: Data readout and processing -* debugged ucar controller and base drive chain node - still not running -* added windows.h; some modifications in ElmoCtrl -> not yet working -* added classes to implement ESD can-itf; incorporated ESD interface as an option in cob_base_drive_chain-node via CanCtrlPltfCOb3; added windows.h to cob_utilities package -* Updated Can Classes to new file structure; removed some leftovers; corrected comments at the beginning considering association to stacks and packages; moved Mutex.h to Utilities; - Debugged compiler error in cob_base_drive_chain -* Implemented base controller - cob_undercarriage_ctrl - based on principle of rigid body motion; controller is not yet tested on hardware; moreover, not yet used: parameterserver for initializing controller, urdf-file to associate joints; also removed some bugs from base_drive_chain -* after merging current review -* adapt launch file to new packages names -* moved files -* renamed to cob_ -* merged master -* renamed packages to cob_ convention -* renamed packages to cob_ -* Contributors: Alexander Bubeck, Christian, Christian Connette, Richard Bormann, abubeck, cob, cpc, cpc-pk, ipa-bnm, ipa-cpc, ipa-fmw, ipa-fxm, ipa-srd diff --git a/cob_base_drive_chain/CMakeLists.txt b/cob_base_drive_chain/CMakeLists.txt deleted file mode 100644 index d25449f45..000000000 --- a/cob_base_drive_chain/CMakeLists.txt +++ /dev/null @@ -1,40 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(cob_base_drive_chain) - -find_package(catkin REQUIRED COMPONENTS cob_canopen_motor cob_generic_can cob_utilities control_msgs diagnostic_msgs message_generation roscpp sensor_msgs std_msgs std_srvs) - -### Message Generatioin ### -add_service_files( - FILES - ElmoRecorderConfig.srv - ElmoRecorderReadout.srv -) - -generate_messages( - DEPENDENCIES -) - -catkin_package( - CATKIN_DEPENDS control_msgs diagnostic_msgs message_runtime roscpp sensor_msgs std_msgs std_srvs -) - -### BUILD ### -include_directories(common/include ${catkin_INCLUDE_DIRS}) - -add_library(${PROJECT_NAME} common/src/CanCtrlPltfCOb3.cpp) - -add_executable(${PROJECT_NAME}_node ros/src/${PROJECT_NAME}.cpp) -add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(${PROJECT_NAME}_node ${PROJECT_NAME} ${catkin_LIBRARIES}) - -add_executable(${PROJECT_NAME}_sim_node ros/src/${PROJECT_NAME}.cpp) -set_target_properties(${PROJECT_NAME}_sim_node PROPERTIES COMPILE_FLAGS "-D__SIM__") -add_dependencies(${PROJECT_NAME}_sim_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(${PROJECT_NAME}_sim_node ${PROJECT_NAME} ${catkin_LIBRARIES}) - -### INSTALL ### -install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node ${PROJECT_NAME}_sim_node - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) diff --git a/cob_base_drive_chain/common/include/cob_base_drive_chain/CanCtrlPltfCOb3.h b/cob_base_drive_chain/common/include/cob_base_drive_chain/CanCtrlPltfCOb3.h deleted file mode 100644 index 839c4530d..000000000 --- a/cob_base_drive_chain/common/include/cob_base_drive_chain/CanCtrlPltfCOb3.h +++ /dev/null @@ -1,435 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -#ifndef CANCTRLPLTFCOB3_INCLUDEDEF_H -#define CANCTRLPLTFCOB3_INCLUDEDEF_H - -//----------------------------------------------- - -// general includes - -// Headers provided by other cob-packages -#include -#include -#include - -// Headers provided by cob-packages which should be avoided/removed -#include -#include - -// remove (not supported) -//#include "stdafx.h" - - -//----------------------------------------------- - -/** - * Represents all CAN components on an arbitrary canbus. - */ -class CanCtrlPltfCOb3 // : public CanCtrlPltfItf -{ -public: - - //--------------------------------- Basic procedures - - /** - * Default constructor. - */ - CanCtrlPltfCOb3(std::string iniDirectory); - - /** - * Default destructor. - */ - ~CanCtrlPltfCOb3(); - - - //--------------------------------- Hardware Specification - - /** - * Specify Cannodes (Identifiers to send velocities to one specific motor) - * This has to be adapted to the hardware-setup! - */ - enum MotorCANNode - { - CANNODE_WHEEL1DRIVEMOTOR, - CANNODE_WHEEL1STEERMOTOR, - CANNODE_WHEEL2DRIVEMOTOR, - CANNODE_WHEEL2STEERMOTOR, - CANNODE_WHEEL3DRIVEMOTOR, - CANNODE_WHEEL3STEERMOTOR, - CANNODE_WHEEL4DRIVEMOTOR, - CANNODE_WHEEL4STEERMOTOR - }; - - - //--------------------------------- Commands for all nodes on the bus - - /** - * Initializes all CAN nodes of the platfrom and performs homing procedure. - * !! The homing routine is hardware-dependent (steering and driving is coupled) !! - * !! If you use this code on other hardware -> make sure te remove or adapt homing sequence !! - */ - bool initPltf(); - - /** - * Reinitializes the nodes on the bus. - * The function might be neccessary after an emergency stop or an hardware failure to reinit drives. - */ - bool resetPltf(); - - /** - * Signs an error of the platform. - * @return true if there is an error. - */ - bool isPltfError(); - - /** - * Shutdown of the platform. - * Disables motors, enables brake and disconnects. - */ - bool shutdownPltf(); - - /** - * Starts the watchdog of the CAN components. - */ - bool startWatchdog(bool bStarted); - - /** - * Triggers evaluation of the can-buffer. - */ - int evalCanBuffer(); - - - //--------------------------------- Commands specific for motor controller nodes - - /** - * Sends veolocities to the can node. - * Status is requested, too. - * @param iCanIdent choose a can node - * @param dVelGearRadS joint-velocity in radian per second - */ - int setVelGearRadS(int iCanIdent, double dVelGearRadS); - - /** - * Sends torques to the can node. - * Status is requested, too. - * @param iCanIdent choose a can node - * @param dTorqueNM motor-torque in Newtonmeter - */ - void setMotorTorque(int iCanIdent, double dTorqueNm); - - /** - * Requests the status of the drive. - * Status-Msg includes whether motor is in error-state - * (with source, e.g. undercurrent, overheated) - * or operational (enabled/disabled, at its limits, ...) - */ - void requestDriveStatus(); - - /** - * Requests position and velocity of the drive. - * (This is not implemented for CanDriveHarmonica. - * sending an Velocity command to an ELMO Harmonica-Ctrl - * triggers a aswer transmitting the current velocity) - * @param iCanIdent choose a can node - */ - int requestMotPosVel(int iCanIdent); - - /** - * Requests motor-torque (in fact active current) of the drive. - * @param iCanIdent choose a can node - */ - void requestMotorTorque(); - - /** - * Gets the position and velocity. - * @param iCanIdent choose a can node - * @param pdAngleGearRad joint-position in radian - * @param pdVelGearRadS joint-velocity in radian per second - */ - int getGearPosVelRadS(int iCanIdent, double* pdAngleGearRad, double* pdVelGearRadS); - - /** - * Gets the delta joint-angle since the last call and the velocity. - * @param iCanIdent choose a can node - * @param pdDeltaAngleGearRad delta joint-position since the last call in radian - * @param pdVelGearRadS joint-velocity in radian per second - */ - int getGearDeltaPosVelRadS(int iCanIdent, double* pdDeltaAngleGearRad, double* pdVelGearRadS); - - /** - * Gets the status and temperature in degree celcius. - * (Not implemented for CanDriveHarmonica) - * @param iCanIdent choose a CANNode enumatraion - */ - void getStatus(int iCanIdent, int* piStatus, int* piTempCel); - - /** - * Gets the motor torque (calculated from motor active current). - * @param iCanIdent choose a can node - * @param pdTorqueNm motor-torque in Newtonmeter - */ - void getMotorTorque(int iCanIdent, double* pdTorqueNm); - - - - //--------------------------------- Commands specific for a certain motor controller - // have to be implemented here, to keep the CanDriveItf generic - - /** - * Provides several functions for drive information recording purposes using the built in ElmoRecorder, which allows to record drive information at a high frequency. - * @param iFlag To keep the interface slight, use iParam to command the recorder: - * 0: Configure the Recorder to record the sources Main Speed(1), Main position(2), Active current(10), Speed command(16). With iParam = iRecordingGap you specify every which time quantum (4*90usec) a new data point (of 1024 points in total) is recorded; - * 1: Query Upload of recorded source (1=Main Speed, 2=Main position, 10=Active Current, 16=Speed command) with iParam and log data to file sParam = file prefix. Filename is extended with _MotorNumber_RecordedSource.log - * 99: Abort and clear current SDO readout process - * 100: Request status of readout. Gives back 0 if all transmissions have finished and no CAN polling is needed anymore. - * @return -1: Unknown flag set; 0: Success; 1: Recorder hasn't been configured yet; 2: data collection still in progress - * - */ - int ElmoRecordings(int iFlag, int iParam, std::string sString); - - //--------------------------------- Commands for other nodes - - - - -protected: - - //--------------------------------- internal functions - - /** - * Reads configuration of can node and components from Inifile - * (should be adapted to use ROS-Parameter file) - */ - std::string sIniDirectory; - std::string sComposed; - void readConfiguration(); - - /** - * Starts up can node - */ - void sendNetStartCanOpen(); - - - //--------------------------------- Types - - /** - * Parameters of the class CanCtrlPltfCOb3. - */ - struct ParamType - { - // Platform config - - int iHasWheel1DriveMotor; - int iHasWheel1SteerMotor; - int iHasWheel2DriveMotor; - int iHasWheel2SteerMotor; - int iHasWheel3DriveMotor; - int iHasWheel3SteerMotor; - int iHasWheel4DriveMotor; - int iHasWheel4SteerMotor; - - double dWheel1SteerDriveCoupling; - double dWheel2SteerDriveCoupling; - double dWheel3SteerDriveCoupling; - double dWheel4SteerDriveCoupling; - - double dHomeVeloRadS; - - int iRadiusWheelMM; - int iDistSteerAxisToDriveWheelMM; - - int iHasRelayBoard; - int iHasIOBoard; - int iHasUSBoard; - int iHasGyroBoard; - int iHasRadarBoard; - - double dCanTimeout; - }; - - /** - * Parameters charaterising combination of gears and drives - */ - struct GearMotorParamType - { - int iEncIncrPerRevMot; - double dVelMeasFrqHz; - double dGearRatio; - double dBeltRatio; - int iSign; - double dVelMaxEncIncrS; - double dAccIncrS2; - double dDecIncrS2; - double dScaleToMM; - int iEncOffsetIncr; - bool bIsSteer; - double dCurrentToTorque; - double dCurrMax; - int iHomingDigIn; - }; - - /** - * CAN IDs for Neobotix boards. (Default Values) - */ - struct CanNeoIDType - { - int IOBoard_rx_ID; - int IOBoard_tx_ID; - - int DriveNeo_W1Drive_rx_ID; - int DriveNeo_W1Drive_tx_ID; - int DriveNeo_W1Steer_rx_ID; - int DriveNeo_W1Steer_tx_ID; - - int DriveNeo_W2Drive_rx_ID; - int DriveNeo_W2Drive_tx_ID; - int DriveNeo_W2Steer_rx_ID; - int DriveNeo_W2Steer_tx_ID; - - int DriveNeo_W3Drive_rx_ID; - int DriveNeo_W3Drive_tx_ID; - int DriveNeo_W3Steer_rx_ID; - int DriveNeo_W3Steer_tx_ID; - - int DriveNeo_W4Drive_rx_ID; - int DriveNeo_W4Drive_tx_ID; - int DriveNeo_W4Steer_rx_ID; - int DriveNeo_W4Steer_tx_ID; - - int USBoard_rx_ID; - int USBoard_tx_ID; - int GyroBoard_rx_ID; - int GyroBoard_tx_ID; - int RadarBoard_rx_ID; - int RadarBoard_tx_ID; - }; - - /** - * CAN IDs for motor drive Harmonica. - * (actually this should be read from inifile) - */ - struct CanOpenIDType - { - // Wheel 1 - // can adresse motor 1 - int TxPDO1_W1Drive; - int TxPDO2_W1Drive; - int RxPDO2_W1Drive; - int TxSDO_W1Drive; - int RxSDO_W1Drive; - // can adresse motor 2 - int TxPDO1_W1Steer; - int TxPDO2_W1Steer; - int RxPDO2_W1Steer; - int TxSDO_W1Steer; - int RxSDO_W1Steer; - - // Wheel 2 - // can adresse motor 7 - int TxPDO1_W2Drive; - int TxPDO2_W2Drive; - int RxPDO2_W2Drive; - int TxSDO_W2Drive; - int RxSDO_W2Drive; - // can adresse motor 8 - int TxPDO1_W2Steer; - int TxPDO2_W2Steer; - int RxPDO2_W2Steer; - int TxSDO_W2Steer; - int RxSDO_W2Steer; - - // Wheel 3 - // can adresse motor 5 - int TxPDO1_W3Drive; - int TxPDO2_W3Drive; - int RxPDO2_W3Drive; - int TxSDO_W3Drive; - int RxSDO_W3Drive; - // can adresse motor 6 - int TxPDO1_W3Steer; - int TxPDO2_W3Steer; - int RxPDO2_W3Steer; - int TxSDO_W3Steer; - int RxSDO_W3Steer; - - // Wheel 4 - // can adresse motor 3 - int TxPDO1_W4Drive; - int TxPDO2_W4Drive; - int RxPDO2_W4Drive; - int TxSDO_W4Drive; - int RxSDO_W4Drive; - // can adresse motor 4 - int TxPDO1_W4Steer; - int TxPDO2_W4Steer; - int RxPDO2_W4Steer; - int TxSDO_W4Steer; - int RxSDO_W4Steer; - }; - - //--------------------------------- Parameter - ParamType m_Param; -// CanNeoIDType m_CanNeoIDParam; - CanOpenIDType m_CanOpenIDParam; - - // Prms for all Motor/Gear combos - GearMotorParamType m_GearMotDrive1; - GearMotorParamType m_GearMotDrive2; - GearMotorParamType m_GearMotDrive3; - GearMotorParamType m_GearMotDrive4; - GearMotorParamType m_GearMotSteer1; - GearMotorParamType m_GearMotSteer2; - GearMotorParamType m_GearMotSteer3; - GearMotorParamType m_GearMotSteer4; - - //--------------------------------- Variables - CanMsg m_CanMsgRec; - Mutex m_Mutex; - bool m_bWatchdogErr; - - //--------------------------------- Components - // Can-Interface - CanItf* m_pCanCtrl; - IniFile m_IniFile; - - int m_iNumMotors; - int m_iNumDrives; - - // Motor-Controllers -/* CanDriveItf* m_pW1DriveMotor; - CanDriveItf* m_pW1SteerMotor; - CanDriveItf* m_pW2DriveMotor; - CanDriveItf* m_pW2SteerMotor; - CanDriveItf* m_pW3DriveMotor; - CanDriveItf* m_pW3SteerMotor; - CanDriveItf* m_pW4DriveMotor; - CanDriveItf* m_pW4SteerMotor;*/ - // pointer to each motors Can-Itf - std::vector m_vpMotor; - // vector with enums (specifying hardware-structure) -> simplifies cmd-check - // this has to be adapted in c++ file to your hardware - std::vector m_viMotorID; - - // other - - -}; - - -//----------------------------------------------- -#endif diff --git a/cob_base_drive_chain/common/src/CanCtrlPltfCOb3.cpp b/cob_base_drive_chain/common/src/CanCtrlPltfCOb3.cpp deleted file mode 100644 index 111e572ed..000000000 --- a/cob_base_drive_chain/common/src/CanCtrlPltfCOb3.cpp +++ /dev/null @@ -1,1490 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -// general includes -#include -#include - -// Headers provided by other cob-packages -#include -#include -#include -#include - -#include - -//----------------------------------------------- - -CanCtrlPltfCOb3::CanCtrlPltfCOb3(std::string iniDirectory) -{ - sIniDirectory = iniDirectory; - IniFile iniFile; - iniFile.SetFileName(sIniDirectory + "Platform.ini", "PltfHardwareCoB3.h"); - - // get max Joint-Velocities (in rad/s) for Steer- and Drive-Joint - iniFile.GetKeyInt("Config", "NumberOfMotors", &m_iNumMotors, true); - iniFile.GetKeyInt("Config", "NumberOfWheels", &m_iNumDrives, true); - - if(m_iNumMotors < 2 || m_iNumMotors > 8) { - m_iNumMotors = 8; - m_iNumDrives = 4; - } - - // ------------- first of all set used CanItf - m_pCanCtrl = NULL; - - // ------------- init hardware-specific vectors and set default values - m_vpMotor.resize(m_iNumMotors); - - for(int i=0; i= 1) - m_viMotorID[0] = CANNODE_WHEEL1DRIVEMOTOR; - if(m_iNumMotors >= 2) - m_viMotorID[1] = CANNODE_WHEEL1STEERMOTOR; - if(m_iNumMotors >= 3) - m_viMotorID[2] = CANNODE_WHEEL2DRIVEMOTOR; - if(m_iNumMotors >= 4) - m_viMotorID[3] = CANNODE_WHEEL2STEERMOTOR; - if(m_iNumMotors >= 5) - m_viMotorID[4] = CANNODE_WHEEL3DRIVEMOTOR; - if(m_iNumMotors >= 6) - m_viMotorID[5] = CANNODE_WHEEL3STEERMOTOR; - if(m_iNumMotors >= 7) - m_viMotorID[6] = CANNODE_WHEEL4DRIVEMOTOR; - if(m_iNumMotors == 8) - m_viMotorID[7] = CANNODE_WHEEL4STEERMOTOR; - - // ------------- parameters - m_Param.dCanTimeout = 7; - - if(m_iNumMotors >= 1) - m_Param.iHasWheel1DriveMotor = 0; - if(m_iNumMotors >= 2) - m_Param.iHasWheel1SteerMotor = 0; - if(m_iNumMotors >= 3) - m_Param.iHasWheel2DriveMotor = 0; - if(m_iNumMotors >= 4) - m_Param.iHasWheel2SteerMotor = 0; - if(m_iNumMotors >= 5) - m_Param.iHasWheel3DriveMotor = 0; - if(m_iNumMotors >= 6) - m_Param.iHasWheel3SteerMotor = 0; - if(m_iNumMotors >= 7) - m_Param.iHasWheel4DriveMotor = 0; - if(m_iNumMotors == 8) - m_Param.iHasWheel4SteerMotor = 0; - - m_Param.iHasRelayBoard = 0; - m_Param.iHasIOBoard = 0; - m_Param.iHasUSBoard = 0; - m_Param.iHasGyroBoard = 0; - m_Param.iHasRadarBoard = 0; - - m_bWatchdogErr = false; - - // ------------ CanIds - - // ------------ For CanOpen (Harmonica) - // Wheel 1 - // can adresse motor 1 - if(m_iNumMotors >= 1) - { - m_CanOpenIDParam.TxPDO1_W1Drive = 0x182; - m_CanOpenIDParam.TxPDO2_W1Drive = 0x282; - m_CanOpenIDParam.RxPDO2_W1Drive = 0x302; - m_CanOpenIDParam.TxSDO_W1Drive = 0x582; - m_CanOpenIDParam.RxSDO_W1Drive = 0x602; - } - // can adresse motor 2 - if(m_iNumMotors >= 2) - { - m_CanOpenIDParam.TxPDO1_W1Steer = 0x181; - m_CanOpenIDParam.TxPDO2_W1Steer = 0x281; - m_CanOpenIDParam.RxPDO2_W1Steer = 0x301; - m_CanOpenIDParam.TxSDO_W1Steer = 0x581; - m_CanOpenIDParam.RxSDO_W1Steer = 0x601; - } - // Wheel 2 - // can adresse motor 7 - if(m_iNumMotors >= 3) - { - m_CanOpenIDParam.TxPDO1_W2Drive = 0x184; - m_CanOpenIDParam.TxPDO2_W2Drive = 0x284; - m_CanOpenIDParam.RxPDO2_W2Drive = 0x304; - m_CanOpenIDParam.TxSDO_W2Drive = 0x584; - m_CanOpenIDParam.RxSDO_W2Drive = 0x604; - } - // can adresse motor 8 - if(m_iNumMotors >= 4) - { - m_CanOpenIDParam.TxPDO1_W2Steer = 0x183; - m_CanOpenIDParam.TxPDO2_W2Steer = 0x283; - m_CanOpenIDParam.RxPDO2_W2Steer = 0x303; - m_CanOpenIDParam.TxSDO_W2Steer = 0x583; - m_CanOpenIDParam.RxSDO_W2Steer = 0x603; - } - // Wheel 3 - // can adresse motor 5 - if(m_iNumMotors >= 5) - { - m_CanOpenIDParam.TxPDO1_W3Drive = 0x188; - m_CanOpenIDParam.TxPDO2_W3Drive = 0x288; - m_CanOpenIDParam.RxPDO2_W3Drive = 0x308; - m_CanOpenIDParam.TxSDO_W3Drive = 0x588; - m_CanOpenIDParam.RxSDO_W3Drive = 0x608; - } - // can adresse motor 6 - if(m_iNumMotors >= 6) - { - m_CanOpenIDParam.TxPDO1_W3Steer = 0x187; - m_CanOpenIDParam.TxPDO2_W3Steer = 0x287; - m_CanOpenIDParam.RxPDO2_W3Steer = 0x307; - m_CanOpenIDParam.TxSDO_W3Steer = 0x587; - m_CanOpenIDParam.RxSDO_W3Steer = 0x607; - } - // Wheel 4 - // can adresse motor 3 - if(m_iNumMotors >= 7) - { - m_CanOpenIDParam.TxPDO1_W4Drive = 0x186; - m_CanOpenIDParam.TxPDO2_W4Drive = 0x286; - m_CanOpenIDParam.RxPDO2_W4Drive = 0x306; - m_CanOpenIDParam.TxSDO_W4Drive = 0x586; - m_CanOpenIDParam.RxSDO_W4Drive = 0x606; - } - // can adresse motor 4 - if(m_iNumMotors == 8) - { - m_CanOpenIDParam.TxPDO1_W4Steer = 0x185; - m_CanOpenIDParam.TxPDO2_W4Steer = 0x285; - m_CanOpenIDParam.RxPDO2_W4Steer = 0x305; - m_CanOpenIDParam.TxSDO_W4Steer = 0x585; - m_CanOpenIDParam.RxSDO_W4Steer = 0x605; - } -} - -//----------------------------------------------- -CanCtrlPltfCOb3::~CanCtrlPltfCOb3() -{ - - if (m_pCanCtrl != NULL) - { - delete m_pCanCtrl; - } - - for(unsigned int i = 0; i < m_vpMotor.size(); i++) - { - if (m_vpMotor[i] != NULL) - { - delete m_vpMotor[i]; - } - } - -} - -//----------------------------------------------- -void CanCtrlPltfCOb3::readConfiguration() -{ - - int iTypeCan = 0; - int iMaxMessages = 0; - - DriveParam DriveParamW1DriveMotor; - DriveParam DriveParamW1SteerMotor; - DriveParam DriveParamW2DriveMotor; - DriveParam DriveParamW2SteerMotor; - DriveParam DriveParamW3DriveMotor; - DriveParam DriveParamW3SteerMotor; - DriveParam DriveParamW4DriveMotor; - DriveParam DriveParamW4SteerMotor; - - std::string strTypeDrive; - std::string strTypeSteer; - - double dScaleToMM; - - // read Platform.ini (Coupling of Drive/Steer for Homing) - m_IniFile.SetFileName(sIniDirectory + "Platform.ini", "CanCtrlPltfCOb3.cpp"); - - m_IniFile.GetKeyInt("Geom", "RadiusWheel", &m_Param.iRadiusWheelMM, true); - m_IniFile.GetKeyInt("Geom", "DistSteerAxisToDriveWheelCenter", &m_Param.iDistSteerAxisToDriveWheelMM, true); - - m_IniFile.GetKeyDouble("DrivePrms","HomingVelocityRadS", &m_Param.dHomeVeloRadS, true); - - if(m_iNumDrives >= 1) - m_IniFile.GetKeyDouble("DrivePrms", "Wheel1SteerDriveCoupling", &m_Param.dWheel1SteerDriveCoupling, true); - if(m_iNumDrives >= 2) - m_IniFile.GetKeyDouble("DrivePrms", "Wheel2SteerDriveCoupling", &m_Param.dWheel2SteerDriveCoupling, true); - if(m_iNumDrives >= 3) - m_IniFile.GetKeyDouble("DrivePrms", "Wheel3SteerDriveCoupling", &m_Param.dWheel3SteerDriveCoupling, true); - if(m_iNumDrives == 4) - m_IniFile.GetKeyDouble("DrivePrms", "Wheel4SteerDriveCoupling", &m_Param.dWheel4SteerDriveCoupling, true); - - - // read CanCtrl.ini - m_IniFile.SetFileName(sIniDirectory + "CanCtrl.ini", "CanCtrlPltfCOb3.cpp"); - - std::cout << "Can configuration of the platform:" << std::endl; - - // read Configuration of the Can-Network (CanCtrl.ini) - m_IniFile.GetKeyInt("TypeCan", "Can", &iTypeCan, true); - if (iTypeCan == 0) - { - sComposed = sIniDirectory; - sComposed += "CanCtrl.ini"; - m_pCanCtrl = new CanPeakSys(sComposed.c_str()); - std::cout << "Uses CAN-Peak-Systems dongle" << std::endl; - } - else if (iTypeCan == 1) - { - sComposed = sIniDirectory; - sComposed += "CanCtrl.ini"; - m_pCanCtrl = new CANPeakSysUSB(sComposed.c_str()); - std::cout << "Uses CAN-Peak-USB" << std::endl; - } - else if (iTypeCan == 2) - { - sComposed = sIniDirectory; - sComposed += "CanCtrl.ini"; - m_pCanCtrl = new CanESD(sComposed.c_str(), false); - std::cout << "Uses CAN-ESD-card" << std::endl; - } - - // CanOpenId's ----- Default values (DESIRE) - // Wheel 1 - // DriveMotor - if(m_iNumDrives >= 1) - { - m_IniFile.GetKeyInt("CanOpenIDs", "TxPDO1_W1Drive", &m_CanOpenIDParam.TxPDO1_W1Drive, true); - m_IniFile.GetKeyInt("CanOpenIDs", "TxPDO2_W1Drive", &m_CanOpenIDParam.TxPDO2_W1Drive, true); - m_IniFile.GetKeyInt("CanOpenIDs", "RxPDO2_W1Drive", &m_CanOpenIDParam.RxPDO2_W1Drive, true); - m_IniFile.GetKeyInt("CanOpenIDs", "TxSDO_W1Drive", &m_CanOpenIDParam.TxSDO_W1Drive, true); - m_IniFile.GetKeyInt("CanOpenIDs", "RxSDO_W1Drive", &m_CanOpenIDParam.RxSDO_W1Drive, true); - } - // SteerMotor - if(m_iNumDrives >= 1) - { - m_IniFile.GetKeyInt("CanOpenIDs", "TxPDO1_W1Steer", &m_CanOpenIDParam.TxPDO1_W1Steer, true); - m_IniFile.GetKeyInt("CanOpenIDs", "TxPDO2_W1Steer", &m_CanOpenIDParam.TxPDO2_W1Steer, true); - m_IniFile.GetKeyInt("CanOpenIDs", "RxPDO2_W1Steer", &m_CanOpenIDParam.RxPDO2_W1Steer, true); - m_IniFile.GetKeyInt("CanOpenIDs", "TxSDO_W1Steer", &m_CanOpenIDParam.TxSDO_W1Steer, true); - m_IniFile.GetKeyInt("CanOpenIDs", "RxSDO_W1Steer", &m_CanOpenIDParam.RxSDO_W1Steer, true); - } - - // Wheel 2 - // DriveMotor - if(m_iNumDrives >= 2) - { - m_IniFile.GetKeyInt("CanOpenIDs", "TxPDO1_W2Drive", &m_CanOpenIDParam.TxPDO1_W2Drive, true); - m_IniFile.GetKeyInt("CanOpenIDs", "TxPDO2_W2Drive", &m_CanOpenIDParam.TxPDO2_W2Drive, true); - m_IniFile.GetKeyInt("CanOpenIDs", "RxPDO2_W2Drive", &m_CanOpenIDParam.RxPDO2_W2Drive, true); - m_IniFile.GetKeyInt("CanOpenIDs", "TxSDO_W2Drive", &m_CanOpenIDParam.TxSDO_W2Drive, true); - m_IniFile.GetKeyInt("CanOpenIDs", "RxSDO_W2Drive", &m_CanOpenIDParam.RxSDO_W2Drive, true); - } - // SteerMotor - if(m_iNumDrives >= 2) - { - m_IniFile.GetKeyInt("CanOpenIDs", "TxPDO1_W2Steer", &m_CanOpenIDParam.TxPDO1_W2Steer, true); - m_IniFile.GetKeyInt("CanOpenIDs", "TxPDO2_W2Steer", &m_CanOpenIDParam.TxPDO2_W2Steer, true); - m_IniFile.GetKeyInt("CanOpenIDs", "RxPDO2_W2Steer", &m_CanOpenIDParam.RxPDO2_W2Steer, true); - m_IniFile.GetKeyInt("CanOpenIDs", "TxSDO_W2Steer", &m_CanOpenIDParam.TxSDO_W2Steer, true); - m_IniFile.GetKeyInt("CanOpenIDs", "RxSDO_W2Steer", &m_CanOpenIDParam.RxSDO_W2Steer, true); - } - - // Wheel 3 - // DriveMotor - if(m_iNumDrives >= 3) - { - m_IniFile.GetKeyInt("CanOpenIDs", "TxPDO1_W3Drive", &m_CanOpenIDParam.TxPDO1_W3Drive, true); - m_IniFile.GetKeyInt("CanOpenIDs", "TxPDO2_W3Drive", &m_CanOpenIDParam.TxPDO2_W3Drive, true); - m_IniFile.GetKeyInt("CanOpenIDs", "RxPDO2_W3Drive", &m_CanOpenIDParam.RxPDO2_W3Drive, true); - m_IniFile.GetKeyInt("CanOpenIDs", "TxSDO_W3Drive", &m_CanOpenIDParam.TxSDO_W3Drive, true); - m_IniFile.GetKeyInt("CanOpenIDs", "RxSDO_W3Drive", &m_CanOpenIDParam.RxSDO_W3Drive, true); - } - // SteerMotor - if(m_iNumDrives >= 3) - { - m_IniFile.GetKeyInt("CanOpenIDs", "TxPDO1_W3Steer", &m_CanOpenIDParam.TxPDO1_W3Steer, true); - m_IniFile.GetKeyInt("CanOpenIDs", "TxPDO2_W3Steer", &m_CanOpenIDParam.TxPDO2_W3Steer, true); - m_IniFile.GetKeyInt("CanOpenIDs", "RxPDO2_W3Steer", &m_CanOpenIDParam.RxPDO2_W3Steer, true); - m_IniFile.GetKeyInt("CanOpenIDs", "TxSDO_W3Steer", &m_CanOpenIDParam.TxSDO_W3Steer, true); - m_IniFile.GetKeyInt("CanOpenIDs", "RxSDO_W3Steer", &m_CanOpenIDParam.RxSDO_W3Steer, true); - } - - // Wheel 4 - // DriveMotor - if(m_iNumDrives >= 4) - { - m_IniFile.GetKeyInt("CanOpenIDs", "TxPDO1_W4Drive", &m_CanOpenIDParam.TxPDO1_W4Drive, true); - m_IniFile.GetKeyInt("CanOpenIDs", "TxPDO2_W4Drive", &m_CanOpenIDParam.TxPDO2_W4Drive, true); - m_IniFile.GetKeyInt("CanOpenIDs", "RxPDO2_W4Drive", &m_CanOpenIDParam.RxPDO2_W4Drive, true); - m_IniFile.GetKeyInt("CanOpenIDs", "TxSDO_W4Drive", &m_CanOpenIDParam.TxSDO_W4Drive, true); - m_IniFile.GetKeyInt("CanOpenIDs", "RxSDO_W4Drive", &m_CanOpenIDParam.RxSDO_W4Drive, true); - } - // SteerMotor - if(m_iNumDrives == 4) - { - m_IniFile.GetKeyInt("CanOpenIDs", "TxPDO1_W4Steer", &m_CanOpenIDParam.TxPDO1_W4Steer, true); - m_IniFile.GetKeyInt("CanOpenIDs", "TxPDO2_W4Steer", &m_CanOpenIDParam.TxPDO2_W4Steer, true); - m_IniFile.GetKeyInt("CanOpenIDs", "RxPDO2_W4Steer", &m_CanOpenIDParam.RxPDO2_W4Steer, true); - m_IniFile.GetKeyInt("CanOpenIDs", "TxSDO_W4Steer", &m_CanOpenIDParam.TxSDO_W4Steer, true); - m_IniFile.GetKeyInt("CanOpenIDs", "RxSDO_W4Steer", &m_CanOpenIDParam.RxSDO_W4Steer, true); - } - - // read configuration of the Drives (CanCtrl.ini) - /* Drivemotor1-Parameters Old - m_IniFile.GetKeyString("TypeDrive", "Drive1", &strTypeDrive, true); - m_IniFile.GetKeyInt(strTypeDrive.c_str(), "EncIncrPerRevMot", &(m_GearMotDrive1.iEncIncrPerRevMot), true); - m_IniFile.GetKeyDouble(strTypeDrive.c_str(), "VelMeasFrqHz", &(m_GearMotDrive1.dVelMeasFrqHz), true); - m_IniFile.GetKeyDouble(strTypeDrive.c_str(), "BeltRatio", &(m_GearMotDrive1.dBeltRatio), true); - m_IniFile.GetKeyDouble(strTypeDrive.c_str(), "GearRatio", &(m_GearMotDrive1.dGearRatio), true); - m_IniFile.GetKeyInt(strTypeDrive.c_str(), "Sign", &(m_GearMotDrive1.iSign), true); - m_IniFile.GetKeyDouble(strTypeDrive.c_str(), "VelMaxEncIncrS", &(m_GearMotDrive1.dVelMaxEncIncrS), true); - m_IniFile.GetKeyDouble(strTypeDrive.c_str(), "AccIncrS", &(m_GearMotDrive1.dAccIncrS2), true); - m_IniFile.GetKeyDouble(strTypeDrive.c_str(), "DecIncrS", &(m_GearMotDrive1.dDecIncrS2), true); - m_IniFile.GetKeyDouble(strTypeDrive.c_str(), "EncOffsetIncr",&(m_GearMotDrive1.iEncOffsetIncr),true); - */ - - // "Drive Motor Type1" drive parameters - if(m_iNumDrives >= 1) - { - m_IniFile.GetKeyInt("Drive1", "EncIncrPerRevMot", &(m_GearMotDrive1.iEncIncrPerRevMot), true); - m_IniFile.GetKeyDouble("Drive1", "VelMeasFrqHz", &(m_GearMotDrive1.dVelMeasFrqHz), true); - m_IniFile.GetKeyDouble("Drive1", "BeltRatio", &(m_GearMotDrive1.dBeltRatio), true); - m_IniFile.GetKeyDouble("Drive1", "GearRatio", &(m_GearMotDrive1.dGearRatio), true); - m_IniFile.GetKeyInt("Drive1", "Sign", &(m_GearMotDrive1.iSign), true); - m_IniFile.GetKeyDouble("Drive1", "VelMaxEncIncrS", &(m_GearMotDrive1.dVelMaxEncIncrS), true); - m_IniFile.GetKeyDouble("Drive1", "AccIncrS", &(m_GearMotDrive1.dAccIncrS2), true); - m_IniFile.GetKeyDouble("Drive1", "DecIncrS", &(m_GearMotDrive1.dDecIncrS2), true); - m_IniFile.GetKeyInt("Drive1", "EncOffsetIncr",&(m_GearMotDrive1.iEncOffsetIncr),true); - m_IniFile.GetKeyBool("Drive1", "IsSteering", &(m_GearMotDrive1.bIsSteer), true); - m_IniFile.GetKeyDouble("Drive1", "CurrentToTorque", &(m_GearMotDrive1.dCurrentToTorque), false); - m_IniFile.GetKeyDouble("Drive1", "CurrMax", &(m_GearMotDrive1.dCurrMax), false); - m_IniFile.GetKeyInt("Drive1", "HomingDigIn", &(m_GearMotDrive1.iHomingDigIn), false); - } - - // "Drive Motor Type2" drive parameters - if(m_iNumDrives >= 2) - { - m_IniFile.GetKeyInt("Drive2", "EncIncrPerRevMot", &(m_GearMotDrive2.iEncIncrPerRevMot), true); - m_IniFile.GetKeyDouble("Drive2", "VelMeasFrqHz", &(m_GearMotDrive2.dVelMeasFrqHz), true); - m_IniFile.GetKeyDouble("Drive2", "BeltRatio", &(m_GearMotDrive2.dBeltRatio), true); - m_IniFile.GetKeyDouble("Drive2", "GearRatio", &(m_GearMotDrive2.dGearRatio), true); - m_IniFile.GetKeyInt("Drive2", "Sign", &(m_GearMotDrive2.iSign), true); - m_IniFile.GetKeyDouble("Drive2", "VelMaxEncIncrS", &(m_GearMotDrive2.dVelMaxEncIncrS), true); - m_IniFile.GetKeyDouble("Drive2", "AccIncrS", &(m_GearMotDrive2.dAccIncrS2), true); - m_IniFile.GetKeyDouble("Drive2", "DecIncrS", &(m_GearMotDrive2.dDecIncrS2), true); - m_IniFile.GetKeyInt("Drive2", "EncOffsetIncr",&(m_GearMotDrive2.iEncOffsetIncr),true); - m_IniFile.GetKeyBool("Drive2", "IsSteering", &(m_GearMotDrive2.bIsSteer), true); - m_IniFile.GetKeyDouble("Drive2", "CurrentToTorque", &(m_GearMotDrive2.dCurrentToTorque), false); - m_IniFile.GetKeyDouble("Drive2", "CurrMax", &(m_GearMotDrive2.dCurrMax), false); - m_IniFile.GetKeyInt("Drive2", "HomingDigIn", &(m_GearMotDrive2.iHomingDigIn), false); - } - - // "Drive Motor Type3" drive parameters - if(m_iNumDrives >= 3) - { - m_IniFile.GetKeyInt("Drive3", "EncIncrPerRevMot", &(m_GearMotDrive3.iEncIncrPerRevMot), true); - m_IniFile.GetKeyDouble("Drive3", "VelMeasFrqHz", &(m_GearMotDrive3.dVelMeasFrqHz), true); - m_IniFile.GetKeyDouble("Drive3", "BeltRatio", &(m_GearMotDrive3.dBeltRatio), true); - m_IniFile.GetKeyDouble("Drive3", "GearRatio", &(m_GearMotDrive3.dGearRatio), true); - m_IniFile.GetKeyInt("Drive3", "Sign", &(m_GearMotDrive3.iSign), true); - m_IniFile.GetKeyDouble("Drive3", "VelMaxEncIncrS", &(m_GearMotDrive3.dVelMaxEncIncrS), true); - m_IniFile.GetKeyDouble("Drive3", "AccIncrS", &(m_GearMotDrive3.dAccIncrS2), true); - m_IniFile.GetKeyDouble("Drive3", "DecIncrS", &(m_GearMotDrive3.dDecIncrS2), true); - m_IniFile.GetKeyInt("Drive3", "EncOffsetIncr",&(m_GearMotDrive3.iEncOffsetIncr),true); - m_IniFile.GetKeyBool("Drive3", "IsSteering", &(m_GearMotDrive3.bIsSteer), true); - m_IniFile.GetKeyDouble("Drive3", "CurrentToTorque", &(m_GearMotDrive3.dCurrentToTorque), false); - m_IniFile.GetKeyDouble("Drive3", "CurrMax", &(m_GearMotDrive3.dCurrMax), false); - m_IniFile.GetKeyInt("Drive3", "HomingDigIn", &(m_GearMotDrive3.iHomingDigIn), false); - } - - // "Drive Motor Type4" drive parameters - if(m_iNumDrives == 4) - { - m_IniFile.GetKeyInt("Drive4", "EncIncrPerRevMot", &(m_GearMotDrive4.iEncIncrPerRevMot), true); - m_IniFile.GetKeyDouble("Drive4", "VelMeasFrqHz", &(m_GearMotDrive4.dVelMeasFrqHz), true); - m_IniFile.GetKeyDouble("Drive4", "BeltRatio", &(m_GearMotDrive4.dBeltRatio), true); - m_IniFile.GetKeyDouble("Drive4", "GearRatio", &(m_GearMotDrive4.dGearRatio), true); - m_IniFile.GetKeyInt("Drive4", "Sign", &(m_GearMotDrive4.iSign), true); - m_IniFile.GetKeyDouble("Drive4", "VelMaxEncIncrS", &(m_GearMotDrive4.dVelMaxEncIncrS), true); - m_IniFile.GetKeyDouble("Drive4", "AccIncrS", &(m_GearMotDrive4.dAccIncrS2), true); - m_IniFile.GetKeyDouble("Drive4", "DecIncrS", &(m_GearMotDrive4.dDecIncrS2), true); - m_IniFile.GetKeyInt("Drive4", "EncOffsetIncr",&(m_GearMotDrive4.iEncOffsetIncr),true); - m_IniFile.GetKeyBool("Drive4", "IsSteering", &(m_GearMotDrive4.bIsSteer), true); - m_IniFile.GetKeyDouble("Drive4", "CurrentToTorque", &(m_GearMotDrive4.dCurrentToTorque), false); - m_IniFile.GetKeyDouble("Drive4", "CurrMax", &(m_GearMotDrive4.dCurrMax), false); - m_IniFile.GetKeyInt("Drive4", "HomingDigIn", &(m_GearMotDrive4.iHomingDigIn), false); - } - - // "Steer Motor Type1" drive parameters - if(m_iNumDrives >= 1) - { - m_IniFile.GetKeyInt("Steer1", "EncIncrPerRevMot", &(m_GearMotSteer1.iEncIncrPerRevMot), true); - m_IniFile.GetKeyDouble("Steer1", "VelMeasFrqHz", &(m_GearMotSteer1.dVelMeasFrqHz), true); - m_IniFile.GetKeyDouble("Steer1", "BeltRatio", &(m_GearMotSteer1.dBeltRatio), true); - m_IniFile.GetKeyDouble("Steer1", "GearRatio", &(m_GearMotSteer1.dGearRatio), true); - m_IniFile.GetKeyInt("Steer1", "Sign", &(m_GearMotSteer1.iSign), true); - m_IniFile.GetKeyDouble("Steer1", "VelMaxEncIncrS", &(m_GearMotSteer1.dVelMaxEncIncrS), true); - m_IniFile.GetKeyDouble("Steer1", "AccIncrS", &(m_GearMotSteer1.dAccIncrS2), true); - m_IniFile.GetKeyDouble("Steer1", "DecIncrS", &(m_GearMotSteer1.dDecIncrS2), true); - m_IniFile.GetKeyInt("Steer1", "EncOffsetIncr",&(m_GearMotSteer1.iEncOffsetIncr),true); - m_IniFile.GetKeyBool("Steer1", "IsSteering", &(m_GearMotSteer1.bIsSteer), true); - m_IniFile.GetKeyDouble("Steer1", "CurrentToTorque", &(m_GearMotSteer1.dCurrentToTorque), false); - m_IniFile.GetKeyDouble("Steer1", "CurrMax", &(m_GearMotSteer1.dCurrMax), false); - m_IniFile.GetKeyInt("Steer1", "HomingDigIn", &(m_GearMotSteer1.iHomingDigIn), false); - } - - // "Steer Motor Type2" drive parameters - if(m_iNumDrives >= 2) - { - m_IniFile.GetKeyInt("Steer2", "EncIncrPerRevMot", &(m_GearMotSteer2.iEncIncrPerRevMot), true); - m_IniFile.GetKeyDouble("Steer2", "VelMeasFrqHz", &(m_GearMotSteer2.dVelMeasFrqHz), true); - m_IniFile.GetKeyDouble("Steer2", "BeltRatio", &(m_GearMotSteer2.dBeltRatio), true); - m_IniFile.GetKeyDouble("Steer2", "GearRatio", &(m_GearMotSteer2.dGearRatio), true); - m_IniFile.GetKeyInt("Steer2", "Sign", &(m_GearMotSteer2.iSign), true); - m_IniFile.GetKeyDouble("Steer2", "VelMaxEncIncrS", &(m_GearMotSteer2.dVelMaxEncIncrS), true); - m_IniFile.GetKeyDouble("Steer2", "AccIncrS", &(m_GearMotSteer2.dAccIncrS2), true); - m_IniFile.GetKeyDouble("Steer2", "DecIncrS", &(m_GearMotSteer2.dDecIncrS2), true); - m_IniFile.GetKeyInt("Steer2", "EncOffsetIncr",&(m_GearMotSteer2.iEncOffsetIncr),true); - m_IniFile.GetKeyBool("Steer2", "IsSteering", &(m_GearMotSteer2.bIsSteer), true); - m_IniFile.GetKeyDouble("Steer2", "CurrentToTorque", &(m_GearMotSteer2.dCurrentToTorque), false); - m_IniFile.GetKeyDouble("Steer2", "CurrMax", &(m_GearMotSteer2.dCurrMax), false); - m_IniFile.GetKeyInt("Steer2", "HomingDigIn", &(m_GearMotSteer2.iHomingDigIn), false); - } - - // "Steer Motor Type3" drive parameters - if(m_iNumDrives >= 3) - { - m_IniFile.GetKeyInt("Steer3", "EncIncrPerRevMot", &(m_GearMotSteer3.iEncIncrPerRevMot), true); - m_IniFile.GetKeyDouble("Steer3", "VelMeasFrqHz", &(m_GearMotSteer3.dVelMeasFrqHz), true); - m_IniFile.GetKeyDouble("Steer3", "BeltRatio", &(m_GearMotSteer3.dBeltRatio), true); - m_IniFile.GetKeyDouble("Steer3", "GearRatio", &(m_GearMotSteer3.dGearRatio), true); - m_IniFile.GetKeyInt("Steer3", "Sign", &(m_GearMotSteer3.iSign), true); - m_IniFile.GetKeyDouble("Steer3", "VelMaxEncIncrS", &(m_GearMotSteer3.dVelMaxEncIncrS), true); - m_IniFile.GetKeyDouble("Steer3", "AccIncrS", &(m_GearMotSteer3.dAccIncrS2), true); - m_IniFile.GetKeyDouble("Steer3", "DecIncrS", &(m_GearMotSteer3.dDecIncrS2), true); - m_IniFile.GetKeyInt("Steer3", "EncOffsetIncr",&(m_GearMotSteer3.iEncOffsetIncr),true); - m_IniFile.GetKeyBool("Steer3", "IsSteering", &(m_GearMotSteer3.bIsSteer), true); - m_IniFile.GetKeyDouble("Steer3", "CurrentToTorque", &(m_GearMotSteer3.dCurrentToTorque), false); - m_IniFile.GetKeyDouble("Steer3", "CurrMax", &(m_GearMotSteer3.dCurrMax), false); - m_IniFile.GetKeyInt("Steer3", "HomingDigIn", &(m_GearMotSteer3.iHomingDigIn), false); - } - - // "Steer Motor Type4" drive parameters - if(m_iNumDrives == 4) - { - m_IniFile.GetKeyInt("Steer4", "EncIncrPerRevMot", &(m_GearMotSteer4.iEncIncrPerRevMot), true); - m_IniFile.GetKeyDouble("Steer4", "VelMeasFrqHz", &(m_GearMotSteer4.dVelMeasFrqHz), true); - m_IniFile.GetKeyDouble("Steer4", "BeltRatio", &(m_GearMotSteer4.dBeltRatio), true); - m_IniFile.GetKeyDouble("Steer4", "GearRatio", &(m_GearMotSteer4.dGearRatio), true); - m_IniFile.GetKeyInt("Steer4", "Sign", &(m_GearMotSteer4.iSign), true); - m_IniFile.GetKeyDouble("Steer4", "VelMaxEncIncrS", &(m_GearMotSteer4.dVelMaxEncIncrS), true); - m_IniFile.GetKeyDouble("Steer4", "AccIncrS", &(m_GearMotSteer4.dAccIncrS2), true); - m_IniFile.GetKeyDouble("Steer4", "DecIncrS", &(m_GearMotSteer4.dDecIncrS2), true); - m_IniFile.GetKeyInt("Steer4", "EncOffsetIncr",&(m_GearMotSteer4.iEncOffsetIncr),true); - m_IniFile.GetKeyBool("Steer4", "IsSteering", &(m_GearMotSteer4.bIsSteer), true); - m_IniFile.GetKeyDouble("Steer4", "CurrentToTorque", &(m_GearMotSteer4.dCurrentToTorque), false); - m_IniFile.GetKeyDouble("Steer4", "CurrMax", &(m_GearMotSteer4.dCurrMax), false); - m_IniFile.GetKeyInt("Steer4", "HomingDigIn", &(m_GearMotSteer4.iHomingDigIn), false); - } - - if(m_iNumDrives >= 1) - { - DriveParamW1DriveMotor.setParam( - 0, - m_GearMotDrive1.iEncIncrPerRevMot, - m_GearMotDrive1.dVelMeasFrqHz, - m_GearMotDrive1.dBeltRatio, - m_GearMotDrive1.dGearRatio, - m_GearMotDrive1.iSign, - m_GearMotDrive1.dVelMaxEncIncrS, - m_GearMotDrive1.dAccIncrS2, - m_GearMotDrive1.dDecIncrS2, - m_GearMotDrive1.iEncOffsetIncr, - m_GearMotDrive1.bIsSteer, - m_GearMotDrive1.dCurrentToTorque, - m_GearMotDrive1.dCurrMax, - m_GearMotDrive1.iHomingDigIn); - } - - if(m_iNumDrives >= 1) - { - DriveParamW1SteerMotor.setParam( - 1, - m_GearMotSteer1.iEncIncrPerRevMot, - m_GearMotSteer1.dVelMeasFrqHz, - m_GearMotSteer1.dBeltRatio, - m_GearMotSteer1.dGearRatio, - m_GearMotSteer1.iSign, - m_GearMotSteer1.dVelMaxEncIncrS, - m_GearMotSteer1.dAccIncrS2, - m_GearMotSteer1.dDecIncrS2, - m_GearMotSteer1.iEncOffsetIncr, - m_GearMotSteer1.bIsSteer, - m_GearMotSteer1.dCurrentToTorque, - m_GearMotSteer1.dCurrMax, - m_GearMotSteer1.iHomingDigIn); - } - - if(m_iNumDrives >= 2) - { - DriveParamW2DriveMotor.setParam( - 2, - m_GearMotDrive2.iEncIncrPerRevMot, - m_GearMotDrive2.dVelMeasFrqHz, - m_GearMotDrive2.dBeltRatio, - m_GearMotDrive2.dGearRatio, - m_GearMotDrive2.iSign, - m_GearMotDrive2.dVelMaxEncIncrS, - m_GearMotDrive2.dAccIncrS2, - m_GearMotDrive2.dDecIncrS2, - m_GearMotDrive2.iEncOffsetIncr, - m_GearMotDrive2.bIsSteer, - m_GearMotDrive2.dCurrentToTorque, - m_GearMotDrive2.dCurrMax, - m_GearMotDrive2.iHomingDigIn); - } - - if(m_iNumDrives >= 2) - { - DriveParamW2SteerMotor.setParam( - 3, - m_GearMotSteer2.iEncIncrPerRevMot, - m_GearMotSteer2.dVelMeasFrqHz, - m_GearMotSteer2.dBeltRatio, - m_GearMotSteer2.dGearRatio, - m_GearMotSteer2.iSign, - m_GearMotSteer2.dVelMaxEncIncrS, - m_GearMotSteer2.dAccIncrS2, - m_GearMotSteer2.dDecIncrS2, - m_GearMotSteer2.iEncOffsetIncr, - m_GearMotSteer2.bIsSteer, - m_GearMotSteer2.dCurrentToTorque, - m_GearMotSteer2.dCurrMax, - m_GearMotSteer2.iHomingDigIn); - } - - if(m_iNumDrives >= 3) - { - DriveParamW3DriveMotor.setParam( - 4, - m_GearMotDrive3.iEncIncrPerRevMot, - m_GearMotDrive3.dVelMeasFrqHz, - m_GearMotDrive3.dBeltRatio, - m_GearMotDrive3.dGearRatio, - m_GearMotDrive3.iSign, - m_GearMotDrive3.dVelMaxEncIncrS, - m_GearMotDrive3.dAccIncrS2, - m_GearMotDrive3.dDecIncrS2, - m_GearMotDrive3.iEncOffsetIncr, - m_GearMotDrive3.bIsSteer, - m_GearMotDrive3.dCurrentToTorque, - m_GearMotDrive3.dCurrMax, - m_GearMotDrive3.iHomingDigIn); - } - - if(m_iNumDrives >= 3) - { - DriveParamW3SteerMotor.setParam( - 5, - m_GearMotSteer3.iEncIncrPerRevMot, - m_GearMotSteer3.dVelMeasFrqHz, - m_GearMotSteer3.dBeltRatio, - m_GearMotSteer3.dGearRatio, - m_GearMotSteer3.iSign, - m_GearMotSteer3.dVelMaxEncIncrS, - m_GearMotSteer3.dAccIncrS2, - m_GearMotSteer3.dDecIncrS2, - m_GearMotSteer3.iEncOffsetIncr, - m_GearMotSteer3.bIsSteer, - m_GearMotSteer3.dCurrentToTorque, - m_GearMotSteer3.dCurrMax, - m_GearMotSteer3.iHomingDigIn); - } - - if(m_iNumDrives == 4) - { - DriveParamW4DriveMotor.setParam( - 6, - m_GearMotDrive4.iEncIncrPerRevMot, - m_GearMotDrive4.dVelMeasFrqHz, - m_GearMotDrive4.dBeltRatio, - m_GearMotDrive4.dGearRatio, - m_GearMotDrive4.iSign, - m_GearMotDrive4.dVelMaxEncIncrS, - m_GearMotDrive4.dAccIncrS2, - m_GearMotDrive4.dDecIncrS2, - m_GearMotDrive4.iEncOffsetIncr, - m_GearMotDrive4.bIsSteer, - m_GearMotDrive4.dCurrentToTorque, - m_GearMotDrive4.dCurrMax, - m_GearMotDrive4.iHomingDigIn); - } - - if(m_iNumDrives == 4) - { - DriveParamW4SteerMotor.setParam( - 7, - m_GearMotSteer4.iEncIncrPerRevMot, - m_GearMotSteer4.dVelMeasFrqHz, - m_GearMotSteer4.dBeltRatio, - m_GearMotSteer4.dGearRatio, - m_GearMotSteer4.iSign, - m_GearMotSteer4.dVelMaxEncIncrS, - m_GearMotSteer4.dAccIncrS2, - m_GearMotSteer4.dDecIncrS2, - m_GearMotSteer4.iEncOffsetIncr, - m_GearMotSteer4.bIsSteer, - m_GearMotSteer4.dCurrentToTorque, - m_GearMotSteer4.dCurrMax, - m_GearMotSteer4.iHomingDigIn); - } - - m_IniFile.GetKeyDouble("US", "ScaleToMM", &dScaleToMM, true); - - - // read Platform.ini - m_IniFile.SetFileName(sIniDirectory + "Platform.ini", "CanCtrlPltfCOb3.cpp"); - - - // ------ WHEEL 1 ------ // - // --- Motor Wheel 1 Drive - if(m_iNumDrives >= 1) - { - m_IniFile.GetKeyInt("Config", "Wheel1DriveMotor", &m_Param.iHasWheel1DriveMotor, true); - if (m_Param.iHasWheel1DriveMotor == 0) - { - // No motor - std::cout << "node Wheel1DriveMotor available = 0" << std::endl; - } - else - { - // Motor Harmonica - std::cout << "Wheel1DriveMotor available = type version 2" << std::endl; - m_vpMotor[0] = new CanDriveHarmonica(); - ((CanDriveHarmonica*) m_vpMotor[0])->setCanOpenParam( - m_CanOpenIDParam.TxPDO1_W1Drive, m_CanOpenIDParam.TxPDO2_W1Drive, m_CanOpenIDParam.RxPDO2_W1Drive, - m_CanOpenIDParam.TxSDO_W1Drive, m_CanOpenIDParam.RxSDO_W1Drive); - m_vpMotor[0]->setCanItf(m_pCanCtrl); - m_vpMotor[0]->setDriveParam(DriveParamW1DriveMotor); - } - } - - // --- Motor Wheel 1 Steer - if(m_iNumDrives >= 1) - { - m_IniFile.GetKeyInt("Config", "Wheel1SteerMotor", &m_Param.iHasWheel1SteerMotor, true); - if (m_Param.iHasWheel1SteerMotor == 0) - { - // No motor - std::cout << "node Wheel1SteerMotor available = 0" << std::endl; - } - else - { - // Motor Harmonica - std::cout << "Wheel1SteerMotor available = type version 2" << std::endl; - m_vpMotor[1] = new CanDriveHarmonica(); - ((CanDriveHarmonica*) m_vpMotor[1])->setCanOpenParam( - m_CanOpenIDParam.TxPDO1_W1Steer, m_CanOpenIDParam.TxPDO2_W1Steer, m_CanOpenIDParam.RxPDO2_W1Steer, - m_CanOpenIDParam.TxSDO_W1Steer, m_CanOpenIDParam.RxSDO_W1Steer); - m_vpMotor[1]->setCanItf(m_pCanCtrl); - m_vpMotor[1]->setDriveParam(DriveParamW1SteerMotor); - - } - } - - // ------ WHEEL 2 ------ // - // --- Motor Wheel 2 Drive - if(m_iNumDrives >= 2) - { - m_IniFile.GetKeyInt("Config", "Wheel2DriveMotor", &m_Param.iHasWheel2DriveMotor, true); - if (m_Param.iHasWheel2DriveMotor == 0) - { - // No motor - std::cout << "node Wheel2DriveMotor available = 0" << std::endl; - } - else - { - // Motor Harmonica - std::cout << "Wheel2DriveMotor available = type version 2" << std::endl; - m_vpMotor[2] = new CanDriveHarmonica(); - ((CanDriveHarmonica*) m_vpMotor[2])->setCanOpenParam( - m_CanOpenIDParam.TxPDO1_W2Drive, m_CanOpenIDParam.TxPDO2_W2Drive, m_CanOpenIDParam.RxPDO2_W2Drive, - m_CanOpenIDParam.TxSDO_W2Drive, m_CanOpenIDParam.RxSDO_W2Drive); - m_vpMotor[2]->setCanItf(m_pCanCtrl); - m_vpMotor[2]->setDriveParam(DriveParamW2DriveMotor); - } - } - - // --- Motor Wheel 2 Steer - if(m_iNumDrives >= 2) - { - m_IniFile.GetKeyInt("Config", "Wheel2SteerMotor", &m_Param.iHasWheel2SteerMotor, true); - if (m_Param.iHasWheel2SteerMotor == 0) - { - // No motor - std::cout << "node Wheel2SteerMotor available = 0" << std::endl; - } - else - { - // Motor Harmonica - std::cout << "Wheel2SteerMotor available = type version 2" << std::endl; - m_vpMotor[3] = new CanDriveHarmonica(); - ((CanDriveHarmonica*) m_vpMotor[3])->setCanOpenParam( - m_CanOpenIDParam.TxPDO1_W2Steer, m_CanOpenIDParam.TxPDO2_W2Steer, m_CanOpenIDParam.RxPDO2_W2Steer, - m_CanOpenIDParam.TxSDO_W2Steer, m_CanOpenIDParam.RxSDO_W2Steer); - m_vpMotor[3]->setCanItf(m_pCanCtrl); - m_vpMotor[3]->setDriveParam(DriveParamW2SteerMotor); - - } - } - - // ------ WHEEL 3 ------ // - // --- Motor Wheel 3 Drive - if(m_iNumDrives >= 3) - { - m_IniFile.GetKeyInt("Config", "Wheel3DriveMotor", &m_Param.iHasWheel3DriveMotor, true); - if (m_Param.iHasWheel3DriveMotor == 0) - { - // No motor - std::cout << "node Wheel3DriveMotor available = 0" << std::endl; - } - else - { - // Motor Harmonica - std::cout << "Wheel3DriveMotor available = type version 2" << std::endl; - m_vpMotor[4] = new CanDriveHarmonica(); - ((CanDriveHarmonica*) m_vpMotor[4])->setCanOpenParam( - m_CanOpenIDParam.TxPDO1_W3Drive, m_CanOpenIDParam.TxPDO2_W3Drive, m_CanOpenIDParam.RxPDO2_W3Drive, - m_CanOpenIDParam.TxSDO_W3Drive, m_CanOpenIDParam.RxSDO_W3Drive); - m_vpMotor[4]->setCanItf(m_pCanCtrl); - m_vpMotor[4]->setDriveParam(DriveParamW3DriveMotor); - } - } - - // --- Motor Wheel 3 Steer - if(m_iNumDrives >= 3) - { - m_IniFile.GetKeyInt("Config", "Wheel3SteerMotor", &m_Param.iHasWheel3SteerMotor, true); - if (m_Param.iHasWheel3SteerMotor == 0) - { - // No motor - std::cout << "node Wheel3SteerMotor available = 0" << std::endl; - } - else - { - // Motor Harmonica - std::cout << "Wheel3SteerMotor available = type version 2" << std::endl; - m_vpMotor[5] = new CanDriveHarmonica(); - ((CanDriveHarmonica*) m_vpMotor[5])->setCanOpenParam( - m_CanOpenIDParam.TxPDO1_W3Steer, m_CanOpenIDParam.TxPDO2_W3Steer, m_CanOpenIDParam.RxPDO2_W3Steer, - m_CanOpenIDParam.TxSDO_W3Steer, m_CanOpenIDParam.RxSDO_W3Steer); - m_vpMotor[5]->setCanItf(m_pCanCtrl); - m_vpMotor[5]->setDriveParam(DriveParamW3SteerMotor); - - } - } - - // ------ WHEEL 4 ------ // - // --- Motor Wheel 4 Drive - if(m_iNumDrives == 4) - { - m_IniFile.GetKeyInt("Config", "Wheel4DriveMotor", &m_Param.iHasWheel4DriveMotor, true); - if (m_Param.iHasWheel4DriveMotor == 0) - { - // No motor - std::cout << "node Wheel4DriveMotor available = 0" << std::endl; - } - else - { - // Motor Harmonica - std::cout << "Wheel4DriveMotor available = type version 2" << std::endl; - m_vpMotor[6] = new CanDriveHarmonica(); - ((CanDriveHarmonica*) m_vpMotor[6])->setCanOpenParam( - m_CanOpenIDParam.TxPDO1_W4Drive, m_CanOpenIDParam.TxPDO2_W4Drive, m_CanOpenIDParam.RxPDO2_W4Drive, - m_CanOpenIDParam.TxSDO_W4Drive, m_CanOpenIDParam.RxSDO_W4Drive); - m_vpMotor[6]->setCanItf(m_pCanCtrl); - m_vpMotor[6]->setDriveParam(DriveParamW4DriveMotor); - } - } - - // --- Motor Wheel 4 Steer - if(m_iNumDrives == 4) - { - m_IniFile.GetKeyInt("Config", "Wheel4SteerMotor", &m_Param.iHasWheel4SteerMotor, true); - if (m_Param.iHasWheel4SteerMotor == 0) - { - // No motor - std::cout << "node Wheel4SteerMotor available = 0" << std::endl; - } - else - { - // Motor Harmonica - std::cout << "Wheel4SteerMotor available = type version 2" << std::endl; - m_vpMotor[7] = new CanDriveHarmonica(); - ((CanDriveHarmonica*) m_vpMotor[7])->setCanOpenParam( - m_CanOpenIDParam.TxPDO1_W4Steer, m_CanOpenIDParam.TxPDO2_W4Steer, m_CanOpenIDParam.RxPDO2_W4Steer, - m_CanOpenIDParam.TxSDO_W4Steer, m_CanOpenIDParam.RxSDO_W4Steer); - m_vpMotor[7]->setCanItf(m_pCanCtrl); - m_vpMotor[7]->setDriveParam(DriveParamW4SteerMotor); - - } - } - - m_IniFile.GetKeyInt("Config", "GenericBufferLen", &iMaxMessages, true); - - -} - -//----------------------------------------------- -int CanCtrlPltfCOb3::evalCanBuffer() -{ - bool bRet; -// char cBuf[200]; - - m_Mutex.lock(); - - // as long as there is something in the can buffer -> read out next message - while(m_pCanCtrl->receiveMsg(&m_CanMsgRec) == true) - { - bRet = false; - // check for every motor if message belongs to it - for (unsigned int i = 0; i < m_vpMotor.size(); i++) - { - // if message belongs to this motor write data (Pos, Vel, ...) to internal member vars - bRet |= m_vpMotor[i]->evalReceivedMsg(m_CanMsgRec); - } - - if (bRet == false) - { - std::cout << "evalCanBuffer(): Received CAN_Message with unknown identifier " << m_CanMsgRec.m_iID << std::endl; - } - }; - - - m_Mutex.unlock(); - - return 0; -} - -//----------------------------------------------- -bool CanCtrlPltfCOb3::initPltf() -{ - // read Configuration parameters from Inifile - readConfiguration(); - - // Vectors for drive objects and return values - std::vector vbRetDriveMotor; - std::vector vbRetSteerMotor; - std::vector vpDriveMotor; - std::vector vpSteerMotor; - bool bHomingOk; - -// vbRetDriveMotor.assign(4,0); -// vbRetSteerMotor.assign(4,0); - vbRetDriveMotor.assign(m_iNumDrives,0); - vbRetSteerMotor.assign(m_iNumDrives,0); - - // Homing is done on a wheel-module base (steering and driving needs to be synchronized) - // copy Motor-Pointer into Steer/Drive vector for more insight - for(int i=0; i<=m_iNumMotors; i+=2) - vpDriveMotor.push_back(m_vpMotor[i]); -// vpDriveMotor.push_back(m_vpMotor[2]); -// vpDriveMotor.push_back(m_vpMotor[4]); -// vpDriveMotor.push_back(m_vpMotor[6]); - for(int i=1; i<=m_iNumMotors; i+=2) - vpSteerMotor.push_back(m_vpMotor[i]); -// vpSteerMotor.push_back(m_vpMotor[3]); -// vpSteerMotor.push_back(m_vpMotor[5]); -// vpSteerMotor.push_back(m_vpMotor[7]); - - std::vector vdFactorVel; -// vdFactorVel.assign(4,0); - vdFactorVel.assign(m_iNumDrives,0); - - - // Start can open network - std::cout << "StartCanOpen" << std::endl; - sendNetStartCanOpen(); - - - // initialize drives - - // 1st init watchdogs - std::cout << "Initialization of Watchdogs" << std::endl; - for(int i=0; istartWatchdog(true); - } -/* m_vpMotor[0]->startWatchdog(true); - m_vpMotor[1]->startWatchdog(true); - m_vpMotor[2]->startWatchdog(true); - m_vpMotor[3]->startWatchdog(true); - m_vpMotor[4]->startWatchdog(true); - m_vpMotor[5]->startWatchdog(true); - m_vpMotor[6]->startWatchdog(true); - m_vpMotor[7]->startWatchdog(true); -*/ - usleep(10000); - - // 2nd send watchdogs to bed while initializing drives - for(int i=0; istartWatchdog(false); - } -/* m_vpMotor[0]->startWatchdog(false); - m_vpMotor[1]->startWatchdog(false); - m_vpMotor[2]->startWatchdog(false); - m_vpMotor[3]->startWatchdog(false); - m_vpMotor[4]->startWatchdog(false); - m_vpMotor[5]->startWatchdog(false); - m_vpMotor[6]->startWatchdog(false); - m_vpMotor[7]->startWatchdog(false); -*/ - usleep(100000); - - std::cout << "Initialization of Watchdogs done" << std::endl; - - - // ---------------------- start homing procedurs - // Perform homing of all wheels simultaneously - // o.k. to avoid crashing hardware -> lets check that we have at least the 8 motors, like we have on cob - if( (int)m_vpMotor.size() == m_iNumMotors ) - { - // Initialize and start all motors - for (int i = 0; iinit(); - vbRetSteerMotor[i] = vpSteerMotor[i]->init(); - usleep(10000); - vbRetDriveMotor[i] = vbRetDriveMotor[i] && vpDriveMotor[i]->start(); - vbRetSteerMotor[i] = vbRetSteerMotor[i] && vpSteerMotor[i]->start(); - usleep(10000); - // output State / Errors - if (vbRetDriveMotor[i] && vbRetSteerMotor[i]) - std::cout << "Initialization of Wheel "<< (i+1) << " OK" << std::endl; - else if (!vbRetDriveMotor[i] && vbRetSteerMotor[i]) - std::cout << "Initialization of Wheel "<< (i+1) << " ERROR while initializing DRIVE-Motor" << std::endl; - else if (vbRetDriveMotor[i] && !vbRetSteerMotor[i]) - std::cout << "Initialization of Wheel "<< (i+1) << " ERROR while initializing STEER-Motor" << std::endl; - else - std::cout << "Initialization of Wheel "<< (i+1) << " ERROR while initializing STEER- and DRIVE-Motor" << std::endl; - // Just to be sure: Set vel to zero - vpDriveMotor[i]->setGearVelRadS(0); - vpSteerMotor[i]->setGearVelRadS(0); - } - - // perform homing only when ALL drives are ERROR-Free -// if (vbRetDriveMotor[0] && vbRetDriveMotor[1] && vbRetDriveMotor[2] && vbRetDriveMotor[3] && -// vbRetSteerMotor[0] && vbRetSteerMotor[1] && vbRetSteerMotor[2] && vbRetSteerMotor[3]) - bHomingOk = true; - for(int i=0; i= 1) - vdFactorVel[0] = - m_Param.dWheel1SteerDriveCoupling + double(m_Param.iDistSteerAxisToDriveWheelMM) / double(m_Param.iRadiusWheelMM); - if(m_iNumDrives >= 2) - vdFactorVel[1] = - m_Param.dWheel2SteerDriveCoupling + double(m_Param.iDistSteerAxisToDriveWheelMM) / double(m_Param.iRadiusWheelMM); - if(m_iNumDrives >= 3) - vdFactorVel[2] = - m_Param.dWheel3SteerDriveCoupling + double(m_Param.iDistSteerAxisToDriveWheelMM) / double(m_Param.iRadiusWheelMM); - if(m_iNumDrives == 4) - vdFactorVel[3] = - m_Param.dWheel4SteerDriveCoupling + double(m_Param.iDistSteerAxisToDriveWheelMM) / double(m_Param.iRadiusWheelMM); - - // initialize homing procedure - for (int i = 0; iinitHoming(); - - // make motors move - for (int i = 0; isetGearVelRadS(m_Param.dHomeVeloRadS); - vpDriveMotor[i]->setGearVelRadS(m_Param.dHomeVeloRadS * vdFactorVel[i]); - } - - // wait at least 0.5 sec. - usleep(500000); - - // Get rid of unnecessary can messages - bool bRet; - do - bRet = m_pCanCtrl->receiveMsg(&m_CanMsgRec); - while(bRet == true); - - // arm homing procedure - for (int i = 0; iIntprtSetInt(8, 'H', 'M', 1, 1); - - // wait until all steers are homed - bool bAllDone, bTimeOut=false; - int iCnt = 0; - do - { - // send request for homing status - for (int i = 0; iIntprtSetInt(4, 'H', 'M', 1, 0); - - // eval Can Messages - evalCanBuffer(); - - // set already homed wheels to zero velocity - bAllDone = true; - for (int i = 0; igetStatusLimitSwitch()) - { - vpSteerMotor[i]->setGearVelRadS(0); - vpDriveMotor[i]->setGearVelRadS(0); - } - bAllDone = bAllDone && vpSteerMotor[i]->getStatusLimitSwitch(); - } - - // increment timeout counter - if (iCnt++ > 1000) //cpc-ck has 500 for Cob 3.5 here - bTimeOut = true; - - // Sleep: To avoid can overload - usleep(20000); - } - while(!bAllDone && !bTimeOut); - - // Output State - if (bTimeOut) - { - for (int i=0;isetGearVelRadS(0); - vpDriveMotor[i]->setGearVelRadS(0); - } - std::cout << "Error while Homing: Timeout while waiting for homing signal" << std::endl; - } - - // update Can Buffer once more - // read current can buffer and update position and velocity measurements - evalCanBuffer(); - - // Now make steers move to position: zero - // this could be handled also by the elmos themselve - // however this way synchronization of steering and driving is better - double m_d0 = 2.5; - if (bTimeOut == false) - { - do - { - bAllDone = true; - for (int i = 0; igetGearPosRad(&dCurrentPosRad); - // P-Ctrl - double dDeltaPhi = 0.0 - dCurrentPosRad; - // check if steer is at pos zero - if (fabs(dDeltaPhi) < 0.03) //alter Wert=0.03 - { - dDeltaPhi = 0.0; - } - else - { - bAllDone = false; - } - double dVelCmd = m_d0 * dDeltaPhi; - // set Outputs - vpSteerMotor[i]->setGearVelRadS(dVelCmd); - vpDriveMotor[i]->setGearVelRadS(dVelCmd*vdFactorVel[i]); - } - usleep(20000); - evalCanBuffer(); - } while(!bAllDone); - - - // Homing done. Wheels at position zero (+/- 0.5°) - std::cout << "Wheels homed" << std::endl; - for (int i=0;isetGearVelRadS(0); - vpDriveMotor[i]->setGearVelRadS(0); - } - } - } - } - // ---------------------- end homing procedure - - // homing done -> wake up watchdogs - for(int i=0; istartWatchdog(true); - } -/* m_vpMotor[0]->startWatchdog(true); - m_vpMotor[1]->startWatchdog(true); - m_vpMotor[2]->startWatchdog(true); - m_vpMotor[3]->startWatchdog(true); - m_vpMotor[4]->startWatchdog(true); - m_vpMotor[5]->startWatchdog(true); - m_vpMotor[6]->startWatchdog(true); - m_vpMotor[7]->startWatchdog(true); -*/ -// return ( -// vbRetDriveMotor[0] && vbRetDriveMotor[1] && vbRetDriveMotor[2] && vbRetDriveMotor[3] && -// vbRetSteerMotor[0] && vbRetSteerMotor[1] && vbRetSteerMotor[2] && vbRetSteerMotor[3]); - bHomingOk = true; - for(int i=0; istart(); - if (bRetMotor == true) - { - m_vpMotor[i]->setGearVelRadS(0); - } - else - { - std::cout << "Resetting of Motor " << i << " failed" << std::endl; - } - - bRet &= bRetMotor; - } - return(bRet); -} - -//----------------------------------------------- -bool CanCtrlPltfCOb3::shutdownPltf() -{ - - for(unsigned int i = 0; i < m_vpMotor.size(); i++) - { - m_vpMotor[i]->shutdown(); - } - - return true; -} - -//----------------------------------------------- -bool CanCtrlPltfCOb3::isPltfError() -{ - - bool bErrMotor = false; - std::vector vbErrMotor; - vbErrMotor.resize(m_vpMotor.size()); - - // check all motors for errors - for(unsigned int i = 0; i < m_vpMotor.size(); i++) - { - vbErrMotor[i] = m_vpMotor[i]->isError(); - // if watchdog is not yet in error state but one motor is errorneous Log Out Error-Msg - if( (m_bWatchdogErr == false) && (vbErrMotor[i] == true) ) - { - std::cout << "Motor " << i << " error" << std::endl; - } - // check whether no motor was errorneous - bErrMotor |= vbErrMotor[i]; - } - - // if no motor is in error state force Watchdog to "No Error" state - if(bErrMotor == false) - m_bWatchdogErr = false; - else - m_bWatchdogErr = true; - - if (m_bWatchdogErr) return true; - - - // Check communication - double dWatchTime = 0; - - for(unsigned int i = 0; i < m_vpMotor.size(); i++) - { - dWatchTime = m_vpMotor[i]->getTimeToLastMsg(); - - if(dWatchTime < m_Param.dCanTimeout) - { - m_bWatchdogErr = false; - } - if( (dWatchTime > m_Param.dCanTimeout) && (m_bWatchdogErr == false) ) - { - std::cout << "timeout CAN motor " << i << std::endl; - m_bWatchdogErr = true; - return true; - } - } - - return false; -} - -//----------------------------------------------- -bool CanCtrlPltfCOb3::startWatchdog(bool bStarted) -{ - - bool bRet = true; - - for(unsigned int i = 0; i < m_vpMotor.size(); i++) - { - bRet = m_vpMotor[i]->startWatchdog(bStarted); - } - - return (bRet); -} - -//----------------------------------------------- -void CanCtrlPltfCOb3::sendNetStartCanOpen() -{ - CanMsg msg; - - msg.m_iID = 0; - msg.m_iLen = 2; - msg.set(1,0,0,0,0,0,0,0); - m_pCanCtrl->transmitMsg(msg, false); - - usleep(100000); -} - - -//----------------------------------------------- -// Motor Controlers -//----------------------------------------------- - - -//----------------------------------------------- -int CanCtrlPltfCOb3::setVelGearRadS(int iCanIdent, double dVelGearRadS) -{ - m_Mutex.lock(); - - // If an error was detected and processed in isPltfErr(). - if (m_bWatchdogErr == true) - { - // Error -> Stop motor driving - dVelGearRadS = 0; - // The error is detected and quitted in the funtion isPltfErr(). - } - - for(unsigned int i = 0; i < m_vpMotor.size(); i++) - { - // check if Identifier fits to availlable hardware - if(iCanIdent == m_viMotorID[i]) - { - m_vpMotor[i]->setGearVelRadS(dVelGearRadS); - } - } - - m_Mutex.unlock(); - - return 0; -} - -//----------------------------------------------- -int CanCtrlPltfCOb3::requestMotPosVel(int iCanIdent) -{ - m_Mutex.lock(); - - for(unsigned int i = 0; i < m_vpMotor.size(); i++) - { - // check if Identifier fits to availlable hardware - if(iCanIdent == m_viMotorID[i]) - { - m_vpMotor[i]->requestPosVel(); - } - } - - m_Mutex.unlock(); - - return 0; -} - -//----------------------------------------------- -void CanCtrlPltfCOb3::requestDriveStatus() -{ - m_Mutex.lock(); - - for(unsigned int i = 0; i < m_vpMotor.size(); i++) - { - m_vpMotor[i]->requestStatus(); - } - - m_Mutex.unlock(); -} - -//----------------------------------------------- -int CanCtrlPltfCOb3::getGearPosVelRadS(int iCanIdent, double* pdAngleGearRad, double* pdVelGearRadS) -{ - - // init default outputs - *pdAngleGearRad = 0; - *pdVelGearRadS = 0; - - for(unsigned int i = 0; i < m_vpMotor.size(); i++) - { - // check if Identifier fits to availlable hardware - if(iCanIdent == m_viMotorID[i]) - { - m_vpMotor[i]->getGearPosVelRadS(pdAngleGearRad, pdVelGearRadS); - } - } - - return 0; -} - -//----------------------------------------------- -int CanCtrlPltfCOb3::getGearDeltaPosVelRadS(int iCanIdent, double* pdAngleGearRad, - double* pdVelGearRadS) -{ - - // init default outputs - *pdAngleGearRad = 0; - *pdVelGearRadS = 0; - - for(unsigned int i = 0; i < m_vpMotor.size(); i++) - { - // check if Identifier fits to availlable hardware - if(iCanIdent == m_viMotorID[i]) - { - m_vpMotor[i]->getGearDeltaPosVelRadS(pdAngleGearRad, pdVelGearRadS); - } - } - - return 0; -} - -//----------------------------------------------- -void CanCtrlPltfCOb3::getStatus(int iCanIdent, int* piStatus, int* piTempCel) -{ - // init default outputs - *piStatus = 0; - *piTempCel = 0; - - for(unsigned int i = 0; i < m_vpMotor.size(); i++) - { - // check if Identifier fits to availlable hardware - if(iCanIdent == m_viMotorID[i]) - { - m_vpMotor[i]->getStatus(piStatus, piTempCel); - } - } - -} -//----------------------------------------------- -void CanCtrlPltfCOb3::requestMotorTorque() -{ - m_Mutex.lock(); - - for(unsigned int i = 0; i < m_vpMotor.size(); i++) - { - m_vpMotor[i]->requestMotorTorque(); - } - - m_Mutex.unlock(); -} - -//----------------------------------------------- -void CanCtrlPltfCOb3::getMotorTorque(int iCanIdent, double* pdTorqueNm) -{ - // init default outputs - *pdTorqueNm = 0; - - for(unsigned int i = 0; i < m_vpMotor.size(); i++) - { - // check if Identifier fits to availlable hardware - if(iCanIdent == m_viMotorID[i]) - { - m_vpMotor[i]->getMotorTorque(pdTorqueNm); - } - } - -} -//----------------------------------------------- -void CanCtrlPltfCOb3::setMotorTorque(int iCanIdent, double dTorqueNm) -{ - m_Mutex.lock(); - - for(unsigned int i = 0; i < m_vpMotor.size(); i++) - { - // check if Identifier fits to availlable hardware - if(iCanIdent == m_viMotorID[i]) - { - m_vpMotor[i]->setMotorTorque(dTorqueNm); - } - } - - m_Mutex.unlock(); -} - -//----------------------------------------------- -// Read out Elmo-Recorder via CAN (cpc-pk) -//----------------------------------------------- - -//----------------------------------------------- -int CanCtrlPltfCOb3::ElmoRecordings(int iFlag, int iParam, std::string sString) { - int tempRet = 0; - int bRet = 0; - - switch(iFlag) { - case 0: //Flag = 0 means reset recorder and configure it - for(unsigned int i = 0; i < m_vpMotor.size(); i++) { - m_vpMotor[i]->setRecorder(0, iParam); //Configure Elmo Recorder with RecordingGap and start immediately - } - return 0; - - case 1: //Flag = 1 means start readout process, mustn't be called too early (while Rec is in process..) - for(unsigned int i = 0; i < m_vpMotor.size(); i++) { - if((tempRet = m_vpMotor[i]->setRecorder(1, iParam, sString)) > bRet) { - bRet = tempRet; //Query Readout of Index to Log Directory - } - } - return bRet; - - case 99: - for(unsigned int i = 0; i < m_vpMotor.size(); i++) { - m_vpMotor[i]->setRecorder(99, 0); //Stop any ongoing SDO transfer and clear corresponding data. - } - return 0; - - case 100: - for(unsigned int i = 0; i < m_vpMotor.size(); i++) { - bRet += m_vpMotor[i]->setRecorder(2, 0); //Request state of transmission - } - return bRet; - - default: - return -1; - } -} diff --git a/cob_base_drive_chain/package.xml b/cob_base_drive_chain/package.xml deleted file mode 100644 index fcc671f0a..000000000 --- a/cob_base_drive_chain/package.xml +++ /dev/null @@ -1,29 +0,0 @@ - - cob_base_drive_chain - 0.7.16 - This package contains classes that are able to control the platform of the Care-O-Bot. This means to establish a CAN communication to drive and steering motors of the platform and later send motion commands and receive motor information. - - Apache 2.0 - - http://ros.org/wiki/cob_base_drive_chain - - - Matthias Gruhler - Christian Connette - - catkin - - message_generation - message_runtime - - cob_canopen_motor - cob_generic_can - cob_utilities - control_msgs - diagnostic_msgs - roscpp - sensor_msgs - std_msgs - std_srvs - - diff --git a/cob_base_drive_chain/ros/src/cob_base_drive_chain.cpp b/cob_base_drive_chain/ros/src/cob_base_drive_chain.cpp deleted file mode 100644 index ec19fcdfc..000000000 --- a/cob_base_drive_chain/ros/src/cob_base_drive_chain.cpp +++ /dev/null @@ -1,879 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -//################## -//#### includes #### - -// standard includes -//-- - -// ROS includes -#include - -// ROS message includes -#include -#include -#include -#include -#include -#include - -// ROS service includes -#include -#include -#include - - -// external includes -#include -#include -#include - -//#################### -//#### node class #### -/** -* This node provides control of the care-o-bot platform drives to the ROS-"network". For this purpose it offers several services and publishes data on different topics. -*/ -class NodeClass -{ - public: - // create a handle for this node, initialize node - ros::NodeHandle n; - - // topics to publish - /** - * On this topic "JointState" of type sensor_msgs::JointState the node publishes joint states - */ - ros::Publisher topicPub_JointState; - - ros::Publisher topicPub_ControllerState; - ros::Publisher topicPub_DiagnosticGlobal_; - - - /** - * On this topic "Diagnostic" of type diagnostic_msgs::DiagnosticStatus the node publishes states and error information about the platform. - */ - ros::Publisher topicPub_Diagnostic; - - /** - * Timer to publish global diagnostic messages - */ - ros::Timer glDiagnostics_timer; - - // topics to subscribe, callback is called for new messages arriving - /** - * The node subscribes to the topic "JointStateCmd" and performs the requested motor commands - */ - ros::Subscriber topicSub_JointStateCmd; - - // service servers - /** - * Service requests std_srvs::Trigger and initializes platform and motors - */ - ros::ServiceServer srvServer_Init; - - /** - * Service requests std_srvs::Trigger and resets platform and motors - */ - ros::ServiceServer srvServer_Recover; - - /** - * Service requests std_srvs::Trigger and shuts down platform and motors - */ - ros::ServiceServer srvServer_Shutdown; - - ros::ServiceServer srvServer_SetMotionType; - - /** - * Service requests cob_base_drive_chain::ElmoRecorderSetup. It is used to configure the Elmo Recorder to record predefined sources. - * Parameters are: - * int64 recordinggap #Specify every which time quantum (4*90usec) a new data point (of 1024 points in total) is recorded. the recording process starts immediately. - */ - ros::ServiceServer srvServer_ElmoRecorderConfig; - - /** - * Service requests cob_base_drive_chain::ElmoRecorderGet. It is used to start the read-out process of previously recorded data by the Elmo Recorder. - * Parameters are: - * int64 subindex - * #As Subindex, set the recorded source you want to read out: - * #1: Main Speed - * #2: Main Position - * #10: ActiveCurrent - * #16: Speed Command - * - * string fileprefix - * #Enter the path+file-prefix for the logfile (of an existing directory!) - * #The file-prefix is extended with _MotorNumber_RecordedSource.log - */ - ros::ServiceServer srvServer_ElmoRecorderReadout; - - - // global variables - // generate can-node handle -#ifdef __SIM__ - ros::Publisher br_steer_pub; - ros::Publisher bl_steer_pub; - ros::Publisher fr_steer_pub; - ros::Publisher fl_steer_pub; - ros::Publisher br_caster_pub; - ros::Publisher bl_caster_pub; - ros::Publisher fr_caster_pub; - ros::Publisher fl_caster_pub; - - std::vector m_gazeboPos; - std::vector m_gazeboVel; - ros::Time m_gazeboStamp; - - ros::Subscriber topicSub_GazeboJointStates; -#else - CanCtrlPltfCOb3 *m_CanCtrlPltf; -#endif - bool m_bisInitialized; - int m_iNumMotors; - int m_iNumDrives; - - struct ParamType - { - double dMaxDriveRateRadpS; - double dMaxSteerRateRadpS; - - std::vector vdWheelNtrlPosRad; - }; - ParamType m_Param; - - std::string sIniDirectory; - bool m_bPubEffort; - bool m_bReadoutElmo; - - // Constructor - NodeClass() - { - /// Parameters are set within the launch file - // Read number of drives from iniFile and pass IniDirectory to CobPlatfCtrl. - if (n.hasParam("IniDirectory")) - { - n.getParam("IniDirectory", sIniDirectory); - ROS_INFO("IniDirectory loaded from Parameter-Server is: %s", sIniDirectory.c_str()); - } - else - { - sIniDirectory = "Platform/IniFiles/"; - ROS_WARN("IniDirectory not found on Parameter-Server, using default value: %s", sIniDirectory.c_str()); - } - - n.param("PublishEffort", m_bPubEffort, false); - if(m_bPubEffort) ROS_INFO("You have choosen to publish effort of motors, that charges capacity of CAN"); - - - IniFile iniFile; - iniFile.SetFileName(sIniDirectory + "Platform.ini", "PltfHardwareCoB3.h"); - - // get max Joint-Velocities (in rad/s) for Steer- and Drive-Joint - iniFile.GetKeyInt("Config", "NumberOfMotors", &m_iNumMotors, true); - iniFile.GetKeyInt("Config", "NumberOfWheels", &m_iNumDrives, true); - if(m_iNumMotors < 2 || m_iNumMotors > 8) { - m_iNumMotors = 8; - m_iNumDrives = 4; - } - -#ifdef __SIM__ - bl_caster_pub = n.advertise("/base_bl_caster_r_wheel_controller/command", 1); - br_caster_pub = n.advertise("/base_br_caster_r_wheel_controller/command", 1); - fl_caster_pub = n.advertise("/base_fl_caster_r_wheel_controller/command", 1); - fr_caster_pub = n.advertise("/base_fr_caster_r_wheel_controller/command", 1); - bl_steer_pub = n.advertise("/base_bl_caster_rotation_controller/command", 1); - br_steer_pub = n.advertise("/base_br_caster_rotation_controller/command", 1); - fl_steer_pub = n.advertise("/base_fl_caster_rotation_controller/command", 1); - fr_steer_pub = n.advertise("/base_fr_caster_rotation_controller/command", 1); - - topicSub_GazeboJointStates = n.subscribe("/joint_states", 1, &NodeClass::gazebo_joint_states_Callback, this); - - m_gazeboPos.resize(m_iNumMotors); - m_gazeboVel.resize(m_iNumMotors); -#else - topicPub_JointState = n.advertise("/joint_states", 1); - m_CanCtrlPltf = new CanCtrlPltfCOb3(sIniDirectory); -#endif - - // implementation of topics - // published topics - topicPub_ControllerState = n.advertise("state", 1); - topicPub_DiagnosticGlobal_ = n.advertise("/diagnostics", 1); - - topicPub_Diagnostic = n.advertise("diagnostic", 1); - // subscribed topics - topicSub_JointStateCmd = n.subscribe("joint_command", 1, &NodeClass::topicCallback_JointStateCmd, this); - - // implementation of service servers - srvServer_Init = n.advertiseService("init", &NodeClass::srvCallback_Init, this); - srvServer_ElmoRecorderConfig = n.advertiseService("ElmoRecorderConfig", &NodeClass::srvCallback_ElmoRecorderConfig, this); - srvServer_ElmoRecorderReadout = n.advertiseService("ElmoRecorderReadout", &NodeClass::srvCallback_ElmoRecorderReadout, this); - m_bReadoutElmo = false; - - srvServer_Recover = n.advertiseService("recover", &NodeClass::srvCallback_Recover, this); - srvServer_Shutdown = n.advertiseService("shutdown", &NodeClass::srvCallback_Shutdown, this); - - //Timer for publishing global diagnostics - glDiagnostics_timer = n.createTimer(ros::Duration(1), &NodeClass::publish_globalDiagnostics, this); - - // initialization of variables -#ifdef __SIM__ - m_bisInitialized = initDrives(); -#else - m_bisInitialized = false; -#endif - - } - - // Destructor - ~NodeClass() - { -#ifdef __SIM__ - -#else - m_CanCtrlPltf->shutdownPltf(); -#endif - } - - // topic callback functions - // function will be called when a new message arrives on a topic - void topicCallback_JointStateCmd(const control_msgs::JointTrajectoryControllerState::ConstPtr& msg) - { - ROS_DEBUG("Topic Callback joint_command"); - // only process cmds when system is initialized - if(m_bisInitialized == true) - { - ROS_DEBUG("Topic Callback joint_command - Sending Commands to drives (initialized)"); - sensor_msgs::JointState JointStateCmd; - JointStateCmd.position.resize(m_iNumMotors); - JointStateCmd.velocity.resize(m_iNumMotors); - JointStateCmd.effort.resize(m_iNumMotors); - - for(unsigned int i = 0; i < msg->joint_names.size(); i++) - { - // associate inputs to according steer and drive joints - // ToDo: specify this globally (Prms-File or config-File or via msg-def.) - // check if velocities lie inside allowed boundaries - - //DRIVES - if(msg->joint_names[i] == "fl_caster_r_wheel_joint") - { - JointStateCmd.position[0] = msg->desired.positions[i]; - JointStateCmd.velocity[0] = msg->desired.velocities[i]; - //JointStateCmd.effort[0] = msg->effort[i]; - } - else if(m_iNumDrives>=2 && msg->joint_names[i] == "bl_caster_r_wheel_joint") - { - JointStateCmd.position[2] = msg->desired.positions[i]; - JointStateCmd.velocity[2] = msg->desired.velocities[i]; - //JointStateCmd.effort[2] = msg->effort[i]; - } - else if(m_iNumDrives>=3 && msg->joint_names[i] == "br_caster_r_wheel_joint") - { - JointStateCmd.position[4] = msg->desired.positions[i]; - JointStateCmd.velocity[4] = msg->desired.velocities[i]; - //JointStateCmd.effort[4] = msg->effort[i]; - } - else if(m_iNumDrives>=4 && msg->joint_names[i] == "fr_caster_r_wheel_joint") - { - JointStateCmd.position[6] = msg->desired.positions[i]; - JointStateCmd.velocity[6] = msg->desired.velocities[i]; - //JointStateCmd.effort[6] = msg->effort[i]; - } - //STEERS - else if(msg->joint_names[i] == "fl_caster_rotation_joint") - { - JointStateCmd.position[1] = msg->desired.positions[i]; - JointStateCmd.velocity[1] = msg->desired.velocities[i]; - //JointStateCmd.effort[1] = msg->effort[i]; - } - else if(m_iNumDrives>=2 && msg->joint_names[i] == "bl_caster_rotation_joint") - { - JointStateCmd.position[3] = msg->desired.positions[i]; - JointStateCmd.velocity[3] = msg->desired.velocities[i]; - //JointStateCmd.effort[3] = msg->effort[i]; - } - else if(m_iNumDrives>=3 && msg->joint_names[i] == "br_caster_rotation_joint") - { - JointStateCmd.position[5] = msg->desired.positions[i]; - JointStateCmd.velocity[5] = msg->desired.velocities[i]; - //JointStateCmd.effort[5] = msg->effort[i]; - } - else if(m_iNumDrives>=4 && msg->joint_names[i] == "fr_caster_rotation_joint") - { - JointStateCmd.position[7] = msg->desired.positions[i]; - JointStateCmd.velocity[7] = msg->desired.velocities[i]; - //JointStateCmd.effort[7] = msg->effort[i]; - } - else - { - ROS_ERROR("Unkown joint name %s", (msg->joint_names[i]).c_str()); - } - } - - - // check if velocities lie inside allowed boundaries - for(int i = 0; i < m_iNumMotors; i++) - { -#ifdef __SIM__ -#else - // for steering motors - if( i == 1 || i == 3 || i == 5 || i == 7) // ToDo: specify this via the config-files - { - if (JointStateCmd.velocity[i] > m_Param.dMaxSteerRateRadpS) - { - JointStateCmd.velocity[i] = m_Param.dMaxSteerRateRadpS; - } - if (JointStateCmd.velocity[i] < -m_Param.dMaxSteerRateRadpS) - { - JointStateCmd.velocity[i] = -m_Param.dMaxSteerRateRadpS; - } - } - // for driving motors - else - { - if (JointStateCmd.velocity[i] > m_Param.dMaxDriveRateRadpS) - { - JointStateCmd.velocity[i] = m_Param.dMaxDriveRateRadpS; - } - if (JointStateCmd.velocity[i] < -m_Param.dMaxDriveRateRadpS) - { - JointStateCmd.velocity[i] = -m_Param.dMaxDriveRateRadpS; - } - } -#endif -#ifdef __SIM__ - ROS_DEBUG("Send velocity data to gazebo"); - std_msgs::Float64 fl; - fl.data = JointStateCmd.velocity[i]; - if(msg->joint_names[i] == "fl_caster_r_wheel_joint") - fl_caster_pub.publish(fl); - if(msg->joint_names[i] == "fr_caster_r_wheel_joint") - fr_caster_pub.publish(fl); - if(msg->joint_names[i] == "bl_caster_r_wheel_joint") - bl_caster_pub.publish(fl); - if(msg->joint_names[i] == "br_caster_r_wheel_joint") - br_caster_pub.publish(fl); - - if(msg->joint_names[i] == "fl_caster_rotation_joint") - fl_steer_pub.publish(fl); - if(msg->joint_names[i] == "fr_caster_rotation_joint") - fr_steer_pub.publish(fl); - if(msg->joint_names[i] == "bl_caster_rotation_joint") - bl_steer_pub.publish(fl); - if(msg->joint_names[i] == "br_caster_rotation_joint") - br_steer_pub.publish(fl); - ROS_DEBUG("Successfully sent velicities to gazebo"); -#else - ROS_DEBUG("Send velocity data to drives"); - m_CanCtrlPltf->setVelGearRadS(i, JointStateCmd.velocity[i]); - ROS_DEBUG("Successfully sent velicities to drives"); -#endif - } - -#ifdef __SIM__ - -#else - if(m_bPubEffort) { - m_CanCtrlPltf->requestMotorTorque(); - } -#endif - } - } - - // service callback functions - // function will be called when a service is querried - - // Init Can-Configuration - bool srvCallback_Init(std_srvs::Trigger::Request &req, - std_srvs::Trigger::Response &res ) - { - ROS_DEBUG("Service callback init"); - if(m_bisInitialized == false) - { - m_bisInitialized = initDrives(); - //ROS_INFO("...initializing can-nodes..."); - //m_bisInitialized = m_CanCtrlPltf->initPltf(); - res.success = m_bisInitialized; - if(m_bisInitialized) - { - ROS_INFO("base initialized"); - } - else - { - res.message = "initialization of base failed"; - ROS_ERROR("Initializing base failed"); - } - } - else - { - ROS_WARN("...base already initialized..."); - res.success = true; - res.message = "platform already initialized"; - } - return true; - } - - bool srvCallback_ElmoRecorderConfig(cob_base_drive_chain::ElmoRecorderConfig::Request &req, - cob_base_drive_chain::ElmoRecorderConfig::Response &res ){ - if(m_bisInitialized) - { -#ifdef __SIM__ - res.success = true; -#else - m_CanCtrlPltf->evalCanBuffer(); - res.success = m_CanCtrlPltf->ElmoRecordings(0, req.recordinggap, ""); -#endif - res.message = "Successfully configured all motors for instant record"; - } - - return true; - } - - bool srvCallback_ElmoRecorderReadout(cob_base_drive_chain::ElmoRecorderReadout::Request &req, - cob_base_drive_chain::ElmoRecorderReadout::Response &res ){ - if(m_bisInitialized) { -#ifdef __SIM__ - res.success = true; -#else - m_CanCtrlPltf->evalCanBuffer(); - res.success = m_CanCtrlPltf->ElmoRecordings(1, req.subindex, req.fileprefix); -#endif - if(res.success == 0) { - res.message = "Successfully requested reading out of Recorded data"; - m_bReadoutElmo = true; - ROS_WARN("CPU consuming evalCanBuffer used for ElmoReadout activated"); - } else if(res.success == 1) res.message = "Recorder hasn't been configured well yet"; - else if(res.success == 2) res.message = "A previous transmission is still in progress"; - } - - return true; - } - - - - // reset Can-Configuration - bool srvCallback_Recover(std_srvs::Trigger::Request &req, - std_srvs::Trigger::Response &res ) - { - if(m_bisInitialized) - { - ROS_DEBUG("Service callback recover"); -#ifdef __SIM__ - res.success = true; -#else - res.success = m_CanCtrlPltf->resetPltf(); -#endif - if (res.success) { - ROS_INFO("base resetted"); - } else { - res.message = "reset of base failed"; - ROS_WARN("Resetting base failed"); - } - } - else - { - ROS_WARN("Base not yet initialized."); - res.success = false; - res.message = "Base not yet initialized."; - } - return true; - } - - // shutdown Drivers and Can-Node - bool srvCallback_Shutdown(std_srvs::Trigger::Request &req, - std_srvs::Trigger::Response &res ) - { - ROS_DEBUG("Service callback shutdown"); -#ifdef __SIM__ - res.success = true; -#else - res.success = m_CanCtrlPltf->shutdownPltf(); -#endif - if (res.success) - ROS_INFO("Drives shut down"); - else - ROS_INFO("Shutdown of Drives FAILED"); - - return true; - } - - //publish JointStates cyclical instead of service callback - bool publish_JointStates() - { - // init local variables - int j, k; - bool bIsError; - std::vector vdAngGearRad, vdVelGearRad, vdEffortGearNM; - - // set default values - vdAngGearRad.resize(m_iNumMotors, 0); - vdVelGearRad.resize(m_iNumMotors, 0); - vdEffortGearNM.resize(m_iNumMotors, 0); - - // create temporary (local) JointState/Diagnostics Data-Container - sensor_msgs::JointState jointstate; - diagnostic_msgs::DiagnosticStatus diagnostics; - control_msgs::JointTrajectoryControllerState controller_state; - - // get time stamp for header -#ifdef __SIM__ - jointstate.header.stamp = m_gazeboStamp; - controller_state.header.stamp = m_gazeboStamp; -#else - jointstate.header.stamp = ros::Time::now(); - controller_state.header.stamp = jointstate.header.stamp; -#endif - - // assign right size to JointState - //jointstate.name.resize(m_iNumMotors); - jointstate.position.assign(m_iNumMotors, 0.0); - jointstate.velocity.assign(m_iNumMotors, 0.0); - jointstate.effort.assign(m_iNumMotors, 0.0); - - if(m_bisInitialized == false) - { - // as long as system is not initialized - bIsError = false; - - j = 0; - k = 0; - - // set data to jointstate - for(int i = 0; ievalCanBuffer(); - ROS_DEBUG("Successfully read CAN-Buffer"); -#endif - j = 0; - k = 0; - for(int i = 0; igetGearPosVelRadS(i, &vdAngGearRad[i], &vdVelGearRad[i]); -#endif - - //Get motor torque - if(m_bPubEffort) { - for(int i=0; igetMotorTorque(i, &vdEffortGearNM[i]); //(int iCanIdent, double* pdTorqueNm) -#endif - } - } - - // if a steering motor was read -> correct for offset - if( i == 1 || i == 3 || i == 5 || i == 7) // ToDo: specify this via the config-files - { - // correct for initial offset of steering angle (arbitrary homing position) - vdAngGearRad[i] += m_Param.vdWheelNtrlPosRad[j]; - MathSup::normalizePi(vdAngGearRad[i]); - j = j+1; - } - } - - // set data to jointstate - for(int i = 0; iisPltfError(); -#endif - } - - // set data to diagnostics - if(bIsError) - { - diagnostics.level = 2; - diagnostics.name = "drive-chain can node"; - diagnostics.message = "one or more drives are in Error mode"; - } - else - { - if (m_bisInitialized) - { - diagnostics.level = 0; - diagnostics.name = "drive-chain can node"; - diagnostics.message = "drives operating normal"; - } - else - { - diagnostics.level = 1; - diagnostics.name = "drive-chain can node"; - diagnostics.message = "drives are initializing"; - } - } - - // publish diagnostic message - topicPub_Diagnostic.publish(diagnostics); - ROS_DEBUG("published new drive-chain configuration (JointState message)"); - - - return true; - } - - void publish_globalDiagnostics(const ros::TimerEvent& event) - { - //publish global diagnostic messages - diagnostic_msgs::DiagnosticArray diagnostics_gl; - diagnostics_gl.header.stamp = ros::Time::now(); - diagnostics_gl.status.resize(1); - // set data to diagnostics -#ifdef __SIM__ - if (false) -#else - if(m_bisInitialized && m_CanCtrlPltf->isPltfError()) -#endif - - { - diagnostics_gl.status[0].level = 2; - diagnostics_gl.status[0].name = ros::this_node::getName(); - diagnostics_gl.status[0].message = "Base not initialized or in error"; - } - else - { - if (m_bisInitialized) - { - diagnostics_gl.status[0].level = 0; - diagnostics_gl.status[0].name = ros::this_node::getName(); - diagnostics_gl.status[0].message = "base_drive_chain initialized and running"; - } - else - { - diagnostics_gl.status[0].level = 1; - diagnostics_gl.status[0].name = ros::this_node::getName(); - diagnostics_gl.status[0].message = "base_drive_chain not initialized"; - } - } - // publish diagnostic message - topicPub_DiagnosticGlobal_.publish(diagnostics_gl); - } - - // other function declarations - bool initDrives(); - -#ifdef __SIM__ - void gazebo_joint_states_Callback(const sensor_msgs::JointState::ConstPtr& msg) { - for (unsigned int i=0; iname.size(); i++) { - //Drives - if(msg->name[i] == "fl_caster_r_wheel_joint") { - m_gazeboPos[0] = msg->position[i]; - m_gazeboVel[0] = msg->velocity[i]; - m_gazeboStamp = msg->header.stamp; - } - else if(msg->name[i] == "bl_caster_r_wheel_joint") { - m_gazeboPos[2] = msg->position[i]; - m_gazeboVel[2] = msg->velocity[i]; - } - else if(msg->name[i] == "br_caster_r_wheel_joint") { - m_gazeboPos[4] = msg->position[i]; - m_gazeboVel[4] = msg->velocity[i]; - } - else if(msg->name[i] == "fr_caster_r_wheel_joint") { - m_gazeboPos[6] = msg->position[i]; - m_gazeboVel[6] = msg->velocity[i]; - } - - //Steers - else if(msg->name[i] == "fl_caster_rotation_joint") { - m_gazeboPos[1] = msg->position[i]; - m_gazeboVel[1] = msg->velocity[i]; - } - else if(msg->name[i] == "bl_caster_rotation_joint") { - m_gazeboPos[3] = msg->position[i]; - m_gazeboVel[3] = msg->velocity[i]; - } - else if(msg->name[i] == "br_caster_rotation_joint") { - m_gazeboPos[5] = msg->position[i]; - m_gazeboVel[5] = msg->velocity[i]; - } - else if(msg->name[i] == "fr_caster_rotation_joint") { - m_gazeboPos[7] = msg->position[i]; - m_gazeboVel[7] = msg->velocity[i]; - } - } - } -#else - -#endif -}; - -//####################### -//#### main programm #### -int main(int argc, char** argv) -{ - // initialize ROS, spezify name of node - ros::init(argc, argv, "base_drive_chain"); - - NodeClass nodeClass; - - // specify looprate of control-cycle - ros::Rate loop_rate(100); // Hz - - while(nodeClass.n.ok()) - { -#ifdef __SIM__ - -#else - //Read-out of CAN buffer is only necessary during read-out of Elmo Recorder - if( nodeClass.m_bReadoutElmo ) - { - if(nodeClass.m_bisInitialized) nodeClass.m_CanCtrlPltf->evalCanBuffer(); - if(nodeClass.m_CanCtrlPltf->ElmoRecordings(100, 0, "") == 0) - { - nodeClass.m_bReadoutElmo = false; - ROS_INFO("CPU consuming evalCanBuffer used for ElmoReadout deactivated"); - } - } -#endif - - nodeClass.publish_JointStates(); - - loop_rate.sleep(); - ros::spinOnce(); - } - - return 0; -} - -//################################## -//#### function implementations #### -bool NodeClass::initDrives() -{ - ROS_INFO("Initializing Base Drive Chain"); - - // init member vectors - m_Param.vdWheelNtrlPosRad.assign((m_iNumDrives),0); -// m_Param.vdWheelNtrlPosRad.assign(4,0); - // ToDo: replace the following steps by ROS configuration files - // create Inifile class and set target inifile (from which data shall be read) - IniFile iniFile; - - //n.param("PltfIniLoc", sIniFileName, "Platform/IniFiles/Platform.ini"); - iniFile.SetFileName(sIniDirectory + "Platform.ini", "PltfHardwareCoB3.h"); - - // get max Joint-Velocities (in rad/s) for Steer- and Drive-Joint - iniFile.GetKeyDouble("DrivePrms", "MaxDriveRate", &m_Param.dMaxDriveRateRadpS, true); - iniFile.GetKeyDouble("DrivePrms", "MaxSteerRate", &m_Param.dMaxSteerRateRadpS, true); - -#ifdef __SIM__ - // get Offset from Zero-Position of Steering - if(m_iNumDrives >=1) - m_Param.vdWheelNtrlPosRad[0] = 0.0f; - if(m_iNumDrives >=2) - m_Param.vdWheelNtrlPosRad[1] = 0.0f; - if(m_iNumDrives >=3) - m_Param.vdWheelNtrlPosRad[2] = 0.0f; - if(m_iNumDrives >=4) - m_Param.vdWheelNtrlPosRad[3] = 0.0f; -#else - // get Offset from Zero-Position of Steering - if(m_iNumDrives >=1) - iniFile.GetKeyDouble("DrivePrms", "Wheel1NeutralPosition", &m_Param.vdWheelNtrlPosRad[0], true); - if(m_iNumDrives >=2) - iniFile.GetKeyDouble("DrivePrms", "Wheel2NeutralPosition", &m_Param.vdWheelNtrlPosRad[1], true); - if(m_iNumDrives >=3) - iniFile.GetKeyDouble("DrivePrms", "Wheel3NeutralPosition", &m_Param.vdWheelNtrlPosRad[2], true); - if(m_iNumDrives >=4) - iniFile.GetKeyDouble("DrivePrms", "Wheel4NeutralPosition", &m_Param.vdWheelNtrlPosRad[3], true); - - //Convert Degree-Value from ini-File into Radian: - for(int i=0; iinitPltf(); -#endif - // debug log - ROS_INFO("Initializing done"); - - - return bTemp1; -} diff --git a/cob_base_drive_chain/srv/ElmoRecorderConfig.srv b/cob_base_drive_chain/srv/ElmoRecorderConfig.srv deleted file mode 100644 index 6d60ae950..000000000 --- a/cob_base_drive_chain/srv/ElmoRecorderConfig.srv +++ /dev/null @@ -1,9 +0,0 @@ -#This service is used to configure the Elmo Recorder. - -#Specify every which time quantum (4*90usec) a new data point (of 1024 points in total) is recorded. the recording process starts immediately. -int64 recordinggap - ---- -#You get a success code, that will be 0 -int64 success -string message diff --git a/cob_base_drive_chain/srv/ElmoRecorderReadout.srv b/cob_base_drive_chain/srv/ElmoRecorderReadout.srv deleted file mode 100644 index 73216a6f4..000000000 --- a/cob_base_drive_chain/srv/ElmoRecorderReadout.srv +++ /dev/null @@ -1,24 +0,0 @@ -#This service is used to start the read-out process of previously recorded data by the Elmo Recorder in Harmonica drives. -# - -#As Subindex, set the recorded source you want to read out: -#1: Main Speed -#2: Main Position -#10: ActiveCurrent -#16: Speed Command -int64 subindex - -#Enter the path+file-prefix for the logfile (of an existing directory!) -#The file-prefix is extended with _MotorNumber_RecordedSource.log -string fileprefix - ---- - -#As return you get a succes-code and an according message - -#0: Successfully requested reading out of Recorded data -#1: Recorder hasn't been configured yet -#2: A previous transmission is still in progress -int64 success - -string message diff --git a/cob_driver/package.xml b/cob_driver/package.xml index cbd83795c..e85039c1e 100644 --- a/cob_driver/package.xml +++ b/cob_driver/package.xml @@ -13,7 +13,6 @@ catkin - cob_base_drive_chain cob_bms_driver cob_canopen_motor cob_generic_can From 20ec38f4f461dbabb6998e9d66f56ec1272ffe71 Mon Sep 17 00:00:00 2001 From: fmessmer Date: Wed, 20 Mar 2024 16:55:40 +0100 Subject: [PATCH 08/14] remove cob_canopen_motor --- cob_canopen_motor/CHANGELOG.rst | 224 --- cob_canopen_motor/CMakeLists.txt | 26 - .../cob_canopen_motor/CanDriveHarmonica.h | 464 ------ .../include/cob_canopen_motor/CanDriveItf.h | 246 --- .../include/cob_canopen_motor/DriveParam.h | 388 ----- .../include/cob_canopen_motor/ElmoRecorder.h | 131 -- .../include/cob_canopen_motor/SDOSegmented.h | 99 -- .../common/src/CanDriveHarmonica.cpp | 1404 ----------------- cob_canopen_motor/common/src/ElmoRecorder.cpp | 292 ---- cob_canopen_motor/package.xml | 20 - cob_driver/package.xml | 1 - 11 files changed, 3295 deletions(-) delete mode 100644 cob_canopen_motor/CHANGELOG.rst delete mode 100644 cob_canopen_motor/CMakeLists.txt delete mode 100644 cob_canopen_motor/common/include/cob_canopen_motor/CanDriveHarmonica.h delete mode 100644 cob_canopen_motor/common/include/cob_canopen_motor/CanDriveItf.h delete mode 100644 cob_canopen_motor/common/include/cob_canopen_motor/DriveParam.h delete mode 100644 cob_canopen_motor/common/include/cob_canopen_motor/ElmoRecorder.h delete mode 100644 cob_canopen_motor/common/include/cob_canopen_motor/SDOSegmented.h delete mode 100644 cob_canopen_motor/common/src/CanDriveHarmonica.cpp delete mode 100644 cob_canopen_motor/common/src/ElmoRecorder.cpp delete mode 100644 cob_canopen_motor/package.xml diff --git a/cob_canopen_motor/CHANGELOG.rst b/cob_canopen_motor/CHANGELOG.rst deleted file mode 100644 index 121c68822..000000000 --- a/cob_canopen_motor/CHANGELOG.rst +++ /dev/null @@ -1,224 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package cob_canopen_motor -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.7.16 (2024-02-20) -------------------- - -0.7.15 (2023-11-06) -------------------- - -0.7.14 (2022-11-17) -------------------- - -0.7.13 (2022-07-29) -------------------- - -0.7.12 (2022-03-15) -------------------- - -0.7.11 (2022-01-12) -------------------- - -0.7.10 (2021-12-23) -------------------- - -0.7.9 (2021-11-26) ------------------- - -0.7.8 (2021-10-19) ------------------- - -0.7.7 (2021-08-02) ------------------- - -0.7.6 (2021-05-10) ------------------- - -0.7.5 (2021-04-06) ------------------- - -0.7.4 (2020-10-14) ------------------- -* Merge pull request `#417 `_ from fmessmer/test_noetic - test noetic -* Bump CMake version to avoid CMP0048 warning -* Contributors: Felix Messmer, fmessmer - -0.7.3 (2020-03-18) ------------------- - -0.7.2 (2020-03-18) ------------------- - -0.7.1 (2019-11-07) ------------------- - -0.7.0 (2019-08-06) ------------------- -* Merge pull request `#396 `_ from HannesBachter/indigo_dev - 0.6.15 -* Contributors: Felix Messmer - -0.6.15 (2019-07-17) -------------------- - -0.6.14 (2019-06-07) -------------------- - -0.6.13 (2019-03-14) -------------------- - -0.6.12 (2018-07-21) -------------------- - -0.6.11 (2018-01-07) -------------------- -* Merge remote-tracking branch 'origin/indigo_release_candidate' into indigo_dev -* Merge pull request `#341 `_ from ipa-fxm/APACHE_license - use license apache 2.0 -* use license apache 2.0 -* Contributors: Felix Messmer, ipa-fxm, ipa-uhr-mk - -0.6.10 (2017-07-24) -------------------- - -0.6.9 (2017-07-18) ------------------- -* manually fix changelog -* Contributors: ipa-fxm - -0.6.8 (2016-10-10) ------------------- - -0.6.7 (2016-04-02) ------------------- - -0.6.6 (2016-04-01) ------------------- - -0.6.5 (2015-08-31) ------------------- - -0.6.4 (2015-08-25) ------------------- -* explicit dependency to boost -* remove obsolete autogenerated mainpage.dox files -* remove trailing whitespaces -* migrate to package format 2 -* sort dependencies -* critically review dependencies -* Contributors: ipa-fxm - -0.6.3 (2015-06-17) ------------------- - -0.6.2 (2014-12-15) ------------------- - -0.6.1 (2014-09-17) ------------------- - -0.6.0 (2014-09-09) ------------------- - -0.5.7 (2014-08-26) ------------------- -* Merge pull request `#163 `_ from ipa320/hydro_dev - updates from hydro_dev -* 0.5.6 -* update changelog -* Cleaned up cob_driver with reduced deps to compile on indigo -* Contributors: Alexander Bubeck, Florian Weisshardt - -0.5.6 (2014-08-26) ------------------- -* Merge pull request `#163 `_ from ipa320/hydro_dev - updates from hydro_dev -* Cleaned up cob_driver with reduced deps to compile on indigo -* Contributors: Alexander Bubeck, Florian Weisshardt - -0.5.3 (2014-03-31) ------------------- -* install tags -* Contributors: ipa-fxm - -0.5.2 (2014-03-20) ------------------- - -0.5.1 (2014-03-20) ------------------- -* fix gcc version related build errors -* cleaned up CMakeLists and added install directives -* futher include and linkpath modifications -* compiling but still some linker errors -* Second catkinization push -* First catkinization, still need to update some CMakeLists.txt -* can_open_motor: typo in message -* adaptions for cob_head_axis on cob3-3, included some new parameters instead of hard-coded settings -* camera settings added for head -* cleanup in cob_driver -* Commented out one command in CanDriveHarmonica: That makes headaxis work on cob3-2, but disables base on (at least) cob3-1 -* HeadAxis working -* cob_head_axis working -* base_drive_chain: added main loop with evalCanBuffer to enable ElmoRecorderReadout. NEW: evalCanBuffer is only executed, when and until a readout is in process -* merged with cpc-pk: added ctrl for tricycle-kinematic; specification of limit in CanDriveHarmonica can now be specified via Inifile; base_drive_chain can be operated on variable numbers of motors (lesser or equal to eight); variable setting of path to inifile for UndercarriageCtrlGeom; debugged relaysboard - reads Bus now nonblocking -* removed hard coded entry of camera-axis limit switch in CanDriveHarmonica -* Added HomingDigIn in CanCtrl.ini to specify which digital input gives homing signal. It's read out and passed to the CanNode via DriveParam.h -* update documentation and deleted tf broadcaster -* update on robot -* cob_camera_axis can be homed, moved and returns joint position and velocitiy over a ROS topic -* Successfully moving camera_axis, not getting any pos data yet -* cob_camera_axis can be initialized incl. homing -* Still getting -inf values for wheel velocities, tiny extension to drive_identification -* Replaced some spaces with tabs -* removed splashing std::couts -* removed splashing std::couts -* xml description updated -* m_Filename member variable corrected -* Cahnged cob_base_drive_chain -> watchdogs activated again, evalCanBuffer at rate of 50Hz, services continous, a lot Doxygen documentation in all ElmoRecorder related files -* . -* fclose moved -> not crashing with wrong filename -* Added Half Float conversion for current-measurement, completely working -* Interface polishing, added srvs for base_drive_chain -* Merge branch 'review' into cpc-pk -* small merge error fixed -* Tiny merge -* Tiny RecorderReadoutTry changes -* Working ElmoRecorder Eadout, multiple motors, different objects with StatusRegister check -* Improved interface for Readout control, added readoutRecorderTry using StatusRegister -* Corrected floating conversion error, working state -* Successfully uploaded Recorder Data, Watchdogs deactivated -* Trying to get Readout running. Working system state (with debug outputs) -* cleanup in cob_driver -* - -* Corrected SDO command specifier miss-filtering -* small design things, comments. -* Corrected interpretation of Header information in ElmoRecorder.cpp, email from mr. richter on May 18, 2010 -* toggleBit included into SDOSegmeted class -* Removed elmo_test exec -* Immediate start of Recorder -* Built merge successful -* After merging in review branch -* Added EvalCanBuffer to main loop of base_drive_chain. -* Bugfix: Include stdio -* Introduced a statusFlag in segData instead of FinishedTransmission and locked. -* Changed trigger type to immediate -* Frontend in base_drive_chain added, filenames can be passed now -* Improved logging functionality and provided more methods to the flag-based frontend -* ElmoRecorder: Added basic logging functionality -* ElmoRecorder: Data readout and processing -* added classes to implement ESD can-itf; incorporated ESD interface as an option in cob_base_drive_chain-node via CanCtrlPltfCOb3; added windows.h to cob_utilities package -* Updated Can Classes to new file structure; removed some leftovers; corrected comments at the beginning considering association to stacks and packages; moved Mutex.h to Utilities; - Debugged compiler error in cob_base_drive_chain -* ElmoRecorder.cpp: Debug messages and collection&conversion of data items in internal vector -* CanDriveHarmonica: added function finishedSDOTransfer -* little testing node for code-snippets of Elmo, e.g. flaoting point conversion -* ElmoRecorder: Added binary integer to float conversion according to IEEE 754, works -* Some Test-outs for COB test -* ElmoRecorder.h hasn't been added to the index during merge -* fixed little merging issues, successfully compiled base_drive_chain with canopen_motor -* after merging current review -* renamed to cob_ -* merged master -* renamed packages to cob_ convention -* Contributors: Alexander Bubeck, Christian Connette, Matthieu Herrb, Philipp, Richard Bormann, abubeck, cob, cpc, cpc-pk, ipa-cpc, ipa-fmw, ipa-uhr diff --git a/cob_canopen_motor/CMakeLists.txt b/cob_canopen_motor/CMakeLists.txt deleted file mode 100644 index 9d2d24f8a..000000000 --- a/cob_canopen_motor/CMakeLists.txt +++ /dev/null @@ -1,26 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(cob_canopen_motor) - -find_package(catkin REQUIRED COMPONENTS cob_generic_can cob_utilities roscpp) - -catkin_package( - CATKIN_DEPENDS cob_generic_can cob_utilities roscpp - INCLUDE_DIRS common/include - LIBRARIES ${PROJECT_NAME}_harmonica -) - -### BUILD ### -include_directories(common/include ${catkin_INCLUDE_DIRS}) - -add_library(${PROJECT_NAME}_harmonica common/src/CanDriveHarmonica.cpp common/src/ElmoRecorder.cpp) - -### INSTALL ### -install(TARGETS ${PROJECT_NAME}_harmonica - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -install(DIRECTORY common/include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) diff --git a/cob_canopen_motor/common/include/cob_canopen_motor/CanDriveHarmonica.h b/cob_canopen_motor/common/include/cob_canopen_motor/CanDriveHarmonica.h deleted file mode 100644 index 6c62db935..000000000 --- a/cob_canopen_motor/common/include/cob_canopen_motor/CanDriveHarmonica.h +++ /dev/null @@ -1,464 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -#ifndef CANDRIVEHARMONICA_INCLUDEDEF_H -#define CANDRIVEHARMONICA_INCLUDEDEF_H - -//----------------------------------------------- -#include -#include - -#include -#include -//----------------------------------------------- - -/** - * Driver class for the motor drive of type Harmonica. - * \ingroup DriversCanModul - */ -class CanDriveHarmonica : public CanDriveItf -{ -public: - // ------------------------- Types - /** - * Internal parameters. - */ - struct ParamType - { - int iNumRetryOfSend; - int iDivForRequestStatus; - double dCanTimeout; - }; - - /** - * States of the drive. - */ - enum StateHarmonica - { - ST_PRE_INITIALIZED, - ST_OPERATION_ENABLED, - ST_OPERATION_DISABLED, - ST_MOTOR_FAILURE - }; - - /** - * Identifier of the CAN messages. - */ - struct ParamCanOpenType - { - int iTxPDO1; - int iTxPDO2; - int iRxPDO2; - int iTxSDO; - int iRxSDO; - }; - - // ------------------------- Interface - /** - * Sets the CAN interface. - */ - void setCanItf(CanItf* pCanItf){ m_pCanCtrl = pCanItf; } - - /** - * Initializes the driver. - * Call this function once after construction. - */ - bool init(); - - /** - * Check if the driver is already initialized. - * This is necessary if a drive gets switched off during runtime. - * @return true if initialization occured already, false if not. - */ - bool isInitialized() { return m_bIsInitialized; } - - /** - * Enables the motor. - * After calling the drive accepts velocity and position commands. - */ - bool start(); - - /** - * Disables the motor. - * After calling the drive won't accepts velocity and position commands. - */ - bool stop(); - /** - * Resets the drive. - * The drive changes into the state after initializaton. - */ - bool reset(); - - /** - * Shutdown the motor. - */ - bool shutdown(); - - /** - * Disables the brake. - * This function is not implemented for Harmonica, - * because brakes are released upon power on and - * shut only at emergency stop. - */ - bool disableBrake(bool bDisabled); - - /** - * Inits homing procedure. - */ - bool initHoming(); - - /** - * Performs homing procedure. - */ - bool execHoming(); - - /** - * Performs homing procedure - * Drives wheel in neutral Position for Startup. - */ - bool endHoming(); - - /** - * Returns the elapsed time since the last received message. - */ - double getTimeToLastMsg(); - - /** - * Returns the status of the limit switch needed for homing. - * true = limit switch is reached; false = not reached - */ - bool getStatusLimitSwitch(); - - /** - * Starts the watchdog. - * The Harmonica provides watchdog functionality which means the drive stops if the watchdog - * becomes active. To keep the watchdog inactive a heartbeat message has to be sent - * periodically. The update rate is set to 1s. - * The update is done in function setGearVelRadS(). - */ - bool startWatchdog(bool bStarted); - - /** - * Evals a received message. - * Only messages with fitting identifiers are evaluated. - * @param msg message to be evaluated. - */ - bool evalReceivedMsg(CanMsg& msg); - - /** - * Evals received messages in OBJECT mode. - * @todo To be implemented! - */ - bool evalReceivedMsg() { return true; } - - - /** - * Sets required position and veolocity. - * Use this function only in position mode. - */ - void setGearPosVelRadS(double dPosRad, double dVelRadS); - - /** - * Sets the velocity. - * By calling the function the status is requested, too. - */ - void setGearVelRadS(double dVelEncRadS); - - /** - * Sets the motion type drive. - */ - bool setTypeMotion(int iType); - - /** - * Returns the position and the velocity of the drive. - */ - void getGearPosVelRadS(double* pdAngleGearRad, double* pdVelGearRadS); - - /** - * Returns the change of the position and the velocity. - * The given delta position is given since the last call of this function. - */ - void getGearDeltaPosVelRadS(double* pdDeltaAngleGearRad, double* pdVelGearRadS); - - /** - * Returns the current position. - */ - void getGearPosRad(double* pdPosGearRad); - - /** - * Sets the drive parameter. - */ - void setDriveParam(DriveParam driveParam) { m_DriveParam = driveParam; } - - /** - * Returns true if an error has been detected. - */ - bool isError(); - - /** - * Return a bitfield containing information about the pending errors. - */ - unsigned int getError() { return 0; } - - // ------ Dummy implementations for completing the interface - - /** - * Dummy implementation for completing CanDriveItf. - */ - void requestPosVel(); - - /** - * Requests status :) checks whether motor is operational, switched off or in error state. - * If motor is in which error state it checks which error occured - */ - void requestStatus(); - - /** - * Dummy implementation for completing CanDriveItf. - */ - void getStatus(int* piStatus, int* piTempCel) { *piStatus = 0; *piTempCel = 0; } - - - // ------------------------- Interface: Harmonica specific - /** - * Default constructor. - */ - CanDriveHarmonica(); - - /** - * Returns some received values from the drive. - * @deprecated use the other functions instead. - * @param pdPosGearRad position of the drive - * @param pdVelGearRadS velocity of the drive - * @param piTorqueCtrl torque - * @param piStatusCtrl - */ - void getData(double* pdPosGearRad, double* pdVelGearRadS, - int* piTorqueCtrl, int* piStatusCtrl); - - /** - * Sets the CAN identifiers of the drive node. - * @param iTxPDO1 first transmit process data object - * @param iTxPDO2 second transmit process data object - * @param iRxPDO2 second receive process data object - * @param iTxSDO transmit service data object - * @param iRxSDO receive service data object - */ - void setCanOpenParam( int iTxPDO1, int iTxPDO2, int iRxPDO2, int iTxSDO, int iRxSDO); - - /** - * Sends an integer value to the Harmonica using the built in interpreter. - */ - void IntprtSetInt(int iDataLen, char cCmdChar1, char cCmdChar2, int iIndex, int iData); - - /** - * Sends a heartbeat to the CAN-network to keep all listening watchdogs sleeping - */ - void sendHeartbeat(); - - - bool setEMStop() { - std::cout << "The function setEMStop() is not implemented!!!" << std::endl; - return false; - } - - bool resetEMStop() { - std::cout << "The function resetEMStop() is not implemented!!!" << std::endl; - return false; - } - - /** - * Sets MotorTorque in Nm - * By sending this command the status is requested, too - */ - void setMotorTorque(double dTorqueNm); - - /** - * Sends Requests for "active current" to motor via CAN - * To read Motor current perform the following: - * 1.) request motor current - * m_pW1DriveMotor->requestMotorTorque(); - * 2.) evaluate Can buffer to read motor current and decode it - * evalCanBuffer(); - */ - void requestMotorTorque(); - - /** - * Return member variable m_MotorCurrent - * To update this value call requestMotorCurrent at first - */ - void getMotorTorque(double* dTorqueNm); - - /** - * Provides several functions for drive information recording purposes using the built in ElmoRecorder, which allows to record drive information at a high frequency. - * @param iFlag To keep the interface slight, use iParam to command the recorder: - * 0: Configure the Recorder to record the sources Main Speed(1), Main position(2), Active current(10), Speed command(16). With iParam = iRecordingGap you specify every which time quantum (4*90usec) a new data point (of 1024 points in total) is recorded; - * 1: Query Upload of recorded source (1=Main Speed, 2=Main position, 10=Active Current, 16=Speed command) with iParam and log data to file sParam = file prefix. Filename is extended with _MotorNumber_RecordedSource.log - * 2: Request status of ongoing readout process - * 99: Abort and clear current SDO readout process - * @return 0: Success, 1: Recorder hasn't been configured yet, 2: data collection still in progress - * - */ - int setRecorder(int iFlag, int iParam = 0, std::string sParam = "/home/MyLog_"); - - - //-------------------------- - //CanDriveHarmonica specific functions (not from CanDriveItf) - //-------------------------- - /** - * Sends a float value to the Harmonica using the built in interpreter. - */ - void IntprtSetFloat(int iDataLen, char cCmdChar1, char cCmdChar2, int iIndex, float fData); - - /** - * CANopen: Uploads a service data object (device to master). (in expedited transfer mode, means in only one message) - */ - void sendSDOUpload(int iObjIndex, int iObjSub); - - /** - * CANopen: This protocol cancels an active segmented transmission due to the given Error Code - */ - void sendSDOAbort(int iObjIndex, int iObjSubIndex, unsigned int iErrorCode); - - /** - * CANopen: Downloads a service data object (master to device). (in expedited transfer mode, means in only one message) - */ - void sendSDODownload(int iObjIndex, int iObjSub, int iData); - - /** - * CANopen: Evaluates a service data object and gives back object and sub-object ID - */ - void evalSDO(CanMsg& CMsg, int* pIndex, int* pSubindex); - - /** - * Internal use. - */ - int getSDODataInt32(CanMsg& CMsg); - - -protected: - // ------------------------- Parameters - ParamCanOpenType m_ParamCanOpen; - DriveParam m_DriveParam; - bool m_bLimitSwitchEnabled; - ParamType m_Param; - - // ------------------------- Variables - CanItf* m_pCanCtrl; - CanMsg m_CanMsgLast; - - ElmoRecorder* ElmoRec; - - int m_iTypeMotion; - int m_iStatusCtrl; - int m_iTorqueCtrl; - - TimeStamp m_CurrentTime; - TimeStamp m_WatchdogTime; - TimeStamp m_VelCalcTime; - TimeStamp m_FailureStartTime; - TimeStamp m_SendTime; - TimeStamp m_StartTime; - - double m_dAngleGearRadMem; - double m_dVelGearMeasRadS; - double m_dPosGearMeasRad; - - bool m_bLimSwLeft; - bool m_bLimSwRight; - - double m_dOldPos; - - std::string m_sErrorMessage; - - int m_iMotorState; - int m_iNewMotorState; - - int m_iCountRequestDiv; - - bool m_bCurrentLimitOn; - - int m_iNumAttempsRecFail; - - bool m_bOutputOfFailure; - - bool m_bIsInitialized; - - double m_dMotorCurr; - - bool m_bWatchdogActive; - - segData seg_Data; - - - // ------------------------- Member functions - double estimVel(double dPos); - - bool evalStatusRegister(int iStatus); - void evalMotorFailure(int iFailure); - - int m_iPartnerDriveRatio; - int m_iDistSteerAxisToDriveWheelMM; - - bool isBitSet(int iVal, int iNrBit) - { - if( (iVal & (1 << iNrBit)) == 0) - return false; - else - return true; - } - - /** - * CANopen: Answer a data mesage during a segmented SDO transfer including a toggling bit. Current toggle bit is stored in the SDOSegmented container. - * Function is called, when any SDO transfer segment is received (by receivedSDODataSegment) - */ - void sendSDOUploadSegmentConfirmation(bool toggleBit); - - /** - * CANopen: Segment data is stored to the SDOSegmented container. - * Function is called, when a segment during a segmented SDO transfer is received (by evalReceivedMsg). It analyzes the SDO transfer header to see, if the transfer is finished. If it's not finished, sendSDOUploadSegmentConfirmation is called to confirm the receive of the current segment and request the next one. - * -Currently only used for Elmo Recorder read-out - * @see evalReceivedMsg() - * @see sendSDOUploadSegmentConfirmation() - * @see finishedSDOSegmentedTransfer() - */ - int receivedSDODataSegment(CanMsg& msg); - - /** - * CANopen: Evaluates a service data object and gives back object and sub-object ID. - * Function is called, when a SDO initialize transfer segment is received (by evalReceivedMsg) - * @see evalReceivedMsg() - */ - int receivedSDOSegmentedInitiation(CanMsg& msg); - - /** - * CANopen: Function is called by evalReceivedMsg when the current segmented SDO transfer is cancelled with an error code. - * @see evalReceivedMsg() - */ - void receivedSDOTransferAbort(unsigned int iErrorCode); - - /** - * CANopen: Give the collected data of a finished segmented SDO transfer to an appropriate (depending on the current object ID) processing function. - * Function is called by receivedSDODataSegment, when the transfer is finished. - * Currently, only Elmo Recorder data is uploaded segmented and is processed here. - * @see receivedSDODataSegment() - */ - void finishedSDOSegmentedTransfer(); - -}; -//----------------------------------------------- -#endif diff --git a/cob_canopen_motor/common/include/cob_canopen_motor/CanDriveItf.h b/cob_canopen_motor/common/include/cob_canopen_motor/CanDriveItf.h deleted file mode 100644 index 813989f98..000000000 --- a/cob_canopen_motor/common/include/cob_canopen_motor/CanDriveItf.h +++ /dev/null @@ -1,246 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -#ifndef CANDRIVEITF_INCLUDEDEF_H -#define CANDRIVEITF_INCLUDEDEF_H - -//----------------------------------------------- -#include -#include -#include -//----------------------------------------------- - -/** - * Interface for a drive. - * \ingroup DriversCanModul - */ -class CanDriveItf -{ -public: - /** - * Motion type of the controller. - */ - enum MotionType - { - MOTIONTYPE_VELCTRL, - MOTIONTYPE_TORQUECTRL, - MOTIONTYPE_POSCTRL - }; - - /** - * Sets the CAN interface. - */ - virtual void setCanItf(CanItf* pCanItf) = 0; - - /** - * Initializes the driver. - * Call this function once after construction. - */ - virtual bool init() = 0; - - /** - * Check if the driver is already initialized. - * This is necessary if a drive gets switched off during runtime. - * @return true if initialization occured already, false if not. - */ - virtual bool isInitialized() = 0; - - /** - * Enables the motor. - * After calling the drive accepts velocity and position commands. - */ - virtual bool start() = 0; - - /** - * Disables the motor. - * After calling the drive won't accepts velocity and position commands. - */ - virtual bool stop() = 0; - - /** - * Resets the drive. - * The drive changes into the state after initializaton. - */ - virtual bool reset() = 0; - - /** - * Shutdowns the motor. - */ - virtual bool shutdown() = 0; - - /** - * Disables the brake. - * This function is not implemented for Harmonica, - * because brakes are released upon power on and - * shut only at emergency stop. - */ - virtual bool disableBrake(bool bDisabled) = 0; - - /** - * Inits homing procedure. - * Used only in position mode. - */ - virtual bool initHoming() = 0; - - /** - * Performs homing procedure. - * Used only in position mode. - */ - virtual bool execHoming() = 0; - - /** - * Returns the elapsed time since the last received message. - */ - virtual double getTimeToLastMsg() = 0; - - /** - * Returns the status of the limit switch needed for homing. - * true = limit switch is reached; false = not reached - */ - virtual bool getStatusLimitSwitch() = 0; - - /** - * Starts the watchdog. - * The Harmonica provides watchdog functionality which means the drive stops if the watchdog - * becomes active. To keep the watchdog inactive a heartbeat message has to be sent - * periodically. The update rate is set to 1s. - * The update is is done in function setGearVelRadS(). - */ - virtual bool startWatchdog(bool bStarted) = 0; - - /** - * Evals a received message. - * Only messages with fitting identifiers are evaluated. - * @param msg message to be evaluated. - */ - virtual bool evalReceivedMsg(CanMsg& msg) = 0; - - /** - * Evals received messages in OBJECT mode. - * The CAN drives have to implement which identifiers they are interested in. - * @return true on success, false on failure. - */ - virtual bool evalReceivedMsg() = 0; - - /** - * Sets required position and veolocity. - * Use this function only in position mode. - * By calling the function the status is requested, too. - */ - virtual void setGearPosVelRadS(double dPosRad, double dVelRadS) = 0; - - /** - * Sets the velocity. - * By calling the function the status is requested, too. - */ - virtual void setGearVelRadS(double dVelRadS) = 0; - - /** - * Sets the motion type drive. - * The function is not implemented for Harmonica. - * The harmonica drive is configured just for velocity mode. - */ - virtual bool setTypeMotion(int iType) = 0; - - /** - * Returns the position and the velocity of the drive. - */ - virtual void getGearPosVelRadS(double* pdAngleGearRad, double* pdVelGearRadS) = 0; - - /** - * Returns the change of the position and the velocity. - * The given delta position is given since the last call of this function. - */ - virtual void getGearDeltaPosVelRadS(double* pdDeltaAngleGearRad, double* pdVelGearRadS) = 0; - - /** - * Returns the current position. - */ - virtual void getGearPosRad(double* pdPosGearRad) = 0; - - /** - * Sets the drive parameter. - */ - virtual void setDriveParam(DriveParam driveParam) = 0; - - /** - * Returns true if an error has been detected. - */ - virtual bool isError() = 0; - - /** - * Return a bitfield containing information about the pending errors. - */ - virtual unsigned int getError() = 0; - - /** - * Requests position and velocity. - */ - virtual void requestPosVel() = 0; - - /** - * Requests status. - */ - virtual void requestStatus() = 0; - - /** - * Returns the measured temperature. - */ - virtual void getStatus(int* piStatus, int* piTempCel) = 0; - - /** - * Enable the emergency stop. - */ - virtual bool setEMStop() = 0; - - /** - * Disable the emergency stop. - */ - virtual bool resetEMStop() = 0; - - /** - * Sends an integer value to the Harmonica using the built in interpreter. cpc-kischa - */ - virtual void IntprtSetInt(int iDataLen, char cCmdChar1, char cCmdChar2, int iIndex, int iData) = 0; - - /** - *Provides several functions for drive information recording purposes. By now, only implemented for the Elmo-recorder. cpc-pk - */ - virtual int setRecorder(int iFlag, int iParam = 0, std::string sParam = "/home/MyLog") = 0; - - /** - * Sends Requests for "active current" to motor via CAN - */ - virtual void requestMotorTorque() = 0; - - /** - * Returns member variable m_MotorCurrent - * To update this value call requestMotorCurrent before - * and evaluate CAN buffer, or wait one cycle - */ - virtual void getMotorTorque(double* dTorqueNm) = 0; - - /** - * Sends command for motor Torque (in Nm) - */ - virtual void setMotorTorque(double dTorqueNm) = 0; -}; - - -//----------------------------------------------- -#endif - diff --git a/cob_canopen_motor/common/include/cob_canopen_motor/DriveParam.h b/cob_canopen_motor/common/include/cob_canopen_motor/DriveParam.h deleted file mode 100644 index 88957e74a..000000000 --- a/cob_canopen_motor/common/include/cob_canopen_motor/DriveParam.h +++ /dev/null @@ -1,388 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -#ifndef DRIVEPARAM_INCLUDEDEF_H -#define DRIVEPARAM_INCLUDEDEF_H - -//----------------------------------------------- - - -/** - * Parameters and conversion functionality of a motor drive. - * \ingroup DriversCanModul - */ -class DriveParam -{ - -private: - - int m_iDriveIdent; - int m_iEncIncrPerRevMot; // encoder increments per revolution motor shaft - double m_dVelMeasFrqHz; // only used for Neo drive, else = 1 - double m_dGearRatio; // - double m_dBeltRatio; // if drive has belt set ratio, else = 1 - int m_iSign; // direction of motion - double m_dVelMaxEncIncrS; // max. veloctiy - double m_dAccIncrS2; // max. acceleration - double m_dDecIncrS2; // max. decelration - double m_dPosGearRadToPosMotIncr; - int m_iEncOffsetIncr; // Position in Increments of Steerwheel when Homingposition - // is reached (only needed forCoB3) - int m_iHomingDigIn; // specifies which digital input is used for homing signal, standart 11 is good for COB3, 19 for Cob3_5 - bool m_bIsSteer; // needed to distinguish motors for initializing - double m_dCurrToTorque; // factor to convert motor active current [A] into torque [Nm] - double m_dCurrMax; // max. current allowed - -public: - - /** - * Default constructor. - */ - DriveParam() - { - - m_bIsSteer = true; //has to be set, because it is checked for absolute / relative positioning - - } - - /** - * Sets the drive parameters. - * @param iDriveIdent identifier for the drive - * @param iEncIncrPerRevMot encoder increments per revolution of the motor shaft - * @param dVelMeasFrqHz set this value to 1 - * @param dGearRatio ratio of the gear - * @param iSign change -1 for changing the motion direction - * @param dVelMaxEncIncrS maximum velocity given in encoder increments per second - * @param dAccIncrS2 acceleration in encoder increments per s^2 - * @param dDecIncrS2 deceleration in encoder increments per s^2 - */ - - void setParam( - int iDriveIdent, - int iEncIncrPerRevMot, - double dVelMeasFrqHz, - double dBeltRatio, - double dGearRatio, - int iSign, - double dVelMaxEncIncrS, - double dAccIncrS2, - double dDecIncrS2) - { - - m_iDriveIdent = iDriveIdent; - m_iEncIncrPerRevMot = iEncIncrPerRevMot; - m_dVelMeasFrqHz = dVelMeasFrqHz; - m_dBeltRatio = dBeltRatio; - m_dGearRatio = dGearRatio; - m_iSign = iSign; - m_dVelMaxEncIncrS = dVelMaxEncIncrS; - m_dAccIncrS2 = dAccIncrS2; - m_dDecIncrS2 = dDecIncrS2; - - m_iHomingDigIn = 11; //for Cob3 - - double dPI = 3.14159265358979323846; - - m_dPosGearRadToPosMotIncr = m_iEncIncrPerRevMot * m_dGearRatio - * m_dBeltRatio / (2. * dPI); - } - - //Overloaded Method for CoB3 - void setParam( - int iDriveIdent, - int iEncIncrPerRevMot, - double dVelMeasFrqHz, - double dBeltRatio, - double dGearRatio, - int iSign, - double dVelMaxEncIncrS, - double dAccIncrS2, - double dDecIncrS2, - int iEncOffsetIncr, - bool bIsSteer, - double dCurrToTorque, - double dCurrMax) - { - - m_iDriveIdent = iDriveIdent; - m_iEncIncrPerRevMot = iEncIncrPerRevMot; - m_dVelMeasFrqHz = dVelMeasFrqHz; - m_dBeltRatio = dBeltRatio; - m_dGearRatio = dGearRatio; - m_iSign = iSign; - m_dVelMaxEncIncrS = dVelMaxEncIncrS; - m_dAccIncrS2 = dAccIncrS2; - m_dDecIncrS2 = dDecIncrS2; - m_iEncOffsetIncr = iEncOffsetIncr; - m_bIsSteer = bIsSteer; - - m_iHomingDigIn = 11; //for Cob3 - - double dPI = 3.14159265358979323846; - - m_dPosGearRadToPosMotIncr = m_iEncIncrPerRevMot * m_dGearRatio - * m_dBeltRatio / (2. * dPI); - - m_dCurrToTorque = dCurrToTorque; - m_dCurrMax = dCurrMax; - } - - //Overloaded Method for CoB3 including new feature HomingDigIn, for compatibility reasons overloaded - void setParam( - int iDriveIdent, - int iEncIncrPerRevMot, - double dVelMeasFrqHz, - double dBeltRatio, - double dGearRatio, - int iSign, - double dVelMaxEncIncrS, - double dAccIncrS2, - double dDecIncrS2, - int iEncOffsetIncr, - bool bIsSteer, - double dCurrToTorque, - double dCurrMax, - int iHomingDigIn) - { - - m_iDriveIdent = iDriveIdent; - m_iEncIncrPerRevMot = iEncIncrPerRevMot; - m_dVelMeasFrqHz = dVelMeasFrqHz; - m_dBeltRatio = dBeltRatio; - m_dGearRatio = dGearRatio; - m_iSign = iSign; - m_dVelMaxEncIncrS = dVelMaxEncIncrS; - m_dAccIncrS2 = dAccIncrS2; - m_dDecIncrS2 = dDecIncrS2; - m_iEncOffsetIncr = iEncOffsetIncr; - m_bIsSteer = bIsSteer; - - double dPI = 3.14159265358979323846; - - m_dPosGearRadToPosMotIncr = m_iEncIncrPerRevMot * m_dGearRatio - * m_dBeltRatio / (2. * dPI); - - m_dCurrToTorque = dCurrToTorque; - m_dCurrMax = dCurrMax; - m_iHomingDigIn = iHomingDigIn; - } - - /** - * Returns the identifier of the drive. - */ - int getDriveIdent() - { - return m_iDriveIdent; - } - - /** - * Returns the sign for the motion direction. - */ - int getSign() - { - return m_iSign; - } - - /** - * Gets the maximum velocity of the drive in increments per second. - */ - double getVelMax() - { - return m_dVelMaxEncIncrS; - } - - /** - * Converts position and velocity. - * @param dPosRad position in radiant - * @param dVelRadS velocity in radiant per seconds - * @param piPosIncr converted position in increments - * @param piVelIncrPeriod converted velocity in increments of period - */ - void PosVelRadToIncr(double dPosRad, double dVelRadS, int* piPosIncr, int* piVelIncrPeriod) - { - *piPosIncr = PosGearRadToPosMotIncr(dPosRad); - *piVelIncrPeriod = VelGearRadSToVelMotIncrPeriod(dVelRadS); - } - - /** - * Converts the temperature in degree Celsius. - * The temperature measure is only supported for the drive neo. - * @param iTempIncr temperature in a special internal unit - */ - int TempMeasIncrToGradCel(int iTempIncr) - { - double dTempMeasGradCel; - - dTempMeasGradCel = 0.0002 * (iTempIncr * iTempIncr) - 0.2592 * iTempIncr + 105; - - return (int)dTempMeasGradCel; - } - - /** - * Converts revolution angle form radian to encoder increments. - * @param dPosGearRad angle in radian - */ - int PosGearRadToPosMotIncr(double dPosGearRad) - { - return ((int)(dPosGearRad * m_dPosGearRadToPosMotIncr)); - } - - /// Conversions of encoder increments to gear position in radians. - double PosMotIncrToPosGearRad(int iPosIncr) - { - return ((double)iPosIncr / m_dPosGearRadToPosMotIncr); - } - - /// Conversions of gear velocity in rad/s to encoder increments per measurment period. - int VelGearRadSToVelMotIncrPeriod(double dVelGearRadS) - { - return ((int)(dVelGearRadS * m_dPosGearRadToPosMotIncr / m_dVelMeasFrqHz)); - } - - /// Conversions of encoder increments per measurment period to gear velocity in rad/s. - double VelMotIncrPeriodToVelGearRadS(int iVelMotIncrPeriod) - { - return ((double)iVelMotIncrPeriod / m_dPosGearRadToPosMotIncr * m_dVelMeasFrqHz); - } - - /** - * Set the maximum acceleration. - * @param dMaxAcc Maximum acceleration - */ - void setMaxAcc(double dMaxAcc) - { - m_dAccIncrS2 = dMaxAcc; - } - - /** - * Get the maximum acceleration. - * @return Maximum acceleration - */ - double getMaxAcc() - { - return m_dAccIncrS2; - } - - /** - * Set the maximum deceleration. - * @param dMaxAcc Maximum deceleration - */ - void setMaxDec(double dMaxDec) - { - m_dDecIncrS2 = dMaxDec; - } - - /** - * Get the maximum deceleration. - * @return Maximum deceleration - */ - double getMaxDec() - { - return m_dDecIncrS2; - } - - /** - * Set the maximum velocity. - * @param dMaxVel Maximum velocity - */ - void setMaxVel(double dMaxVel) - { - m_dVelMaxEncIncrS = dMaxVel; - } - - /** - * Get the maximum velocity in increments per second. - * @return Maximum velocity [inc/sec]. - */ - double getMaxVel() - { - return m_dVelMaxEncIncrS; - } - - /** - * Get the gear ratio. - * @return The gear ratio. - */ - double getGearRatio() - { - return m_dGearRatio; - } - /** - * Get the belt ratio. - * @return The belt ratio. - */ - double getBeltRatio() - { - return m_dBeltRatio; - } - - /** - * Get the EncoderOffset - * @return the Encoderoffset - */ - int getEncOffset() - { - return m_iEncOffsetIncr; - } - - /** - * Get the DriveType - If it's a Steering or Driving Motor - * @return the Encoderoffset - */ - bool getIsSteer() - { - return m_bIsSteer; - } - /** - * Get the DriveType - If it's a Steering or Driving Motor - * @return the Encoderoffset - */ - int getEncIncrPerRevMot() - { - return m_iEncIncrPerRevMot; - } - /** - * Get factor to convert motor active current [A] into torque [Nm] - */ - double getCurrToTorque() - { - return m_dCurrToTorque; - } - /** - * Get maximum current allowed - */ - double getCurrMax() - { - return m_dCurrMax; - } - /** - * Get digital Input for Homing signal - */ - int getHomingDigIn() - { - return m_iHomingDigIn; - } - /** - * Set digital Input for Homing signal - */ - void setHomingDigIn(int HomingDigIn) - { - m_iHomingDigIn = HomingDigIn; - } -}; -//----------------------------------------------- -#endif diff --git a/cob_canopen_motor/common/include/cob_canopen_motor/ElmoRecorder.h b/cob_canopen_motor/common/include/cob_canopen_motor/ElmoRecorder.h deleted file mode 100644 index f61a3962f..000000000 --- a/cob_canopen_motor/common/include/cob_canopen_motor/ElmoRecorder.h +++ /dev/null @@ -1,131 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -#ifndef _ElmoRecorder_H -#define _ElmoRecorder_H - -#include -#include - -class CanDriveHarmonica; - -/** - * The in Elmo drives integrated Recorder allows the user to record drive information at a high frequency. - * - * The Recorder firstly has to be configured properly. It can be started immidiately after configuring or later, triggered by a signal (e.g. a begin-motion-command). - * When the recording is finished, you can read out the data via CANopen using a segmented SDO transfer. Appropriate functions for this CANopen specific process are provided by the CanDriveHarmonica class. - * This class brings all the functions to use the Elmo Recorder in a comfortable way. - */ -class ElmoRecorder { - public: - /** - * @param pParentHarmonicaDrive This pointer is used to give ElmoRecorder the ability to take use of CANopen functions of CanDriveHarmonica - */ - ElmoRecorder(CanDriveHarmonica * pParentHarmonicaDrive); - - ~ElmoRecorder(); - - /** - * Processes the collected Elmo Recorder data and saves them into a logfile. - */ - int processData(segData& SDOData); - - /** - * Configures the Elmo Recorder to log internal data at a high frequency - * (This can be used for identification of the drive chain) - * You can read the data out after recording process has finished, using readoutRecorderTry(). - * @param iRecordingGap iRecordingGap = N indicates that a new sample should be taken once per N time quanta (= 4 * 90 usec) - * @param driveID Enter the drive ID of the current motor, to later identify the logfiles - * @param startImmediately 1: start the recording process immediately, 0: start recording at next BG (begin motion) command - * @see readoutRecorderTry() - */ - int configureElmoRecorder(int iRecordingGap, int driveID, int startImmediately = 1); - - /** - * @param initNow Enter true to set the initialization state to true, enter false to only request the state. - * @return Return the initialization state of the recorder. - */ - bool isInitialized(bool initNow); - - /** - * Start point of the actual read-out process. The following flow looks as follows: Request SR -> readoutRecorderTryStatus() If recorder ready -> readoutRecorder() start SDO read-out -> when finished -> processData() process data -> logToFile() save to file. - * This function takes use of the SR (status register) that tells the recorder state. In this function, the SR is requested. - */ - int readoutRecorderTry(int iObjSubIndex); - - /** - * This function is called, when a new SR message is received. If the Recorder is finished and ready to upload data, readoutRecorder() is called to actually start the read-out process. - * @param iStatusReg Status register received from the binary interpreter response. - */ - int readoutRecorderTryStatus(int iStatusReg, segData& SDOData); - - /** - * @param sLogFileprefix Path (to an existing directory!) and file-prefix for the created logfile. It is extended with _MotorNumber_RecordedSource.log - */ - int setLogFilename(std::string sLogFileprefix); - - private: - /** - * Stores the targeted object from the time of requesting the read-out to the actual begin after "Recorder has finished" confirmation by SR - */ - int m_iCurrentObject; - - float m_fRecordingStepSec; - - std::string m_sLogFilename; - - /** - * A flag that tells, whether we are waiting for read-out until the confirmation by SR, that the recorder is ready for read-out - */ - int m_iReadoutRecorderTry; - - CanDriveHarmonica* m_pHarmonicaDrive; - - /** - * Hold the drive ID to later identify the logfiles (in the same directory) - */ - int m_iDriveID; - - /** - * Elmo Recorder is intialized from the time of the first configuration on. - */ - bool m_bIsInitialized; - - /** - * @param iObjSubIndex Requests the SDO Upload of the recorder object 0x2030 of the selected iObjSubIndex-source - */ - int readoutRecorder(int iObjSubIndex); - - /** - * After processing the collected Recorder data log it to a file. - * @param vtValues[] A 2 x N vector with a time stamp in the first column and the according data point value in teh second - * @param filename Path and file-prefix to an existing directory! It is extended with _MotorNumber_RecordedSource.log - */ - int logToFile(std::string filename, std::vector vtValues[]); - - /** - * Convert the 32bit binary representation of a float to an actual 32bit float value - */ - float convertBinaryToFloat(unsigned int binaryRepresentation); - - /** - * Convert the 16bit binary representation of a float to an actual 16bit (half)float value - */ - float convertBinaryToHalfFloat(unsigned int iBinaryRepresentation); -}; - -#endif diff --git a/cob_canopen_motor/common/include/cob_canopen_motor/SDOSegmented.h b/cob_canopen_motor/common/include/cob_canopen_motor/SDOSegmented.h deleted file mode 100644 index ce278dafa..000000000 --- a/cob_canopen_motor/common/include/cob_canopen_motor/SDOSegmented.h +++ /dev/null @@ -1,99 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -#ifndef _SDOSegmented_H -#define _SDOSegmented_H - -#include - -/** -* This class is used to collect data that is uploaded to the master in an segmented SDO transfer. Additionally, it includes some administrative functions for this proccess. -* It can be seen as a SDO segmented collector. -*/ -class segData { - - public: - - /** - * States, that are used to describe the current state of the transmission process of the collected segmented SDO transfer. - */ - enum SDOStatusFlag { - SDO_SEG_FREE = 0, /**< SDO collector is ready for a new transmission */ - SDO_SEG_WAITING = 3, /**< SDO collector is waiting for the first bytes to receive */ - SDO_SEG_COLLECTING = 2, /**< SDO collector is currently collecting data in a segmented SDO transfer */ - SDO_SEG_PROCESSING = 1, /**< collection of data is finished but still has to be processed */ - }; - - segData() { - objectID = 0x0000; - objectSubID = 0x00; - toggleBit = false; - statusFlag = SDO_SEG_FREE; - } - - ~segData() {} - - /** - * Clear the SDO segmented collector - */ - void resetTransferData() { - data.clear(); - objectID = 0x0000; - objectSubID = 0x00; - toggleBit = false; - statusFlag = SDO_SEG_FREE; - } - - //public attributes - //all attributes are public, as this class is used only as ~data array - - /** - * combines different status flags and represents the workflow from 3 to 0: - * 3: SDORequest sent, waiting for transmission !If you are expecting a Segmented answer, this must be set during the request! - * 2: SDO process initiated, collecting data - * 1: finished transmission, waiting for data processing - * 0: SDO workflow finished, free for new transmission - */ - int statusFlag; - - /** - * Holds the ID of the currently uploading object - */ - int objectID; - - /** - * Holds the Sub-ID of the currently uploading object - */ - int objectSubID; - - /** - * The toggle bit, that has to be alternated in each confirmation response to a received segment. - */ - bool toggleBit; - - /** - * Contains the total number of bytes to be uploaded (if specified by SDO sehmented header) - */ - unsigned int numTotalBytes; - - /** - * This vector holds the received data byte-wise - */ - std::vector data; -}; - -#endif diff --git a/cob_canopen_motor/common/src/CanDriveHarmonica.cpp b/cob_canopen_motor/common/src/CanDriveHarmonica.cpp deleted file mode 100644 index 9f41e32fe..000000000 --- a/cob_canopen_motor/common/src/CanDriveHarmonica.cpp +++ /dev/null @@ -1,1404 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -#include -#include -#include - -//----------------------------------------------- -CanDriveHarmonica::CanDriveHarmonica() -{ - // Parameter - m_Param.iDivForRequestStatus = 10; - m_Param.dCanTimeout = 6; - - // Variables - m_pCanCtrl = NULL; - - m_iStatusCtrl = 0; - m_dPosGearMeasRad = 0; - m_dAngleGearRadMem = 0; - m_dVelGearMeasRadS = 0; - - m_VelCalcTime.SetNow(); - - m_bLimSwLeft = false; - m_bLimSwRight = false; - - m_bLimitSwitchEnabled = false; - - m_iCountRequestDiv = 0; - - m_iMotorState = ST_PRE_INITIALIZED; - m_bCurrentLimitOn = false; - - m_iNumAttempsRecFail = 0; - - m_SendTime.SetNow(); - m_StartTime.SetNow(); - - m_bOutputOfFailure = false; - - m_bIsInitialized = false; - - - ElmoRec = new ElmoRecorder(this); - -} - -//----------------------------------------------- -void CanDriveHarmonica::setCanOpenParam( int iTxPDO1, int iTxPDO2, int iRxPDO2, int iTxSDO, int iRxSDO ) -{ - m_ParamCanOpen.iTxPDO1 = iTxPDO1; - m_ParamCanOpen.iTxPDO2 = iTxPDO2; - m_ParamCanOpen.iRxPDO2 = iRxPDO2; - m_ParamCanOpen.iTxSDO = iTxSDO; - m_ParamCanOpen.iRxSDO = iRxSDO; - -} - -//----------------------------------------------- -bool CanDriveHarmonica::evalReceivedMsg(CanMsg& msg) -{ - bool bRet = false; - int iDigIn; - int iFailure; - int iPara; - - int iHomeDigIn = 0x0001; // 0x0001 for CoB3 steering drive homing input; 0x0400 for Scara - int iTemp1, iTemp2; - - m_CanMsgLast = msg; - - //----------------------- - // eval answers from PDO1 - transmitted on SYNC msg - if (msg.m_iID == m_ParamCanOpen.iTxPDO1) - { - iTemp1 = (msg.getAt(3) << 24) | (msg.getAt(2) << 16) - | (msg.getAt(1) << 8) | (msg.getAt(0) ); - - m_dPosGearMeasRad = m_DriveParam.getSign() * m_DriveParam. - PosMotIncrToPosGearRad(iTemp1); - - iTemp2 = (msg.getAt(7) << 24) | (msg.getAt(6) << 16) - | (msg.getAt(5) << 8) | (msg.getAt(4) ); - - m_dVelGearMeasRadS = m_DriveParam.getSign() * m_DriveParam. - VelMotIncrPeriodToVelGearRadS(iTemp2); - - m_WatchdogTime.SetNow(); - - bRet = true; - } - - //----------------------- - // eval answers from binary interpreter - if (msg.m_iID == m_ParamCanOpen.iTxPDO2) - { - if( (msg.getAt(0) == 'P') && (msg.getAt(1) == 'X') ) // current pos - { - } - - else if( (msg.getAt(0) == 'P') && (msg.getAt(1) == 'A') ) // position absolute - { - } - - else if( (msg.getAt(0) == 'J') && (msg.getAt(1) == 'V') ) // current velocity - { - } - - else if( (msg.getAt(0) == 'B') && (msg.getAt(1) == 'G') ) // begin motion - { - } - - else if( (msg.getAt(0) == 'U') && (msg.getAt(1) == 'M') ) // user mode - { - iDigIn = 0x1FFFFF & ( (msg.getAt(7) << 24) | (msg.getAt(6) << 16) - | (msg.getAt(5) << 8) | (msg.getAt(4)) ); - } - - else if( (msg.getAt(0) == 'I') && (msg.getAt(1) == 'P') ) // digital in == limit switches - { - iDigIn = 0x1FFFFF & ( (msg.getAt(7) << 24) | (msg.getAt(6) << 16) - | (msg.getAt(5) << 8) | (msg.getAt(4)) ); - iDigIn = 0x1FFFFF & ( (msg.getAt(7) << 24) | (msg.getAt(6) << 16) - | (msg.getAt(5) << 8) | (msg.getAt(4)) ); - - if( (iDigIn & iHomeDigIn) != 0x0000 ) - { - m_bLimSwRight = true; - } - } - - else if( (msg.getAt(0) == 'S') && (msg.getAt(1) == 'R') ) // status - { - m_iStatusCtrl = (msg.getAt(7) << 24) | (msg.getAt(6) << 16) - | (msg.getAt(5) << 8) | (msg.getAt(4) ); - - evalStatusRegister(m_iStatusCtrl); - ElmoRec->readoutRecorderTryStatus(m_iStatusCtrl, seg_Data); - - } - - else if( (msg.getAt(0) == 'M') && (msg.getAt(1) == 'F') ) // motor failure - { - iFailure = (msg.getAt(7) << 24) | (msg.getAt(6) << 16) - | (msg.getAt(5) << 8) | (msg.getAt(4) ); - - evalMotorFailure(iFailure); - } - - // debug eval - else if( (msg.getAt(0) == 'U') && (msg.getAt(1) == 'M') ) - { - iPara = (msg.getAt(7) << 24) | (msg.getAt(6) << 16) - | (msg.getAt(5) << 8) | (msg.getAt(4) ); - - std::cout << "um " << iPara << std::endl; - } - - else if( (msg.getAt(0) == 'P') && (msg.getAt(1) == 'M') ) - { - iPara = (msg.getAt(7) << 24) | (msg.getAt(6) << 16) - | (msg.getAt(5) << 8) | (msg.getAt(4) ); - - std::cout << "pm " << iPara << std::endl; - } - - else if( (msg.getAt(0) == 'A') && (msg.getAt(1) == 'C') ) - { - iPara = (msg.getAt(7) << 24) | (msg.getAt(6) << 16) - | (msg.getAt(5) << 8) | (msg.getAt(4) ); - - std::cout << "ac " << iPara << std::endl; - } - - else if( (msg.getAt(0) == 'D') && (msg.getAt(1) == 'C') ) - { - iPara = (msg.getAt(7) << 24) | (msg.getAt(6) << 16) - | (msg.getAt(5) << 8) | (msg.getAt(4) ); - - std::cout << "dc " << iPara << std::endl; - } - else if( (msg.getAt(0) == 'H') && (msg.getAt(1) == 'M') ) - { - // status message (homing armed = 1 / disarmed = 0) is encoded in 5th byte - if(msg.getAt(4) == 0) - { - // if 0 received: elmo disarmed homing after receiving the defined event - m_bLimSwRight = true; - } - } - else if( (msg.getAt(0) == 'I') && (msg.getAt(1) == 'Q') ) - { - int iVal=0; - iVal = (msg.getAt(7) << 24) | (msg.getAt(6) << 16) - | (msg.getAt(5) << 8) | (msg.getAt(4) ); - float* pfVal; - pfVal=(float*)&iVal; - m_dMotorCurr = *pfVal; - } - - else - { - } - - m_WatchdogTime.SetNow(); - - bRet = true; - } - - //----------------------- - // eval answer from SDO - if (msg.m_iID == m_ParamCanOpen.iTxSDO) - { - m_WatchdogTime.SetNow(); - - if( (msg.getAt(0) >> 5) == 0) { //Received Upload SDO Segment (scs = 0) - //std::cout << "SDO Upload Segment received" << std::endl; - receivedSDODataSegment(msg); - - } else if( (msg.getAt(0) & 0xE2) == 0x40) { //Received Initiate SDO Upload, that is not expedited -> start segmented upload (scs = 2 AND expedited flag = 0) - //std::cout << "SDO Initiate Segmented Upload received, Object ID: " << (msg.getAt(1) | (msg.getAt(2) << 8) ) << std::endl; - receivedSDOSegmentedInitiation(msg); - - } else if( (msg.getAt(0) >> 5) == 4) { // Received an Abort SDO Transfer message, cs = 4 - unsigned int iErrorNum = (msg.getAt(4) | msg.getAt(5) << 8 | msg.getAt(6) << 16 | msg.getAt(7) << 24); - receivedSDOTransferAbort(iErrorNum); - } - - bRet = true; - } - - return bRet; -} - -//----------------------------------------------- -bool CanDriveHarmonica::init() -{ - int iCnt, iPosCnt; - bool bRet = true; - CanMsg Msg; - - m_iMotorState = ST_PRE_INITIALIZED; - - - // Set Values for Modulo-Counting. Neccessary to preserve absolute position for homed motors (after encoder overflow) - int iIncrRevWheel = int( (double)m_DriveParam.getGearRatio() * (double)m_DriveParam.getBeltRatio() - * (double)m_DriveParam.getEncIncrPerRevMot() * 3 ); - IntprtSetInt(8, 'M', 'O', 0, 0); - usleep(20000); - IntprtSetInt(8, 'X', 'M', 2, iIncrRevWheel * 5000); - usleep(20000); - IntprtSetInt(8, 'X', 'M', 1, -iIncrRevWheel * 5000); - usleep(20000); - - - setTypeMotion(MOTIONTYPE_VELCTRL); - // ---------- set position counter to zero - IntprtSetInt(8, 'P', 'X', 0, 0); - - iCnt = 0; - while(true) - { - m_pCanCtrl->receiveMsg(&Msg); - - if( (Msg.getAt(0) == 'P') && (Msg.getAt(1) == 'X') ) - { - iPosCnt = (Msg.getAt(7) << 24) | (Msg.getAt(6) << 16) - | (Msg.getAt(5) << 8) | (Msg.getAt(4) ); - - m_dPosGearMeasRad = m_DriveParam.getSign() * m_DriveParam.PosMotIncrToPosGearRad(iPosCnt); - m_dAngleGearRadMem = m_dPosGearMeasRad; - break; - } - - if ( iCnt > 300 ) - { - std::cout << "CanDriveHarmonica: initial position not set" << std::endl; - bRet = false; - break; - } - - usleep(10000); - iCnt++; - } - - // ---------- set PDO mapping - // Mapping of TPDO1: - // - position - // - velocity - - // stop all emissions of TPDO1 - sendSDODownload(0x1A00, 0, 0); - - // position 4 byte of TPDO1 - sendSDODownload(0x1A00, 1, 0x60640020); - - // velocity 4 byte of TPDO1 - sendSDODownload(0x1A00, 2, 0x60690020); - - // transmission type "synch" - sendSDODownload(0x1800, 2, 1); - - // activate mapped objects - sendSDODownload(0x1A00, 0, 2); - - m_bWatchdogActive = false; - - if( bRet ) - m_bIsInitialized = true; - - return bRet; -} -//----------------------------------------------- -bool CanDriveHarmonica::stop() -{ - bool bRet = true; - // motor off - IntprtSetInt(8, 'M', 'O', 0, 0); - usleep(20000); - return bRet; -} -//----------------------------------------------- -bool CanDriveHarmonica::start() -{ - // motor on - IntprtSetInt(8, 'M', 'O', 0, 1); - usleep(20000); - - // ------------------- request status - int iCnt; - bool bRet = true; - int iStatus; - CanMsg Msg; - - // clear the can buffer - do - { - bRet = m_pCanCtrl->receiveMsg(&Msg); - } - while(bRet == true); - - // send request - IntprtSetInt(4, 'S', 'R', 0, 0); - - iCnt = 0; - while(true) - { - m_pCanCtrl->receiveMsg(&Msg); - - if( (Msg.getAt(0) == 'S') && (Msg.getAt(1) == 'R') ) - { - iStatus = (Msg.getAt(7) << 24) | (Msg.getAt(6) << 16) - | (Msg.getAt(5) << 8) | (Msg.getAt(4) ); - - bRet = evalStatusRegister(iStatus); - break; - } - - if ( iCnt > 300 ) - { - std::cout << "CanDriveHarmonica::enableMotor(): No answer on status request" << std::endl; - bRet = false; - break; - } - - usleep(10000); - iCnt++; - } - - // ------------------- start watchdog timer - m_WatchdogTime.SetNow(); - m_SendTime.SetNow(); - - return bRet; -} - -//----------------------------------------------- -bool CanDriveHarmonica::reset() -{ - // repeat initialization - - // start network - CanMsg msg; - msg.m_iID = 0; - msg.m_iLen = 2; - msg.set(1,0,0,0,0,0,0,0); - m_pCanCtrl->transmitMsg(msg); - - // init and start - bool bRet = init(); - bRet |= start(); - - return bRet; -} -//----------------------------------------------- -bool CanDriveHarmonica::shutdown() -{ - std::cout << "shutdown drive " << m_DriveParam.getDriveIdent() << std::endl; - - IntprtSetInt(8, 'M', 'O', 0, 0); - - return true; -} - -//----------------------------------------------- -bool CanDriveHarmonica::startWatchdog(bool bStarted) -{ - if (bStarted == true) - { - //save Watchdog state into member variable - m_bWatchdogActive = true; - // ------- init watchdog - // Harmonica checks PC hearbeat - // note: the COB-ID for a heartbeat message = 0x700 + Device ID - - const int c_iHeartbeatTimeMS = 1000; - const int c_iNMTNodeID = 0x00; - - // consumer (PC) heartbeat time - sendSDODownload(0x1016, 1, (c_iNMTNodeID << 16) | c_iHeartbeatTimeMS); - - // error behavior after failure: 0=pre-operational, 1=no state change, 2=stopped" - sendSDODownload(0x1029, 1, 2); - - // motor behavior after heartbeat failre: "quick stop" - sendSDODownload(0x6007, 0, 3); - - // acivate emergency events: "heartbeat event" - // Object 0x2F21 = "Emergency Events" which cause an Emergency Message - // Bit 3 is responsible for Heartbeart-Failure.--> Hex 0x08 - sendSDODownload(0x2F21, 0, 0x08); - usleep(20000); - - } - else - { - //save Watchdog state into member variable - m_bWatchdogActive = false; - - //Motor action after Hearbeat-Error: No Action - sendSDODownload(0x6007, 0, 0); - - //Error Behavior: No state change - sendSDODownload(0x1029, 1, 1); - - // Deacivate emergency events: "heartbeat event" - // Object 0x2F21 = "Emergency Events" which cause an Emergency Message - // Bit 3 is responsible for Heartbeart-Failure. - sendSDODownload(0x2F21, 0, 0x00); - usleep(25000); - - - } - - return true; -} - -//----------------------------------------------- -bool CanDriveHarmonica::disableBrake(bool bDisabled) -{ - return true; -} - -//----------------------------------------------- -double CanDriveHarmonica::getTimeToLastMsg() -{ - m_CurrentTime.SetNow(); - - return m_CurrentTime - m_WatchdogTime; -} -//----------------------------------------------- -bool CanDriveHarmonica::getStatusLimitSwitch() -{ - return m_bLimSwRight; -} -//----------------------------------------------- -bool CanDriveHarmonica::initHoming() -{ - const int c_iPosRef = m_DriveParam.getEncOffset(); - - // 1. make sure that, if on elmo controller still a pending homing from a previous startup is running (in case of warm-start without switching of the whole robot), this old sequence is disabled - // disarm homing process - IntprtSetInt(8, 'H', 'M', 1, 0); - - // always give can and controller some time to understand the command - usleep(20000); - - /* THIS is needed for head_axis on cob3-2! - - //set input logic to 'general purpose' - IntprtSetInt(8, 'I', 'L', 2, 7); - usleep(20000); - */ - - // 2. configure the homing sequence - // 2.a set the value to which the increment counter shall be reseted as soon as the homing event occurs - // value to load at homing event - IntprtSetInt(8, 'H', 'M', 2, c_iPosRef); - usleep(20000); - - // 2.b choose the chanel/switch on which the controller listens for a change or defined logic level (the homing event) (high/low/falling/rising) - // home event - // iHomeEvent = 5 : event according to defined FLS switch (for scara arm) - // iHomeEvent = 9 : event according to definded DIN1 switch (for full steerable wheels COb3) - // iHomeEvent =11 : event according to ?? (for COb3 Head-Axis) - IntprtSetInt(8, 'H', 'M', 3, m_DriveParam.getHomingDigIn()); - //IntprtSetInt(8, 'H', 'M', 3, 11); //cob3-2 - usleep(20000); - - - // 2.c choose the action that the controller shall perform after the homing event occured - // HM[4] = 0 : after Event stop immediately - // HM[4] = 2 : Do nothing! - IntprtSetInt(8, 'H', 'M', 4, 2); - usleep(20000); - - // 2.d choose the setting of the position counter (i.e. to the value defined in 2.a) after the homing event occured - // HM[5] = 0 : absolute setting of position counter: PX = HM[2] - IntprtSetInt(8, 'H', 'M', 5, 0); - usleep(20000); - - // 3. let the motor turn some time to give him the possibility to escape the approximation sensor if accidently in home position already at the beginning of the sequence (done in CanCtrlPltf...) - - return true; -} - - -//----------------------------------------------- -bool CanDriveHarmonica::execHoming() //not used by CanCtrlPltf, that has its own homing implementation -{ - - int iCnt; - CanMsg Msg; - bool bRet = true; - - int iNrDrive = m_DriveParam.getDriveIdent(); - - // 4. arm the homing process -> as soon as the approximation sensor is reached and the homing event occurs the commands set in 2. take effect - // arm homing process - IntprtSetInt(8, 'H', 'M', 1, 1); - - // 5. clear the can buffer to get rid of all uneccessary and potentially disturbing commands still floating through the wires - do - { - // read from can - bRet = m_pCanCtrl->receiveMsg(&Msg); - } - while(bRet == true); - - // 6. now listen for status of homing, to synchronize program flow -> proceed only after homing was succesful (homing disarmed by elmo) or timeout occured - - // set timeout counter to zero - iCnt = 0; - - do - { - // 6.a ask for status of homing process (armed/disarmed) - // ask for first byte in Homing Configuration - IntprtSetInt(4, 'H', 'M', 1, 0); - - // 6.b read message from can - m_pCanCtrl->receiveMsgRetry(&Msg, 10); - - // 6.c see if received message is answer of request and if so what is the status - if( (Msg.getAt(0) == 'H') && (Msg.getAt(1) == 'M') ) - { - // status message (homing armed = 1 / disarmed = 0) is encoded in 5th byte - if(Msg.getAt(4) == 0) - { - // if 0 received: elmo disarmed homing after receiving the defined event - std::cout << "Got Homing-Signal " << std::endl; - m_bLimSwRight = true; - break; - } - } - - // increase count for timeout - usleep(10000); - iCnt++; - - } - while((m_bLimSwRight == false) && (iCnt<2000)); // wait some time - - // 7. see why finished (homed or timeout) and log out - if(iCnt>=2000) - { - std::cout << "Homing failed - limit switch " << iNrDrive << " not reached" << std::endl; - bRet = false; - } - else - { - std::cout << "Homing successful - limit switch " << iNrDrive << " ok" << std::endl; - bRet = true; - } - //IntprtSetInt(8, 'I', 'L', 2, 9); //cob3-2 | ----------------------------------------------------------------------------------- - //usleep(20000); - - return bRet; -} -//----------------------------------------------- -void CanDriveHarmonica::setGearPosVelRadS(double dPosGearRad, double dVelGearRadS) -{ - int iPosEncIncr; - int iVelEncIncrPeriod; - - m_DriveParam.PosVelRadToIncr(dPosGearRad, dVelGearRadS, &iPosEncIncr, &iVelEncIncrPeriod); - - if(iVelEncIncrPeriod > m_DriveParam.getVelMax()) - { - iVelEncIncrPeriod = (int)m_DriveParam.getVelMax(); - } - - if(iVelEncIncrPeriod < -m_DriveParam.getVelMax()) - { - iVelEncIncrPeriod = (int)-m_DriveParam.getVelMax(); - } - - if(m_iTypeMotion == MOTIONTYPE_POSCTRL) - { - //new: set VELOCITY for PTP Motion - IntprtSetInt(8, 'S', 'P', 0, iVelEncIncrPeriod); - - // Position Relativ ("PR") , because of positioning of driving wheel - // which is not initialized to zero on a specific position - // only when command is for homed steering wheel set absolute - if (m_DriveParam.getIsSteer() == true) - IntprtSetInt(8, 'P', 'A', 0, iPosEncIncr); - else - IntprtSetInt(8, 'P', 'R', 0, iPosEncIncr); - - IntprtSetInt(4, 'B', 'G', 0, 0); - - } - - if(m_iTypeMotion == MOTIONTYPE_VELCTRL) - { - iVelEncIncrPeriod *= m_DriveParam.getSign(); - IntprtSetInt(8, 'J', 'V', 0, iVelEncIncrPeriod); - IntprtSetInt(4, 'B', 'G', 0, 0); - } - - // request pos and vel by TPDO1, triggered by SYNC msg - // (to request pos by SDO usesendSDOUpload(0x6064, 0) ) - CanMsg msg; - msg.m_iID = 0x80; - msg.m_iLen = 0; - msg.set(0,0,0,0,0,0,0,0); - m_pCanCtrl->transmitMsg(msg); -} - -//----------------------------------------------- -void CanDriveHarmonica::setGearVelRadS(double dVelGearRadS) -{ - int iVelEncIncrPeriod; - - // calc motor velocity from joint velocity - iVelEncIncrPeriod = m_DriveParam.getSign() * m_DriveParam.VelGearRadSToVelMotIncrPeriod(dVelGearRadS); - - if(iVelEncIncrPeriod > m_DriveParam.getVelMax()) - { - std::cout << "SteerVelo asked for " << iVelEncIncrPeriod << " EncIncrements" << std::endl; - iVelEncIncrPeriod = (int)m_DriveParam.getVelMax(); - } - - if(iVelEncIncrPeriod < -m_DriveParam.getVelMax()) - { - std::cout << "SteerVelo asked for " << iVelEncIncrPeriod << " EncIncrements" << std::endl; - iVelEncIncrPeriod = -1 * (int)m_DriveParam.getVelMax(); - } - - IntprtSetInt(8, 'J', 'V', 0, iVelEncIncrPeriod); - IntprtSetInt(4, 'B', 'G', 0, 0); - - // request pos and vel by TPDO1, triggered by SYNC msg - // (to request pos by SDO use sendSDOUpload(0x6064, 0) ) - // sync msg is: iID 0x80 with msg (0,0,0,0,0,0,0,0) - CanMsg msg; - msg.m_iID = 0x80; - msg.m_iLen = 0; - msg.set(0,0,0,0,0,0,0,0); - m_pCanCtrl->transmitMsg(msg); - - // send heartbeat to keep watchdog inactive - msg.m_iID = 0x700; - msg.m_iLen = 5; - msg.set(0x00,0,0,0,0,0,0,0); - m_pCanCtrl->transmitMsg(msg); - - m_CurrentTime.SetNow(); - double dt = m_CurrentTime - m_SendTime; - if ((dt > 1.0) && m_bWatchdogActive) - { - std::cout << "Time between send velocity of motor " << m_DriveParam.getDriveIdent() - << " is too large: " << dt << " s" << std::endl; - } - m_SendTime.SetNow(); - - - // request status - m_iCountRequestDiv++; - if (m_iCountRequestDiv > m_Param.iDivForRequestStatus) - { - requestStatus(); - m_iCountRequestDiv = 0; - } -} - -//----------------------------------------------- -void CanDriveHarmonica::getGearPosRad(double* dGearPosRad) -{ - *dGearPosRad = m_dPosGearMeasRad; -} - -//----------------------------------------------- -void CanDriveHarmonica::getGearPosVelRadS(double* pdAngleGearRad, double* pdVelGearRadS) -{ - *pdAngleGearRad = m_dPosGearMeasRad; - *pdVelGearRadS = m_dVelGearMeasRadS; -} - -//----------------------------------------------- -void CanDriveHarmonica::getGearDeltaPosVelRadS(double* pdAngleGearRad, double* pdVelGearRadS) -{ - *pdAngleGearRad = m_dPosGearMeasRad - m_dAngleGearRadMem; - *pdVelGearRadS = m_dVelGearMeasRadS; - m_dAngleGearRadMem = m_dPosGearMeasRad; -} - -//----------------------------------------------- -void CanDriveHarmonica::getData(double* pdPosGearRad, double* pdVelGearRadS, - int* piTorqueCtrl, int* piStatusCtrl) -{ - *pdPosGearRad = m_dPosGearMeasRad; - *pdVelGearRadS = m_dVelGearMeasRadS; - *piTorqueCtrl = m_iTorqueCtrl; - *piStatusCtrl = m_iStatusCtrl; -} - -//----------------------------------------------- -void CanDriveHarmonica::requestPosVel() -{ - // request pos and vel by TPDO1, triggered by SYNC msg - CanMsg msg; - msg.m_iID = 0x80; - msg.m_iLen = 0; - msg.set(0,0,0,0,0,0,0,0); - m_pCanCtrl->transmitMsg(msg); - // (to request pos by SDO use sendSDOUpload(0x6064, 0) ) -} - -//----------------------------------------------- -void CanDriveHarmonica::sendHeartbeat() -{ - CanMsg msg; - msg.m_iID = 0x700; - msg.m_iLen = 5; - msg.set(0x00,0,0,0,0,0,0,0); - m_pCanCtrl->transmitMsg(msg); -} - -//----------------------------------------------- -void CanDriveHarmonica::requestStatus() -{ - IntprtSetInt(4, 'S', 'R', 0, 0); -} - -//----------------------------------------------- -void CanDriveHarmonica::requestMotorTorque() -{ - // send command for requesting motor current: - IntprtSetInt(4, 'I', 'Q', 0, 0); // active current - //IntprtSetInt(4, 'I', 'D', 0, 0); // reactive current -} - -//----------------------------------------------- -bool CanDriveHarmonica::isError() -{ - if (m_iMotorState != ST_MOTOR_FAILURE) - { - // Check timeout of can communication - double dWatchTime = getTimeToLastMsg(); - - if (dWatchTime>m_Param.dCanTimeout) - { - if ( m_bOutputOfFailure == false) - { - std::cout << "Motor " << m_DriveParam.getDriveIdent() << - " has no can communication for " << dWatchTime << " s." << std::endl; - } - - m_iMotorState = ST_MOTOR_FAILURE; - m_FailureStartTime.SetNow(); - } - - } - - return (m_iMotorState == ST_MOTOR_FAILURE); -} -//----------------------------------------------- -bool CanDriveHarmonica::setTypeMotion(int iType) -{ - int iMaxAcc = int(m_DriveParam.getMaxAcc()); - int iMaxDcc = int(m_DriveParam.getMaxDec()); - CanMsg Msg; - - if (iType == MOTIONTYPE_POSCTRL) - { - // 1.) Switch to UnitMode = 5 (Single Loop Position Control) // - - // switch off Motor to change Unit-Mode - IntprtSetInt(8, 'M', 'O', 0, 0); - usleep(20000); - // switch Unit-Mode - IntprtSetInt(8, 'U', 'M', 0, 5); - - // set Target Radius to X Increments - IntprtSetInt(8, 'T', 'R', 1, 15); - // set Target Time to X ms - IntprtSetInt(8, 'T', 'R', 2, 100); - - // set maximum Acceleration to X Incr/s^2 - IntprtSetInt(8, 'A', 'C', 0, iMaxAcc); - // set maximum decceleration to X Incr/s^2 - IntprtSetInt(8, 'D', 'C', 0, iMaxDcc); - usleep(100000); - - - } - else if (iType == MOTIONTYPE_TORQUECTRL) - { - // Switch to TorqueControll-Mode - // switch off Motor to change Unit-Mode - IntprtSetInt(8, 'M', 'O', 0, 0); - usleep(50000); - // switch Unit-Mode 1: Torque Controlled - IntprtSetInt(8, 'U', 'M', 0, 1); - // disable external compensation input - // to avoid noise from that input pin - IntprtSetInt(8, 'R', 'M', 0, 0); - - // debugging: - std::cout << "Motor"<> 8) & 0x3F; // The two MSB must be 0. Cf. DSP 301 Implementation guide p. 39. - - cInt[0] = iData; - cInt[1] = iData >> 8; - cInt[2] = iData >> 16; - cInt[3] = iData >> 24; - - CMsgTr.set(cCmdChar1, cCmdChar2, cIndex[0], cIndex[1], cInt[0], cInt[1], cInt[2], cInt[3]); - m_pCanCtrl->transmitMsg(CMsgTr); -} - -//----------------------------------------------- -void CanDriveHarmonica::IntprtSetFloat(int iDataLen, char cCmdChar1, char cCmdChar2, int iIndex, float fData) -{ - char cIndex[2]; - char cFloat[4]; - CanMsg CMsgTr; - char* pTempFloat = NULL; - - CMsgTr.m_iID = m_ParamCanOpen.iRxPDO2; - CMsgTr.m_iLen = iDataLen; - - cIndex[0] = iIndex; - // for sending float values bit 6 has to be zero and bit 7 one (according to Elmo Implementation guide) - cIndex[1] = (iIndex >> 8) & 0x3F; // setting bit 6 to zero with mask 0b10111111->0xBF - cIndex[1] = cIndex[1] | 0x80; // setting bit 7 to one with mask 0b10000000 ->0x80 - - pTempFloat = (char*)&fData; - for( int i=0; i<4; i++ ) - cFloat[i] = pTempFloat[i]; - - CMsgTr.set(cCmdChar1, cCmdChar2, cIndex[0], cIndex[1], cFloat[0], cFloat[1], cFloat[2], cFloat[3]); - m_pCanCtrl->transmitMsg(CMsgTr); -} - -//----------------------------------------------- -// SDO Communication Protocol, most functions are used for general SDO Segmented Transfer -//----------------------------------------------- - -//----------------------------------------------- -void CanDriveHarmonica::sendSDOAbort(int iObjIndex, int iObjSubIndex, unsigned int iErrorCode) -{ - CanMsg CMsgTr; - const int ciAbortTransferReq = 0x04 << 5; - - CMsgTr.m_iLen = 8; - CMsgTr.m_iID = m_ParamCanOpen.iRxSDO; - - unsigned char cMsg[8]; - - cMsg[0] = ciAbortTransferReq; - cMsg[1] = iObjIndex; - cMsg[2] = iObjIndex >> 8; - cMsg[3] = iObjSubIndex; - cMsg[4] = iErrorCode; - cMsg[5] = iErrorCode >> 8; - cMsg[6] = iErrorCode >> 16; - cMsg[7] = iErrorCode >> 24; - - CMsgTr.set(cMsg[0], cMsg[1], cMsg[2], cMsg[3], cMsg[4], cMsg[5], cMsg[6], cMsg[7]); - m_pCanCtrl->transmitMsg(CMsgTr); -} - -//----------------------------------------------- -void CanDriveHarmonica::receivedSDOTransferAbort(unsigned int iErrorCode){ - std::cout << "SDO Abort Transfer received with error code: " << iErrorCode; - seg_Data.statusFlag = segData::SDO_SEG_FREE; -} - -//----------------------------------------------- -void CanDriveHarmonica::sendSDOUpload(int iObjIndex, int iObjSubIndex) -{ - CanMsg CMsgTr; - const int ciInitUploadReq = 0x40; - - CMsgTr.m_iLen = 8; - CMsgTr.m_iID = m_ParamCanOpen.iRxSDO; - - unsigned char cMsg[8]; - - cMsg[0] = ciInitUploadReq; - cMsg[1] = iObjIndex; - cMsg[2] = iObjIndex >> 8; - cMsg[3] = iObjSubIndex; - cMsg[4] = 0x00; - cMsg[5] = 0x00; - cMsg[6] = 0x00; - cMsg[7] = 0x00; - - CMsgTr.set(cMsg[0], cMsg[1], cMsg[2], cMsg[3], cMsg[4], cMsg[5], cMsg[6], cMsg[7]); - m_pCanCtrl->transmitMsg(CMsgTr); -} - -//----------------------------------------------- -void CanDriveHarmonica::sendSDODownload(int iObjIndex, int iObjSubIndex, int iData) -{ - CanMsg CMsgTr; - - const int ciInitDownloadReq = 0x20; - const int ciNrBytesNoData = 0x00; - const int ciExpedited = 0x02; - const int ciDataSizeInd = 0x01; - - CMsgTr.m_iLen = 8; - CMsgTr.m_iID = m_ParamCanOpen.iRxSDO; - - unsigned char cMsg[8]; - - cMsg[0] = ciInitDownloadReq | (ciNrBytesNoData << 2) | ciExpedited | ciDataSizeInd; - cMsg[1] = iObjIndex; - cMsg[2] = iObjIndex >> 8; - cMsg[3] = iObjSubIndex; - cMsg[4] = iData; - cMsg[5] = iData >> 8; - cMsg[6] = iData >> 16; - cMsg[7] = iData >> 24; - - CMsgTr.set(cMsg[0], cMsg[1], cMsg[2], cMsg[3], cMsg[4], cMsg[5], cMsg[6], cMsg[7]); - m_pCanCtrl->transmitMsg(CMsgTr); -} - -//----------------------------------------------- -void CanDriveHarmonica::evalSDO(CanMsg& CMsg, int* pIndex, int* pSubindex) -{ - *pIndex = (CMsg.getAt(2) << 8) | CMsg.getAt(1); - *pSubindex = CMsg.getAt(3); -} - -//----------------------------------------------- -int CanDriveHarmonica::getSDODataInt32(CanMsg& CMsg) -{ - int iData = (CMsg.getAt(7) << 24) | (CMsg.getAt(6) << 16) | - (CMsg.getAt(5) << 8) | CMsg.getAt(4); - - return iData; -} - -//----------------------------------------------- -int CanDriveHarmonica::receivedSDOSegmentedInitiation(CanMsg& msg) { - - if(seg_Data.statusFlag == segData::SDO_SEG_FREE || seg_Data.statusFlag == segData::SDO_SEG_WAITING) { //only accept new SDO Segmented Upload if seg_Data is free - seg_Data.resetTransferData(); - seg_Data.statusFlag = segData::SDO_SEG_COLLECTING; - - //read out objectIDs - evalSDO(msg, &seg_Data.objectID, &seg_Data.objectSubID); - - //data in byte 4 to 7 contain the number of bytes to be uploaded (if Size indicator flag is set) - if( (msg.getAt(0) & 0x01) == 1) { - seg_Data.numTotalBytes = msg.getAt(7) << 24 | msg.getAt(6) << 16 | msg.getAt(5) << 8 | msg.getAt(4); - } else seg_Data.numTotalBytes = 0; - - sendSDOUploadSegmentConfirmation(seg_Data.toggleBit); - } - - return 0; - -} - -//----------------------------------------------- -int CanDriveHarmonica::receivedSDODataSegment(CanMsg& msg){ - - int numEmptyBytes = 0; - - //Read SDO Upload Protocol: - //Byte 0: SSS T NNN C | SSS=Cmd-Specifier, T=ToggleBit, NNN=num of empty bytes, C=Finished - //Byte 1 to 7: Data - - if( (msg.getAt(0) & 0x10) != (seg_Data.toggleBit << 4) ) { - std::cout << "Toggle Bit error, send Abort SDO with \"Toggle bit not alternated\" error" << std::endl; - sendSDOAbort(seg_Data.objectID, seg_Data.objectSubID, 0x05030000); //Send SDO Abort with error code Toggle-Bit not alternated - return 1; - } - - if( (msg.getAt(0) & 0x01) == 0x00) { //Is finished-bit not set? - seg_Data.statusFlag = segData::SDO_SEG_COLLECTING; - } else { - //std::cout << "SDO Segmented Transfer finished bit found!" << std::endl; - seg_Data.statusFlag = segData::SDO_SEG_PROCESSING; - }; - - numEmptyBytes = (msg.getAt(0) >> 1) & 0x07; - //std::cout << "NUM empty bytes in SDO :" << numEmptyBytes << std::endl; - - for(int i=1; i<=7-numEmptyBytes; i++) { - seg_Data.data.push_back(msg.getAt(i)); - } - - if(seg_Data.statusFlag == segData::SDO_SEG_PROCESSING) { - finishedSDOSegmentedTransfer(); - } else { - seg_Data.toggleBit = !seg_Data.toggleBit; - sendSDOUploadSegmentConfirmation(seg_Data.toggleBit); - } - - return 0; -} - -//----------------------------------------------- -void CanDriveHarmonica::sendSDOUploadSegmentConfirmation(bool toggleBit) { - - CanMsg CMsgTr; - int iConfirmSegment = 0x60; //first three bits must be css = 3 : 011 00000 - iConfirmSegment = iConfirmSegment | (toggleBit << 4); //fourth bit is toggle bit: 011T0000 - - CMsgTr.m_iLen = 8; - CMsgTr.m_iID = m_ParamCanOpen.iRxSDO; - - unsigned char cMsg[8]; - - cMsg[0] = iConfirmSegment; - cMsg[1] = 0x00; - cMsg[2] = 0x00; - cMsg[3] = 0x00; - cMsg[4] = 0x00; - cMsg[5] = 0x00; - cMsg[6] = 0x00; - cMsg[7] = 0x00; - - CMsgTr.set(cMsg[0], cMsg[1], cMsg[2], cMsg[3], cMsg[4], cMsg[5], cMsg[6], cMsg[7]); - m_pCanCtrl->transmitMsg(CMsgTr); -} - -//----------------------------------------------- -void CanDriveHarmonica::finishedSDOSegmentedTransfer() { - seg_Data.statusFlag = segData::SDO_SEG_PROCESSING; - - if( (seg_Data.data.size() != seg_Data.numTotalBytes) & (seg_Data.numTotalBytes != 0) ) { - std::cout << "WARNING: SDO tranfer finished but number of collected bytes " - << seg_Data.data.size() << " != expected number of bytes: " << seg_Data.numTotalBytes << std::endl; - //abort processing? - } - - if(seg_Data.objectID == 0x2030) { - if(ElmoRec->processData(seg_Data) == 0) seg_Data.statusFlag = segData::SDO_SEG_FREE; - } -} - -//----------------------------------------------- -double CanDriveHarmonica::estimVel(double dPos) -{ - double dVel; - double dt; - - m_CurrentTime.SetNow(); - - dt = m_CurrentTime - m_VelCalcTime; - - dVel = (dPos - m_dOldPos)/dt; - - m_dOldPos = dPos; - m_VelCalcTime.SetNow(); - - return dVel; -} -//----------------------------------------------- -bool CanDriveHarmonica::evalStatusRegister(int iStatus) -{ - bool bNoError; - - // --------- Error status - if( isBitSet(iStatus, 0) ) - { - // ------------ Error - if ( m_bOutputOfFailure == false ) - { - std::cout << "Error of drive: " << m_DriveParam.getDriveIdent() << std::endl; - - if( (iStatus & 0x0000000E) == 2) - std::cout << "- drive error under voltage" << std::endl; - - if( (iStatus & 0x0000000E) == 4) - std::cout << "- drive error over voltage" << std::endl; - - if( (iStatus & 0x0000000E) == 10) - std::cout << "- drive error short circuit" << std::endl; - - if( (iStatus & 0x0000000E) == 12) - std::cout << "- drive error overheating" << std::endl; - - // Request detailed description of failure - IntprtSetInt(4, 'M', 'F', 0, 0); - } - - m_iNewMotorState = ST_MOTOR_FAILURE; - - bNoError = false; - } - else if ( isBitSet(iStatus, 6) ) - { - // General failure - if ( m_bOutputOfFailure == false ) - { - std::cout << "Motor " << m_DriveParam.getDriveIdent() << " failure latched" << std::endl; - - // Request detailed description of failure - IntprtSetInt(4, 'M', 'F', 0, 0); - - m_FailureStartTime.SetNow(); - } - m_iNewMotorState = ST_MOTOR_FAILURE; - - bNoError = false; - } - else - { - // ---------- No error - bNoError = true; - - // Clear flag for failure output only if at least one - // status message without error has been received. - // Printing an error message on recovery is avoided. - m_bOutputOfFailure = false; - - // --------- General status bits - // check if Bit 4 (-> Motor is ON) ist set - if( isBitSet(iStatus, 4) ) - { - if (m_iMotorState != ST_OPERATION_ENABLED) - { - std::cout << "Motor " << m_DriveParam.getDriveIdent() << " operation enabled" << std::endl; - m_FailureStartTime.SetNow(); - } - - m_iNewMotorState = ST_OPERATION_ENABLED; - } - else - { - if (m_iMotorState != ST_OPERATION_DISABLED) - { - std::cout << "Motor " << m_DriveParam.getDriveIdent() << " operation disabled" << std::endl; - } - - m_iNewMotorState = ST_OPERATION_DISABLED; - } - - // Current limit - if( isBitSet(iStatus, 13) ) - { - if (m_bCurrentLimitOn == false) - std::cout << "Motor " << m_DriveParam.getDriveIdent() << "current limit on" << std::endl; - - m_bCurrentLimitOn = true; - } - else - m_bCurrentLimitOn = false; - } - - // Change state - m_iMotorState = m_iNewMotorState; - - if (m_iMotorState == ST_MOTOR_FAILURE) - m_bOutputOfFailure = true; - - return bNoError; -} - -//----------------------------------------------- -void CanDriveHarmonica::evalMotorFailure(int iFailure) -{ - - std::cout << "Motor " << m_DriveParam.getDriveIdent() << " has a failure: " << iFailure << std::endl; - - if( isBitSet(iFailure, 2) ) - { - std::cout << "- feedback loss" << std::endl; - } - - if( isBitSet(iFailure, 3) ) - { - std::cout << "- peak current excced" << std::endl; - } - - if( isBitSet(iFailure, 7) ) - { - std::cout << "- speed track error" << std::endl; - } - - if( isBitSet(iFailure, 8) ) - { - std::cout << "- position track error" << std::endl; - } - - if( isBitSet(iFailure, 17) ) - { - std::cout << "- speed limit exceeded" << std::endl; - } - - if( isBitSet(iFailure, 21) ) - { - std::cout << "- motor stuck" << std::endl; - } -} - -//----------------------------------------------- -void CanDriveHarmonica::setMotorTorque(double dTorqueNm) -{ - // convert commanded motor current into amperes - float fMotCurr = m_DriveParam.getSign() * dTorqueNm / m_DriveParam.getCurrToTorque(); - - // check for limitations - if (fMotCurr > m_DriveParam.getCurrMax()) - { - fMotCurr = m_DriveParam.getCurrMax(); - std::cout << "Torque command too high: " << fMotCurr << " Nm. Torque has been limitited." << std::endl; - } - if (fMotCurr < -m_DriveParam.getCurrMax()) - { - fMotCurr = -m_DriveParam.getCurrMax(); - std::cout << "Torque command too high: " << fMotCurr << " Nm. Torque has been limitited." << std::endl; - } - - // send Command - IntprtSetFloat(8, 'T', 'C', 0, fMotCurr); - - // request pos and vel by TPDO1, triggered by SYNC msg - CanMsg msg; - msg.m_iID = 0x80; - msg.m_iLen = 0; - msg.set(0,0,0,0,0,0,0,0); - m_pCanCtrl->transmitMsg(msg); - - // send heartbeat to keep watchdog inactive - sendHeartbeat(); - - m_CurrentTime.SetNow(); - double dt = m_CurrentTime - m_SendTime; - if (dt > 1.0) - { - std::cout << "Time between send current/torque of motor " << m_DriveParam.getDriveIdent() - << " is too large: " << dt << " s" << std::endl; - } - m_SendTime.SetNow(); - - - // request status - m_iCountRequestDiv++; - if (m_iCountRequestDiv > m_Param.iDivForRequestStatus) - { - requestStatus(); - m_iCountRequestDiv = 0; - } - -} - -//----------------------------------------------- -void CanDriveHarmonica::getMotorTorque(double* dTorqueNm) -{ - // With motor sign: - *dTorqueNm = m_DriveParam.getSign() * m_dMotorCurr * m_DriveParam.getCurrToTorque(); - -} - - - -//---------------- -//---------------- -//Function, that proceeds (Elmo-) recorder readout - -//----------------------------------------------- -int CanDriveHarmonica::setRecorder(int iFlag, int iParam, std::string sParam) { - - switch(iFlag) { - case 0: //Configure Elmo Recorder for new Record, param = iRecordingGap, which specifies every which time quantum (4*90usec) a new data point is recorded - if(iParam < 1) iParam = 1; - ElmoRec->isInitialized(true); - ElmoRec->configureElmoRecorder(iParam, m_DriveParam.getDriveIdent()); //int startImmediately is default = 1 - return 0; - - case 1: //Query upload of previous recorded data, data is being proceeded after complete upload, param = recorded ID, filename - if(!ElmoRec->isInitialized(false)) return 1; - - if(seg_Data.statusFlag == segData::SDO_SEG_FREE) { - if( (iParam != 1) && (iParam != 2) && (iParam != 10) && (iParam != 16) ) { - iParam = 1; - std::cout << "Changed the Readout object to #1 as your selected object hasn't been recorded!" << std::endl; - } - ElmoRec->setLogFilename(sParam); - seg_Data.statusFlag = segData::SDO_SEG_WAITING; - ElmoRec->readoutRecorderTry(iParam); //as subindex, give the recorded variable - return 0; - } else { - std::cout << "Previous transmission not finished or colected data hasn't been proceeded yet" << std::endl; - return 2; - } - - break; - - case 2: //request status, still collecting data during ReadOut process? - if(seg_Data.statusFlag == segData::SDO_SEG_COLLECTING) { - //std::cout << "Transmission of data is still in progress" << std::endl; - return 2; - } else if(seg_Data.statusFlag == segData::SDO_SEG_PROCESSING) { - //std::cout << "Transmission of data finished, data not proceeded yet" << std::endl; - return 2; - } else if(seg_Data.statusFlag == segData::SDO_SEG_WAITING) { - //std::cout << "Still waiting for transmission to begin" << std::endl; - return 2; - } else { //finished transmission and finished proceeding - return 0; - } - - break; - - case 99: //Abort ongoing SDO data Transmission and clear collected data - sendSDOAbort(0x2030, 0x00, 0x08000020); //send general error abort - seg_Data.resetTransferData(); //!overwrites previous collected data (even from other processes) - return 0; - } - - return 0; -} diff --git a/cob_canopen_motor/common/src/ElmoRecorder.cpp b/cob_canopen_motor/common/src/ElmoRecorder.cpp deleted file mode 100644 index 17646a208..000000000 --- a/cob_canopen_motor/common/src/ElmoRecorder.cpp +++ /dev/null @@ -1,292 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -#include -#include -#include -#include -#include -#include - -ElmoRecorder::ElmoRecorder(CanDriveHarmonica * pParentHarmonicaDrive) { - m_pHarmonicaDrive = pParentHarmonicaDrive; - - m_bIsInitialized = false; - m_iReadoutRecorderTry = 0; -} - -ElmoRecorder::~ElmoRecorder() { -} - -bool ElmoRecorder::isInitialized(bool initNow) { - if(initNow) m_bIsInitialized = true; - return m_bIsInitialized; -} - -int ElmoRecorder::configureElmoRecorder(int iRecordingGap, int driveID, int startImmediately){ //iRecordingGap = N indicates that a new sample should be taken once per N time quanta - m_iDriveID = driveID; - - if(startImmediately >=2 ) startImmediately = 1; - - m_pHarmonicaDrive->IntprtSetInt(8, 'R', 'R', 0, 0); // Stop Recorder if it's active - // Record Main speed (index 0, ID1) - // Active Current (index 9, ID10) - // Main Position (index 1, ID2) - // Speed Command (index 15, ID16) - // RC = 2^(Signal1Index) + 2^(Signal2Index) + ..; e.g.: 2^0 + 2^1 + 2^9 + 2^15 = 33283; - m_pHarmonicaDrive->IntprtSetInt(8, 'R', 'C', 0, 33283); - // Set trigger type to immediate - m_pHarmonicaDrive->IntprtSetInt(8, 'R', 'P', 3, 0); - // Set Recording Gap - m_pHarmonicaDrive->IntprtSetInt(8, 'R', 'G', 0, iRecordingGap); - // Set Recording Length - // RL = (4096 / Number of Signals) - m_pHarmonicaDrive->IntprtSetInt(8, 'R', 'L', 0, 1024); - - // Set Time Quantum, Default: RP=0 -> TS * 4; TS is 90us by default - // m_pHarmonicaDrive->IntprtSetInt(8, 'R', 'P', 0, 0); - // ----> Total Recording Time = 90us * 4 * RG * RL - - m_pHarmonicaDrive->IntprtSetInt(8, 'R', 'R', 0, startImmediately + 1); //2 launches immediately (8, 'R', 'R', 0, 1) launches at next BG - - m_fRecordingStepSec = 0.000090 * 4 * iRecordingGap; - - return 0; -} - -int ElmoRecorder::readoutRecorderTry(int iObjSubIndex) { - //Request the SR (status register) and begin all the read-out process with this action. - //SDOData.statusFlag is segData::SDO_SEG_WAITING; - - m_iReadoutRecorderTry = 1; - m_iCurrentObject = iObjSubIndex; - - m_pHarmonicaDrive->requestStatus(); - - return 0; -} - -int ElmoRecorder::readoutRecorderTryStatus(int iStatusReg, segData& SDOData) { - if(m_iReadoutRecorderTry == 0) return 0; //only evaluate the SR, if we are really waiting for it (to save time and not to unintionally start a read-out) - - m_iReadoutRecorderTry = 0; - - //Bits 16-17 of status register contain recorder information - int iRecorderStatus = (0x30000 & iStatusReg) >> 16; - - if(iRecorderStatus == 0) { - std::cout << "Recorder " << m_iDriveID << " inactive with no valid data to upload" << std::endl; - SDOData.statusFlag = segData::SDO_SEG_FREE; - } else if(iRecorderStatus == 1) { - std::cout << "Recorder " << m_iDriveID << " waiting for a trigger event" << std::endl; - SDOData.statusFlag = segData::SDO_SEG_FREE; - } else if(iRecorderStatus == 2) { - std::cout << "Recorder " << m_iDriveID << " finished, valid data ready for use" << std::endl; - readoutRecorder(m_iCurrentObject); - //already set to SDOData.statusFlag = segData::SDO_SEG_WAITING; - } else if(iRecorderStatus == 3) { - std::cout << "Recorder " << m_iDriveID << " is still recording" << std::endl; - SDOData.statusFlag = segData::SDO_SEG_FREE; - } - - return 0; -} - -int ElmoRecorder::readoutRecorder(int iObjSubIndex){ - //initialize Upload of Recorded Data (object 0x2030) - int iObjIndex = 0x2030; - - m_pHarmonicaDrive->sendSDOUpload(iObjIndex, iObjSubIndex); - m_iCurrentObject = iObjSubIndex; - - return 0; -} - -int ElmoRecorder::processData(segData& SDOData) { - int iItemSize = 4; - int iItemCount = 0; - unsigned int iNumDataItems = 0; - bool bCollectFloats = true; - float fFloatingPointFactor = 0; - - std::vector vfResData[2]; - - //see SimplIQ CANopen DS 301 Implementation Guide, object 0x2030 - - //HEADER - //-------------------------------------- - //First 7 Bytes of the data sequence contain header information: - //Byte 0: First four bits: 4 = Long Int data type, 1 = Half Float data type, 5 = Double Float - // Next four bits: Recording frequency in 1 per n * TS => deltaT = n * 90µsec - //Byte 2, Byte 3: Number of recorded data points - //Byte 3 to 6: Floating point factor for data to be multiplied with - // - //Byte 7 to Byte (7+ iNumdataItems * 4) contain data - - //B[0]: Time quantum and data type - switch ((SDOData.data[0] >> 4) ) { - case 4: - bCollectFloats = false; - iItemSize = 4; - break; - case 5: - bCollectFloats = true; - iItemSize = 4; - break; - case 1: - bCollectFloats = true; - iItemSize = 2; - break; - default: - bCollectFloats = false; - iItemSize = 4; - break; - } - std::cout << ">>>>>ElmoRec: HEADER INFOS<<<<<\nData type is: " << (SDOData.data[0] >> 4) << std::endl; - - //std::cout << "fTimeQuantum from Header is " << fTimeQuantum << " m_fRecordingStepSec is " << m_fRecordingStepSec << std::endl; - - - //B[1]..[2] //Number of recorded items - iNumDataItems = (SDOData.data[2] << 8 | SDOData.data[1]); - //std::cout << "Number of recorded data points: " << iNumDataItems << std::endl; - - //B[3] ... [6] //Floating point factor - fFloatingPointFactor = convertBinaryToFloat( (SDOData.data[6] << 24) | (SDOData.data[5] << 16) | (SDOData.data[4] << 8) | (SDOData.data[3]) ); - std::cout << "Floating point factor for recorded values is: " << fFloatingPointFactor << std::endl; - - - if( ((SDOData.numTotalBytes-7)/iItemSize) != iNumDataItems) - std::cout << "SDODataSize announced in SDO-Header" << ((SDOData.numTotalBytes-7)/iItemSize) << " differs from NumDataItems by ElmoData-Header" << iNumDataItems << std::endl; - //END HEADER - //-------------------------------------- - - vfResData[0].assign(iNumDataItems, 0.0); - vfResData[1].assign(iNumDataItems, 0.0); - iItemCount = 0; - - //extract values from data stream, consider Little Endian conversion for every single object! - for(unsigned int i=7;i<=SDOData.data.size() - iItemSize; i=i+iItemSize) { - if(bCollectFloats) { - if(iItemSize == 4) - vfResData[1][iItemCount] = fFloatingPointFactor * convertBinaryToFloat( (SDOData.data[i] << 0) | (SDOData.data[i+1] << 8) | (SDOData.data[i+2] << 16) | (SDOData.data[i+3] << 24) ); - - //DEBUG - if(iItemCount == 120) - std::cout << (unsigned int)( (SDOData.data[i] << 0) | (SDOData.data[i+1] << 8) | (SDOData.data[i+2] << 16) | (SDOData.data[i+3] << 24) ) << std::endl; - - else vfResData[1][iItemCount] = fFloatingPointFactor * convertBinaryToHalfFloat( (SDOData.data[i] << 0) | (SDOData.data[i+1] << 8) | (SDOData.data[i+2] << 16) | (SDOData.data[i+3] << 24) ); - iItemCount ++; - } else { - vfResData[1][iItemCount] = fFloatingPointFactor * (float)( (SDOData.data[i] << 0) | (SDOData.data[i+1] << 8) | (SDOData.data[i+2] << 16) | (SDOData.data[i+3] << 24) ); - iItemCount ++; - } - - vfResData[0][iItemCount] = m_fRecordingStepSec * iItemCount; - } - - logToFile(m_sLogFilename, vfResData); - - SDOData.statusFlag = segData::SDO_SEG_FREE; - return 0; -} - -int ElmoRecorder::setLogFilename(std::string sLogFileprefix) { - m_sLogFilename = sLogFileprefix; - return 0; -} - - - -float ElmoRecorder::convertBinaryToFloat(unsigned int iBinaryRepresentation) { - //Converting binary-numbers to 32bit float values according to IEEE 754 see http://de.wikipedia.org/wiki/IEEE_754 - int iSign; - int iExponent; - unsigned int iMantissa; - float iNumMantissa = 0.0f; - - if((iBinaryRepresentation & (1 << 31)) == 0) //first bit is sign bit: 0 = +, 1 = - - iSign = 1; - else - iSign = -1; - - iExponent = ((iBinaryRepresentation >> 23) & 0xFF) - 127; //take away Bias(127) for positive and negative exponents - - iMantissa = (iBinaryRepresentation & 0x7FFFFF); //only keep mantissa part of binary number - - iNumMantissa = 1.0f; - - for(int i=1; i<=23; i++) { //calculate decimal places (convert binary mantissa to decimal number - if((iMantissa & (1 << (23-i))) > 0) { - iNumMantissa = iNumMantissa + pow(2,(-1)*i); - } - } - - return iSign * pow(2,iExponent) * iNumMantissa; -} - -float ElmoRecorder::convertBinaryToHalfFloat(unsigned int iBinaryRepresentation) { - //Converting binary-numbers to 16bit float values according to IEEE 754 see http://de.wikipedia.org/wiki/IEEE_754 - int iSign; - int iExponent; - unsigned int iMantissa; - float iNumMantissa = 0.0f; - - if((iBinaryRepresentation & (1 << 15)) == 0) //first bit is sign bit: 0 = +, 1 = - - iSign = 1; - else - iSign = -1; - - iExponent = ((iBinaryRepresentation >> 10) & 0x1F) - 15; //take away Bias(15) for positive and negative exponents - - iMantissa = (iBinaryRepresentation & 0x3FF); //only keep mantissa part of binary number - - iNumMantissa = 1.0f; - - for(int i=1; i<=10; i++) { //calculate decimal places (convert binary mantissa to decimal number - if((iMantissa & (1 << (10-i))) > 0) { - iNumMantissa = iNumMantissa + pow(2,(-1)*i); - } - } - - return iSign * pow(2,iExponent) * iNumMantissa; -} - -// Function for writing Logfile -int ElmoRecorder::logToFile(std::string filename, std::vector vtValues[]) { - std::stringstream outputFileName; - outputFileName << filename << "mot_" << m_iDriveID << "_" << m_iCurrentObject << ".log"; - - FILE* pFile; - //open FileStream - pFile = fopen(outputFileName.str().c_str(), "w"); - - //Check if there was a problem - if( pFile == NULL ) - { - std::cout << "Error while writing file: " << outputFileName.str() << " Maybe the selected folder does'nt exist." << std::endl; - } - else - { - // write all data from vector to file - for (unsigned int i = 0; i < vtValues[0].size(); i++) - fprintf(pFile, "%e %e\n", vtValues[0][i], vtValues[1][i]); - fclose(pFile); - } - - return true; -} diff --git a/cob_canopen_motor/package.xml b/cob_canopen_motor/package.xml deleted file mode 100644 index d03861139..000000000 --- a/cob_canopen_motor/package.xml +++ /dev/null @@ -1,20 +0,0 @@ - - cob_canopen_motor - 0.7.16 - The package cob_canopen_motor implements a controller-drive component which is connected to a can-bus and works with a canopen-interface. "CanDriveItf" provides a - more or less - generic interface to the controller-drive components. "CanDrvie..." then implements a specific setup, e.g. an ELMO Harmonica Controller in case of the "CanDriveHarmonica". - - Apache 2.0 - - http://ros.org/wiki/cob_canopen_motor - - - Matthias Gruhler - Christian Connette - - catkin - - cob_generic_can - cob_utilities - roscpp - - diff --git a/cob_driver/package.xml b/cob_driver/package.xml index e85039c1e..4d27efd83 100644 --- a/cob_driver/package.xml +++ b/cob_driver/package.xml @@ -14,7 +14,6 @@ catkin cob_bms_driver - cob_canopen_motor cob_generic_can cob_light cob_mimic From 39de833f1c28c949062761140e1e132f374c151a Mon Sep 17 00:00:00 2001 From: fmessmer Date: Wed, 20 Mar 2024 16:56:30 +0100 Subject: [PATCH 09/14] remove cob_generic_can --- cob_driver/package.xml | 1 - cob_generic_can/CHANGELOG.rst | 214 ---------- cob_generic_can/CMakeLists.txt | 34 -- .../common/include/cob_generic_can/CanESD.h | 90 ----- .../common/include/cob_generic_can/CanItf.h | 129 ------ .../common/include/cob_generic_can/CanMsg.h | 209 ---------- .../include/cob_generic_can/CanPeakSys.h | 54 --- .../include/cob_generic_can/CanPeakSysUSB.h | 61 --- .../include/cob_generic_can/SocketCan.h | 57 --- cob_generic_can/common/src/CanESD.cpp | 379 ------------------ cob_generic_can/common/src/CanPeakSys.cpp | 260 ------------ cob_generic_can/common/src/CanPeakSysUSB.cpp | 339 ---------------- cob_generic_can/common/src/SocketCan.cpp | 173 -------- cob_generic_can/package.xml | 20 - 14 files changed, 2020 deletions(-) delete mode 100644 cob_generic_can/CHANGELOG.rst delete mode 100644 cob_generic_can/CMakeLists.txt delete mode 100644 cob_generic_can/common/include/cob_generic_can/CanESD.h delete mode 100644 cob_generic_can/common/include/cob_generic_can/CanItf.h delete mode 100644 cob_generic_can/common/include/cob_generic_can/CanMsg.h delete mode 100644 cob_generic_can/common/include/cob_generic_can/CanPeakSys.h delete mode 100644 cob_generic_can/common/include/cob_generic_can/CanPeakSysUSB.h delete mode 100644 cob_generic_can/common/include/cob_generic_can/SocketCan.h delete mode 100644 cob_generic_can/common/src/CanESD.cpp delete mode 100644 cob_generic_can/common/src/CanPeakSys.cpp delete mode 100644 cob_generic_can/common/src/CanPeakSysUSB.cpp delete mode 100644 cob_generic_can/common/src/SocketCan.cpp delete mode 100644 cob_generic_can/package.xml diff --git a/cob_driver/package.xml b/cob_driver/package.xml index 4d27efd83..815d3ec15 100644 --- a/cob_driver/package.xml +++ b/cob_driver/package.xml @@ -14,7 +14,6 @@ catkin cob_bms_driver - cob_generic_can cob_light cob_mimic cob_phidgets diff --git a/cob_generic_can/CHANGELOG.rst b/cob_generic_can/CHANGELOG.rst deleted file mode 100644 index b304bbc91..000000000 --- a/cob_generic_can/CHANGELOG.rst +++ /dev/null @@ -1,214 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package cob_generic_can -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.7.16 (2024-02-20) -------------------- - -0.7.15 (2023-11-06) -------------------- - -0.7.14 (2022-11-17) -------------------- - -0.7.13 (2022-07-29) -------------------- - -0.7.12 (2022-03-15) -------------------- - -0.7.11 (2022-01-12) -------------------- - -0.7.10 (2021-12-23) -------------------- - -0.7.9 (2021-11-26) ------------------- - -0.7.8 (2021-10-19) ------------------- - -0.7.7 (2021-08-02) ------------------- - -0.7.6 (2021-05-10) ------------------- - -0.7.5 (2021-04-06) ------------------- - -0.7.4 (2020-10-14) ------------------- -* Merge pull request `#417 `_ from fmessmer/test_noetic - test noetic -* Bump CMake version to avoid CMP0048 warning -* Contributors: Felix Messmer, fmessmer - -0.7.3 (2020-03-18) ------------------- - -0.7.2 (2020-03-18) ------------------- -* Merge pull request `#408 `_ from fmessmer/ci_updates - [travis] ci updates -* catkin_lint fixes -* Contributors: Felix Messmer, fmessmer - -0.7.1 (2019-11-07) ------------------- - -0.7.0 (2019-08-06) ------------------- -* Merge pull request `#380 `_ from ipa-jba/fix/boost_shared_ptr - [Melodic] combined melodify pr -* use all the pointer names I could find -* replace boost in cob_generic_can -* Merge pull request `#396 `_ from HannesBachter/indigo_dev - 0.6.15 -* Contributors: Felix Messmer, Jannik Abbenseth - -0.6.15 (2019-07-17) -------------------- - -0.6.14 (2019-06-07) -------------------- - -0.6.13 (2019-03-14) -------------------- - -0.6.12 (2018-07-21) -------------------- - -0.6.11 (2018-01-07) -------------------- -* Merge remote-tracking branch 'origin/indigo_release_candidate' into indigo_dev -* Merge pull request `#341 `_ from ipa-fxm/APACHE_license - use license apache 2.0 -* use license apache 2.0 -* Contributors: Felix Messmer, ipa-fxm, ipa-uhr-mk - -0.6.10 (2017-07-24) -------------------- - -0.6.9 (2017-07-18) ------------------- -* SocketCAN support (`#269 `_) - * SocketCAN finished implementaiton. - * Licence update and formating. - * Run socketcan_interface in the new thread. - * Removed cmake_modules for boost. - * Thread initialisation moved to front and added sleep to let enough time to start the thread since this caused problems in some cases (on some computers). - * Using ThreadedSocketCANInterface. - * BufferedReader working - * Updated dependecies and removed initCAN() function. - * Clean SocketCAN implementation and set Timeout to correct unit (MicroSeconds) -* manually fix changelog -* Contributors: Denis Štogl, ipa-fxm - -0.6.8 (2016-10-10) ------------------- - -0.6.7 (2016-04-02) ------------------- - -0.6.6 (2016-04-01) ------------------- -* Update accorting to comments from ipa-fxm -* Correction of include and name -* Correct interface. -* compiles... -* Starting with SocketCAN -* Contributors: Denis Štogl - -0.6.5 (2015-08-31) ------------------- - -0.6.4 (2015-08-25) ------------------- -* explicit dependency to boost -* remove obsolete autogenerated mainpage.dox files -* remove trailing whitespaces -* migrate to package format 2 -* sort dependencies -* critically review dependencies -* Contributors: ipa-fxm - -0.6.3 (2015-06-17) ------------------- - -0.6.2 (2014-12-15) ------------------- - -0.6.1 (2014-09-17) ------------------- - -0.6.0 (2014-09-09) ------------------- -* Fixed typo -* Extended with ReciveMsgTimeout method. -* Correction... -* Extended CanPeakSysUSB for usage without (obsolete) Ini-file, some stuff clearly written -* Contributors: Denis Štogl - -0.5.7 (2014-08-26) ------------------- -* Merge pull request `#163 `_ from ipa320/hydro_dev - updates from hydro_dev -* 0.5.6 -* update changelog -* Cleaned up cob_driver with reduced deps to compile on indigo -* Contributors: Alexander Bubeck, Florian Weisshardt - -0.5.6 (2014-08-26) ------------------- -* Merge pull request `#163 `_ from ipa320/hydro_dev - updates from hydro_dev -* Cleaned up cob_driver with reduced deps to compile on indigo -* Contributors: Alexander Bubeck, Florian Weisshardt - -0.5.3 (2014-03-31) ------------------- -* install tags -* Contributors: ipa-fxm - -0.5.2 (2014-03-20) ------------------- - -0.5.1 (2014-03-20) ------------------- -* added missing install tags -* Some small dependency tweaks. -* fixed gcc4.7 build error (sleep and usleep undefined) -* cleaned up CMakeLists and added install directives -* further modifications for catkin, now everything is compiling and linking -* futher include and linkpath modifications -* compiling but still some linker errors -* Second catkinization push -* First catkinization, still need to update some CMakeLists.txt -* cob_generic_can: selectable __DEBUG__ output -* cob_generic_can: error message filtering -* cob_generic_can: restructured and added diagnostic outputs for CAN errors -* merge -* remove compiler error -* cob_generic_can: debug outputs filtered -* cob_generic_can: detecting heavy CAN-bus loads -* cob_generic_can: trying to detect BUSOFF of can and restart -* cob_generic_can: Starting debug on base crash error -* camera settings added for head -* cleanup in cob_driver -* added inifile strings for pcan devices -* cob_head_axis: correctly working, but front and back is switched -* update documentation and deleted tf broadcaster -* cleanup in stacks -* cleanup in cob_driver -* merge wit cpc -* After merging in review branch -* debugging cob_camera_axis; not yet running -* added windows.h; some modifications in ElmoCtrl -> not yet working -* added classes to implement ESD can-itf; incorporated ESD interface as an option in cob_base_drive_chain-node via CanCtrlPltfCOb3; added windows.h to cob_utilities package -* Updated Can Classes to new file structure; removed some leftovers; corrected comments at the beginning considering association to stacks and packages; moved Mutex.h to Utilities; - Debugged compiler error in cob_base_drive_chain -* renamed to cob_ -* merged master -* renamed packages to cob_ convention -* Contributors: Alexander Bubeck, Christian Connette, Richard Bormann, abubeck, cob, cpc, cpc-pk, ipa-bnm, ipa-cpc, ipa-fmw diff --git a/cob_generic_can/CMakeLists.txt b/cob_generic_can/CMakeLists.txt deleted file mode 100644 index 39c43a698..000000000 --- a/cob_generic_can/CMakeLists.txt +++ /dev/null @@ -1,34 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(cob_generic_can) - -find_package(catkin REQUIRED COMPONENTS cob_utilities libntcan libpcan socketcan_interface) - -catkin_package( - CATKIN_DEPENDS cob_utilities libntcan libpcan socketcan_interface - INCLUDE_DIRS common/include - LIBRARIES ${PROJECT_NAME}_peaksysusb ${PROJECT_NAME}_peaksys ${PROJECT_NAME}_esd ${PROJECT_NAME}_socketcan -) - -### BUILD ### -include_directories(common/include ${catkin_INCLUDE_DIRS}) - -add_library(${PROJECT_NAME}_esd common/src/CanESD.cpp) -add_library(${PROJECT_NAME}_peaksys common/src/CanPeakSys.cpp) -add_library(${PROJECT_NAME}_peaksysusb common/src/CanPeakSysUSB.cpp) -add_library(${PROJECT_NAME}_socketcan common/src/SocketCan.cpp) - -target_link_libraries(${PROJECT_NAME}_esd ${catkin_LIBRARIES}) -target_link_libraries(${PROJECT_NAME}_peaksys ${catkin_LIBRARIES}) -target_link_libraries(${PROJECT_NAME}_peaksysusb ${catkin_LIBRARIES}) -target_link_libraries(${PROJECT_NAME}_socketcan ${catkin_LIBRARIES}) - -### INSTALL ### -install(TARGETS ${PROJECT_NAME}_esd ${PROJECT_NAME}_peaksys ${PROJECT_NAME}_peaksysusb ${PROJECT_NAME}_socketcan - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -install(DIRECTORY common/include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) diff --git a/cob_generic_can/common/include/cob_generic_can/CanESD.h b/cob_generic_can/common/include/cob_generic_can/CanESD.h deleted file mode 100644 index dd639e7ee..000000000 --- a/cob_generic_can/common/include/cob_generic_can/CanESD.h +++ /dev/null @@ -1,90 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -#ifndef CANESD_INCLUDEDEF_H -#define CANESD_INCLUDEDEF_H -//----------------------------------------------- - -#include -// general includes -#include -#include -#include - -// Headers provided by other cob-packages -#include - -// Headers provided by other cob-packages which should be avoided/removed -#include -#include - - -//----------------------------------------------- -/** - * Driver of the CAN controller of ESD. - */ -class CanESD : public CanItf -{ -private: - - BYTE m_DeviceNr; - BYTE m_BaudRate; - NTCAN_HANDLE m_Handle; - int m_LastID; - bool m_bObjectMode; - bool m_bIsTXError; - Mutex m_Mutex; - - IniFile m_IniFile; - - void initIntern(); - -public: - CanESD(const char* cIniFile, bool bObjectMode = false); - ~CanESD(); - bool init_ret(); - void init(){}; - bool transmitMsg(CanMsg CMsg, bool bBlocking = true); - bool receiveMsgRetry(CanMsg* pCMsg, int iNrOfRetry); - bool receiveMsg(CanMsg* pCMsg); - bool receiveMsgTimeout(CanMsg* pCMsg, int nMicroSeconds); - bool isObjectMode() { return m_bObjectMode; } - bool isTransmitError() { return m_bIsTXError; } - -protected: - - /*! - \fn CanESD::invert(int id) - */ - /** - * Invert a give ID in 1-complement. - * Note: Only 11 bits are used, i.e. the range is from 0x00 to 0x7FF. - * @param id The id to be inverted. - * @return The inverted id. - */ - int invert(int id) - { - return (~id) & 0x7F8; - } - - int canIdAddGroup(NTCAN_HANDLE handle, int id); - - std::string GetErrorStr(int ntstatus) const; - int readEvent(); -}; -//----------------------------------------------- -#endif diff --git a/cob_generic_can/common/include/cob_generic_can/CanItf.h b/cob_generic_can/common/include/cob_generic_can/CanItf.h deleted file mode 100644 index 3281c39c0..000000000 --- a/cob_generic_can/common/include/cob_generic_can/CanItf.h +++ /dev/null @@ -1,129 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -#ifndef CANITF_INCLUDEDEF_H -#define CANITF_INCLUDEDEF_H -//----------------------------------------------- -#include -//----------------------------------------------- - -// for types and baudrates see: https://github.com/ipa320/cob_robots/blob/hydro_dev/cob_hardware_config/raw3-5/config/base/CanCtrl.ini -#define CANITFTYPE_CAN_PEAK 0 -#define CANITFTYPE_CAN_PEAK_USB 1 -#define CANITFTYPE_CAN_ESD 2 -#define CANITFTYPE_CAN_DUMMY 3 -#define CANITFTYPE_CAN_BECKHOFF 4 -#define CANITFTYPE_SOCKET_CAN 5 - -#define CANITFBAUD_1M 0x0 -#define CANITFBAUD_500K 0x2 -#define CANITFBAUD_250K 0x4 -#define CANITFBAUD_125K 0x6 -#define CANITFBAUD_50K 0x9 -#define CANITFBAUD_20K 0xB -#define CANITFBAUD_10K 0xD - -/** - * General interface of the CAN bus. - * \ingroup DriversCanModul - */ -class CanItf -{ -public: - enum CanItfType { - CAN_PEAK = 0, - CAN_PEAK_USB = 1, - CAN_ESD = 2, - CAN_DUMMY = 3, - CAN_BECKHOFF = 4, - CAN_SOCKETCAN = 5 - }; - - /** - * The destructor does not necessarily have to be overwritten. - * But it makes sense to close any resources like handles. - */ - virtual ~CanItf() { - } - - /** - * Initializes the CAN bus and returns success. - */ - virtual bool init_ret() = 0; - - /** - * Initializes the CAN bus. - */ - virtual void init() = 0; - - /** - * Sends a CAN message. - * @param pCMsg CAN message - * @param bBlocking specifies whether send should be blocking or non-blocking - */ - virtual bool transmitMsg(CanMsg CMsg, bool bBlocking = true) = 0; - - /** - * Reads a CAN message. - * @return true if a message is available - */ - virtual bool receiveMsg(CanMsg* pCMsg) = 0; - - /** - * Reads a CAN message. - * The function blocks between the attempts. - * @param pCMsg CAN message - * @param iNrOfRetry number of retries - * @return true if a message is available - */ - virtual bool receiveMsgRetry(CanMsg* pCMsg, int iNrOfRetry) = 0; - - /** - * Reads a CAN message with timeout. - * @param pCMsg CAN message - * @param nMicroSecTimeout timeout in us - * @return true if a message is available - */ - virtual bool receiveMsgTimeout(CanMsg* pCMsg, int nMicroSecTimeout) = 0; - - /** - * Check if the current CAN interface was opened on OBJECT mode. - * @return true if opened in OBJECT mode, false if not. - */ - virtual bool isObjectMode() = 0; - - /** - * Set the CAN interface type. This is necessary to implement - * a proper CAN bus simulation. - * @param iType The CAN interface type. - */ - void setCanItfType(CanItfType iType) { m_iCanItfType = iType; } - - /** - * Get the CAN interface type. This is necessary to implement - * a proper CAN bus simulation. - * @return The CAN interface type. - */ - CanItfType getCanItfType() { return m_iCanItfType; } - -private: - /// The CAN interface type. - CanItfType m_iCanItfType; -}; -//----------------------------------------------- - -#endif diff --git a/cob_generic_can/common/include/cob_generic_can/CanMsg.h b/cob_generic_can/common/include/cob_generic_can/CanMsg.h deleted file mode 100644 index e83cc50df..000000000 --- a/cob_generic_can/common/include/cob_generic_can/CanMsg.h +++ /dev/null @@ -1,209 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -#ifndef CANMSG_INCLUDEDEF_H -#define CANMSG_INCLUDEDEF_H -//----------------------------------------------- -#include -//----------------------------------------------- - -/** - * Represents a CAN message. - * \ingroup DriversCanModul - */ -class CanMsg -{ -public: - /// Include typedefs from windows.h - typedef unsigned char BYTE; - /// @todo This should be private. - int m_iID; - /// @todo This should be private. - int m_iLen; - /// @todo This should be private. - int m_iType; - -protected: - /** - * A CAN message consists of eight bytes. - */ - BYTE m_bDat[8]; - -public: - /** - * Default constructor. - */ - CanMsg() - { - m_iID = 0; - m_iLen = 8; - m_iType = 0x00; - } - - /** - * Sets the bytes to the telegram. - */ - void set(BYTE Data0=0, BYTE Data1=0, BYTE Data2=0, BYTE Data3=0, BYTE Data4=0, BYTE Data5=0, BYTE Data6=0, BYTE Data7=0) - { - m_bDat[0] = Data0; - m_bDat[1] = Data1; - m_bDat[2] = Data2; - m_bDat[3] = Data3; - m_bDat[4] = Data4; - m_bDat[5] = Data5; - m_bDat[6] = Data6; - m_bDat[7] = Data7; - } - - /** - * Set the byte at the given position. - */ - void setAt(BYTE data, int iNr) - { - m_bDat[iNr] = data; - } - - /** - * Gets the bytes of the telegram. - */ - void get(BYTE* pData0, BYTE* pData1, BYTE* pData2, BYTE* pData3, BYTE* pData4, BYTE* pData5, BYTE* pData6, BYTE* pData7) - { - *pData0 = m_bDat[0]; - *pData1 = m_bDat[1]; - *pData2 = m_bDat[2]; - *pData3 = m_bDat[3]; - *pData4 = m_bDat[4]; - *pData5 = m_bDat[5]; - *pData6 = m_bDat[6]; - *pData7 = m_bDat[7]; - } - - /** - * Returns a spezific byte of the telegram. - * @param iNr number of the byte. - */ - int getAt(int iNr) - { - return m_bDat[iNr]; - } - - /** - * Prints the telegram to the standard output. - * @deprecated function uses a spetific format of the telegram. - */ - int printCanIdentMsgStatus() - { - if(getStatus() == 0) - { - std::cout << "ID= " << m_iID << " " << "Cmd= " << getCmd() << " " << "Msg_OK" << std::endl; - return 0; - } - else - { - std::cout << "ID= " << m_iID << " " << "Cmd= " << getCmd() << " " << "Msg_Error" << std::endl; - return -1; - } - } - - /** - * Prints the telegram. - */ - void print() - { - std::cout << "id= " << m_iID << " type= " << m_iType << " len= " << m_iLen << " data= " << - (int)m_bDat[0] << " " << (int)m_bDat[1] << " " << (int)m_bDat[2] << " " << (int)m_bDat[3] << " " << - (int)m_bDat[4] << " " << (int)m_bDat[5] << " " << (int)m_bDat[6] << " " << (int)m_bDat[7] << std::endl; - } - - /** - * @deprecated function uses a spetific format of the telegram. - */ - int getStatus() - { - //bit 0 and bit 1 contain MsgStatus - return (int)(m_bDat[7] & 0x0003); - } - - /** - * @deprecated function uses a spetific format of the telegram. - */ - int getCmd() - { - return (m_bDat[7] >> 2); - } - - /** - * Get the identifier stored in this message structure. - * @return the message identifier. - */ - int getID() - { - return m_iID; - } - - /** - * Set the message identifier within this message structure. - * @param id The message identifier. Its value must be in the range [0..2047], i.e. - * 29-bit identifiers are not supported here. - */ - void setID(int id) - { - if( (0 <= id) && (id <= 2047) ) - m_iID = id; - } - - /** - * Get the message length set within this data structure. - * @return The message length in the range [0..8]. - */ - int getLength() - { - return m_iLen; - } - - /** - * Set the message length within this message structure. - * @param len The message length. Its value must be in the range [0..8]. - */ - void setLength(int len) - { - if( (0 <= len) && (len <= 8) ) - m_iLen = len; - } - - /** - * Get the message type. By default, the type is 0x00. - * @return The message type. - */ - int getType() - { - return m_iType; - } - - /** - * Set the message type. By default, the type is 0x00. - * @param type The message type. - */ - void setType(int type) - { - m_iType = type; - } - - -}; -//----------------------------------------------- -#endif diff --git a/cob_generic_can/common/include/cob_generic_can/CanPeakSys.h b/cob_generic_can/common/include/cob_generic_can/CanPeakSys.h deleted file mode 100644 index 0ed7a0d0b..000000000 --- a/cob_generic_can/common/include/cob_generic_can/CanPeakSys.h +++ /dev/null @@ -1,54 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -#ifndef CANPEAKSYS_INCLUDEDEF_H -#define CANPEAKSYS_INCLUDEDEF_H -//----------------------------------------------- -#include -#include -#include -//----------------------------------------------- - -class CanPeakSys : public CanItf -{ -public: - // --------------- Interface - CanPeakSys(const char* cIniFile); - ~CanPeakSys(); - bool init_ret(); - void init(); - void destroy() {} - bool transmitMsg(CanMsg CMsg, bool bBlocking = true); - bool receiveMsg(CanMsg* pCMsg); - bool receiveMsgRetry(CanMsg* pCMsg, int iNrOfRetry); - bool receiveMsgTimeout(CanMsg* pCMsg, int nSecTimeout); - bool isObjectMode() { return false; } - -private: - // --------------- Types - HANDLE m_handle; - - bool m_bInitialized; - IniFile m_IniFile; - bool m_bSimuEnabled; - - static const int c_iInterrupt; - static const int c_iPort; -}; -//----------------------------------------------- -#endif - diff --git a/cob_generic_can/common/include/cob_generic_can/CanPeakSysUSB.h b/cob_generic_can/common/include/cob_generic_can/CanPeakSysUSB.h deleted file mode 100644 index b75bd23f6..000000000 --- a/cob_generic_can/common/include/cob_generic_can/CanPeakSysUSB.h +++ /dev/null @@ -1,61 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -#ifndef CANPEAKSYSUSB_INCLUDEDEF_H -#define CANPEAKSYSUSB_INCLUDEDEF_H -//----------------------------------------------- -#include -#include -#include -//----------------------------------------------- - -class CANPeakSysUSB : public CanItf -{ -public: - // --------------- Interface - CANPeakSysUSB(const char* device, int baudrate); - CANPeakSysUSB(const char* cIniFile); - ~CANPeakSysUSB(); - bool init_ret(); - void init(); - void destroy() {}; - bool transmitMsg(CanMsg CMsg, bool bBlocking = true); - bool receiveMsg(CanMsg* pCMsg); - bool receiveMsgRetry(CanMsg* pCMsg, int iNrOfRetry); - bool receiveMsgTimeout(CanMsg* pCMsg, int nMicroSeconds); - bool isObjectMode() { return false; } - -private: - // --------------- Types - HANDLE m_handle; - - bool m_bInitialized; - IniFile m_IniFile; - bool m_bSimuEnabled; - const char* p_cDevice; - int m_iBaudrateVal; - - static const int c_iInterrupt; - static const int c_iPort; - - bool initCAN(); - - void outputDetailedStatus(); -}; -//----------------------------------------------- -#endif - diff --git a/cob_generic_can/common/include/cob_generic_can/SocketCan.h b/cob_generic_can/common/include/cob_generic_can/SocketCan.h deleted file mode 100644 index 4e89f6eda..000000000 --- a/cob_generic_can/common/include/cob_generic_can/SocketCan.h +++ /dev/null @@ -1,57 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -#ifndef SOCKETCAN_INCLUDEDEF_H -#define SOCKETCAN_INCLUDEDEF_H -//----------------------------------------------- -#include - -#include -#include -#include -#include -//----------------------------------------------- - -class SocketCan : public CanItf -{ -public: - // --------------- Interface - SocketCan ( const char* device, int baudrate ); - SocketCan ( const char* device ); - ~SocketCan(); - bool init_ret(); - void init(); - bool transmitMsg ( CanMsg CMsg, bool bBlocking = true ); - bool receiveMsg ( CanMsg* pCMsg ); - bool receiveMsgRetry ( CanMsg* pCMsg, int iNrOfRetry ); - bool receiveMsgTimeout ( CanMsg* pCMsg, int nMicroSecTimeout ); - bool isObjectMode() { - return false; - } - -private: - // --------------- Types - can::ThreadedSocketCANInterfaceSharedPtr m_handle; - can::BufferedReader m_reader; - - bool m_bInitialized; - const char* p_cDevice; - - void print_error(const can::State& state); -}; -//----------------------------------------------- -#endif diff --git a/cob_generic_can/common/src/CanESD.cpp b/cob_generic_can/common/src/CanESD.cpp deleted file mode 100644 index 69abc0533..000000000 --- a/cob_generic_can/common/src/CanESD.cpp +++ /dev/null @@ -1,379 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -// general includes - -// Headers provided by other cob-packages -#include - -// Headers provided by other cob-packages which should be avoided/removed - - -//----------------------------------------------- -CanESD::CanESD(const char* cIniFile, bool bObjectMode) -{ - m_bObjectMode = bObjectMode; - m_bIsTXError = false; - m_IniFile.SetFileName(cIniFile, "CanESD.cpp"); - initIntern(); -} - -//----------------------------------------------- -/** - * Destructor. - * Release the allocated resources. - */ -CanESD::~CanESD() -{ - std::cout << "Closing CAN handle" << std::endl; - canClose(m_Handle); -} - -//----------------------------------------------- -bool CanESD::init_ret() -{ - // Not implemented yet - return false; -} - -//----------------------------------------------- -void CanESD::initIntern() -{ - int ret=0; - ret = 0; - int iCanNet = 1; - m_IniFile.GetKeyInt( "CanCtrl", "NetESD", &iCanNet, true); - - int iBaudrateVal = 2; - m_IniFile.GetKeyInt( "CanCtrl", "BaudrateVal", &iBaudrateVal, true); - - std::cout << "Initializing CAN network with id =" << iCanNet << ", baudrate=" << iBaudrateVal << std::endl; - - int iRet; - if( m_bObjectMode ) - iRet = canOpen(iCanNet, NTCAN_MODE_OBJECT, 10000, 10000, 1000, 0, &m_Handle); - else - iRet = canOpen(iCanNet, 0, 10000, 10000, 1000, 0, &m_Handle); - Sleep(300); - - if(iRet == NTCAN_SUCCESS) - std::cout << "CanESD::CanESD(), init ok" << std::endl; - else - std::cout << "error in CANESD::receiveMsg: " << GetErrorStr(iRet) << std::endl; - - iRet = canSetBaudrate(m_Handle, iBaudrateVal); - if(iRet == NTCAN_SUCCESS) - std::cout << "CanESD::CanESD(), canSetBaudrate ok" << std::endl; - else - std::cout << "error in CANESD::receiveMsg: " << GetErrorStr(iRet) << std::endl; - Sleep(300); - - //long lArg; - iRet = canIoctl(m_Handle, NTCAN_IOCTL_FLUSH_RX_FIFO, NULL); - - // MMB/24.02.2006: Add all 11-bit identifiers as there is no loss in performance. - for( int i=0; i<=0x7FF; ++i ) { - iRet = canIdAdd( m_Handle, i ); - if(iRet != NTCAN_SUCCESS) - std::cout << "error in CANESD::receiveMsg: " << GetErrorStr(iRet) << std::endl; - } - - - Sleep(300); - - m_LastID = -1; -} - -//----------------------------------------------- -/** - * Transmit a message via the CAN bus. - * Additionally, an error flag is set internally if the transmission does not succeed. - * @param CMsg Structure containing the CAN message. - * @return true on success, false on failure. - */ -bool CanESD::transmitMsg(CanMsg CMsg, bool bBlocking) -{ - CMSG NTCANMsg; - NTCANMsg.id = CMsg.m_iID; - NTCANMsg.len = CMsg.m_iLen; - - for(int i=0; i<8; i++) - NTCANMsg.data[i] = CMsg.getAt(i); - - int ret; - int32_t len; - bool bRet = true; - - len = 1; - - if (bBlocking) - ret = canWrite(m_Handle, &NTCANMsg, &len, NULL); - else - ret = canSend(m_Handle, &NTCANMsg, &len); - - if( ret != NTCAN_SUCCESS) - { - std::cout << "error in CANESD::transmitMsg: " << GetErrorStr(ret) << std::endl; - bRet = false; - } - - m_LastID = (int)NTCANMsg.data[0]; - - m_bIsTXError = !bRet; - return bRet; -} - -//----------------------------------------------- -bool CanESD::receiveMsgRetry(CanMsg* pCMsg, int iNrOfRetry) -{ - //int id = pCMsg->m_iID; - CMSG NTCANMsg; - NTCANMsg.len = 8; - - int32_t len; - int i, ret; - bool bRet = true; - - i=0; - - len = 1; - - do - { - len = 1; - ret = canTake(m_Handle, &NTCANMsg, &len); - i++; - Sleep(10); - } - - while((len == 0) && (i < iNrOfRetry)); - - if(i == iNrOfRetry) - { - if( ret != NTCAN_SUCCESS ) - std::cout << "error in CANESD::receiveMsgRetry: " << GetErrorStr(ret) << std::endl; - - bRet = false; - } - else - { - pCMsg->m_iID = NTCANMsg.id; - pCMsg->m_iLen = NTCANMsg.len; - pCMsg->set(NTCANMsg.data[0], NTCANMsg.data[1], NTCANMsg.data[2], NTCANMsg.data[3], - NTCANMsg.data[4], NTCANMsg.data[5], NTCANMsg.data[6], NTCANMsg.data[7]); - } - - return bRet; -} - -//----------------------------------------------- -bool CanESD::receiveMsg(CanMsg* pCMsg) -{ - CMSG NTCANMsg; - NTCANMsg.len = 8; - - int ret; - int32_t len; - bool bRet = true; - - len = 1; - - // Debug valgrind - NTCANMsg.data[0] = 0; - NTCANMsg.data[1] = 0; - NTCANMsg.data[2] = 0; - NTCANMsg.data[3] = 0; - NTCANMsg.data[4] = 0; - NTCANMsg.data[5] = 0; - NTCANMsg.data[6] = 0; - NTCANMsg.data[7] = 0; - NTCANMsg.msg_lost = 0; - NTCANMsg.id = 0; - NTCANMsg.len = 0; - - pCMsg->set(0,0,0,0,0,0,0,0); - - - if( !isObjectMode() ) { - pCMsg->m_iID = 0; - } else { - NTCANMsg.id = pCMsg->m_iID; - } - - ret = canTake(m_Handle, &NTCANMsg, &len); - - if( !isObjectMode() ) { - if( (len == 1) && (ret == NTCAN_SUCCESS) ) - { - // message received - pCMsg->m_iID = NTCANMsg.id; - pCMsg->m_iLen = NTCANMsg.len; - pCMsg->set(NTCANMsg.data[0], NTCANMsg.data[1], NTCANMsg.data[2], NTCANMsg.data[3], - NTCANMsg.data[4], NTCANMsg.data[5], NTCANMsg.data[6], NTCANMsg.data[7]); - bRet = true; - } - else - { - // no message - if( ret != NTCAN_SUCCESS) - { - // error - std::cout << "error in CANESD::receiveMsg: " << GetErrorStr(ret) << std::endl; - } - - pCMsg->m_iID = NTCANMsg.id; - pCMsg->set(0,0,0,0,0,0,0,0); - - bRet = false; - } - } else { - if( len == 16 ) { - // No message was received yet. - pCMsg->m_iID = NTCANMsg.id; - pCMsg->set(0,0,0,0,0,0,0,0); - bRet = false; - } else { - pCMsg->m_iID = NTCANMsg.id; - pCMsg->m_iLen = NTCANMsg.len; - pCMsg->set(NTCANMsg.data[0], NTCANMsg.data[1], NTCANMsg.data[2], NTCANMsg.data[3], - NTCANMsg.data[4], NTCANMsg.data[5], NTCANMsg.data[6], NTCANMsg.data[7]); - bRet = true; - } - } - - if( NTCANMsg.msg_lost != 0 ) - std::cout << (int)(NTCANMsg.msg_lost) << " messages lost!" << std::endl; - - return bRet; -} - - -//----------------------------------------------- -bool CanESD::receiveMsgTimeout(CanMsg* pCMsg, int nMicroSecTimeout) -{ - // Not implemented yet - return false; -} - -/** - * Add a group of CAN identifier to the handle, so it can be received. - * The identifiers are generated by inverting the id and adding each value between 0 and 7 - * This is used for generating the answer commands by the RCS5000. - * @param handle The handle to add the identifiers to. - * @param id The command id sent to the RCS5000. - * @return NTCAN_SUCESS if ok, or an error code. - */ -int CanESD::canIdAddGroup(NTCAN_HANDLE handle, int id) -{ - int result = NTCAN_SUCCESS; - int i = 0; - int iRes = 0; - int cmd_id = invert(id); - - for( i=0; i<8; ++i) { - iRes = canIdAdd(m_Handle, cmd_id+i); - - if( iRes != NTCAN_SUCCESS ) { - std::cout << "Adding CAN ID " << cmd_id+i << " failed with errorcode: " << iRes << " " << GetErrorStr(iRes) << std::endl; - result = iRes; - } - } - - return result; -} - -//----------------------------------------------- -std::string CanESD::GetErrorStr(int ntstatus) const -{ - switch (ntstatus) - { - case NTCAN_SUCCESS: return "NTCAN_SUCCESS"; - case NTCAN_RX_TIMEOUT: return "NTCAN_RX_TIMEOUT"; - case NTCAN_TX_TIMEOUT: return "NTCAN_TX_TIMEOUT"; - case NTCAN_TX_ERROR: return "NTCAN_TX_ERROR"; - case NTCAN_CONTR_OFF_BUS: return "NTCAN_CONTR_OFF_BUS"; - case NTCAN_CONTR_BUSY: return "NTCAN_CONTR_BUSY"; - case NTCAN_CONTR_WARN: return "NTCAN_CONTR_WARN"; - case NTCAN_NO_ID_ENABLED: return "NTCAN_NO_ID_ENABLED"; - case NTCAN_ID_ALREADY_ENABLED: return "NTCAN_ID_ALREADY_ENABLED"; - case NTCAN_ID_NOT_ENABLED: return "NTCAN_ID_NOT_ENABLED"; - - case NTCAN_INVALID_FIRMWARE: return "NTCAN_INVALID_FIRMWARE"; - case NTCAN_MESSAGE_LOST: return "NTCAN_MESSAGE_LOST"; - case NTCAN_INVALID_HARDWARE: return "NTCAN_INVALID_HARDWARE"; - - case NTCAN_PENDING_WRITE: return "NTCAN_PENDING_WRITE"; - case NTCAN_PENDING_READ: return "NTCAN_PENDING_READ"; - case NTCAN_INVALID_DRIVER: return "NTCAN_INVALID_DRIVER"; - - case NTCAN_INVALID_PARAMETER: return "NTCAN_INVALID_PARAMETER"; - case NTCAN_INVALID_HANDLE: return "NTCAN_INVALID_HANDLE"; - case NTCAN_NET_NOT_FOUND: return "NTCAN_NET_NOT_FOUND"; - case NTCAN_INSUFFICIENT_RESOURCES: return "NTCAN_INSUFFICIENT_RESOURCES"; - - case NTCAN_OPERATION_ABORTED: return "NTCAN_OPERATION_ABORTED"; - } - char msg[100]; - sprintf(msg, "unknown error code %d", ntstatus); - return msg; -} - -/** - * Check if errors occured on the CAN bus. - * @return - 0 if everthing is fine. - * - -1 if an error occured. - * - -3 if messages were lost. - * - -5 if a FIFO overflow occured. - * - -6 if the CAN controller is BUS OFF. - * - -7 if the CAN controller is WARN, i.e. error passive. - */ -int CanESD::readEvent() -{ - EVMSG evmsg; - int iRet = 0; - int ret; - - ret = canReadEvent(m_Handle, &evmsg, NULL); - - if(ret == NTCAN_SUCCESS) - { - if( (int)evmsg.evid == NTCAN_EV_CAN_ERROR ) { - switch( evmsg.evdata.s[0] ) { - case 0x00: - iRet = 0; - break; - case 0xC0: - iRet = -6; - std::cout << "BUS OFF" << std::endl; - break; - case 0x40: - iRet = -7; - std::cout << "ERROR PASSIVE" << std::endl; - break; - } - if( evmsg.evdata.s[3] != 0 ) { - iRet = -3; - std::cout << "Lost " << (int)evmsg.evdata.s[3] << " messages" << std::endl; - } else if( evmsg.evdata.s[5] != 0 ) { - iRet = -5; - std::cout << "Lost " << (int)evmsg.evdata.s[5] << " messages from fifo" << std::endl; - } - } - } - return iRet; -} - diff --git a/cob_generic_can/common/src/CanPeakSys.cpp b/cob_generic_can/common/src/CanPeakSys.cpp deleted file mode 100644 index 185c65dc7..000000000 --- a/cob_generic_can/common/src/CanPeakSys.cpp +++ /dev/null @@ -1,260 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -#include -#include -#include -#include -#include -#include -#include -#include -//----------------------------------------------- - -const int CanPeakSys::c_iInterrupt = 7; -const int CanPeakSys::c_iPort = 0x378; - -//----------------------------------------------- -CanPeakSys::CanPeakSys(const char* cIniFile) -{ - m_bInitialized = false; - - // read IniFile - m_IniFile.SetFileName(cIniFile, "CanPeakSys.cpp"); - init(); -} - -//----------------------------------------------- -CanPeakSys::~CanPeakSys() -{ - if (m_bInitialized) - { - CAN_Close(m_handle); - } -} - -//----------------------------------------------- -bool CanPeakSys::init_ret() -{ - // Not implemented yet - return false; -} - -//----------------------------------------------- -void CanPeakSys::init() -{ - std::string sCanDevice; - if( m_IniFile.GetKeyString( "TypeCan", "DevicePath", &sCanDevice, false) != 0) { - sCanDevice = "/dev/pcan32"; - } else std::cout << "CAN-device path read from ini-File: " << sCanDevice << std::endl; - m_handle = LINUX_CAN_Open(sCanDevice.c_str(), O_RDWR); - - - if (! m_handle) - { - // Fatal error - std::cout << "Cannot open CAN-dongle on parallel port: " << strerror(errno) << std::endl; - sleep(3); - exit(0); - } - - - int ret = CAN_ERR_OK; - int iBaudrateVal = 0; - m_IniFile.GetKeyInt( "CanCtrl", "BaudrateVal", &iBaudrateVal, true); - - switch(iBaudrateVal) - { - case CANITFBAUD_1M: - ret = CAN_Init(m_handle, CAN_BAUD_1M, CAN_INIT_TYPE_ST); - break; - case CANITFBAUD_500K: - ret = CAN_Init(m_handle, CAN_BAUD_500K, CAN_INIT_TYPE_ST); - break; - case CANITFBAUD_250K: - ret = CAN_Init(m_handle, CAN_BAUD_250K, CAN_INIT_TYPE_ST); - break; - case CANITFBAUD_125K: - ret = CAN_Init(m_handle, CAN_BAUD_125K, CAN_INIT_TYPE_ST); - break; - case CANITFBAUD_50K: - ret = CAN_Init(m_handle, CAN_BAUD_50K, CAN_INIT_TYPE_ST); - break; - case CANITFBAUD_20K: - ret = CAN_Init(m_handle, CAN_BAUD_20K, CAN_INIT_TYPE_ST); - break; - case CANITFBAUD_10K: - ret = CAN_Init(m_handle, CAN_BAUD_10K, CAN_INIT_TYPE_ST); - break; - } - - if(ret) - { - std::cout << "CanPeakSys::CanPeakSys(), error in init" << std::endl; - } - else - { - std::cout << "CanPeakSys::CanpeakSys(), init ok" << std::endl; - m_bInitialized = true; - } -} - -//------------------------------------------- -bool CanPeakSys::transmitMsg(CanMsg CMsg, bool bBlocking) -{ - TPCANMsg TPCMsg; - bool bRet = true; - - if (m_bInitialized == false) return false; - - // copy CMsg to TPCmsg - TPCMsg.LEN = CMsg.m_iLen; - TPCMsg.ID = CMsg.m_iID; - TPCMsg.MSGTYPE = CMsg.m_iType; - for(int i=0; i<8; i++) - TPCMsg.DATA[i] = CMsg.getAt(i); - - // write msg - int iRet; - iRet = CAN_Write(m_handle, &TPCMsg); - iRet = CAN_Status(m_handle); - - if(iRet < 0) - { - std::cout << "CanPeakSys::transmitMsg, errorcode= " << nGetLastError() << std::endl; - bRet = false; - } - - - return bRet; -} - -//------------------------------------------- -bool CanPeakSys::receiveMsg(CanMsg* pCMsg) -{ - TPCANRdMsg TPCMsg; - TPCMsg.Msg.LEN = 8; - TPCMsg.Msg.MSGTYPE = 0; - TPCMsg.Msg.ID = 0; - - int iRet = CAN_ERR_OK; - bool bRet = false; - - - if (m_bInitialized == false) return false; - - iRet = LINUX_CAN_Read_Timeout(m_handle, &TPCMsg, 0); - - if (iRet == CAN_ERR_OK) - { - pCMsg->m_iID = TPCMsg.Msg.ID; - pCMsg->set(TPCMsg.Msg.DATA[0], TPCMsg.Msg.DATA[1], TPCMsg.Msg.DATA[2], TPCMsg.Msg.DATA[3], - TPCMsg.Msg.DATA[4], TPCMsg.Msg.DATA[5], TPCMsg.Msg.DATA[6], TPCMsg.Msg.DATA[7]); - bRet = true; - } - else if (CAN_Status(m_handle) != CAN_ERR_QRCVEMPTY) - { - std::cout << "CanPeakSys::receiveMsg ERROR: iRet = " << iRet << std::endl; - pCMsg->set(0, 0, 0, 0, 0, 0, 0, 0); - } - else - { - // make sure there's never an undefined state (even when can drivers fail) - pCMsg->set(0, 0, 0, 0, 0, 0, 0, 0); - } - - return bRet; -} - -//------------------------------------------- -bool CanPeakSys::receiveMsgRetry(CanMsg* pCMsg, int iNrOfRetry) -{ - int i, iRet; - - TPCANRdMsg TPCMsg; - TPCMsg.Msg.LEN = 8; - TPCMsg.Msg.MSGTYPE = 0; - TPCMsg.Msg.ID = 0; - - if (m_bInitialized == false) return false; - - // wait until msg in buffer - bool bRet = true; - iRet = CAN_ERR_OK; - i=0; - do - { - iRet = LINUX_CAN_Read_Timeout(m_handle, &TPCMsg, 0); - - if(iRet == CAN_ERR_OK) - break; - - i++; - usleep(100000); - } - while(i < iNrOfRetry); - - // eval return value - if(iRet != CAN_ERR_OK) - { - std::cout << "CanPeakSys::receiveMsgRetry: " << strerror(errno) << std::endl; - pCMsg->set(0, 0, 0, 0, 0, 0, 0, 0); - bRet = false; - } - else - { - pCMsg->m_iID = TPCMsg.Msg.ID; - pCMsg->set(TPCMsg.Msg.DATA[0], TPCMsg.Msg.DATA[1], TPCMsg.Msg.DATA[2], TPCMsg.Msg.DATA[3], - TPCMsg.Msg.DATA[4], TPCMsg.Msg.DATA[5], TPCMsg.Msg.DATA[6], TPCMsg.Msg.DATA[7]); - } - - return bRet; -} - -//------------------------------------------- -bool CanPeakSys::receiveMsgTimeout(CanMsg* pCMsg, int nMicroSecTimeout) -{ - int iRet = CAN_ERR_OK; - - TPCANRdMsg TPCMsg; - TPCMsg.Msg.LEN = 8; - TPCMsg.Msg.MSGTYPE = 0; - TPCMsg.Msg.ID = 0; - - if (m_bInitialized == false) return false; - - bool bRet = true; - - iRet = LINUX_CAN_Read_Timeout(m_handle, &TPCMsg, nMicroSecTimeout); - - // eval return value - if(iRet != CAN_ERR_OK) - { - std::cout << "CANPeakSysUSB::receiveMsgRetry, errorcode= " << nGetLastError() << std::endl; - pCMsg->set(0, 0, 0, 0, 0, 0, 0, 0); - bRet = false; - } - else - { - pCMsg->setID(TPCMsg.Msg.ID); - pCMsg->setLength(TPCMsg.Msg.LEN); - pCMsg->set(TPCMsg.Msg.DATA[0], TPCMsg.Msg.DATA[1], TPCMsg.Msg.DATA[2], TPCMsg.Msg.DATA[3], - TPCMsg.Msg.DATA[4], TPCMsg.Msg.DATA[5], TPCMsg.Msg.DATA[6], TPCMsg.Msg.DATA[7]); - } - - return bRet; -} diff --git a/cob_generic_can/common/src/CanPeakSysUSB.cpp b/cob_generic_can/common/src/CanPeakSysUSB.cpp deleted file mode 100644 index 6230663e9..000000000 --- a/cob_generic_can/common/src/CanPeakSysUSB.cpp +++ /dev/null @@ -1,339 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -//#define __DEBUG__ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -//----------------------------------------------- -CANPeakSysUSB::CANPeakSysUSB(const char* device, int baudrate) -{ - m_bInitialized = false; - - p_cDevice = device; - m_iBaudrateVal = baudrate; -} - -CANPeakSysUSB::CANPeakSysUSB(const char* cIniFile) -{ - m_bInitialized = false; - - // read IniFile - m_IniFile.SetFileName(cIniFile, "CanPeakSysUSB.cpp"); - - init(); -} - -//----------------------------------------------- -CANPeakSysUSB::~CANPeakSysUSB() -{ - if (m_bInitialized) - { - CAN_Close(m_handle); - } -} - -//----------------------------------------------- -bool CANPeakSysUSB::init_ret() -{ - bool ret = true; - - // init() - part - m_handle = LINUX_CAN_Open(p_cDevice, O_RDWR); - - if (! m_handle) - { - std::cout << "Cannot open CAN on USB: " << strerror(errno) << std::endl; - ret = false; - } - else - { - ret = initCAN(); - } - - return ret; -} - -//----------------------------------------------- -void CANPeakSysUSB::init() -{ - std::string sCanDevice; - - if( m_IniFile.GetKeyString( "TypeCan", "DevicePath", &sCanDevice, false) != 0) { - sCanDevice = "/dev/pcan32"; - } else std::cout << "CAN-device path read from ini-File: " << sCanDevice << std::endl; - - //m_handle = LINUX_CAN_Open("/dev/pcan32", O_RDWR | O_NONBLOCK); - m_handle = LINUX_CAN_Open(sCanDevice.c_str(), O_RDWR); - - if (! m_handle) - { - // Fatal error - std::cout << "Cannot open CAN on USB: " << strerror(errno) << std::endl; - sleep(3); - exit(0); - } - - m_iBaudrateVal = 0; - m_IniFile.GetKeyInt( "CanCtrl", "BaudrateVal", &m_iBaudrateVal, true); - - initCAN(); -} - -//------------------------------------------- -bool CANPeakSysUSB::transmitMsg(CanMsg CMsg, bool bBlocking) -{ - TPCANMsg TPCMsg; - bool bRet = true; - - if (m_bInitialized == false) return false; - - // copy CMsg to TPCmsg - TPCMsg.LEN = CMsg.getLength(); - TPCMsg.ID = CMsg.getID(); - TPCMsg.MSGTYPE = CMsg.getType(); - for(int i=0; i<8; i++) - TPCMsg.DATA[i] = CMsg.getAt(i); - - //TODO Hier stürtzt die Base ab.. verwende libpcan.h pcan.h um Fehler auszulesen, diagnostizieren, ausgeben und CAN_INIT erneut aufzurufen = neustart can-hardware. - - int iRet; - //iRet = CAN_Write(m_handle, &TPCMsg); - iRet = LINUX_CAN_Write_Timeout(m_handle, &TPCMsg, 25); //Timeout in micrsoseconds - - if(iRet != CAN_ERR_OK) { -#ifdef __DEBUG__ - std::cout << "CANPeakSysUSB::transmitMsg An error occured while sending..." << iRet << std::endl; - outputDetailedStatus(); -#endif - bRet = false; - } - -#ifdef __DEBUG__ - //is this necessary? try iRet==CAN_Status(m_handle) ? - iRet = CAN_Status(m_handle); - - if(iRet < 0) - { - std::cout << "CANPeakSysUSB::transmitMsg, system error: " << iRet << std::endl; - bRet = false; - } else if((iRet & CAN_ERR_BUSOFF) != 0) { - std::cout << "CANPeakSysUSB::transmitMsg, BUSOFF detected" << std::endl; - //Try to restart CAN-Device - std::cout << "Trying to re-init Hardware..." << std::endl; - bRet = initCAN(); - - } else if((iRet & CAN_ERR_ANYBUSERR) != 0) { - std::cout << "CANPeakSysUSB::transmitMsg, ANYBUSERR" << std::endl; - - } else if( (iRet & (~CAN_ERR_QRCVEMPTY)) != 0) { - std::cout << "CANPeakSysUSB::transmitMsg, CAN_STATUS: " << iRet << std::endl; - bRet = false; - } -#endif - - return bRet; -} - -//------------------------------------------- -bool CANPeakSysUSB::receiveMsg(CanMsg* pCMsg) -{ - TPCANRdMsg TPCMsg; - TPCMsg.Msg.LEN = 8; - TPCMsg.Msg.MSGTYPE = 0; - TPCMsg.Msg.ID = 0; - - int iRet = CAN_ERR_OK; - - bool bRet = false; - - if (m_bInitialized == false) return false; - - iRet = LINUX_CAN_Read_Timeout(m_handle, &TPCMsg, 0); - - if (iRet == CAN_ERR_OK) - { - pCMsg->setID(TPCMsg.Msg.ID); - pCMsg->setLength(TPCMsg.Msg.LEN); - pCMsg->set(TPCMsg.Msg.DATA[0], TPCMsg.Msg.DATA[1], TPCMsg.Msg.DATA[2], TPCMsg.Msg.DATA[3], - TPCMsg.Msg.DATA[4], TPCMsg.Msg.DATA[5], TPCMsg.Msg.DATA[6], TPCMsg.Msg.DATA[7]); - bRet = true; - } - else if( (iRet & (~CAN_ERR_QRCVEMPTY)) != 0) //no"empty-queue"-status - { - std::cout << "CANPeakSysUSB::receiveMsg, CAN_STATUS: " << iRet << std::endl; - pCMsg->set(0, 0, 0, 0, 0, 0, 0, 0); - } - - //catch status messages, these could be further processed in overlying software to identify and handle CAN errors - if( TPCMsg.Msg.MSGTYPE == MSGTYPE_STATUS ) { - std::cout << "CANPeakSysUSB::receiveMsg, status message catched:\nData is (CAN_ERROR_...) " << TPCMsg.Msg.DATA[3] << std::endl; - pCMsg->set(0, 0, 0, 0, 0, 0, 0, 0); - } - - return bRet; -} - -//------------------------------------------- -bool CANPeakSysUSB::receiveMsgRetry(CanMsg* pCMsg, int iNrOfRetry) -{ - int i, iRet; - - TPCANRdMsg TPCMsg; - TPCMsg.Msg.LEN = 8; - TPCMsg.Msg.MSGTYPE = 0; - TPCMsg.Msg.ID = 0; - - if (m_bInitialized == false) return false; - - // wait until msg in buffer - bool bRet = true; - iRet = CAN_ERR_OK; - i=0; - do - { - iRet = LINUX_CAN_Read_Timeout(m_handle, &TPCMsg, 0); - - if(iRet == CAN_ERR_OK) - break; - - i++; - usleep(10000); - } - while(i < iNrOfRetry); - - // eval return value - if(iRet != CAN_ERR_OK) - { - std::cout << "CANPeakSysUSB::receiveMsgRetry, errorcode= " << nGetLastError() << std::endl; - pCMsg->set(0, 0, 0, 0, 0, 0, 0, 0); - bRet = false; - } - else - { - pCMsg->setID(TPCMsg.Msg.ID); - pCMsg->setLength(TPCMsg.Msg.LEN); - pCMsg->set(TPCMsg.Msg.DATA[0], TPCMsg.Msg.DATA[1], TPCMsg.Msg.DATA[2], TPCMsg.Msg.DATA[3], - TPCMsg.Msg.DATA[4], TPCMsg.Msg.DATA[5], TPCMsg.Msg.DATA[6], TPCMsg.Msg.DATA[7]); - } - - return bRet; -} - -//------------------------------------------- -bool CANPeakSysUSB::receiveMsgTimeout(CanMsg* pCMsg, int nMicroSecTimeout) -{ - int iRet = CAN_ERR_OK; - - TPCANRdMsg TPCMsg; - TPCMsg.Msg.LEN = 8; - TPCMsg.Msg.MSGTYPE = 0; - TPCMsg.Msg.ID = 0; - - if (m_bInitialized == false) return false; - - bool bRet = true; - - iRet = LINUX_CAN_Read_Timeout(m_handle, &TPCMsg, nMicroSecTimeout); - - // eval return value - if(iRet != CAN_ERR_OK) - { - std::cout << "CANPeakSysUSB::receiveMsgTimeout, errorcode= " << nGetLastError() << std::endl; - pCMsg->set(0, 0, 0, 0, 0, 0, 0, 0); - bRet = false; - } - else - { - pCMsg->setID(TPCMsg.Msg.ID); - pCMsg->setLength(TPCMsg.Msg.LEN); - pCMsg->set(TPCMsg.Msg.DATA[0], TPCMsg.Msg.DATA[1], TPCMsg.Msg.DATA[2], TPCMsg.Msg.DATA[3], - TPCMsg.Msg.DATA[4], TPCMsg.Msg.DATA[5], TPCMsg.Msg.DATA[6], TPCMsg.Msg.DATA[7]); - } - - return bRet; -} - -bool CANPeakSysUSB::initCAN() { - int ret = CAN_ERR_OK; - bool bRet = true; - - switch(m_iBaudrateVal) - { - case CANITFBAUD_1M: - ret = CAN_Init(m_handle, CAN_BAUD_1M, CAN_INIT_TYPE_ST); - break; - case CANITFBAUD_500K: - ret = CAN_Init(m_handle, CAN_BAUD_500K, CAN_INIT_TYPE_ST); - break; - case CANITFBAUD_250K: - ret = CAN_Init(m_handle, CAN_BAUD_250K, CAN_INIT_TYPE_ST); - break; - case CANITFBAUD_125K: - ret = CAN_Init(m_handle, CAN_BAUD_125K, CAN_INIT_TYPE_ST); - break; - case CANITFBAUD_50K: - ret = CAN_Init(m_handle, CAN_BAUD_50K, CAN_INIT_TYPE_ST); - break; - case CANITFBAUD_20K: - ret = CAN_Init(m_handle, CAN_BAUD_20K, CAN_INIT_TYPE_ST); - break; - case CANITFBAUD_10K: - ret = CAN_Init(m_handle, CAN_BAUD_10K, CAN_INIT_TYPE_ST); - break; - } - - if(ret) - { - std::cout << "CANPeakSysUSB::CANPeakSysUSB(), error in init" << std::endl; - m_bInitialized = false; - bRet = false; - } - else - { - std::cout << "CANPeakSysUSB::CanpeakSys(), init ok" << std::endl; - m_bInitialized = true; - bRet = true; - } - - return bRet; -} - -void CANPeakSysUSB::outputDetailedStatus() { - TPDIAG diag; - - LINUX_CAN_Statistics(m_handle, &diag); - - std::cout << "*************************\n" - << "*** Detailed status output of CANPeakSys\n" - << "*************************" - << "\nIRQ-Level: " << diag.wIrqLevel - << "\nNo reads: " << diag.dwReadCounter - << "\nNo writes: " << diag.dwWriteCounter - << "\nNo interrupts: " << diag.dwIRQcounter - << "\nNo errors: " << diag.dwErrorCounter - << "\nError flag: " << diag.wErrorFlag - << "\nLast error: " << diag.nLastError - << std::endl; -} diff --git a/cob_generic_can/common/src/SocketCan.cpp b/cob_generic_can/common/src/SocketCan.cpp deleted file mode 100644 index aa18601e1..000000000 --- a/cob_generic_can/common/src/SocketCan.cpp +++ /dev/null @@ -1,173 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -SocketCan::SocketCan(const char* device, int baudrate) -{ - m_bInitialized = false; - - p_cDevice = device; - m_handle.reset(new can::ThreadedSocketCANInterface()); -} - -SocketCan::SocketCan(const char* device) -{ - m_bInitialized = false; - - p_cDevice = device; - m_handle.reset(new can::ThreadedSocketCANInterface()); -} - -//----------------------------------------------- -SocketCan::~SocketCan() -{ - if (m_bInitialized) - { - m_handle->shutdown(); - } -} - -//----------------------------------------------- -bool SocketCan::init_ret() -{ - bool ret = true; - if (!m_handle->init(p_cDevice, false)) - { - print_error(m_handle->getState()); - ret = false; - } - else - { - m_reader.listen((can::CommInterfaceSharedPtr)m_handle); - m_bInitialized = true; - bool bRet = true; - ret = true; - } - return ret; -} - -//----------------------------------------------- -void SocketCan::init() -{ - if (!init_ret()) - { - sleep(3); - exit(0); - } -} - - -//------------------------------------------- -bool SocketCan::transmitMsg(CanMsg CMsg, bool bBlocking) -{ - can::Header header(CMsg.getID(), false, false, false); - can::Frame message(header, CMsg.getLength()); - for (int i = 0; i < CMsg.getLength(); i++) - { - message.data[i] = CMsg.getAt(i); - } - return m_handle->send(message); -} - -//------------------------------------------- -bool SocketCan::receiveMsg(CanMsg* pCMsg) -{ - if (!m_bInitialized) - { - return false; - } - - bool bRet = false; - can::Frame frame; - - if (m_reader.read(&frame, boost::chrono::seconds(1))) - { - pCMsg->setID(frame.id); - pCMsg->setLength(frame.dlc); - pCMsg->set(frame.data[0], frame.data[1], frame.data[2], frame.data[3], - frame.data[4], frame.data[5], frame.data[6], frame.data[7]); - bRet = true; - } - return bRet; -} - -//------------------------------------------- -bool SocketCan::receiveMsgRetry(CanMsg* pCMsg, int iNrOfRetry) -{ - if (!m_bInitialized) - { - return false; - } - - can::Frame frame; - bool bRet = false; - int i = 0; - - do - { - if (m_reader.read(&frame, boost::chrono::milliseconds(10))) - { - pCMsg->setID(frame.id); - pCMsg->setLength(frame.dlc); - pCMsg->set(frame.data[0], frame.data[1], frame.data[2], frame.data[3], - frame.data[4], frame.data[5], frame.data[6], frame.data[7]); - bRet = true; - break; - } - i++; - } - while ((i < iNrOfRetry && bRet != true)); - return bRet; -} - -//------------------------------------------- -bool SocketCan::receiveMsgTimeout(CanMsg* pCMsg, int nMicroSecTimeout) -{ - if (!m_bInitialized) - { - return false; - } - - bool bRet = false; - can::Frame frame; - - if (m_reader.read(&frame, boost::chrono::microseconds(nMicroSecTimeout))) - { - pCMsg->setID(frame.id); - pCMsg->setLength(frame.dlc); - pCMsg->set(frame.data[0], frame.data[1], frame.data[2], frame.data[3], frame.data[4], frame.data[5], frame.data[6], frame.data[7]); - bRet = true; - } - return bRet; -} - -void SocketCan::print_error(const can::State& state) -{ - std::string err; - std::cout << "ERROR: state=" << std::endl; - m_handle->translateError(state.internal_error, err); - std::cout << "ERROR: state=" << state.driver_state << " internal_error=" << state.internal_error << "('" << err << "') asio: " << state.error_code << std::endl; -} diff --git a/cob_generic_can/package.xml b/cob_generic_can/package.xml deleted file mode 100644 index 2f8433f79..000000000 --- a/cob_generic_can/package.xml +++ /dev/null @@ -1,20 +0,0 @@ - - cob_generic_can - 0.7.16 - The package cob_generic_can provides an interface for nodes on a can-bus and examplary wrappers for two PeakSys-can-libs. When a can-bus-device is generated (for an example see base_dirve_chain) you can use generic_can to create as many itfs as there will be components communicating via this can-bus. Assign type of the can communication device (e.g. usb-to-can or can-card of a specific vendor) and can-address of the target device. This package comes with wrappers for PeakSys and PeakSysUSB adapters. - - Apache 2.0 - - http://ros.org/wiki/cob_generic_can - - - Matthias Gruhler - Christian Connette - - catkin - - cob_utilities - libntcan - libpcan - socketcan_interface - From 223d871b7830ea666e166127e51b874676dfab7f Mon Sep 17 00:00:00 2001 From: fmessmer Date: Wed, 20 Mar 2024 16:57:34 +0100 Subject: [PATCH 10/14] remove cob_utilities --- cob_driver/package.xml | 1 - cob_utilities/CHANGELOG.rst | 160 ----- cob_utilities/CMakeLists.txt | 25 - .../common/include/cob_utilities/IniFile.h | 305 --------- .../common/include/cob_utilities/MathSup.h | 307 --------- .../common/include/cob_utilities/Mutex.h | 74 -- .../common/include/cob_utilities/StrUtil.h | 80 --- .../common/include/cob_utilities/TimeStamp.h | 97 --- .../common/include/cob_utilities/windows.h | 56 -- cob_utilities/common/src/IniFile.cpp | 646 ------------------ cob_utilities/common/src/MathSup.cpp | 22 - cob_utilities/common/src/StrUtil.cpp | 120 ---- cob_utilities/common/src/TimeStamp.cpp | 159 ----- cob_utilities/package.xml | 17 - 14 files changed, 2069 deletions(-) delete mode 100644 cob_utilities/CHANGELOG.rst delete mode 100644 cob_utilities/CMakeLists.txt delete mode 100644 cob_utilities/common/include/cob_utilities/IniFile.h delete mode 100644 cob_utilities/common/include/cob_utilities/MathSup.h delete mode 100644 cob_utilities/common/include/cob_utilities/Mutex.h delete mode 100644 cob_utilities/common/include/cob_utilities/StrUtil.h delete mode 100644 cob_utilities/common/include/cob_utilities/TimeStamp.h delete mode 100644 cob_utilities/common/include/cob_utilities/windows.h delete mode 100644 cob_utilities/common/src/IniFile.cpp delete mode 100644 cob_utilities/common/src/MathSup.cpp delete mode 100644 cob_utilities/common/src/StrUtil.cpp delete mode 100644 cob_utilities/common/src/TimeStamp.cpp delete mode 100644 cob_utilities/package.xml diff --git a/cob_driver/package.xml b/cob_driver/package.xml index 815d3ec15..3a7cebe7a 100644 --- a/cob_driver/package.xml +++ b/cob_driver/package.xml @@ -21,7 +21,6 @@ cob_scan_unifier cob_sick_s300 cob_sound - cob_utilities diff --git a/cob_utilities/CHANGELOG.rst b/cob_utilities/CHANGELOG.rst deleted file mode 100644 index 16316b684..000000000 --- a/cob_utilities/CHANGELOG.rst +++ /dev/null @@ -1,160 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package cob_utilities -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.7.16 (2024-02-20) -------------------- - -0.7.15 (2023-11-06) -------------------- - -0.7.14 (2022-11-17) -------------------- - -0.7.13 (2022-07-29) -------------------- - -0.7.12 (2022-03-15) -------------------- - -0.7.11 (2022-01-12) -------------------- - -0.7.10 (2021-12-23) -------------------- - -0.7.9 (2021-11-26) ------------------- - -0.7.8 (2021-10-19) ------------------- - -0.7.7 (2021-08-02) ------------------- - -0.7.6 (2021-05-10) ------------------- - -0.7.5 (2021-04-06) ------------------- - -0.7.4 (2020-10-14) ------------------- -* Merge pull request `#417 `_ from fmessmer/test_noetic - test noetic -* Bump CMake version to avoid CMP0048 warning -* Contributors: Felix Messmer, fmessmer - -0.7.3 (2020-03-18) ------------------- - -0.7.2 (2020-03-18) ------------------- -* Merge pull request `#405 `_ from fmessmer/fix_warnings - fix compile warnings -* fix -Wunused-result in cob_utilities -* Contributors: Felix Messmer, fmessmer - -0.7.1 (2019-11-07) ------------------- - -0.7.0 (2019-08-06) ------------------- -* Merge pull request `#396 `_ from HannesBachter/indigo_dev - 0.6.15 -* Contributors: Felix Messmer - -0.6.15 (2019-07-17) -------------------- - -0.6.14 (2019-06-07) -------------------- - -0.6.13 (2019-03-14) -------------------- - -0.6.12 (2018-07-21) -------------------- - -0.6.11 (2018-01-07) -------------------- -* Merge remote-tracking branch 'origin/indigo_release_candidate' into indigo_dev -* Merge pull request `#341 `_ from ipa-fxm/APACHE_license - use license apache 2.0 -* use license apache 2.0 -* Contributors: Felix Messmer, ipa-fxm, ipa-uhr-mk - -0.6.10 (2017-07-24) -------------------- - -0.6.9 (2017-07-18) ------------------- -* manually fix changelog -* Contributors: ipa-fxm - -0.6.8 (2016-10-10) ------------------- - -0.6.7 (2016-04-02) ------------------- - -0.6.6 (2016-04-01) ------------------- - -0.6.5 (2015-08-31) ------------------- - -0.6.4 (2015-08-25) ------------------- -* remove obsolete autogenerated mainpage.dox files -* remove trailing whitespaces -* migrate to package format 2 -* sort dependencies -* critically review dependencies -* Contributors: ipa-fxm - -0.6.3 (2015-06-17) ------------------- - -0.6.2 (2014-12-15) ------------------- - -0.6.1 (2014-09-17) ------------------- - -0.6.0 (2014-09-09) ------------------- - -0.5.7 (2014-08-26) ------------------- -* Merge pull request `#163 `_ from ipa320/hydro_dev - updates from hydro_dev -* 0.5.6 -* update changelog -* Cleaned up cob_driver with reduced deps to compile on indigo -* Contributors: Alexander Bubeck, Florian Weisshardt - -0.5.6 (2014-08-26) ------------------- -* Merge pull request `#163 `_ from ipa320/hydro_dev - updates from hydro_dev -* Cleaned up cob_driver with reduced deps to compile on indigo -* Contributors: Alexander Bubeck, Florian Weisshardt - -0.5.3 (2014-03-31) ------------------- -* install tags -* Contributors: ipa-fxm - -0.5.2 (2014-03-20) ------------------- - -0.5.1 (2014-03-20) ------------------- -* cleaned up CMakeLists and added install directives -* futher include and linkpath modifications -* compiling but still some linker errors -* Second catkinization push -* First catkinization, still need to update some CMakeLists.txt -* moved to cob_driver -* Contributors: Alexander Bubeck, abubeck, ipa-fmw diff --git a/cob_utilities/CMakeLists.txt b/cob_utilities/CMakeLists.txt deleted file mode 100644 index 8741ce0a3..000000000 --- a/cob_utilities/CMakeLists.txt +++ /dev/null @@ -1,25 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(cob_utilities) - -find_package(catkin REQUIRED COMPONENTS) - -catkin_package( - INCLUDE_DIRS common/include - LIBRARIES ${PROJECT_NAME} -) - -### BUILD ### -include_directories(common/include ${catkin_INCLUDE_DIRS}) - -add_library(${PROJECT_NAME} common/src/IniFile.cpp common/src/MathSup.cpp common/src/StrUtil.cpp common/src/TimeStamp.cpp) - -### INSTALL ### -install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -install(DIRECTORY common/include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) diff --git a/cob_utilities/common/include/cob_utilities/IniFile.h b/cob_utilities/common/include/cob_utilities/IniFile.h deleted file mode 100644 index e7e8b8880..000000000 --- a/cob_utilities/common/include/cob_utilities/IniFile.h +++ /dev/null @@ -1,305 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -#ifndef _Inifile_H -#define _Inifile_H - -#include -#include -#include -#include - -//------------------------------------------------------------------- - - -/** - * Used to store persistend program configuration in INI-Files. - * The INI-File is organized into sections and Keys (variables) like - * ordinary Windows INI-Files. - * The write functions create a temporary file in the root directory. - * If there is no write permission in this directory, they don't work. - * @par sections: - * identifcator between '[' and ']', a section headline must not have blanks - * at the beginning neither right after or before the '[ ]'. - * - * @par sections contains keys: - * identificator followed by '=' and the value, no blanks at the - * beginning of the line, nor after the '=' sign or between key - * and '=' sign. the pBuf will contain the eventual blanks after - * the '=' sign. - * - * @par example: - * @code - * [section1] - * - * key11=234 - * key12=yellow submarine - * key13=13.5 - * - * [section2] - * - * key21= - * key22=13 - * @endcode - * - * \ingroup UtilitiesModul - */ -class IniFile -{ -public: - - /** - * Default constructor. - */ - IniFile(); - - /** - * Constructor. - * @param fileName file name. - */ - IniFile(std::string fileName); - ~IniFile(); - - /** - * Sets file path of ini-file. - * Also verifies that file exists. - * @param fileName file name - * @param strIniFileUsedBy the name of the source file using the ini-file. - * This name will be printed if an error occurs. - * @param bCreate if true: create new file if the file does not exist (default: false) - * @return 0 if file exists or new file has been created sucesfully - */ - int SetFileName(std::string fileName, std::string strIniFileUsedBy = "", bool bCreate = false); - - - /** - * Write character string to INI-File. - * Like Windows Fn WriteProfileString(). - * Comments in the same line as the variables will be deleted during write operations. - * @param pSect pointer to a null ended string containing the section title without '[' and ']' - * @param pKey pointer to a null ended string containing the key - * @param StrToWrite null ended string to write. - * @param bWarnIfNotfound print a warning message if the section is not found and therefore is created newly. - */ - int WriteKeyString(const char* pSect, const char* pKey, const std::string* pStrToWrite, bool bWarnIfNotfound = true); - - /** - * Write integer to INI-File. - * Like Windows Fn WriteProfileInt(). - * Comments in the same line as the variables will be deleted during write operations. - * @param pSect pointer to a null ended string containing the section title without '[' and ']' - * @param pKey pointer to a null ended string containing the key - * @param nValue integer to write. - * @param bWarnIfNotfound print a warning message if the section is not found and therefore is created newly. - */ - int WriteKeyInt(const char* pSect,const char* pKey,int nValue, bool bWarnIfNotfound = true); - - /** - * Write double to INI-File. - * Comments in the same line as the variables will be deleted during write operations. - * @param pSect pointer to a null ended string containing the section title without '[' and ']' - * @param pKey pointer to a null ended string containing the key - * @param dValue double to write. - * @param StringLen total length of string into which the double will be converted - * @param decimals of double to store - * @param bWarnIfNotfound print a warning message if the section is not found and therefore is created newly. - */ - int WriteKeyDouble(const char* pSect,const char* pKey,double dValue,int StringLen=12,int decimals=5, bool bWarnIfNotfound = true); - - /** - * Read boolean from INI-File. - * The value writen will be either 'true' or 'false'. - * @param pSect pointer to a null ended string containing the section title without '[' and ']' - * @param pKey pointer to a null ended string containing the key - * @param pValue pointer to boolean which will contain the value of the key. - * If the section or the key is not found, the value of *pValue remains unchanged - * @param bWarnIfNotfound print a warning message if the section is not found and therefore is created newly. - */ - int WriteKeyBool(const char* pSect, const char* pKey, bool bValue, bool bWarnIfNotfound = true); - - /** - * Read character string from INI-File. - * Like Windows Fn GetProfileString(). - * @param pSect pointer to a null ended string containing the section title without '[' and ']' - * @param pKey pointer to a null ended string containing the key - * @param pStrToRead will contain string read - * If the section or the key is not found, the value of *pStrToRead remains unchanged - */ - int GetKeyString(const char* pSect,const char* pKey, std::string* pStrToRead, - bool bWarnIfNotfound = true); - - /** - * Read integer from INI-File. - * Like Windows Fn GetProfileInt(). - */ - int GetKeyInt(const char* pSect,const char* pKey,int* pValue, - bool bWarnIfNotfound = true); - - /** - * Read long from INI-File. - */ - int GetKeyLong(const char* pSect,const char* pKey,long* pValue, - bool bWarnIfNotfound = true); - - /** - * Read boolean from INI-File. - * The value can be either 'true' or 'false'. - * @param pSect pointer to a null ended string containing the section title without '[' and ']' - * @param pKey pointer to a null ended string containing the key - * @param pValue pointer to boolean which will contain the value of the key. - * If the section or the key is not found, the value of *pValue remains unchanged - */ - int GetKeyBool(const char* pSect, const char* pKey, bool* pValue, - bool bWarnIfNotfound = true); - - /** - * Read double from INI-File. - * Current accuracy: 9 chars!! - * @param pSect pointer to a null ended string containing the section title without '[' and ']' - * @param pKey pointer to a null ended string containing the key - * @param pValue pointer to double which will contain the value of the key. - * If the section or the key is not found, the vlaue of *pValue remains unchanged - */ - int GetKeyDouble(const char* pSect,const char* pKey,double* pValue, - bool bWarnIfNotfound = true); - - /** - * Read double from INI-File. - * Current accuracy: 9 chars!! - * @param pSect pointer to a null ended string containing the section title without '[' and ']' - * @param pKey pointer to a null ended string containing the key - * @param pValue pointer to double which will contain the value of the key. - * If the section or the key is not found, the vlaue of *pValue remains unchanged - */ - int GetKeyDouble(const char* pSect,const char* pKey,double* pValue, double dDefault, - bool bWarnIfNotfound = true); - - /** - * Read character string from INI-File. - * Like Windows Fn GetProfileString(). - * @param pSect pointer to a null ended string containing the section title without '[' and ']' - * @param pKey pointer to a null ended string containing the key - * @param StrToRead will contain string read - * If the section or the key is not found, the value of *pStrToRead remains unchanged - */ - int GetKey(const char* pSect,const char* pKey, std::string* pStrToRead, bool bWarnIfNotfound = true); - - /** - * Read integer from INI-File. - * Like Windows Fn GetProfileInt(). - */ - int GetKey(const char* pSect,const char* pKey,int* pValue, bool bWarnIfNotfound = true); - - /** - * Read boolean from INI-File. - * The value can be either 'true' or 'false'. - * @param pSect pointer to a null ended string containing the section title without '[' and ']' - * @param pKey pointer to a null ended string containing the key - * @param pValue pointer to boolean which will contain the value of the key. - * If the section or the key is not found, the value of *pValue remains unchanged - */ - int GetKey(const char* pSect, const char* pKey, bool* pValue, bool bWarnIfNotfound = true); - - /** - * Read double from INI-File. - * Current accuracy: 9 chars!! - * @param pSect pointer to a null ended string containing the section title without '[' and ']' - * @param pKey pointer to a null ended string containing the key - * @param pValue pointer to double which will contain the value of the key. - * If the section or the key is not found, the vlaue of *pValue remains unchanged - */ - int GetKey(const char* pSect,const char* pKey,double* pValue, bool bWarnIfNotfound = true); - - /** - * Find the section name after the given section. - * If prevSect is NULL, get the first section name. - * @param pSect pointer to a null ended string which will contain the section title without '[' and ']'. - * @param prevSect pointer to a null ended string contraing the previous section title without '[' and ']'. - * @param bWarnIfNotfound print a warning message if the section is not found. - * If the section is not found, the value of *sect is not defined. - */ - int FindNextSection(std::string* pSect, std::string prevSect, bool bWarnIfNotfound = true); - -private: - - int FindSection(const char* sect, bool bWarnIfNotfound = true); - int FindKey(const char* skey, bool bWarnIfNotfound = true); - int FindNextLine(std::vector& NewLine, int& CharInd); - - /** - * Write character string to INI-File. - * Like Windows Fn WriteProfileString(). - * @param pSect pointer to a null ended string containing the section title without '[' and ']' - * @param pKey pointer to a null ended string containing the key - * @param pBuf null ended string to write. - * @param bWarnIfNotfound print a warning message if the section is not found and therefore is created newly. - */ - int WriteKeyValue(const char* pSect,const char* pKey,const char* pBuf, bool bWarnIfNotfound = true); - - /** - * Read character string from INI-File. - * Like Windows Fn GetProfileString(). - * @param pSect pointer to a null ended string containing the section title without '[' and ']' - * @param pKey pointer to a null ended string containing the key - * @param pBuf pointer to a character buffer of length lenBuf string which will - * contain the value of the key. - * If the section or the key is not found, the vlaue of *pBuf remains unchanged - * @param lenBuf the maximal length of szBuf (including terminating \0). - * @return the length of the string read or RF_E if error - */ - int GetKeyValue(const char* pSect,const char* pKey, char* pBuf, int lenBuf, - bool bWarnIfNotfound = true); - - /** - * Skips chars in line until Endchar. - * return: - Nr of chars skipped if successful - * - RF_E if end of line (\n) or end of file before Endchar is found - */ - int SkipLineUntil(FILE* pFile, const char EndChar); - - /** - * Reads chars in line until Endchar into string. - * return: - Nr of chars read if successful - * - RF_E if end of line (\n) or end of file before Endchar is found. - * In this case, the string will contain the chars read so far. - */ - int ReadLineUntil(FILE* pFile, const char EndChar, std::string& ReadIntoStr); - - bool m_bFileOK; - - /** - * Vector to store the chars of the most recently read line. - */ - std::vector m_CurLine; - - /** - * Size of the vector CurLine. - */ - const int m_vectorSize; - - /** - * Index of the current character in the current line. - */ - int m_CurCharInd; - - std::string m_fileName; - std::string m_strIniFileUsedBy; //used for debug to inidcate user class of ini-file - FILE* f; -}; - -#endif - diff --git a/cob_utilities/common/include/cob_utilities/MathSup.h b/cob_utilities/common/include/cob_utilities/MathSup.h deleted file mode 100644 index 1983b810d..000000000 --- a/cob_utilities/common/include/cob_utilities/MathSup.h +++ /dev/null @@ -1,307 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -#ifndef MATHSUP_INCLUDEDEFX_H -#define MATHSUP_INCLUDEDEFX_H - -#include - - -//----------------------------------------------- -/** - * Provides diverse mathematical utilities and functions. - * \ingroup MathUtilsModul - */ -class MathSup -{ -public: - // -------- Constants - /// Constant for PI. - static const double PI; - - /// Constant for 2*PI - static const double TWO_PI; - - /// Constant for PI/2 - static const double HALF_PI; - - // -------- Angle conversion - /** - * Converts radian to degree. - */ - static double convRadToDeg(const double& dAngRad) - { - return (dAngRad * 180.0 / PI); - } - - /** - * Converts degree to radian. - */ - static double convDegToRad(const double& dAngDeg) - { - return ( dAngDeg * PI / 180.0 ); - } - - /** - * Normalizes angle to the interval [0,2pi[. - */ - static void normalize2Pi(double& angle) - { - angle-=floor(angle/(2 * PI))* 2 * PI; - } - - /** - * Normalizes angle to the interval ]-pi,pi]. - */ - static void normalizePi(double& angle) - { - normalize2Pi(angle); - if ( angle> PI ) angle-=2 * PI; - } - - /** - * Normalizes angle to the interval ]-pi/2,pi/2] for lines or segments. - */ - static void normalizePiHalf(double& angle) - { - normalize2Pi(angle); - if (angle > PI) angle-=2*PI; - if (angle > PI/2.0) angle -= PI; - if (angle <= -PI/2.0) angle += PI; - } - - /** - * Returns the sign. - */ - static double sign(const double& x) - { - if (x<0) - return -1.0; - else - return 1.0; - } - - /** - * Returns the minimum. - */ - static double getMin(const double& a, const double& b) - { - if (a < b) - return a; - else - return b; - } - - /** - * Returns the maximum. - */ - static double getMax(const double& a, const double& b) - { - if (a > b) - return a; - else - return b; - } - - /** - * Calculates the difference angle a-b. - * The difference ange is normalized to the interval ]-pi,pi]. - */ - static double calcDeltaAng(const double& a, const double& b) - { - double c = a-b; - normalizePi(c); - return c; - } - - - /** - * Calculates the arcus tangens and removes ambiguity in quadrant. - */ - static double atan4quad(double y, double x) - { - double result; - - if((x==0.0) && (y==0.0)) - result = 0.0; - else if((x==0.0) && (y > 0.0)) - result = HALF_PI; - else if((x==0.0) && (y < 0.0)) - result = -HALF_PI; - else if((y==0.0) && (x > 0.0)) - result = 0.0; - else if((y==0.0) && (x < 0.0)) - result = PI; - else - { - result = atan(y/x); - if(x<0.0) - { - if(y>0.0) // Quadrant 2 -> correct - result += PI; - else // Quadrant 3 -> correct - result -= PI; - } - } - normalizePi(result); - return result; - } - - - /** - * Calculates the euclidean distance of two points. - */ - static double distance(double x1, double y1, double x2, double y2) - { - return sqrt( distanceSq( x1, y1, x2, y2 ) ); - } - - /** - * Calculates the squared euclidean distance of two points. - */ - static double distanceSq(double x1, double y1, double x2, double y2) - { - double dx = x2 - x1; - double dy = y2 - y1; - - return dx*dx + dy*dy; - } - - /** - * Checks if a bit is set. - */ - static bool isBitSet(int iVal, int iNrBit) - { - if( (iVal & (1 << iNrBit)) == 0) - return false; - else - return true; - } - - /** - * Converts a float to a 4 byte integer value according to - * IEEE specification. - */ - static double convFloatToInt4Byte(double dVal) - { - //Todo - return 1.0; - } - - /** - * Converts a 4 byte integer value to float according to - * IEEE specification. - */ - static double convInt4ByteToFloat(int iVal) - { - unsigned char c[4]; - double dSign; - double dFraction; - double dExp; - double dVal; - - c[0] = iVal >> 24; - c[1] = iVal >> 16; - c[2] = iVal >> 8; - c[3] = iVal; - - if( (c[0] & 0x80) == 0) - dSign = 1; - else - dSign = -1; - - dFraction = (iVal & 0xFFFFFF) | 0x800000; - dFraction = dFraction * pow(2., -23); - - dExp = ((c[0] << 1) | (c[1] >> 7)) - 127; - - dVal = dSign * dFraction * pow(10., dExp); - - return dVal; - } - - /** - * Limits a variable to the interval [-dLimit, dLimit]. - * @param pdToLimit variable to be limited - * @param dLimit bound of the interval [-dLimit, dLimit] - * @return 0: value is in the interval, 1: value has been bound to the lower bound, - * 2: value has been bound to the upper bound - */ - static int limit(double* pdToLimit, double dLimit) - { - int iRet = 0; - - if(*pdToLimit < -dLimit) - { - *pdToLimit = -dLimit; - iRet = 1; - } - if(*pdToLimit > dLimit) - { - *pdToLimit = dLimit; - iRet = 2; - } - - return iRet; - } - - /** - * Limits a variable to the interval [-dLimit, dLimit]. - * @param piToLimit variable to be limited - * @param iLimit bound of the interval [-dLimit, dLimit] - * @return 0: value is in interval, 1: value has been bound to the lower bound, - * 2: value has been bound to the upper bound - */ - static int limit(int* piToLimit, int iLimit) - { - int iRet = 0; - - if(*piToLimit < -iLimit) - { - *piToLimit = -iLimit; - iRet = 1; - } - if(*piToLimit > iLimit) - { - *piToLimit = iLimit; - iRet = 2; - } - - return iRet; - } - - /** - * Checks value to be in an interval [dLow, dHigh]. - * @param dLow lower bound - * @param dHigh upper bound - * @param dVal value - * @return true if value is in the interval - */ - static bool isInInterval(double dLow, double dHigh, double dVal) - { - if( (dVal >= dLow) && (dVal <= dHigh) ) - return true; - - else - return false; - } - -}; - - -//----------------------------------------------- -#endif diff --git a/cob_utilities/common/include/cob_utilities/Mutex.h b/cob_utilities/common/include/cob_utilities/Mutex.h deleted file mode 100644 index c2244cc7f..000000000 --- a/cob_utilities/common/include/cob_utilities/Mutex.h +++ /dev/null @@ -1,74 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -#ifndef MUTEX_INCLUDEDEF_H -#define MUTEX_INCLUDEDEF_H -//----------------------------------------------- -#include - -const unsigned int INFINITE = 0; - - -class Mutex -{ -private: - pthread_mutex_t m_hMutex; - -public: - Mutex() - { - pthread_mutex_init(&m_hMutex, 0); - } - - Mutex( std::string sName) - { -// no named Mutexes for POSIX - pthread_mutex_init(&m_hMutex, 0); - } - - ~Mutex() - { - pthread_mutex_destroy(&m_hMutex); - } - - /** Returns true if log was successful. - */ - bool lock( unsigned int uiTimeOut = INFINITE ) - { - int ret; - - if (uiTimeOut == INFINITE) - { - ret = pthread_mutex_lock(&m_hMutex); - } - else - { - timespec abstime = { time(0) + uiTimeOut, 0 }; - ret = pthread_mutex_timedlock(&m_hMutex, &abstime); - } - - return ! ret; - } - - void unlock() - { - pthread_mutex_unlock(&m_hMutex); - } -}; -//----------------------------------------------- -#endif - diff --git a/cob_utilities/common/include/cob_utilities/StrUtil.h b/cob_utilities/common/include/cob_utilities/StrUtil.h deleted file mode 100644 index 736ff46a4..000000000 --- a/cob_utilities/common/include/cob_utilities/StrUtil.h +++ /dev/null @@ -1,80 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -#ifndef STRUTIL_H -#define STRUTIL_H - -#include -#include - -/** change the string to upper case - @param strToConvert the string to be converted - */ -std::string StringToUpper(std::string strToConvert); - -/** change the string to lower case - @param strToConvert the string to be converted - */ -std::string StringToLower(std::string strToConvert); - -/** convert the number to a string - @param n the integer number to be converted - */ -std::string NumToString(const int n); - -/** convert the number to a string - @param n the unsigned integer number to be converted - */ -std::string NumToString(const unsigned int n); - -/** convert the number to a string - @param l the long integer number to be converted - */ -std::string NumToString(const long l); - -/** convert the number to a string - @param f the float point number to be converted - @param width format of the width of the floating number, default = 10 - @param precise format of the precise of the floating number, default = 7 - */ -std::string NumToString(const float f, unsigned int width=10, unsigned int precise=7); - -/** convert the number to a string - @param d the double precise number to be converted - @param width format of the width of the number, default = 16 - @param precise format of the precise of the number, default = 12 - */ -std::string NumToString(const double d, unsigned int width=16, unsigned int precise=12); - -/** - * C++ version char* style "itoa": Convert the number to a char* - * @param value The value to be converted. - * @param result The char pointer to contain the result. - * @param base The num base which has to be in the range 2 .. 16. - * @return The converted result. If an error occured, the result is a null pointer. - */ -char* itoa( int value, char* result, int base ) ; - -/** - * C++ version std::string style "itoa": Convert the number to a string. - * @param value The value to be converted. - * @param base The num base which has to be in the range 2 .. 16. - * @return The converted result. If an error occured, the result is a null pointer. - */ -std::string itoa(int value, int base); - -#endif diff --git a/cob_utilities/common/include/cob_utilities/TimeStamp.h b/cob_utilities/common/include/cob_utilities/TimeStamp.h deleted file mode 100644 index 642f7bc13..000000000 --- a/cob_utilities/common/include/cob_utilities/TimeStamp.h +++ /dev/null @@ -1,97 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -#ifndef _TimeStamp_H -#define _TimeStamp_H - -#include -#include - -//------------------------------------------------------------------- - -/** Measure system time. - * Use this class for measure system time accurately. Under Windows, it uses - * QueryPerformanceCounter(), which has a resolution of approx. one micro-second. - * The difference between two time stamps can be calculated. - */ -class TimeStamp -{ - public: - /// Constructor. - TimeStamp(); - - /// Destructor. - virtual ~TimeStamp() {}; - - /// Makes time measurement. - void SetNow(); - - /// Retrieves time difference in seconds. - double operator- ( const TimeStamp& EarlierTime ) const; - - /// Increase the timestamp by TimeS seconds. - /** @param TimeS must be >0!. - */ - void operator+= ( double TimeS ); - - /// Reduces the timestamp by TimeS seconds. - /** @param TimeS must be >0!. - */ - void operator-= ( double TimeS ); - - /// Checks if this time is after time "Time". - bool operator> ( const TimeStamp& Time ); - - /// Checks if this time is before time "Time". - bool operator< ( const TimeStamp& Time ); - - /** - * Gets seconds and nanoseconds of the timestamp. - */ - void getTimeStamp ( long& lSeconds, long& lNanoSeconds ); - - /** - * Sets timestamp from seconds and nanoseconds. - */ - void setTimeStamp ( const long& lSeconds, const long& lNanoSeconds ); - - /** - * return the current time as string, in long format YYYY-MM-DD HH:MM:SS.ssssss - *** Attention *** call SetNow() before calling this function - */ - std::string CurrentToString(); - - std::string ToString(); - - protected: - - /// Internal time stamp data. - timespec m_TimeStamp; - - private: - - /// Conversion timespec -> double - static double TimespecToDouble ( const ::timespec& LargeInt ); - - /// Conversion double -> timespec - static ::timespec DoubleToTimespec ( double TimeS ); - -}; - - -#endif - diff --git a/cob_utilities/common/include/cob_utilities/windows.h b/cob_utilities/common/include/cob_utilities/windows.h deleted file mode 100644 index 10e0b002a..000000000 --- a/cob_utilities/common/include/cob_utilities/windows.h +++ /dev/null @@ -1,56 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -#ifndef WINDOWS_H -#define WINDOWS_H - - -#include - -inline void Sleep(long dwMilliseconds) -{ - ::timeval sleepTime = {0, dwMilliseconds * 1000}; - ::select(0, 0, 0, 0, &sleepTime); -} - - -//#ifndef HANDLE -//typedef int HANDLE; -//#endif -//typedef int DWORD; -typedef unsigned char BYTE; -enum { - FALSE = false, - TRUE = true -}; - - -inline int min(int a, int b) -{ - return (a < b) ? a : b; -} - - -inline int max(int a, int b) -{ - return (a > b) ? a : b; -} - - - -#endif - diff --git a/cob_utilities/common/src/IniFile.cpp b/cob_utilities/common/src/IniFile.cpp deleted file mode 100644 index 9c3dcbccb..000000000 --- a/cob_utilities/common/src/IniFile.cpp +++ /dev/null @@ -1,646 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -#include - -#include -#include -#include -#include - -using namespace std; - -//------------------------------------------------------------------- -IniFile::IniFile(): m_vectorSize(500), m_CurCharInd(0) -{ - m_bFileOK=false; - m_CurLine.resize(m_vectorSize); -} -//-------------------------------------------------------------------------------- - -IniFile::IniFile(std::string fileName): m_vectorSize(500), m_CurCharInd(0) -{ - m_bFileOK=false; - m_CurLine.resize(m_vectorSize); - if(fileName != "") - SetFileName(fileName); -} -//-------------------------------------------------------------------------------- -IniFile::~IniFile() -{ -} -//-------------------------------------------------------------------------------- -int IniFile::SetFileName(std::string fileName, std::string strIniFileUsedBy, bool bCreate) -{ - m_fileName = fileName; - m_strIniFileUsedBy = strIniFileUsedBy; - - if ((f = fopen(m_fileName.c_str(),"r")) == NULL) - { - if (bCreate == true) - { - f = fopen(m_fileName.c_str(),"w"); // create new file - std::cout << "Creating new INI-File " << m_fileName.c_str() << std::endl; - fclose(f); - } - else - { - std::cout << "INI-File not found " << m_fileName.c_str() << std::endl; - return -1; - } - } - else fclose(f); - m_bFileOK = true; - return 0; -} -//-------------------------------------------------------------------------------- -int IniFile::WriteKeyString(const char* pSect, const char* pKey, const std::string* pStrToWrite, bool bWarnIfNotfound) -{ - std::string StrWithDelimeters = '"' + *pStrToWrite + '"'; - return WriteKeyValue(pSect, pKey, StrWithDelimeters.c_str(), bWarnIfNotfound); -} -//-------------------------------------------------------------------------------- -int IniFile::WriteKeyValue(const char* szSect,const char* szKey,const char* szValue, bool bWarnIfNotfound) -{ - if (!m_bFileOK) return -1; - - FILE* ftemp; - int lS,lK,i,bEoff; - int bFoundSect,bFoundKey; - char c; - long fpos; - - -/*------------------------verifications*/ - - bFoundSect = 1 /* true*/; - bFoundKey = 0 /* */; - bEoff = 0; - lS = strlen(szSect); - lK = strlen(szKey); - if ((lS * lK) == 0) return -1; - -/*--------------------- file opening */ - - f = fopen(m_fileName.c_str(),"r"); - if (f == NULL) - { - std::cout << "INI-File not found " << m_fileName.c_str() << std::endl; - return -1; - } - if ((ftemp = tmpfile()) == NULL) - { - std::cout << "tmpfile() did not work!" << std::endl; - return -1; - } - - -/* ---------------------- search section and key */ - if (FindSection(szSect, bWarnIfNotfound) != 0) - { - bFoundSect = 0; - } - fpos = ftell(f); - if (bFoundSect) - { - if (!FindKey(szKey, false)) bFoundKey = 1; - fpos = ftell(f); - } - if (feof(f)) bEoff = 1; - -/* --------------------- updating the file */ - fseek(f,0,SEEK_SET); - for (i=0;i0x3A)) - { - iNumLength = z-1; - break; - } - } - *pValue = 0; - for (int i=0; i<=(iNumLength); i++) - { - //convert hex-string from character into int - *pValue += (buf[i+2]-0x30) * (int)pow((double)16, (iNumLength-i)); - } - } - else - { - *pValue = atoi(buf); - } - return 0; - - } - else return -1; -} -//-------------------------------------------------------------------------------- -int IniFile::GetKeyLong(const char* szSect,const char* szKey,long* pValue, - bool bWarnIfNotfound) -{ - char buf[9]; - if (GetKeyValue(szSect,szKey,buf,9, bWarnIfNotfound)!=-1) - { - *pValue = atol(buf); - return 0; - } - else return -1; -} -//-------------------------------------------------------------------------------- -int IniFile::GetKeyDouble(const char* szSect,const char* szKey,double* pValue, - bool bWarnIfNotfound) -{ - char buf[50]; - if (GetKeyValue(szSect, szKey, buf, 50, bWarnIfNotfound) == -1) - { - if( bWarnIfNotfound ) - std::cout << "Setting parameter " << szKey <<" = " << *pValue << " of section '" << szSect << - "' in File '" << m_fileName.c_str() << std::endl; - return -1; - } - - *pValue = atof(buf); - return 0; -} -//----------------------------------------------- -int IniFile::GetKeyDouble(const char* pSect,const char* pKey,double* pValue, - double dDefault, bool bWarnIfNotfound) -{ - (*pValue) = dDefault; - return GetKeyDouble(pSect, pKey, pValue, bWarnIfNotfound); -} -//-------------------------------------------------------------------------------- -int IniFile::GetKeyValue(const char* szSect,const char* szKey,char* szBuf, - int lenBuf, bool bWarnIfNotfound) -{ - if (!m_bFileOK) return -1; - - int lS,lK; - - lS = strlen(szSect); - lK = strlen(szKey); - if ((lS * lK) == 0) return -1; - if ((f = fopen(m_fileName.c_str(),"r")) == NULL) - { - std::cout << "INI-File not found " << m_fileName.c_str() << std::endl; - return -1; - } - if ( FindSection(szSect, bWarnIfNotfound) ) - { fclose(f); - return -1; - } - if ( FindKey(szKey, bWarnIfNotfound) ) - { fclose(f); - return -1; - } - - if (feof(f)) - { - fclose(f); - return -1; - } - - //----------- read szBuf bytes from file into szKey - int BytesRead = fread( szBuf, 1, lenBuf, f ); - - // terminate string - int StrLen; - if(BytesRead < lenBuf) - { - if( BytesRead == 0 && (!feof(f)) ) - { - std::cout << "file read" << std::endl; - } - StrLen = BytesRead; - } - else - { - StrLen = lenBuf-1; - } - szBuf[StrLen] = '\0'; - - fclose(f); - return StrLen; -} -//-------------------------------------------------------------------------------- -int IniFile::GetKeyString(const char* szSect,const char* szKey, std::string* pStrToRead, - bool bWarnIfNotfound) -{ - if (!m_bFileOK) return -1; - - int lS,lK; - - lS = strlen(szSect); - lK = strlen(szKey); - if ((lS * lK) == 0) return -1; - if ((f = fopen(m_fileName.c_str(),"r")) == NULL) - { - std::cout << "INI-File not found " << m_fileName.c_str() << std::endl; - return -1; - } - if ( FindSection(szSect, bWarnIfNotfound) ) - { fclose(f); - return -1; - } - if ( FindKey(szKey, bWarnIfNotfound) ) - { fclose(f); - return -1; - } - - if (feof(f)) - { - fclose(f); - return -1; - } - - //----------- read szBuf bytes from file into szKey - int res = SkipLineUntil(f, '"'); // find begin of string - if(res == -1) - { if(bWarnIfNotfound) - { - std::cout << "GetKeyString section " << szSect << " key " << szKey << " first \" not found" << std::endl; - } - fclose(f); - return -1; - } - - std::string strRead; - res = ReadLineUntil(f, '"', strRead); // read string - if(res == -1) - { - if(bWarnIfNotfound) - { - std::cout << "GetKeyString section " << szSect << " key " << szKey << " string not found" << std::endl; - } - fclose(f); - return -1; - } - - // success - *pStrToRead = strRead; - fclose(f); - return 0; -} -//-------------------------------------------------------------------------------- -int IniFile::SkipLineUntil(FILE* pFile, const char EndChar) -{ - int CharsRead = 0; - while (1) - { - int Char = fgetc(pFile); - - if (Char == EndChar) // end found? - return CharsRead; // read finished - - if (Char == EOF || Char == '\n') - return -1; // end not found - - CharsRead++; - } -} -//-------------------------------------------------------------------------------- -int IniFile::ReadLineUntil(FILE* pFile, const char EndChar, std::string& ReadIntoStr) -{ - int CharsRead = 0; - while (1) - { - int Char = fgetc(pFile); - - if (Char == EndChar) // end found? - return CharsRead; // read finished - - if (Char == EOF || Char == '\n') - return -1; // end not found - - ReadIntoStr.append(1, char(Char)); - - CharsRead++; - } -} -//-------------------------------------------------------------------------------- -int IniFile::FindNextLine(std::vector& NewLine, int& CharInd) -{ - if (!feof(f)) - { - std::string line = fgets(&NewLine[0], NewLine.size(), f); // store next line in NewLine - CharInd=0; // makes CharInd reference the first char of the line - return 0; - } - return -1; -} -//-------------------------------------------------------------------------------- -int IniFile::FindNextSection(std::string* pSect, std::string prevSect, bool bWarnIfNotfound) -{ - std::vector line; - //int charInd = 0; - //int res = -1; - - if (!m_bFileOK) return -1; - - // Make sure that there is no old data. - pSect->erase(); - -/*--------------------- file opening */ - f = fopen(m_fileName.c_str(),"r"); - if (f == NULL) - { - std::cout << "INI-File not found " << m_fileName.c_str() << std::endl; - return -1; - } - if (feof(f)) return -1; - -/*--------------------- search the section */ - if( prevSect != "" ) { - FindSection( prevSect.c_str(), bWarnIfNotfound ); - } else { - fseek(f,0,SEEK_SET); - } - - FindNextLine(m_CurLine, m_CurCharInd); //read first line of file - do - { - if (m_CurLine[0] == '[') - { - while( m_CurCharInd < (int)m_CurLine.size() ) { - m_CurCharInd++; - if (m_CurLine[m_CurCharInd] == ']') // if found section name equals searched one - { - for( int i=1; iappend(1, char(m_CurLine[i])); - return 0; - } - } - } - else - { - FindNextLine(m_CurLine, m_CurCharInd); - } - }while (!feof(f)); - -/*--------------------- file closing */ - fclose(f); - - return 0; -} -//-------------------------------------------------------------------------------- -int IniFile::FindSection(const char* sect, - bool bWarnIfNotfound) -{ - int lS; - lS = strlen(sect); - if (feof(f)) return -1; - - FindNextLine(m_CurLine, m_CurCharInd); //read first line of file - do - { - if (m_CurLine[0] == '[') - { - m_CurCharInd++; - if ((strncmp(&m_CurLine[m_CurCharInd], sect, lS) == 0) && (m_CurLine[m_CurCharInd+lS] == ']')) // if found section name equals searched one - { - return 0; - } - else - { - FindNextLine(m_CurLine, m_CurCharInd); - } - } - else if (m_CurLine[m_CurCharInd] == ' ') // if a blank is found - { - m_CurCharInd++; - } - else - { - FindNextLine(m_CurLine, m_CurCharInd); - } - }while (!feof(f)); - - // not found - if(bWarnIfNotfound) - { - std::cout << "Section [" << sect << "] in IniFile " << m_fileName.c_str() << " used by " - << m_strIniFileUsedBy << " not found" << std::endl; - } - - return -1; -} -//-------------------------------------------------------------------------------- -int IniFile::FindKey(const char* skey, - bool bWarnIfNotfound) -{ - int lS; - long fpos = 0l; - lS = strlen(skey); - if (feof(f)) return -1; - - do - { - fpos=ftell(f);// pointer to the begin of the last read line - FindNextLine(m_CurLine, m_CurCharInd); - - while ( m_CurLine[m_CurCharInd] == ' ' ) // skip blanks - { - m_CurCharInd++; - fpos++; - } - - if (m_CurLine[m_CurCharInd] == '[') // next section? - break; // not found - - if (strncmp(&m_CurLine[m_CurCharInd], skey, lS) == 0) //Found - { - m_CurCharInd+=lS; - fpos+=lS; // set file pointer to end of found key - while ( m_CurLine[m_CurCharInd] == ' ' ) // skip blanks - { - m_CurCharInd++; - fpos++; - } - if ( m_CurLine[m_CurCharInd] == '=' ) - { - m_CurCharInd++; // set index to first char after the = - fpos++; - fseek(f,fpos,SEEK_SET);// set file pointer to first char after the = - return 0; - } - - } - - }while (!feof(f)); - - if(bWarnIfNotfound) - { - std::cout << "Key " << skey << " in IniFile '" << m_fileName.c_str() << "' used by " - << m_strIniFileUsedBy << " not found" << std::endl; - } - return -1; -} -//----------------------------------------------- -int IniFile::GetKey(const char* pSect,const char* pKey, std::string* pStrToRead, bool bWarnIfNotfound) -{ - return GetKeyString(pSect,pKey,pStrToRead,bWarnIfNotfound); -} -//----------------------------------------------- -int IniFile::GetKey(const char* pSect,const char* pKey,int* pValue, bool bWarnIfNotfound) -{ - return GetKeyInt(pSect,pKey,pValue,bWarnIfNotfound); -} -//----------------------------------------------- - -int IniFile::GetKey(const char* pSect, const char* pKey, bool* pValue, bool bWarnIfNotfound) -{ - return GetKeyBool(pSect,pKey,pValue,bWarnIfNotfound); -} -//----------------------------------------------- -int IniFile::GetKey(const char* pSect,const char* pKey,double* pValue, bool bWarnIfNotfound) -{ - return GetKeyDouble(pSect,pKey,pValue,bWarnIfNotfound); -} - diff --git a/cob_utilities/common/src/MathSup.cpp b/cob_utilities/common/src/MathSup.cpp deleted file mode 100644 index 71fc5091a..000000000 --- a/cob_utilities/common/src/MathSup.cpp +++ /dev/null @@ -1,22 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -#include - -const double MathSup::PI = 3.14159265358979323846; -const double MathSup::TWO_PI = 6.283185307179586476925286766559; -const double MathSup::HALF_PI = 1.5707963267948966192313216916398; diff --git a/cob_utilities/common/src/StrUtil.cpp b/cob_utilities/common/src/StrUtil.cpp deleted file mode 100644 index 43bdc8991..000000000 --- a/cob_utilities/common/src/StrUtil.cpp +++ /dev/null @@ -1,120 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -#include -#include - -std::string StringToUpper(std::string strToConvert) { - for(unsigned int i=0;i 16) { - *result = 0; return result; - } - - char* out = result; - int quotient = value; - - do { - *out = "0123456789abcdef"[ std::abs( quotient % base ) ]; - ++out; - quotient /= base; - } while ( quotient ); - - // Only apply negative sign for base 10 - if ( value < 0 && base == 10) *out++ = '-'; - - std::reverse( result, out ); - *out = 0; - return result; -} - -/** - * C++ version std::string style "itoa": - */ -std::string itoa(int value, int base) -{ - enum { kMaxDigits = 35 }; - std::string buf; - - buf.reserve( kMaxDigits ); // Pre-allocate enough space. - - // check that the base if valid - if (base < 2 || base > 16) return buf; - - int quotient = value; - - // Translating number to string with base: - do { - buf += "0123456789abcdef"[ std::abs( quotient % base ) ]; - quotient /= base; - } while ( quotient ); - - // Append the negative sign for base 10 - if ( value < 0 && base == 10) buf += '-'; - std::reverse( buf.begin(), buf.end() ); - return buf; -} - diff --git a/cob_utilities/common/src/TimeStamp.cpp b/cob_utilities/common/src/TimeStamp.cpp deleted file mode 100644 index e9a0cbf99..000000000 --- a/cob_utilities/common/src/TimeStamp.cpp +++ /dev/null @@ -1,159 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -#include - -//----------------------------------------------------------------------------- - -TimeStamp::TimeStamp() -{ - m_TimeStamp.tv_sec = 0; - m_TimeStamp.tv_nsec = 0; -} - -void TimeStamp::SetNow() -{ - ::clock_gettime(CLOCK_REALTIME, &m_TimeStamp); -} - -double TimeStamp::TimespecToDouble(const ::timespec& LargeInt) -{ - return double(LargeInt.tv_sec) + double(LargeInt.tv_nsec) / 1e9; -} - -::timespec TimeStamp::DoubleToTimespec(double TimeS) -{ - ::timespec DeltaTime; - if (! ( TimeS < 4e9 && TimeS > 0.0 )) - { - DeltaTime.tv_sec = 0; - DeltaTime.tv_nsec = 0; - return DeltaTime; - } - - DeltaTime.tv_sec = ::time_t(TimeS); - DeltaTime.tv_nsec - = static_cast((TimeS - double(DeltaTime.tv_sec)) * 1e9); - - return DeltaTime; -} - -double TimeStamp::operator-(const TimeStamp& EarlierTime) const -{ - ::timespec Res; - - Res.tv_sec = m_TimeStamp.tv_sec - EarlierTime.m_TimeStamp.tv_sec; - Res.tv_nsec = m_TimeStamp.tv_nsec - EarlierTime.m_TimeStamp.tv_nsec; - - if (Res.tv_nsec < 0) { - Res.tv_sec--; - Res.tv_nsec += 1000000000; - } - - return TimespecToDouble(Res); -} - -void TimeStamp::operator+=(double TimeS) -{ - ::timespec Dbl = DoubleToTimespec(TimeS); - m_TimeStamp.tv_sec += Dbl.tv_sec; - m_TimeStamp.tv_nsec += Dbl.tv_nsec; - if (m_TimeStamp.tv_nsec > 1000000000) - { - m_TimeStamp.tv_sec++; - m_TimeStamp.tv_nsec -= 1000000000; - } -} - -void TimeStamp::operator-=(double TimeS) -{ - ::timespec Dbl = DoubleToTimespec(TimeS); - m_TimeStamp.tv_sec -= Dbl.tv_sec; - m_TimeStamp.tv_nsec -= Dbl.tv_nsec; - if (m_TimeStamp.tv_nsec < 0.0) - { - m_TimeStamp.tv_sec--; - m_TimeStamp.tv_nsec += 1000000000; - } -} - -bool TimeStamp::operator>(const TimeStamp& Time) -{ - if (m_TimeStamp.tv_sec > Time.m_TimeStamp.tv_sec) return true; - if ((m_TimeStamp.tv_sec == Time.m_TimeStamp.tv_sec) && - (m_TimeStamp.tv_nsec > Time.m_TimeStamp.tv_nsec)) return true; - return false; -} - -bool TimeStamp::operator<(const TimeStamp& Time) -{ - if (m_TimeStamp.tv_sec < Time.m_TimeStamp.tv_sec) return true; - if ((m_TimeStamp.tv_sec == Time.m_TimeStamp.tv_sec) && - (m_TimeStamp.tv_nsec < Time.m_TimeStamp.tv_nsec)) return true; - return false; -} - -void TimeStamp::getTimeStamp(long& lSeconds, long& lNanoSeconds) -{ - lSeconds = m_TimeStamp.tv_sec; - lNanoSeconds = m_TimeStamp.tv_nsec; -}; - -void TimeStamp::setTimeStamp(const long& lSeconds, const long& lNanoSeconds) -{ - m_TimeStamp.tv_sec = lSeconds; - m_TimeStamp.tv_nsec = lNanoSeconds; -}; - -std::string TimeStamp::CurrentToString() -{ -# define TIME_SIZE 400 - - const struct tm *tm; - size_t len; - time_t now; - char pres[TIME_SIZE]; - std::string s; - - now = time ( NULL ); - tm = localtime ( &now ); - len = strftime ( pres, TIME_SIZE, "%Y-%m-%d %H:%M:%S.", tm ); - - s = (std::string)pres + NumToString(m_TimeStamp.tv_nsec / 1000); - - return s; -# undef TIME_SIZE -} - -std::string TimeStamp::ToString() -{ -# define TIME_SIZE 4000 - - const struct tm *tm; - size_t len; - //time_t now; - char pres[TIME_SIZE]; - std::string s; - - tm = localtime ( &m_TimeStamp.tv_sec ); - len = strftime ( pres, TIME_SIZE, "%Y-%m-%d %H:%M:%S.", tm ); - - s = (std::string)pres + NumToString(m_TimeStamp.tv_nsec / 1000); - - return s; -# undef TIME_SIZE -} diff --git a/cob_utilities/package.xml b/cob_utilities/package.xml deleted file mode 100644 index 0d6181698..000000000 --- a/cob_utilities/package.xml +++ /dev/null @@ -1,17 +0,0 @@ - - cob_utilities - 0.7.16 - Deprecated - "cob_utilities" subsumes a number of classes, which are used in the original COb3 software. E.g. "IniFile.h" supports the original inifile structure of Care-O-bot 3. "MathSup.h" provides some basic functions like conversion from degree to radion or norming of angles within +/- PI. The package is currently used while the drivers are ported to ROS and Orocos respectively. Midterm it shall be removed and the ROS structures shall be used for reading parameters during initialization. So, don't use this package in new code! - - Apache 2.0 - - http://ros.org/wiki/cob_utilities - - - Matthias Gruhler - Christian Connette - - catkin - - From 4d0290b2c1e0d0b36e0e2ba5b5a8c527820c9e34 Mon Sep 17 00:00:00 2001 From: fmessmer Date: Wed, 20 Mar 2024 16:58:53 +0100 Subject: [PATCH 11/14] remove cob_relayboard --- cob_driver/package.xml | 1 - cob_relayboard/CHANGELOG.rst | 310 ----------- cob_relayboard/CMakeLists.txt | 36 -- .../include/cob_relayboard/CmdRelaisBoard.h | 33 -- .../common/include/cob_relayboard/Mutex.h | 74 --- .../include/cob_relayboard/SerRelayBoard.h | 175 ------ .../common/include/cob_relayboard/SerialIO.h | 213 ------- .../common/include/cob_relayboard/StrUtil.h | 80 --- cob_relayboard/common/src/SerRelayBoard.cpp | 526 ------------------ cob_relayboard/common/src/SerialIO.cpp | 417 -------------- cob_relayboard/common/src/StrUtil.cpp | 120 ---- cob_relayboard/package.xml | 21 - cob_relayboard/ros/src/analysis.R | 35 -- .../ros/src/cob_relayboard_node.cpp | 266 --------- cob_relayboard/ros/src/relayboard_sim.py | 47 -- 15 files changed, 2354 deletions(-) delete mode 100644 cob_relayboard/CHANGELOG.rst delete mode 100644 cob_relayboard/CMakeLists.txt delete mode 100644 cob_relayboard/common/include/cob_relayboard/CmdRelaisBoard.h delete mode 100644 cob_relayboard/common/include/cob_relayboard/Mutex.h delete mode 100644 cob_relayboard/common/include/cob_relayboard/SerRelayBoard.h delete mode 100644 cob_relayboard/common/include/cob_relayboard/SerialIO.h delete mode 100644 cob_relayboard/common/include/cob_relayboard/StrUtil.h delete mode 100644 cob_relayboard/common/src/SerRelayBoard.cpp delete mode 100644 cob_relayboard/common/src/SerialIO.cpp delete mode 100644 cob_relayboard/common/src/StrUtil.cpp delete mode 100644 cob_relayboard/package.xml delete mode 100644 cob_relayboard/ros/src/analysis.R delete mode 100644 cob_relayboard/ros/src/cob_relayboard_node.cpp delete mode 100755 cob_relayboard/ros/src/relayboard_sim.py diff --git a/cob_driver/package.xml b/cob_driver/package.xml index 3a7cebe7a..d359c7b9e 100644 --- a/cob_driver/package.xml +++ b/cob_driver/package.xml @@ -17,7 +17,6 @@ cob_light cob_mimic cob_phidgets - cob_relayboard cob_scan_unifier cob_sick_s300 cob_sound diff --git a/cob_relayboard/CHANGELOG.rst b/cob_relayboard/CHANGELOG.rst deleted file mode 100644 index 6a1b90117..000000000 --- a/cob_relayboard/CHANGELOG.rst +++ /dev/null @@ -1,310 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package cob_relayboard -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.7.16 (2024-02-20) -------------------- - -0.7.15 (2023-11-06) -------------------- - -0.7.14 (2022-11-17) -------------------- - -0.7.13 (2022-07-29) -------------------- - -0.7.12 (2022-03-15) -------------------- - -0.7.11 (2022-01-12) -------------------- - -0.7.10 (2021-12-23) -------------------- - -0.7.9 (2021-11-26) ------------------- - -0.7.8 (2021-10-19) ------------------- - -0.7.7 (2021-08-02) ------------------- - -0.7.6 (2021-05-10) ------------------- - -0.7.5 (2021-04-06) ------------------- -* Merge pull request `#418 `_ from fmessmer/fix_catkin_lint - fix catkin_lint -* fix catkin_lint -* Contributors: Felix Messmer, fmessmer - -0.7.4 (2020-10-14) ------------------- -* Merge pull request `#417 `_ from fmessmer/test_noetic - test noetic -* Bump CMake version to avoid CMP0048 warning -* Contributors: Felix Messmer, fmessmer - -0.7.3 (2020-03-18) ------------------- - -0.7.2 (2020-03-18) ------------------- -* Merge pull request `#409 `_ from LoyVanBeek/feature/python3_compatibility - [ci_updates] pylint + Python3 compatibility -* fix pylint errors -* Merge pull request `#408 `_ from fmessmer/ci_updates - [travis] ci updates -* catkin_lint fixes -* Contributors: Felix Messmer, fmessmer - -0.7.1 (2019-11-07) ------------------- - -0.7.0 (2019-08-06) ------------------- -* Merge pull request `#396 `_ from HannesBachter/indigo_dev - 0.6.15 -* Contributors: Felix Messmer - -0.6.15 (2019-07-17) -------------------- - -0.6.14 (2019-06-07) -------------------- - -0.6.13 (2019-03-14) -------------------- - -0.6.12 (2018-07-21) -------------------- - -0.6.11 (2018-01-07) -------------------- -* Merge remote-tracking branch 'origin/indigo_release_candidate' into indigo_dev -* Merge pull request `#341 `_ from ipa-fxm/APACHE_license - use license apache 2.0 -* use license apache 2.0 -* Contributors: Felix Messmer, ipa-fxm, ipa-uhr-mk - -0.6.10 (2017-07-24) -------------------- - -0.6.9 (2017-07-18) ------------------- -* manually fix changelog -* Contributors: ipa-fxm - -0.6.8 (2016-10-10) ------------------- - -0.6.7 (2016-04-02) ------------------- - -0.6.6 (2016-04-01) ------------------- -* use aggregated power state message -* tabs vs. spaces -* harmonize publisher queue sizes -* relaynode delete slashes topics -* Contributors: Felix Gruber, ipa-fmw, ipa-fxm - -0.6.5 (2015-08-31) ------------------- - -0.6.4 (2015-08-25) ------------------- -* explicit dependency to boost -* remove obsolete autogenerated mainpage.dox files -* remove trailing whitespaces -* add_dependencies EXPORTED_TARGETS -* migrate to package format 2 -* sort dependencies -* critically review dependencies -* Contributors: ipa-fxm - -0.6.3 (2015-06-17) ------------------- - -0.6.2 (2014-12-15) ------------------- - -0.6.1 (2014-09-17) ------------------- -* Deleted CurrentMeasurement.msg -* Re-add relayboard_sim -* fix install tags -* Current Measurement -* Current measurement -* Removed emergency model and custom Relayboard -* Merge branch 'groovy_dev' of https://github.com/ipa320/cob_driver into groovy_dev - Conflicts: - cob_relayboard/ros/src/new_method.py - cob_relayboard/ros/src/relayboard_sim.py -* Thrash -* Emergency stop modeling - * First Working Version for the Care-o-Bot -* Contributors: Thiago de Freitas, ipa-cob4-2, ipa-nhg, thiagodefreitas, thiagodefreitas@gmail.com - -0.6.0 (2014-09-09) ------------------- - -0.5.7 (2014-08-26) ------------------- -* Merge pull request `#163 `_ from ipa320/hydro_dev - updates from hydro_dev -* 0.5.6 -* update changelog -* move EmergencyStopState.msg to cob_msgs + PowerBoardState works again -* Added queue_size=1 paramter to Publisher initialisation. -* changes due to introduction of cob_msgs -* small changes to remove warnings -* Cleaned up cob_driver with reduced deps to compile on indigo -* fix tabs vs spaces -* Fix python indentation problems -* Contributors: Alexander Bubeck, Denis Štogl, Felix Messmer, Florian Weisshardt, Scott K Logan, ipa-fxm - -0.5.6 (2014-08-26) ------------------- -* Merge pull request `#163 `_ from ipa320/hydro_dev - updates from hydro_dev -* move EmergencyStopState.msg to cob_msgs + PowerBoardState works again -* Added queue_size=1 paramter to Publisher initialisation. -* changes due to introduction of cob_msgs -* small changes to remove warnings -* Cleaned up cob_driver with reduced deps to compile on indigo -* fix tabs vs spaces -* Fix python indentation problems -* Contributors: Alexander Bubeck, Denis Štogl, Felix Messmer, Florian Weisshardt, Scott K Logan, ipa-fxm - -0.5.3 (2014-03-31) ------------------- -* install tags -* Contributors: ipa-fxm - -0.5.2 (2014-03-20) ------------------- - -0.5.1 (2014-03-20) ------------------- -* some install tag updates -* cleaup merge hell -* Revert "added pr2_msgs to dependencies" as this change has been introduced in commit cc92fd0e590b607b29ea73bcdd - This reverts commit 203b52f3d9d4b26ef0db8e415e1aa3e883fbb708. -* merge changes from frederikhegger, `#80 `_ -* added pr2_msgs to dependencies -* changes for hydro -* fix compile bug -* merge -* common/src/SerRelayBoard.cpp -* Missed CmdRelaisBoard.h file -* Added version 3 for relayboard -* fix compiling bug for ubuntu > precise -* Installation stuff -* use v not mV -* use V and not mV -* removed power state publisher -* cleaned up CMakeLists and added install directives -* further modifications for catkin, now everything is compiling and linking -* compiling but still some linker errors -* Second catkinization push -* First catkinization, still need to update some CMakeLists.txt -* More organization to the voltage commit -* Organizing the voltage filter commit -* Reverting new_method -* First robot changes -* IPA PC -* added line buffering -* record voltage update -* discharge analysis -* discharge analysis -* voltage test -* added topic to publish voltage -* remove test -* move relayboard_sim launch file to cob_bringup -* add TODOs -* emergency stop message: misused wireless field from power_board/state message as scanner stop filed -* add simulated relayboard again -* removed deprecated yaml and launch files -* emergency stop topic for simulation -* using private namespace -* merge -* changed relayboard topics and reversed em state to work with pr2_dashboard -* additional config files for cob3-bosch -* additional battery interface -* added PowerState and PowerBoardState messages to relayboard for dashboard usage, fix naming issue in the powercubechain follow trajectory interface -* added roslaunch tests -* added cob3-4 configs -* additional config files for cob3-bosch -* added rostest -* missing file -* relayboard_sim.launch for icob -* icob changes -* update cob3-3 -* rearranging cob_camera_sensors launch files -* config for cob3-3 -* config for cob3-3 -* camera settings added for head -* moved ultiple message files out of cob_msgs to their own packages -* added camera tests -* configurable relayboard -* release update for cob3-1 -* cob_relayboard: Added support for different protocol versions as yaml parameter (esspecially NUM_BYTE_SEND) -* merge -* added configs for desire -* changed NUM_CHAR_BYTE: TODO as parameter for cob3-2 -* small changes on relayboard -* edited Relayboard: now publishes only on topic, when succesfully connected to relayboard; when connection to relayboard breaks, publish EM_STOP_ACTIVE messages -* removed dependency to generic can -* cleanup in cob_driver -* new rostest file for relayboard -* included new rostest file relayboard.test -* renamed as relayboard.test -* hztest for emergency_stop_state -* included rostest -* devs for cob3-2 -* restructured base_controller -* Now also with ElmoRecorderReadout feature low CPU costs in base_drive_chain -* much ado about nothing -* Modified launch files of cob_base_drive_chain, cob_relayboard, cob_undercaariage_ctrl and cob_teleop_ucar and made them hierarchic -* removed hard coded entry of camera-axis limit switch in CanDriveHarmonica -* small launch file adaptions -* relayboard fixed -* Fixed relayboard-bug - at first go (by opening serial connection with O_NONBLOCK flag) -* update documentation and deleted tf broadcaster -* modification on cob3-2 -* adapted launchfiles and added relative drive service -* Renamed and worked on cob_drive_identification, moved Elmo Recorder services to cob_srvs -* merged in master and manually solved conflicts in base_drive_chain.cpp -* cleanup in stacks -* added dependency -* Merge branch 'review' into cpc-pk -* Deployment of undercarriage controller debugged and finished: launch-script cob_ucar_joy starts up relayboard, base_drive_chain and controller; also remaps topics and services in correct namespaces. Debugging of controller itself is work in progress: simplified and removed old stuff - code compiles - controller runs but appaerently has some bugs -> may not yet be used -* Merge branch 'review-cpc' -* Merge branch 'review' into cpc-pk -* debugging undercarriage drivers (base_drive_chain + relayboard + ucar_ctrl) - work in progress -* cleanup in cob_driver -* Makefile for cob_relayboard -* after merging current review -* modifications to cob_relayboard -* Added EmergencyStop Message containing the current em signals as well as current state (e.g. confirmed after using the key-switch); accordingly adapted the relayboard-node to output the EMState together with EM signals; Last but not least: Fixed a typing error in the platform node -* renamed to cob_ -* renamed packages to cob_ convention -* Moved StrUtil and TimeStamp from canopen_motor to cob_utilities; Adapted CMakelists and manifest of related packages -* Merged Relayboard into master branch -* simple_drive_test on COB3 -* Header copyright infos adapted -* cob_relayboard: adapted launchfile for setting com port there. Platform.ini not anymore used there -* Removed unused features of relayboard from cob_relayboard. Cleaned interface and added readable error responses -* Debugged Relaysboard Node - checked differnt cyclerates - tested operation -* cob_relayboard node is publishing EmergencyStop States (EM-Stop & ScannerStop) correctly (tested) -* Simple Publisher fpr EmergencyStopStates including launch file -* node added *g* -* Very basic RelayBoard node added, which only has ability to publish EmergencyStopStates -* cob_relayboard: SerialIO library added, SerRelBoard library added, properly linked and compiled, ready for starting with ros node.. -* Added new package cob_relayboard in stack cob_driver -* Contributors: Alexander Bubeck, Christian, Christian Connette, Florian Weißhardt, Richard Bormann, Thiago de Freitas, abubeck, cob, cpc, cpc-pk, fmw, ipa, ipa-cob3-7, ipa-cpc, ipa-fmw, ipa-mig, ipa-nhg, ipa-tys, ipa-uhr, mfueller, thiagodefreitas, uh diff --git a/cob_relayboard/CMakeLists.txt b/cob_relayboard/CMakeLists.txt deleted file mode 100644 index 28739b5b7..000000000 --- a/cob_relayboard/CMakeLists.txt +++ /dev/null @@ -1,36 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(cob_relayboard) - -find_package(catkin REQUIRED COMPONENTS cob_msgs roscpp std_msgs) - -catkin_package( - CATKIN_DEPENDS cob_msgs roscpp std_msgs - INCLUDE_DIRS common/include - LIBRARIES ${PROJECT_NAME}_SerialIO ${PROJECT_NAME} -) - -### BUILD ### -include_directories(common/include ${catkin_INCLUDE_DIRS}) - -add_library(${PROJECT_NAME}_SerialIO common/src/SerialIO.cpp) -add_library(${PROJECT_NAME} common/src/SerRelayBoard.cpp common/src/StrUtil.cpp) -target_link_libraries(${PROJECT_NAME} ${PROJECT_NAME}_SerialIO) - -add_executable(${PROJECT_NAME}_node ros/src/${PROJECT_NAME}_node.cpp) -add_dependencies(${PROJECT_NAME}_node ${catkin_EXPORTED_TARGETS}) -target_link_libraries(${PROJECT_NAME}_node ${PROJECT_NAME} ${catkin_LIBRARIES}) - -### INSTALL ### -install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_SerialIO ${PROJECT_NAME}_node - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -install(DIRECTORY common/include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) - -catkin_install_python(PROGRAMS ros/src/relayboard_sim.py - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) diff --git a/cob_relayboard/common/include/cob_relayboard/CmdRelaisBoard.h b/cob_relayboard/common/include/cob_relayboard/CmdRelaisBoard.h deleted file mode 100644 index 067174d5d..000000000 --- a/cob_relayboard/common/include/cob_relayboard/CmdRelaisBoard.h +++ /dev/null @@ -1,33 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -#ifndef CMDRELAISBOARD_INCLUDEDEF_H -#define CMDRELAISBOARD_INCLUDEDEF_H - -enum CMD_RELAISBOARD -{ - CMD_RELAISBOARD_CONNECT, - CMD_RELAISBOARD_GET_DATA, - CMD_RELAISBOARD_SET_DEBUG_DATA, - CMD_RELAISBOARD_GET_DEBUG_DATA, - CMD_RELAISBOARD_UNKNOWN -}; - -#endif - - - diff --git a/cob_relayboard/common/include/cob_relayboard/Mutex.h b/cob_relayboard/common/include/cob_relayboard/Mutex.h deleted file mode 100644 index c2244cc7f..000000000 --- a/cob_relayboard/common/include/cob_relayboard/Mutex.h +++ /dev/null @@ -1,74 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -#ifndef MUTEX_INCLUDEDEF_H -#define MUTEX_INCLUDEDEF_H -//----------------------------------------------- -#include - -const unsigned int INFINITE = 0; - - -class Mutex -{ -private: - pthread_mutex_t m_hMutex; - -public: - Mutex() - { - pthread_mutex_init(&m_hMutex, 0); - } - - Mutex( std::string sName) - { -// no named Mutexes for POSIX - pthread_mutex_init(&m_hMutex, 0); - } - - ~Mutex() - { - pthread_mutex_destroy(&m_hMutex); - } - - /** Returns true if log was successful. - */ - bool lock( unsigned int uiTimeOut = INFINITE ) - { - int ret; - - if (uiTimeOut == INFINITE) - { - ret = pthread_mutex_lock(&m_hMutex); - } - else - { - timespec abstime = { time(0) + uiTimeOut, 0 }; - ret = pthread_mutex_timedlock(&m_hMutex, &abstime); - } - - return ! ret; - } - - void unlock() - { - pthread_mutex_unlock(&m_hMutex); - } -}; -//----------------------------------------------- -#endif - diff --git a/cob_relayboard/common/include/cob_relayboard/SerRelayBoard.h b/cob_relayboard/common/include/cob_relayboard/SerRelayBoard.h deleted file mode 100644 index 54092820d..000000000 --- a/cob_relayboard/common/include/cob_relayboard/SerRelayBoard.h +++ /dev/null @@ -1,175 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -#ifndef SerRelayBoard_INCLUDEDEF_H -#define SerRelayBoard_INCLUDEDEF_H - -//----------------------------------------------- -#include -#include -#include - -//----------------------------------------------- - -/** - * Driver class for communication with a Neobotix RelayBoard. - * Uses RS422 with 420 kBaud. - */ -class SerRelayBoard -{ -public: - - SerRelayBoard(std::string ComPort, int ProtocolVersion = 1); - - ~SerRelayBoard(); - - // Main control functions - bool init(); - bool reset(); - bool shutdown(); - - int evalRxBuffer(); //needs to be calles to read new data from relayboard - int sendRequest(); //sends collected data and requests response - - //Services by relayboard - int setDigOut(int iChannel, bool bOn); - int getAnalogIn(int* piAnalogIn); - int getDigIn(); - bool isEMStop(); - bool isScannerStop(); - int getBatteryVoltage() - { - return m_iRelBoardBattVoltage; - } - int getChargeCurrent() - { - return m_iChargeCurrent; - } - - - enum RelBoardReturns - { - NO_ERROR = 0, - NOT_INITIALIZED = 1, - GENERAL_SENDING_ERROR = 2, - TOO_LESS_BYTES_IN_QUEUE = 3, - NO_MESSAGES = 4, //for a long time, no message have been received, check com port! - CHECKSUM_ERROR = 5, - }; - - enum RelBoardCmd - { - CMD_SET_CHARGE_RELAY = 1, - CMD_RESET_POS_CNT = 2, - CMD_QUICK_STOP = 4, - CMD_SET_RELAY1 = 8, - CMD_SET_RELAY2 = 16, - CMD_SET_RELAY3 = 32, - CMD_SET_RELAY4 = 64, - CMD_SET_RELAY5 = 128, - CMD_SET_RELAY6 = 256, - CMD_ZERO_GYRO = 512 - }; - - enum RelBoardConfig - { - CONFIG_HAS_IOBOARD = 1, - CONFIG_HAS_USBOARD = 2, - CONFIG_HAS_GYROBOARD = 4, - CONFIG_HAS_RADARBOARD1 = 8, - CONFIG_HAS_RADARBOARD2 = 16, - CONFIG_HAS_DRIVES = 32, - }; - - enum TypeLCD - { - LCD_20CHAR_TEXT, - LCD_60CHAR_TEXT, - RELAY_BOARD_1_4 - }; - -protected: - - - std::string m_sNumComPort; - - void txCharArray(); - void rxCharArray(); - - void convDataToSendMsg(unsigned char cMsg[]); - bool convRecMsgToData(unsigned char cMsg[]); - - Mutex m_Mutex; - - int m_iNumBytesSend; - int m_iTypeLCD; - - unsigned char m_cTextDisplay[60]; - - //relayboard 1.4: - int m_iVelCmdMotRearRightEncS; - int m_iVelCmdMotRearLeftEncS; - char m_cSoftEMStop; - char m_cDebugRearRight[4]; - int m_iPosMeasMotRearRightEnc; - int m_iVelMeasMotRearRightEncS; - int m_iPosMeasMotRearLeftEnc; - char m_cDebugRearLeft[4]; - int m_iVelMeasMotRearLeftEncS; - int m_iMotRearRightStatus; - int m_iMotRearLeftStatus; - double m_dLastPosRearRight; - double m_dLastPosRearLeft; - - //----------------------- - // send data - - // RelayBoard - int m_iConfigRelayBoard; - int m_iCmdRelayBoard; - - // IOBoard - int m_iIOBoardDigOut; - - // MotCtrlBoards - int m_iVelCmdMotRightEncS; - int m_iVelCmdMotLeftEncS; - - // USBoard - int m_iUSBoardSensorActive; - - //----------------------- - // rec data - int m_iRelBoardStatus; - int m_iChargeCurrent; - int m_iRelBoardBattVoltage; - int m_iRelBoardKeyPad; - int m_iRelBoardAnalogIn[4]; - int m_iRelBoardTempSensor; - - int m_iDigIn; - int m_iProtocolVersion; - int m_NUM_BYTE_SEND; - - SerialIO m_SerIO; - - bool m_bComInit; -}; - - -//----------------------------------------------- -#endif diff --git a/cob_relayboard/common/include/cob_relayboard/SerialIO.h b/cob_relayboard/common/include/cob_relayboard/SerialIO.h deleted file mode 100644 index ab9025d20..000000000 --- a/cob_relayboard/common/include/cob_relayboard/SerialIO.h +++ /dev/null @@ -1,213 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -#ifndef _SerialIO_H -#define _SerialIO_H - -#include -#include - -#include - -/** - * Wrapper class for serial communication. - */ -class SerialIO -{ -public: - // ---------------------- Constants - /// Constants for defining the handshake. - enum HandshakeFlags - { - HS_NONE, - HS_HARDWARE, - HS_XONXOFF - }; - - /// Constants for defining the parity bits. - enum ParityFlags - { - PA_NONE, - PA_EVEN, - PA_ODD, -// UNIX serial drivers only support even, odd, and no parity bit generation. - PA_MARK, - PA_SPACE - }; - - /// Constants for defining the stop bits. - enum StopBits - { - SB_ONE, - SB_ONE_5, // ????? returns an error ????? - SB_TWO - }; - - /// Default constructor - SerialIO(); - - /// Destructor - virtual ~SerialIO(); - - /** - * Sets the device name - * @param Name 'COM1', 'COM2', ... - */ - void setDeviceName(const char *Name) { m_DeviceName = Name; } - - /** - * Sets the baudrate. - * @param BaudRate baudrate. - */ - void setBaudRate(int BaudRate) { m_BaudRate = BaudRate; } - - /** - * Changes the baudrate. - * The serial port is allready open. - * @param BaudRate new baudrate. - */ - void changeBaudRate(int BaudRate); - - /** - * Sets a multiplier for the baudrate. - * Some serial cards need a specific multiplier for the baudrate. - * @param Multiplier default is one. - */ - void setMultiplier(double Multiplier = 1) { m_Multiplier = Multiplier; }; - - /** - * Sets the message format. - */ - void SetFormat(int ByteSize, ParityFlags Parity, int StopBits) - { m_ByteSize = ByteSize; m_Parity = Parity; m_StopBits = StopBits; } - - /** - * Defines the handshake type. - */ - void setHandshake(HandshakeFlags Handshake) { m_Handshake = Handshake; } - - /** - * Sets the buffer sizes. - * @param ReadBufSize number of bytes of the read buffer. - * @param WriteBufSize number of bytes of the write buffer. - */ - void setBufferSize(int ReadBufSize, int WriteBufSize) - { m_ReadBufSize = ReadBufSize; m_WriteBufSize = WriteBufSize; } - - /** - * Sets the timeout. - * @param Timeout in seconds - */ - void setTimeout(double Timeout); - - /** - * Sets the byte period for transmitting bytes. - * If the period is not equal to 0, the transmit will be repeated with the given - * period until all bytes are transmitted. - * @param default is 0. - */ - void setBytePeriod(double Period); - - /** - * Opens serial port. - * The port has to be configured before. - */ - int openIO(); - - /** - * Closes the serial port. - */ - void closeIO(); - - /** - * Reads the serial port blocking. - * The function blocks until the requested number of bytes have been - * read or the timeout occurs. - * @param Buffer pointer to the buffer. - * @param Length number of bytes to read - */ - int readBlocking(char *Buffer, int Length); - - - /** - * Reads the serial port non blocking. - * The function returns all avaiable bytes but not more than requested. - * @param Buffer pointer to the buffer. - * @param Length number of bytes to read - */ - int readNonBlocking(char *Buffer, int Length); - - /** - * Writes bytes to the serial port. - * @param Buffer buffer of the message - * @param Length number of bytes to send - */ - int writeIO(const char *Buffer, int Length); - - /** - * Returns the number of bytes available in the read buffer. - */ - int getSizeRXQueue(); - - - /** Clears the read and transmit buffer. - */ - void purge() - { - ::tcflush(m_Device, TCIOFLUSH); - } - - /** Clears the read buffer. - */ - void purgeRx() { - tcflush(m_Device, TCIFLUSH); -} - - /** - * Clears the transmit buffer. - * The content of the buffer will not be transmitted. - */ - void purgeTx() { - tcflush(m_Device, TCOFLUSH); - } - - /** - * Sends the transmit buffer. - * All bytes of the transmit buffer will be sent. - */ - void flushTx() { - tcdrain(m_Device); - } - -protected: - ::termios m_tio; - std::string m_DeviceName; - int m_Device; - int m_BaudRate; - double m_Multiplier; - int m_ByteSize, m_StopBits; - ParityFlags m_Parity; - HandshakeFlags m_Handshake; - int m_ReadBufSize, m_WriteBufSize; - double m_Timeout; - ::timeval m_BytePeriod; - bool m_ShortBytePeriod; -}; - - -#endif // - diff --git a/cob_relayboard/common/include/cob_relayboard/StrUtil.h b/cob_relayboard/common/include/cob_relayboard/StrUtil.h deleted file mode 100644 index 736ff46a4..000000000 --- a/cob_relayboard/common/include/cob_relayboard/StrUtil.h +++ /dev/null @@ -1,80 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -#ifndef STRUTIL_H -#define STRUTIL_H - -#include -#include - -/** change the string to upper case - @param strToConvert the string to be converted - */ -std::string StringToUpper(std::string strToConvert); - -/** change the string to lower case - @param strToConvert the string to be converted - */ -std::string StringToLower(std::string strToConvert); - -/** convert the number to a string - @param n the integer number to be converted - */ -std::string NumToString(const int n); - -/** convert the number to a string - @param n the unsigned integer number to be converted - */ -std::string NumToString(const unsigned int n); - -/** convert the number to a string - @param l the long integer number to be converted - */ -std::string NumToString(const long l); - -/** convert the number to a string - @param f the float point number to be converted - @param width format of the width of the floating number, default = 10 - @param precise format of the precise of the floating number, default = 7 - */ -std::string NumToString(const float f, unsigned int width=10, unsigned int precise=7); - -/** convert the number to a string - @param d the double precise number to be converted - @param width format of the width of the number, default = 16 - @param precise format of the precise of the number, default = 12 - */ -std::string NumToString(const double d, unsigned int width=16, unsigned int precise=12); - -/** - * C++ version char* style "itoa": Convert the number to a char* - * @param value The value to be converted. - * @param result The char pointer to contain the result. - * @param base The num base which has to be in the range 2 .. 16. - * @return The converted result. If an error occured, the result is a null pointer. - */ -char* itoa( int value, char* result, int base ) ; - -/** - * C++ version std::string style "itoa": Convert the number to a string. - * @param value The value to be converted. - * @param base The num base which has to be in the range 2 .. 16. - * @return The converted result. If an error occured, the result is a null pointer. - */ -std::string itoa(int value, int base); - -#endif diff --git a/cob_relayboard/common/src/SerRelayBoard.cpp b/cob_relayboard/common/src/SerRelayBoard.cpp deleted file mode 100644 index 0ec84565c..000000000 --- a/cob_relayboard/common/src/SerRelayBoard.cpp +++ /dev/null @@ -1,526 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -#include -#include -#include - -//----------------------------------------------- - - -#define NUM_BYTE_SEND 79 //Total amount of data sent to relayboard in one message, is now passed and set as protocol-version argument in constructor - -#define RS422_BAUDRATE 420000 -#define RS422_RX_BUFFERSIZE 1024 -#define RS422_TX_BUFFERSIZE 1024 - -#define RS422_TIMEOUT 0.025 - -#define NUM_BYTE_REC_MAX 120 -#define NUM_BYTE_REC_HEADER 4 //Header bytes, which are used to identify the beginning of a new received message {0x02, 0x80, 0xD6, 0x02} -#define NUM_BYTE_REC_CHECKSUM 2 //checksum for message, that is built as the sum of all data bytes contained in the message -#define NUM_BYTE_REC 104 //Total amount of data bytes in a received message (from the relayboard) - -#define NUM_BYTE_SEND_RELAYBOARD_14 88 -#define NUM_BYTE_REC_RELAYBOARD_14 124 - - -//----------------------------------------------- -SerRelayBoard::SerRelayBoard(std::string ComPort, int ProtocolVersion) -{ - m_iProtocolVersion = ProtocolVersion; - if(m_iProtocolVersion == 1) - m_NUM_BYTE_SEND = 50; - else if(m_iProtocolVersion == 2) - { m_NUM_BYTE_SEND = 79; - m_iTypeLCD = LCD_60CHAR_TEXT; - } - else if(m_iProtocolVersion == 3) - { - m_NUM_BYTE_SEND = NUM_BYTE_SEND_RELAYBOARD_14; - m_iTypeLCD = RELAY_BOARD_1_4; - } - m_bComInit = false; - m_sNumComPort = ComPort; - - m_iRelBoardBattVoltage = 0; - m_iConfigRelayBoard = 0; - m_iRelBoardKeyPad = 0xFFFF; - m_iCmdRelayBoard = 0; - m_iDigIn = 0; - m_cSoftEMStop = 0; - -} - -//----------------------------------------------- -SerRelayBoard::~SerRelayBoard() -{ - m_SerIO.closeIO(); -} - -//----------------------------------------------- -int SerRelayBoard::evalRxBuffer() -{ - static int siNoMsgCnt = 0; - - int iNumByteRec = NUM_BYTE_REC; - if(m_iTypeLCD == RELAY_BOARD_1_4) - { - iNumByteRec = NUM_BYTE_REC_RELAYBOARD_14; - } - - const int c_iNrBytesMin = NUM_BYTE_REC_HEADER + iNumByteRec + NUM_BYTE_REC_CHECKSUM; - const int c_iSizeBuffer = 4096; - - int i; - int errorFlag = NO_ERROR; - int iNrBytesInQueue, iNrBytesRead, iDataStart; - unsigned char cDat[c_iSizeBuffer]; - unsigned char cTest[4] = {0x02, 0x80, 0xD6, 0x02}; - - if( !m_bComInit ) return NOT_INITIALIZED; - - //enough data in queue? - iNrBytesInQueue = m_SerIO.getSizeRXQueue(); - if(iNrBytesInQueue < c_iNrBytesMin) - { - //there are too less bytes in queue - siNoMsgCnt++; - if(siNoMsgCnt > 29) - { - //std::cerr << "Relayboard: " << siNoMsgCnt << " cycles no msg received"; - siNoMsgCnt = 0; - errorFlag = NO_MESSAGES; - } else errorFlag = TOO_LESS_BYTES_IN_QUEUE; - - return errorFlag; - } - else - { - siNoMsgCnt = 0; - } - - // search most recent data from back of queue - iNrBytesRead = m_SerIO.readBlocking((char*)&cDat[0], iNrBytesInQueue); - for(i = (iNrBytesRead - c_iNrBytesMin); i >= 0 ; i--) - { - //try to find start bytes - if((cDat[i] == cTest[0]) && (cDat[i+1] == cTest[1]) && (cDat[i+2] == cTest[2]) && (cDat[i+3] == cTest[3])) - { - iDataStart = i + 4; - - // checksum ok? - if( convRecMsgToData(&cDat[iDataStart]) ) - { - return errorFlag; - } - else - { - //std::cerr << "Relayboard: checksum error"; - errorFlag = CHECKSUM_ERROR; - return errorFlag; - } - } - } - - return errorFlag; -} - -//----------------------------------------------- -bool SerRelayBoard::init() -{ - m_SerIO.setBaudRate(RS422_BAUDRATE); - m_SerIO.setDeviceName( m_sNumComPort.c_str() ); - m_SerIO.setBufferSize(RS422_RX_BUFFERSIZE, RS422_TX_BUFFERSIZE); - m_SerIO.setTimeout(RS422_TIMEOUT); - - m_SerIO.openIO(); - - m_bComInit = true; - - return true; -} - -//----------------------------------------------- -bool SerRelayBoard::reset() -{ - m_SerIO.closeIO(); - m_bComInit = false; - - init(); - - return true; -} - -//----------------------------------------------- -bool SerRelayBoard::shutdown() -{ - m_SerIO.closeIO(); - - m_bComInit = false; - - return true; -} - - -//----------------------------------------------- -bool SerRelayBoard::isEMStop() -{ - if( (m_iRelBoardStatus & 0x0001) != 0) - { - return true; - } - else - { - return false; - } -} - -//----------------------------------------------- -bool SerRelayBoard::isScannerStop() -{ - if( (m_iRelBoardStatus & 0x0002) != 0) - { - return true; - } - else - { - return false; - } -} - -//----------------------------------------------- -int SerRelayBoard::sendRequest() { - int errorFlag = NO_ERROR; - int iNrBytesWritten; - - unsigned char cMsg[m_NUM_BYTE_SEND]; - - m_Mutex.lock(); - - convDataToSendMsg(cMsg); - - m_SerIO.purgeTx(); - - iNrBytesWritten = m_SerIO.writeIO((char*)cMsg, m_NUM_BYTE_SEND); - - if(iNrBytesWritten < m_NUM_BYTE_SEND) { - //std::cerr << "Error in sending message to Relayboard over SerialIO, lost bytes during writing" << std::endl; - errorFlag = GENERAL_SENDING_ERROR; - } - - m_Mutex.unlock(); - - return errorFlag; -} - -int SerRelayBoard::setDigOut(int iChannel, bool bOn) -{ - switch( iChannel) - { - case 0: - - if(bOn) { m_iCmdRelayBoard |= CMD_SET_CHARGE_RELAY; } - else { m_iCmdRelayBoard &= ~CMD_SET_CHARGE_RELAY; } - - break; - - case 1: - - if(bOn) { m_iCmdRelayBoard |= CMD_SET_RELAY1; } - else { m_iCmdRelayBoard &= ~CMD_SET_RELAY1; } - - break; - - case 2: - - if(bOn) { m_iCmdRelayBoard |= CMD_SET_RELAY2; } - else { m_iCmdRelayBoard &= ~CMD_SET_RELAY2; } - - break; - - case 3: - - if(bOn) { m_iCmdRelayBoard |= CMD_SET_RELAY3; } - else { m_iCmdRelayBoard &= ~CMD_SET_RELAY3; } - - break; - - case 4: - - if(bOn) { m_iCmdRelayBoard |= CMD_SET_RELAY4; } - else { m_iCmdRelayBoard &= ~CMD_SET_RELAY4; } - - break; - - case 5: - - if(bOn) { m_iCmdRelayBoard |= CMD_SET_RELAY5; } - else { m_iCmdRelayBoard &= ~CMD_SET_RELAY5; } - - break; - - case 6: - - if(bOn) { m_iCmdRelayBoard |= CMD_SET_RELAY6; } - else { m_iCmdRelayBoard &= ~CMD_SET_RELAY6; } - - break; - - default: - - return -1; - } - - return 0; -} -//----------------------------------------------- -int SerRelayBoard::getAnalogIn(int* piAnalogIn) -{ - piAnalogIn[0] = m_iChargeCurrent; - piAnalogIn[1] = m_iRelBoardBattVoltage; - piAnalogIn[2] = m_iRelBoardTempSensor; - piAnalogIn[3] = m_iRelBoardKeyPad; - piAnalogIn[4] = m_iRelBoardAnalogIn[0]; - piAnalogIn[5] = m_iRelBoardAnalogIn[1]; - piAnalogIn[6] = m_iRelBoardAnalogIn[2]; - piAnalogIn[7] = m_iRelBoardAnalogIn[3]; - - return 0; -} - -//----------------------------------------------- -int SerRelayBoard::getDigIn() -{ - return m_iDigIn; -} - -void SerRelayBoard::convDataToSendMsg(unsigned char cMsg[]) -{ - int i; - static int j = 0; - int iCnt = 0; - int iChkSum = 0; - - if (m_cSoftEMStop & 0x02) - { - if (j == 1) - { - m_cSoftEMStop &= 0xFD; - j = 0; - } - else if (j == 0) - { - j = 1; - } - } - - cMsg[iCnt++] = CMD_RELAISBOARD_GET_DATA; - - cMsg[iCnt++] = m_iConfigRelayBoard >> 8; - cMsg[iCnt++] = m_iConfigRelayBoard; - - cMsg[iCnt++] = m_iCmdRelayBoard >> 8; - cMsg[iCnt++] = m_iCmdRelayBoard; - - cMsg[iCnt++] = m_iIOBoardDigOut >> 8; - cMsg[iCnt++] = m_iIOBoardDigOut; - - cMsg[iCnt++] = m_iVelCmdMotRightEncS >> 24; - cMsg[iCnt++] = m_iVelCmdMotRightEncS >> 16; - cMsg[iCnt++] = m_iVelCmdMotRightEncS >> 8; - cMsg[iCnt++] = m_iVelCmdMotRightEncS; - - cMsg[iCnt++] = m_iVelCmdMotLeftEncS >> 24; - cMsg[iCnt++] = m_iVelCmdMotLeftEncS >> 16; - cMsg[iCnt++] = m_iVelCmdMotLeftEncS >> 8; - cMsg[iCnt++] = m_iVelCmdMotLeftEncS; - - if(m_iTypeLCD == RELAY_BOARD_1_4) - { - cMsg[iCnt++] = m_iVelCmdMotRearRightEncS >> 24; - cMsg[iCnt++] = m_iVelCmdMotRearRightEncS >> 16; - cMsg[iCnt++] = m_iVelCmdMotRearRightEncS >> 8; - cMsg[iCnt++] = m_iVelCmdMotRearRightEncS; - - cMsg[iCnt++] = m_iVelCmdMotRearLeftEncS >> 24; - cMsg[iCnt++] = m_iVelCmdMotRearLeftEncS >> 16; - cMsg[iCnt++] = m_iVelCmdMotRearLeftEncS >> 8; - cMsg[iCnt++] = m_iVelCmdMotRearLeftEncS; - } - - cMsg[iCnt++] = m_iUSBoardSensorActive >> 8; - cMsg[iCnt++] = m_iUSBoardSensorActive; - - if(m_iTypeLCD == LCD_20CHAR_TEXT) - { - for(i = 0; i < 20; i++) - { - cMsg[iCnt++] = m_cTextDisplay[i]; - } - - // fill remaining msg with 0's - do - { - cMsg[iCnt++] = 0; - } - while(iCnt < (m_NUM_BYTE_SEND - 2)); - } - else - { - for(i = 0; i < 60; i++) - { - cMsg[iCnt++] = m_cTextDisplay[i]; - } - } - - if(m_iTypeLCD == RELAY_BOARD_1_4) - { - cMsg[iCnt++] = m_cSoftEMStop; - } - // calc checksum - for(i = 0; i < (m_NUM_BYTE_SEND - 2); i++) - { - iChkSum %= 0xFF00; - iChkSum += cMsg[i]; - } - - cMsg[m_NUM_BYTE_SEND - 2] = iChkSum >> 8; - cMsg[m_NUM_BYTE_SEND - 1] = iChkSum; - - // reset flags - m_iCmdRelayBoard &= ~CMD_RESET_POS_CNT; - -} - - -//----------------------------------------------- -/*void SerRelayBoard::convDataToSendMsg(unsigned char cMsg[]) -{ - int i; - int iCnt = 0; - int iChkSum = 0; - - cMsg[iCnt++] = 1;//CMD_RELAISBOARD_GET_DATA; - - cMsg[iCnt++] = m_iConfigRelayBoard >> 8; - cMsg[iCnt++] = m_iConfigRelayBoard; - - cMsg[iCnt++] = m_iCmdRelayBoard >> 8; - cMsg[iCnt++] = m_iCmdRelayBoard; - - // fill remaining msg with 0's - do - { - cMsg[iCnt++] = 0; - } - while(iCnt < (m_NUM_BYTE_SEND - 2)); - - // calc checksum: summation of all bytes in the message - for(i = 0; i < (m_NUM_BYTE_SEND - 2); i++) - { - iChkSum += cMsg[i]; - } - - cMsg[m_NUM_BYTE_SEND - 2] = iChkSum >> 8; - cMsg[m_NUM_BYTE_SEND - 1] = iChkSum; - - // reset flags - m_iCmdRelayBoard &= ~CMD_RESET_POS_CNT; -} -*/ -//----------------------------------------------- -bool SerRelayBoard::convRecMsgToData(unsigned char cMsg[]) -{ - - int iNumByteRec = NUM_BYTE_REC; - if(m_iTypeLCD == LCD_20CHAR_TEXT) - { - iNumByteRec = NUM_BYTE_REC; - } - if(m_iTypeLCD == LCD_60CHAR_TEXT) - { - iNumByteRec = NUM_BYTE_REC; - } - if(m_iTypeLCD == RELAY_BOARD_1_4) - { - iNumByteRec = NUM_BYTE_REC_RELAYBOARD_14; - } - - const int c_iStartCheckSum = iNumByteRec; - - int i; - unsigned int iTxCheckSum; - unsigned int iCheckSum; - - m_Mutex.lock(); - - // test checksum: checksum should be sum of all bytes - iTxCheckSum = (cMsg[c_iStartCheckSum + 1] << 8) | cMsg[c_iStartCheckSum]; - - iCheckSum = 0; - for(i = 0; i < c_iStartCheckSum; i++) - { - iCheckSum %= 0xFF00; - iCheckSum += cMsg[i]; - } - - if(iCheckSum != iTxCheckSum) - { - return false; - } - - // convert data - int iCnt = 0; - - //RelayboardStatus bytes contain EM-Stop and Scanner-Stop bits - m_iRelBoardStatus = (cMsg[iCnt + 1] << 8) | cMsg[iCnt]; - iCnt += 2; - - //unused at the moment - m_iChargeCurrent = (cMsg[iCnt + 1] << 8) | cMsg[iCnt]; - iCnt += 2; - - //unused at the moment - m_iRelBoardBattVoltage = (cMsg[iCnt + 1] << 8) | cMsg[iCnt]; - iCnt += 2; - - //unused at the moment - m_iRelBoardKeyPad = (cMsg[iCnt + 1] << 8) | cMsg[iCnt]; - iCnt += 2; - - //unused at the moment - for(i = 0; i < 4; i++) - { - m_iRelBoardAnalogIn[i] = (cMsg[iCnt + 1] << 8) | cMsg[iCnt]; - iCnt += 2; - } - - //unused at the moment - m_iRelBoardTempSensor = (cMsg[iCnt + 1] << 8) | cMsg[iCnt]; - iCnt += 2; - - //Digital Inputs - //unused at the moment - m_iDigIn = (cMsg[iCnt + 1] << 8) | cMsg[iCnt]; - iCnt += 2; - - //Throw away rest of the message, it was used for earlier purposes - - m_Mutex.unlock(); - return true; -} diff --git a/cob_relayboard/common/src/SerialIO.cpp b/cob_relayboard/common/src/SerialIO.cpp deleted file mode 100644 index 20238f7ab..000000000 --- a/cob_relayboard/common/src/SerialIO.cpp +++ /dev/null @@ -1,417 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -//#include "stdafx.h" -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - - - -//#define _PRINT_BYTES - -/* -#ifdef _DEBUG -#define new DEBUG_NEW -#undef THIS_FILE -static char THIS_FILE[] = __FILE__; -#endif -*/ - - -bool getBaudrateCode(int iBaudrate, int* iBaudrateCode) -{ - // baudrate codes are defined in termios.h - // currently upto B1000000 - const int baudTable[] = { - 0, 50, 75, 110, 134, 150, 200, 300, 600, - 1200, 1800, 2400, 4800, - 9600, 19200, 38400, 57600, 115200, 230400, - 460800, 500000, 576000, 921600, 1000000 - }; - const int baudCodes[] = { - B0, B50, B75, B110, B134, B150, B200, B300, B600, - B1200, B1800, B2400, B4800, - B9600, B19200, B38400, B57600, B115200, B230400, - B460800, B500000, B576000, B921600, B1000000 - }; - const int iBaudsLen = sizeof(baudTable) / sizeof(int); - - bool ret = false; - *iBaudrateCode = B38400; - int i; - for( i=0; i= baudTable[iPos + 1]) - && iPos != 0) - { - if (iBaudrate < baudTable[iPos]) - { - iEnd = iPos; - } - else - { - iStart = iPos; - } - iPos = (iStart + iEnd) / 2; - } - - return baudCodes[iPos]; - */ - return ret; -} - - - - -////////////////////////////////////////////////////////////////////// -// Konstruktion/Destruktion -////////////////////////////////////////////////////////////////////// - -SerialIO::SerialIO() - : m_DeviceName(""), - m_Device(-1), - m_BaudRate(9600), - m_Multiplier(1.0), - m_ByteSize(8), - m_StopBits(SB_ONE), - m_Parity(PA_NONE), - m_Handshake(HS_NONE), - m_ReadBufSize(1024), - m_WriteBufSize(m_ReadBufSize), - m_Timeout(0), - m_ShortBytePeriod(false) -{ - m_BytePeriod.tv_sec = 0; - m_BytePeriod.tv_usec = 0; -} - -SerialIO::~SerialIO() -{ - closeIO(); -} - -int SerialIO::openIO() -{ - int Res; - - // open device - m_Device = open(m_DeviceName.c_str(), O_RDWR | O_NOCTTY | O_NONBLOCK); - - if(m_Device < 0) - { - //RF_ERR("Open " << m_DeviceName << " failed, error code " << errno); - std::cout << "Trying to open " << m_DeviceName << " failed: " - << strerror(errno) << " (Error code " << errno << ")"; - - return -1; - } - - // set parameters - Res = tcgetattr(m_Device, &m_tio); - if (Res == -1) - { - std::cerr << "tcgetattr of " << m_DeviceName << " failed: " - << strerror(errno) << " (Error code " << errno << ")"; - - close(m_Device); - m_Device = -1; - - return -1; - } - - // Default values - m_tio.c_iflag = 0; - m_tio.c_oflag = 0; - m_tio.c_cflag = B9600 | CS8 | CREAD | HUPCL | CLOCAL; - m_tio.c_lflag = 0; - cfsetispeed(&m_tio, B9600); - cfsetospeed(&m_tio, B9600); - - m_tio.c_cc[VINTR] = 3; // Interrupt - m_tio.c_cc[VQUIT] = 28; // Quit - m_tio.c_cc[VERASE] = 127; // Erase - m_tio.c_cc[VKILL] = 21; // Kill-line - m_tio.c_cc[VEOF] = 4; // End-of-file - m_tio.c_cc[VTIME] = 0; // Time to wait for data (tenths of seconds) - m_tio.c_cc[VMIN] = 1; // Minimum number of characters to read - m_tio.c_cc[VSWTC] = 0; - m_tio.c_cc[VSTART] = 17; - m_tio.c_cc[VSTOP] = 19; - m_tio.c_cc[VSUSP] = 26; - m_tio.c_cc[VEOL] = 0; // End-of-line - m_tio.c_cc[VREPRINT] = 18; - m_tio.c_cc[VDISCARD] = 15; - m_tio.c_cc[VWERASE] = 23; - m_tio.c_cc[VLNEXT] = 22; - m_tio.c_cc[VEOL2] = 0; // Second end-of-line - - - // set baud rate - int iNewBaudrate = int(m_BaudRate * m_Multiplier + 0.5); - std::cerr << "Setting Baudrate to " << iNewBaudrate; - - int iBaudrateCode = 0; - bool bBaudrateValid = getBaudrateCode(iNewBaudrate, &iBaudrateCode); - - cfsetispeed(&m_tio, iBaudrateCode); - cfsetospeed(&m_tio, iBaudrateCode); - - if( !bBaudrateValid ) { - std::cerr << "Baudrate code not available - setting baudrate directly"; - struct serial_struct ss; - ioctl( m_Device, TIOCGSERIAL, &ss ); - ss.flags |= ASYNC_SPD_CUST; - ss.custom_divisor = ss.baud_base / iNewBaudrate; - ioctl( m_Device, TIOCSSERIAL, &ss ); - } - - - // set data format - m_tio.c_cflag &= ~CSIZE; - switch (m_ByteSize) - { - case 5: - m_tio.c_cflag |= CS5; - break; - case 6: - m_tio.c_cflag |= CS6; - break; - case 7: - m_tio.c_cflag |= CS7; - break; - case 8: - default: - m_tio.c_cflag |= CS8; - } - - m_tio.c_cflag &= ~ (PARENB | PARODD); - - switch (m_Parity) - { - case PA_ODD: - m_tio.c_cflag |= PARODD; - //break; // break must not be active here as we need the combination of PARODD and PARENB on odd parity. - - case PA_EVEN: - m_tio.c_cflag |= PARENB; - break; - - case PA_NONE: - default: {} - } - - switch (m_StopBits) - { - case SB_TWO: - m_tio.c_cflag |= CSTOPB; - break; - - case SB_ONE: - default: - m_tio.c_cflag &= ~CSTOPB; - } - - // hardware handshake - switch (m_Handshake) - { - case HS_NONE: - m_tio.c_cflag &= ~CRTSCTS; - m_tio.c_iflag &= ~(IXON | IXOFF | IXANY); - break; - case HS_HARDWARE: - m_tio.c_cflag |= CRTSCTS; - m_tio.c_iflag &= ~(IXON | IXOFF | IXANY); - break; - case HS_XONXOFF: - m_tio.c_cflag &= ~CRTSCTS; - m_tio.c_iflag |= (IXON | IXOFF | IXANY); - break; - } - - m_tio.c_oflag &= ~OPOST; - m_tio.c_lflag &= ~ICANON; - - // write parameters - Res = tcsetattr(m_Device, TCSANOW, &m_tio); - - if (Res == -1) - { - std::cerr << "tcsetattr " << m_DeviceName << " failed: " - << strerror(errno) << " (Error code " << errno << ")"; - - close(m_Device); - m_Device = -1; - - return -1; - } - - // set buffer sizes -// SetupComm(m_Device, m_ReadBufSize, m_WriteBufSize); - - // set timeout - setTimeout(m_Timeout); - - return 0; -} - -void SerialIO::closeIO() -{ - if (m_Device != -1) - { - close(m_Device); - m_Device = -1; - } -} - -void SerialIO::setTimeout(double Timeout) -{ - m_Timeout = Timeout; - if (m_Device != -1) - { - m_tio.c_cc[VTIME] = cc_t(ceil(m_Timeout * 10.0)); - tcsetattr(m_Device, TCSANOW, &m_tio); - } - -} - -void SerialIO::setBytePeriod(double Period) -{ - m_ShortBytePeriod = false; - m_BytePeriod.tv_sec = time_t(Period); - m_BytePeriod.tv_usec = suseconds_t((Period - m_BytePeriod.tv_sec) * 1000); -} - -//----------------------------------------------- -void SerialIO::changeBaudRate(int iBaudRate) -{ - /* - int iRetVal; - - m_BaudRate = iBaudRate; - - int iNewBaudrate = int(m_BaudRate * m_Multiplier + 0.5); - int iBaudrateCode = getBaudrateCode(iNewBaudrate); - cfsetispeed(&m_tio, iBaudrateCode); - cfsetospeed(&m_tio, iBaudrateCode); - - iRetVal = tcsetattr(m_Device, TCSANOW, &m_tio); - if(iRetVal == -1) - { - LOG_OUT("error in SerialCom::changeBaudRate()"); - char c; - std::cin >> c; - exit(0); - }*/ -} - - -int SerialIO::readBlocking(char *Buffer, int Length) -{ - ssize_t BytesRead; - BytesRead = read(m_Device, Buffer, Length); -#ifdef PRINT_BYTES - printf("%2d Bytes read:", BytesRead); - for(int i=0; i -#include - -std::string StringToUpper(std::string strToConvert) { - for(unsigned int i=0;i 16) { - *result = 0; return result; - } - - char* out = result; - int quotient = value; - - do { - *out = "0123456789abcdef"[ std::abs( quotient % base ) ]; - ++out; - quotient /= base; - } while ( quotient ); - - // Only apply negative sign for base 10 - if ( value < 0 && base == 10) *out++ = '-'; - - std::reverse( result, out ); - *out = 0; - return result; -} - -/** - * C++ version std::string style "itoa": - */ -std::string itoa(int value, int base) -{ - enum { kMaxDigits = 35 }; - std::string buf; - - buf.reserve( kMaxDigits ); // Pre-allocate enough space. - - // check that the base if valid - if (base < 2 || base > 16) return buf; - - int quotient = value; - - // Translating number to string with base: - do { - buf += "0123456789abcdef"[ std::abs( quotient % base ) ]; - quotient /= base; - } while ( quotient ); - - // Append the negative sign for base 10 - if ( value < 0 && base == 10) buf += '-'; - std::reverse( buf.begin(), buf.end() ); - return buf; -} - diff --git a/cob_relayboard/package.xml b/cob_relayboard/package.xml deleted file mode 100644 index f1fa389a8..000000000 --- a/cob_relayboard/package.xml +++ /dev/null @@ -1,21 +0,0 @@ - - cob_relayboard - 0.7.16 - cob_relayboard - Apache 2.0 - - http://ros.org/wiki/cob_relayboard - - - Christian Connette - Matthias Gruhler - - catkin - - cob_msgs - roscpp - std_msgs - - rospy - - diff --git a/cob_relayboard/ros/src/analysis.R b/cob_relayboard/ros/src/analysis.R deleted file mode 100644 index f4e204646..000000000 --- a/cob_relayboard/ros/src/analysis.R +++ /dev/null @@ -1,35 +0,0 @@ -library(e1071) - -filename <- 'desire_LIB_01.csv' -A <- read.csv(filename) -A <- A[ -nrow(A), ] -colnames(A) <- c('time','voltage') -# A[,1] <- A[,1] / 60.0 -A[,2] <- A[,2] / 1000.0 - -A$group = cut(A$time, breaks = seq(0, max(A$time), by=10)) - -A.split <- split(A, A$group) -A.split <- lapply(A.split, function(X) { - if (nrow(X)==0) data.frame(time=c(), median.voltage=c(), mean.voltage=c()) - else data.frame(time=mean(X$time), median.voltage=median(X$voltage), mean.voltage=mean(X$voltage)) - }) -B <- do.call(rbind, A.split) - -m <- svm(median.voltage ~ time, data = B) - -B.predict <- data.frame( time = seq(min(B$time), max(B$time), length=1000) ) -B.predict$voltage <- predict(m, B.predict) - -B.predict$time <- B.predict$time / 60 -B$time <- B$time / 60 -A$time <- A$time / 60 - -y.limits <- c(quantile(A[,2], 0.001) - 0.1, quantile(A[,2], 0.999) + 0.1) -pdf(paste(filename, '.pdf',sep=''), width=12, height=7) -plot(A[,1], A[,2], type='p', xlab='Time [min]', ylab='Voltage [V]', main=paste("Battery discharge (", filename, ")", sep=""), ylim=y.limits) -points(B$time, B$median.voltage, col='orange') -points(B$time, B$mean.voltage, col='green') -lines(B.predict$time, B.predict$voltage, col='red') -grid(col='grey30') -dev.off() diff --git a/cob_relayboard/ros/src/cob_relayboard_node.cpp b/cob_relayboard/ros/src/cob_relayboard_node.cpp deleted file mode 100644 index 6683837fd..000000000 --- a/cob_relayboard/ros/src/cob_relayboard_node.cpp +++ /dev/null @@ -1,266 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -//################## -//#### includes #### - -// standard includes -//-- - -// ROS includes -#include -#include - -// ROS message includes -#include -#include -#include - -// ROS service includes -//-- - -// external includes -//-- - -//#################### -//#### node class #### -class NodeClass -{ - // -public: - // create a handle for this node, initialize node - ros::NodeHandle n; - ros::NodeHandle n_priv; - - // topics to publish - ros::Publisher topicPub_isEmergencyStop; - ros::Publisher topicPub_Voltage; - // topics to subscribe, callback is called for new messages arriving - // -- - - // Constructor - NodeClass() - { - n = ros::NodeHandle(); - n_priv = ros::NodeHandle("~"); - - topicPub_isEmergencyStop = n.advertise("emergency_stop_state", 1); - topicPub_Voltage = n.advertise("voltage", 1); - - // Make sure member variables have a defined state at the beginning - EM_stop_status_ = ST_EM_FREE; - relayboard_available = false; - relayboard_online = false; - relayboard_timeout_ = 2.0; - protocol_version_ = 1; - duration_for_EM_free_ = ros::Duration(1); - } - - // Destructor - ~NodeClass() - { - delete m_SerRelayBoard; - } - - void sendEmergencyStopStates(); - void sendBatteryVoltage(); - int init(); - -private: - std::string sComPort; - SerRelayBoard * m_SerRelayBoard; - - int EM_stop_status_; - ros::Duration duration_for_EM_free_; - ros::Time time_of_EM_confirmed_; - double relayboard_timeout_; - int protocol_version_; - - ros::Time time_last_message_received_; - bool relayboard_online; //the relayboard is sending messages at regular time - bool relayboard_available; //the relayboard has sent at least one message -> publish topic - - // possible states of emergency stop - enum - { - ST_EM_FREE = 0, - ST_EM_ACTIVE = 1, - ST_EM_CONFIRMED = 2 - }; - - int requestBoardStatus(); -}; - -//####################### -//#### main programm #### -int main(int argc, char** argv) -{ - // initialize ROS, spezify name of node - ros::init(argc, argv, "cob_relayboard_node"); - - NodeClass node; - if(node.init() != 0) return 1; - - ros::Rate r(20); //Cycle-Rate: Frequency of publishing EMStopStates - while(node.n.ok()) - { - node.sendEmergencyStopStates(); - - ros::spinOnce(); - r.sleep(); - } - - return 0; -} - -//################################## -//#### function implementations #### - -int NodeClass::init() -{ - if (n_priv.hasParam("ComPort")) - { - n_priv.getParam("ComPort", sComPort); - ROS_INFO("Loaded ComPort parameter from parameter server: %s",sComPort.c_str()); - } - else - { - sComPort ="/dev/ttyUSB0"; - ROS_WARN("ComPort Parameter not available, using default Port: %s",sComPort.c_str()); - } - - n_priv.param("relayboard_timeout", relayboard_timeout_, 2.0); - n_priv.param("protocol_version", protocol_version_, 1); - - m_SerRelayBoard = new SerRelayBoard(sComPort, protocol_version_); - ROS_INFO("Opened Relayboard at ComPort = %s", sComPort.c_str()); - - m_SerRelayBoard->init(); - - // Init member variable for EM State - EM_stop_status_ = ST_EM_ACTIVE; - duration_for_EM_free_ = ros::Duration(1); - - return 0; -} - -int NodeClass::requestBoardStatus() { - int ret; - - // Request Status of RelayBoard - ret = m_SerRelayBoard->sendRequest(); - if(ret != SerRelayBoard::NO_ERROR) { - ROS_ERROR("Error in sending message to Relayboard over SerialIO, lost bytes during writing"); - } - - ret = m_SerRelayBoard->evalRxBuffer(); - if(ret==SerRelayBoard::NOT_INITIALIZED) { - ROS_ERROR("Failed to read relayboard data over Serial, the device is not initialized"); - relayboard_online = false; - } else if(ret==SerRelayBoard::NO_MESSAGES) { - ROS_ERROR("For a long time, no messages from RelayBoard have been received, check com port!"); - if(time_last_message_received_.toSec() - ros::Time::now().toSec() > relayboard_timeout_) {relayboard_online = false;} - } else if(ret==SerRelayBoard::TOO_LESS_BYTES_IN_QUEUE) { - //ROS_ERROR("Relayboard: Too less bytes in queue"); - } else if(ret==SerRelayBoard::CHECKSUM_ERROR) { - ROS_ERROR("A checksum error occurred while reading from relayboard data"); - } else if(ret==SerRelayBoard::NO_ERROR) { - relayboard_online = true; - relayboard_available = true; - time_last_message_received_ = ros::Time::now(); - } - - return 0; -} - -void NodeClass::sendBatteryVoltage() -{ - std_msgs::Float64 voltage; - voltage.data = m_SerRelayBoard->getBatteryVoltage()/1000.0; //normalize from mV to V - topicPub_Voltage.publish(voltage); -} - -void NodeClass::sendEmergencyStopStates() -{ - requestBoardStatus(); - - if(!relayboard_available) return; - - sendBatteryVoltage(); - - - bool EM_signal; - ros::Duration duration_since_EM_confirmed; - cob_msgs::EmergencyStopState EM_msg; - - // assign input (laser, button) specific EM state TODO: Laser and Scanner stop can't be read independently (e.g. if button is stop --> no informtion about scanner, if scanner ist stop --> no informtion about button stop) - EM_msg.emergency_button_stop = m_SerRelayBoard->isEMStop(); - EM_msg.scanner_stop = m_SerRelayBoard->isScannerStop(); - - // determine current EMStopState - EM_signal = (EM_msg.emergency_button_stop || EM_msg.scanner_stop); - - switch (EM_stop_status_) - { - case ST_EM_FREE: - { - if (EM_signal == true) - { - ROS_INFO("Emergency stop was issued"); - EM_stop_status_ = EM_msg.EMSTOP; - } - break; - } - case ST_EM_ACTIVE: - { - if (EM_signal == false) - { - ROS_INFO("Emergency stop was confirmed"); - EM_stop_status_ = EM_msg.EMCONFIRMED; - time_of_EM_confirmed_ = ros::Time::now(); - } - break; - } - case ST_EM_CONFIRMED: - { - if (EM_signal == true) - { - ROS_INFO("Emergency stop was issued"); - EM_stop_status_ = EM_msg.EMSTOP; - } - else - { - duration_since_EM_confirmed = ros::Time::now() - time_of_EM_confirmed_; - if( duration_since_EM_confirmed.toSec() > duration_for_EM_free_.toSec() ) - { - ROS_INFO("Emergency stop released"); - EM_stop_status_ = EM_msg.EMFREE; - } - } - break; - } - }; - - - EM_msg.emergency_state = EM_stop_status_; - - //publish EM-Stop-Active-messages, when connection to relayboard got cut - if(relayboard_online == false) { - EM_msg.emergency_state = EM_msg.EMSTOP; - } - topicPub_isEmergencyStop.publish(EM_msg); -} diff --git a/cob_relayboard/ros/src/relayboard_sim.py b/cob_relayboard/ros/src/relayboard_sim.py deleted file mode 100755 index ffbd722fd..000000000 --- a/cob_relayboard/ros/src/relayboard_sim.py +++ /dev/null @@ -1,47 +0,0 @@ -#!/usr/bin/env python -# -# Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - - -import rospy -from std_msgs.msg import Float64 -from cob_msgs.msg import EmergencyStopState - -def relayboard_sim(): - rospy.init_node('cob_relayboard_sim') - - # emergency_stop topic - pub_em_stop = rospy.Publisher('emergency_stop_state', EmergencyStopState, queue_size=1) - msg_em = EmergencyStopState() - msg_em.emergency_button_stop = False - msg_em.scanner_stop = False - msg_em.emergency_state = 0 - - # voltage topic - pub_voltage = rospy.Publisher('voltage', Float64, queue_size=1) - msg_voltage = Float64() - msg_voltage.data = 48.0 # in simulation battery is always full - - while not rospy.is_shutdown(): - pub_em_stop.publish(msg_em) - pub_voltage.publish(msg_voltage) - rospy.sleep(1.0) - -if __name__ == '__main__': - try: - relayboard_sim() - except rospy.ROSInterruptException: - print("Interupted") - pass From cf94b7e2bacdc597b50f371776c3311717380d40 Mon Sep 17 00:00:00 2001 From: fmessmer Date: Wed, 20 Mar 2024 17:00:54 +0100 Subject: [PATCH 12/14] add laser_scan_densifier to cob_driver meta-package --- cob_driver/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/cob_driver/package.xml b/cob_driver/package.xml index d359c7b9e..69ccdd8d5 100644 --- a/cob_driver/package.xml +++ b/cob_driver/package.xml @@ -20,6 +20,7 @@ cob_scan_unifier cob_sick_s300 cob_sound + laser_scan_densifier From 85e2b58b4cd2a54efaca9aeddbc0d7da7c65701c Mon Sep 17 00:00:00 2001 From: fmessmer Date: Thu, 11 Apr 2024 12:03:30 +0200 Subject: [PATCH 13/14] remove cob_bms_driver --- cob_bms_driver/CHANGELOG.rst | 266 ---------- cob_bms_driver/CMakeLists.txt | 41 -- .../cob_bms_driver/cob_bms_driver_node.h | 122 ----- cob_bms_driver/package.xml | 35 -- cob_bms_driver/src/cob_bms_driver_node.cpp | 473 ------------------ cob_bms_driver/src/fake_bms.py | 91 ---- cob_bms_driver/src/power_state_aggregator.py | 177 ------- cob_driver/package.xml | 1 - 8 files changed, 1206 deletions(-) delete mode 100644 cob_bms_driver/CHANGELOG.rst delete mode 100644 cob_bms_driver/CMakeLists.txt delete mode 100644 cob_bms_driver/include/cob_bms_driver/cob_bms_driver_node.h delete mode 100644 cob_bms_driver/package.xml delete mode 100644 cob_bms_driver/src/cob_bms_driver_node.cpp delete mode 100755 cob_bms_driver/src/fake_bms.py delete mode 100755 cob_bms_driver/src/power_state_aggregator.py diff --git a/cob_bms_driver/CHANGELOG.rst b/cob_bms_driver/CHANGELOG.rst deleted file mode 100644 index 04c8bde52..000000000 --- a/cob_bms_driver/CHANGELOG.rst +++ /dev/null @@ -1,266 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package cob_bms_driver -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.7.16 (2024-02-20) -------------------- - -0.7.15 (2023-11-06) -------------------- - -0.7.14 (2022-11-17) -------------------- -* Merge pull request `#437 `_ from Deleh/fix/power_consume - Fix power consumption of fake BMS -* round remaining capacity for publisher and diagnostics -* remove round of remaining capacity -* Merge pull request `#436 `_ from HannesBachter/audi_scenario - configurable battery details -* get battery details from parameter server -* adjust battery capacity and discharging values to new diff base -* Contributors: Denis Lehmann, Felix Messmer, HannesBachter, fmessmer - -0.7.13 (2022-07-29) -------------------- -* Merge pull request `#434 `_ from floweisshardt/feature/power_state_connected - add explicit power_state.connected -* remove unused subscriber -* add explicit power_state.connected -* Merge pull request `#432 `_ from fmessmer/feature/publish_battery_state - power_state_aggregator now also publishes sensor_msgs.BatteryState -* fix percentage value range - Co-authored-by: Benjamin Maidel -* update current_buffer_size -* power_state_aggregator now also publishes sensor_msgs.BatteryState -* Contributors: Felix Messmer, floweisshardt, fmessmer - -0.7.12 (2022-03-15) -------------------- - -0.7.11 (2022-01-12) -------------------- -* Merge pull request `#429 `_ from fmessmer/fix/invalid_value_warning - catch invalid value warning -* use odd number for buffer_size to prevent 0.0 mean -* catch invalid value warning -* Contributors: Felix Messmer, fmessmer - -0.7.10 (2021-12-23) -------------------- - -0.7.9 (2021-11-26) ------------------- -* Merge pull request `#427 `_ from benmaidel/fix/fake_bms_service - [BMS] fix fakebms set_relative_remaining_capacity service -* fix fakebms set_relative_remaining_capacity service -* Contributors: Benjamin Maidel, Felix Messmer - -0.7.8 (2021-10-19) ------------------- - -0.7.7 (2021-08-02) ------------------- - -0.7.6 (2021-05-10) ------------------- -* Merge pull request `#423 `_ from mikaelarguedas/python2-deps - ROS_PYTHON_VERSION conditional dependency for `python-tk` and `python-numpy` -* convter cob_bms_driver to package format 3 -* ROS_PYTHON_VERSION conditional dependency for python-numpy -* Contributors: Felix Messmer, Mikael Arguedas - -0.7.5 (2021-04-06) ------------------- -* Merge pull request `#418 `_ from fmessmer/fix_catkin_lint - fix catkin_lint -* fix catkin_lint -* Contributors: Felix Messmer, fmessmer - -0.7.4 (2020-10-14) ------------------- -* Merge pull request `#417 `_ from fmessmer/test_noetic - test noetic -* Bump CMake version to avoid CMP0048 warning -* Contributors: Felix Messmer, fmessmer - -0.7.3 (2020-03-18) ------------------- - -0.7.2 (2020-03-18) ------------------- -* Merge pull request `#408 `_ from fmessmer/ci_updates - [travis] ci updates -* catkin_lint fixes -* Contributors: Felix Messmer, fmessmer - -0.7.1 (2019-11-07) ------------------- - -0.7.0 (2019-08-06) ------------------- -* Merge pull request `#380 `_ from ipa-jba/fix/boost_shared_ptr - [Melodic] combined melodify pr -* melodify cob_bms_driver_node -* Merge pull request `#396 `_ from HannesBachter/indigo_dev - 0.6.15 -* Contributors: Felix Messmer, Jannik Abbenseth - -0.6.15 (2019-07-17) ------------ -* 0.6.14 -* update changelogs -* Merge pull request `#393 `_ from fmessmer/add_int_bms_parameter - add int bms parameter types -* get rid of c++11 compile options -* fix key name -* add int bms parameter types -* Merge pull request `#392 `_ from fmessmer/max_time_remaining - clamp time_remaining when current is zero -* clamp time_remaining when current is zero -* Contributors: Felix Messmer, fmessmer - -0.6.14 (2019-06-07) -------------------- -* Merge pull request `#393 `_ from fmessmer/add_int_bms_parameter - add int bms parameter types -* get rid of c++11 compile options -* fix key name -* add int bms parameter types -* Merge pull request `#392 `_ from fmessmer/max_time_remaining - clamp time_remaining when current is zero -* clamp time_remaining when current is zero -* Contributors: Felix Messmer, fmessmer - -0.6.13 (2019-03-14) -------------------- -* Merge pull request `#381 `_ from pholthau/boost-format - include boost/format.hpp -* include boost/format.hpp -* Contributors: Felix Messmer, Patrick Holthaus - -0.6.12 (2018-07-21) -------------------- -* update maintainer -* Merge pull request `#374 `_ from floweisshardt/feature/round_remaining_capacity - round remaining_capacity -* adjust to real driver precision -* round remaining_capacity -* Contributors: Felix Messmer, fmessmer, ipa-fmw, ipa-fxm - -0.6.11 (2018-01-07) -------------------- -* Merge remote-tracking branch 'origin/indigo_release_candidate' into indigo_dev -* Merge pull request `#364 `_ from ipa-fxm/fake_bms_diagnostics - use diagnostic updater in fake_bms -* use diagnostic updater in fake_bms -* Merge pull request `#361 `_ from ipa-fxm/set_relative_remaining_capacity - set relative remaining capacity -* set relative remaining capacity -* Merge pull request `#341 `_ from ipa-fxm/APACHE_license - use license apache 2.0 -* change maintainer -* use license apache 2.0 -* Contributors: Felix Messmer, Florian Weisshardt, ipa-fxm, ipa-uhr-mk - -0.6.10 (2017-07-24) -------------------- - -0.6.9 (2017-07-18) ------------------- -* minor change for handling exception -* made changes which only sets the current that in turn is used by power_aggregator for relative_remaining_capacity calculation -* fix typo -* added emulation of realistic current value -* minor change for publishing a realistic voltage value -* Merge pull request `#310 `_ from souravran/feature/fake_bms - added a fake bms with set_charging and set_relative_remaining_capacity services -* finalize exception handling -* fake current -* consistent naming -* publish diagnostics in fake_bms -* harmonize namespaces of fake_bms -* uses the default parameter value -* poll frequency has been set from the parameter list -* made changes as per the review. - power state elements being published at 20 Hz. - removed junk rospy log and changed division_by_zero error message. -* fake_bms publishing all power_state entities. - added exception handling in power_state_aggregator. - added package dependency and install tags. -* added a fake bms with set_charging and set_relative_remaining_capacity services -* fix typo -* fix powerstate aggregator charging flag (bms is not delivering correct flag for full battery and docked) -* use bms flag for harging -* fix identation -* use spaces for indention in BMS driver -* updated authors -* added support for bit_mask'ed booleans -* make BmsParameter an abstract base class -* BMS driver clean-up -* switch from map of vectors to multimap in BMS driver -* simplified BMS publisher creation and polling list optimization -* simplified BMS config parsing -* manually fix changelog -* Contributors: Felix Messmer, Florian Weisshardt, Mathias Lüdtke, Nadia Hammoudeh García, fmw-ss, ipa-fxm, robot - -0.6.8 (2016-10-10) ------------------- -* restart CAN on failure -* move power_state_phidget node to new package -* invert current + round values -* fix typo -* corrected namespace -* added node to calculate powerstate from phidget board -* Contributors: Benjamin Maidel, Mathias Lüdtke - -0.6.7 (2016-04-02) ------------------- -* add missing dependencies -* Contributors: ipa-fxm - -0.6.6 (2016-04-01) ------------------- -* dependency and package cleanup -* remove config and launch as it is added to cob_robots -* adjust version -* move cob_bms_driver to cob_driver -* Contributors: ipa-fxm - -0.6.5 (2015-08-31) ------------------- - -0.6.4 (2015-08-25) ------------------- - -0.6.3 (2015-06-17) ------------------- - -0.6.2 (2014-12-15) ------------------- - -0.6.1 (2014-09-17) ------------------- - -0.6.0 (2014-09-09) ------------------- - -0.5.7 (2014-08-26 09:47) ------------------------- - -0.5.6 (2014-08-26 09:42) ------------------------- - -0.5.5 (2014-08-26 08:33) ------------------------- - -0.5.4 (2014-08-25) ------------------- - -0.5.3 (2014-03-31) ------------------- - -0.5.2 (2014-03-21) ------------------- - -0.5.1 (2014-03-20 10:54) ------------------------- diff --git a/cob_bms_driver/CMakeLists.txt b/cob_bms_driver/CMakeLists.txt deleted file mode 100644 index 17529ac95..000000000 --- a/cob_bms_driver/CMakeLists.txt +++ /dev/null @@ -1,41 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(cob_bms_driver) - -find_package(catkin REQUIRED COMPONENTS - diagnostic_msgs - diagnostic_updater - roscpp - socketcan_interface - std_msgs -) - -catkin_package( - CATKIN_DEPENDS diagnostic_msgs roscpp std_msgs -) - -########### -## Build ## -########### -include_directories( - include - ${catkin_INCLUDE_DIRS} -) - -add_executable(bms_driver_node src/${PROJECT_NAME}_node.cpp) -add_dependencies(bms_driver_node ${catkin_EXPORTED_TARGETS}) -target_link_libraries(bms_driver_node ${catkin_LIBRARIES}) - -############# -## Install ## -############# -install(TARGETS bms_driver_node - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -catkin_install_python(PROGRAMS - src/fake_bms.py - src/power_state_aggregator.py - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) diff --git a/cob_bms_driver/include/cob_bms_driver/cob_bms_driver_node.h b/cob_bms_driver/include/cob_bms_driver/cob_bms_driver_node.h deleted file mode 100644 index 02f141029..000000000 --- a/cob_bms_driver/include/cob_bms_driver/cob_bms_driver_node.h +++ /dev/null @@ -1,122 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -#ifndef COB_BMS_DRIVER_NODE_H -#define COB_BMS_DRIVER_NODE_H - -#include -#include -#include -#include - -#include -#include - -struct BmsParameter -{ - unsigned int offset; - unsigned int length; - - bool is_signed; - - ros::Publisher publisher; - - diagnostic_msgs::KeyValue kv; - - virtual ~BmsParameter() {} - - virtual void update(const can::Frame &f) = 0; - virtual void advertise(ros::NodeHandle &nh, const std::string &topic) = 0; - - typedef boost::shared_ptr Ptr; -}; - -class CobBmsDriverNode -{ -private: - ros::NodeHandle nh_; - ros::NodeHandle nh_priv_; - - typedef std::multimap ConfigMap; - - //ROS parameters - ConfigMap config_map_; //holds all the information that is provided in the configuration file - int poll_period_for_two_ids_in_ms_; - std::string can_device_; - int bms_id_to_poll_; - ros::Timer updater_timer_; - - boost::mutex data_mutex_; - - //polling lists that contain CAN-ID(s) that are to be polled. Each CAN-ID corresponds to a group of BMS parameters - std::vector polling_list1_; - std::vector polling_list2_; - std::vector::iterator polling_list1_it_; - std::vector::iterator polling_list2_it_; - - //interface to send and recieve CAN frames - can::ThreadedSocketCANInterface socketcan_interface_; - - //pointer to callback function to handle CAN frames from BMS - can::CommInterface::FrameListenerConstSharedPtr frame_listener_; - - //diagnostics data received from BMS - diagnostic_updater::DiagnosticStatusWrapper stat_; - - //function to get ROS parameters from parameter server - bool getParams(); - - //function to interpret the diagnostics XmlRpcValue and save data in config_map_ - bool loadConfigMap(XmlRpc::XmlRpcValue &diagnostics, std::vector &topics); - - //helper function to evaluate poll period from given poll frequency - void evaluatePollPeriodFrom(int poll_frequency); - - //function that goes through config_map_ and fills polling_list1_ and polling_list2_ with CAN-IDs. - //If Topics are found on ROS Parameter Server, they are kept in a separate list (to be polled faster). - //Otherwise, all CAN-ID are divided between both lists. - void optimizePollingLists(); - - //function that polls BMS (bms_id_to_poll_ is used here!). - //Also, this function sleeps for time given by poll_period_for_two_ids_in_ms_ to ensure BMS is polled at the desired frequency - void pollBmsForIds(const uint16_t first_id, const uint16_t second_id); - - //callback function to handle all types of frames received from BMS - void handleFrames(const can::Frame &f); - - //updates the diagnostics data with the new data received from BMS - void produceDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat); - - //calls update function of diagnostics_updater - void diagnosticsTimerCallback(const ros::TimerEvent&); -public: - - //updater for diagnostics data - diagnostic_updater::Updater updater_; - - CobBmsDriverNode(); - ~CobBmsDriverNode(); - - //initlializes SocketCAN interface, saves data from ROS parameter server, loads polling lists and sets up diagnostic updater - bool prepare(); - - //cycles through polling lists and sends 2 ids at a time (one from each list) to the BMS - void pollNextInLists(); -}; - - -#endif //COB_BMS_DRIVER_NODE_H diff --git a/cob_bms_driver/package.xml b/cob_bms_driver/package.xml deleted file mode 100644 index 20cbf365a..000000000 --- a/cob_bms_driver/package.xml +++ /dev/null @@ -1,35 +0,0 @@ - - - - cob_bms_driver - 0.7.16 - - Driver package for interfacing the battery management system (BMS) on Care-O-bot. - - - Felix Messmer - Florian Weisshardt - - Apache 2.0 - - mig-mc - Mathias Lüdtke - - catkin - - diagnostic_msgs - diagnostic_updater - roscpp - socketcan_interface - std_msgs - - cob_msgs - cob_srvs - python-numpy - python3-numpy - rospy - sensor_msgs - - diff --git a/cob_bms_driver/src/cob_bms_driver_node.cpp b/cob_bms_driver/src/cob_bms_driver_node.cpp deleted file mode 100644 index e384dda7a..000000000 --- a/cob_bms_driver/src/cob_bms_driver_node.cpp +++ /dev/null @@ -1,473 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -#include -#include -#include -#include -#include - -#include -#include -#include - -#include - -using boost::make_shared; - -//template to be used to read CAN frames received from the BMS -template void big_endian_to_host(const void* in, void* out); -template<> void big_endian_to_host<1>(const void* in, void* out){ *(uint8_t*)out = *(uint8_t*)in;} -template<> void big_endian_to_host<2>(const void* in, void* out){ *(uint16_t*)out = be16toh(*(uint16_t*)in);} -template<> void big_endian_to_host<4>(const void* in, void* out){ *(uint32_t*)out = be32toh(*(uint32_t*)in);} -template<> void big_endian_to_host<8>(const void* in, void* out){ *(uint64_t*)out = be64toh(*(uint64_t*)in);} - -template T read_value(const can::Frame &f, uint8_t offset){ - T res; - big_endian_to_host(&f.data[offset], &res); - return res; -} -template bool readTypedValue(const can::Frame &f, const BmsParameter ¶m, T &data){ - switch (param.length) - { - case 1: - data = param.is_signed ? read_value (f, param.offset) : read_value (f, param.offset); - break; - - case 2: - data = param.is_signed ? read_value (f, param.offset) : read_value (f, param.offset); - break; - - case 4: - data = param.is_signed ? read_value (f, param.offset) : read_value (f, param.offset); - break; - - default: - ROS_WARN_STREAM("Unknown length of BmsParameter: " << param.kv.key << ". Cannot read data!"); - return false; - } - return true; -} - -template struct TypedBmsParameter : BmsParameter { - T msg_; - - void publish(){ - //if the BmsParameter is a topic, publish data to the topic - if (static_cast(publisher)) - { - publisher.publish(msg_); - } - } - virtual void advertise(ros::NodeHandle &nh, const std::string &topic){ - publisher = nh.advertise (topic, 1, true); - } -}; - -struct FloatBmsParameter : TypedBmsParameter { - double factor; - FloatBmsParameter(double factor) : factor(factor) {} - void update(const can::Frame &f){ - readTypedValue(f, *this, msg_.data); - msg_.data *= factor; - - //save data for diagnostics updater (and round to two digits for readability) - std::stringstream sstream; - sstream << std::setprecision(2) << msg_.data; - kv.value = sstream.str(); - publish(); - } -}; - -struct IntBmsParameter : TypedBmsParameter { - IntBmsParameter() {} - void update(const can::Frame &f){ - readTypedValue(f, *this, msg_.data); - - kv.value = (boost::format("%lld") % msg_.data).str(); - publish(); - } -}; - -struct UIntBmsParameter : TypedBmsParameter { - UIntBmsParameter() {} - void update(const can::Frame &f){ - readTypedValue(f, *this, msg_.data); - - kv.value = (boost::format("%llu") % msg_.data).str(); - publish(); - } -}; - -struct BooleanBmsParameter : TypedBmsParameter { - int bit_mask; - BooleanBmsParameter(int bit_mask) : bit_mask(bit_mask) { is_signed = true; } - void update(const can::Frame &f){ - int value; - readTypedValue(f, *this, value); - msg_.data = (value & bit_mask) == bit_mask; - - kv.value = msg_.data ? "True" : "False"; - publish(); - } -}; - -CobBmsDriverNode::CobBmsDriverNode() -: nh_priv_("~") -{} - -CobBmsDriverNode::~CobBmsDriverNode() -{ - socketcan_interface_.shutdown(); -} - -//initlializes SocketCAN interface, saves data from ROS parameter server, loads polling lists and sets up diagnostic updater -bool CobBmsDriverNode::prepare() -{ - //reads parameters from ROS parameter server and saves them in respective member variable: config_map_, poll_period_for_two_ids_in_ms_, can_device_, bms_id_to_poll_ - if (!getParams()) - { - ROS_ERROR("Could not prepare driver for start up"); - return false; - } - - optimizePollingLists(); - - //initalize polling lists iterators - polling_list1_it_ = polling_list1_.begin(); - polling_list2_it_ = polling_list2_.begin(); - - updater_.setHardwareID("bms"); - updater_.add("cob_bms_dagnostics_updater", this, &CobBmsDriverNode::produceDiagnostics); - - updater_timer_ = nh_.createTimer(ros::Duration(updater_.getPeriod()), &CobBmsDriverNode::diagnosticsTimerCallback, this); - - //initialize the socketcan interface - if(!socketcan_interface_.init(can_device_, false)) { - ROS_ERROR("cob_bms_driver initialization failed"); - return false; - } - - //create listener for CAN frames - frame_listener_ = socketcan_interface_.createMsgListener(can::CommInterface::FrameDelegate(this, &CobBmsDriverNode::handleFrames)); - - return true; -} - -//function to get ROS parameters from parameter server -bool CobBmsDriverNode::getParams() -{ - //local declarations - XmlRpc::XmlRpcValue diagnostics; - std::vector topics; - int poll_frequency_hz; - - if (!nh_priv_.getParam("topics", topics)) - { - ROS_INFO_STREAM("Did not find \"topics\" on parameter server"); - } - if (topics.empty()) - { - ROS_INFO("Topic list is empty. No publisher created"); - } - - if (!nh_priv_.getParam("diagnostics", diagnostics)) - { - ROS_ERROR_STREAM("Did not find \"diagnostics\" on parameter server"); - return false; - } - try { - if(!loadConfigMap(diagnostics, topics)) return false; - } - catch(XmlRpc::XmlRpcException &e){ - ROS_ERROR_STREAM("Could not parse 'diagnostics': "<< e.getMessage()); - return false; - } - - if (!topics.empty()) - { - for(std::vector::iterator it = topics.begin(); it != topics.end(); ++it){ - ROS_ERROR_STREAM("Could not find entry for topic '" << *it << "'."); - } - return false; - } - - if (!nh_priv_.getParam("can_device", can_device_)) - { - ROS_INFO_STREAM("Did not find \"can_device\" on parameter server. Using default value: can0"); - can_device_ = "can0"; - } - - if (!nh_priv_.getParam("bms_id_to_poll", bms_id_to_poll_)) - { - ROS_INFO_STREAM("Did not find \"bms_id_to_poll\" on parameter server. Using default value: 0x200"); - bms_id_to_poll_ = 0x200; - } - - if (!nh_priv_.getParam("poll_frequency_hz", poll_frequency_hz)) - { - ROS_INFO_STREAM("Did not find \"poll_frequency\" on parameter server. Using default value: 20 Hz"); - poll_frequency_hz = 20; - } - evaluatePollPeriodFrom(poll_frequency_hz); - return true; -} - -//function to interpret the diagnostics XmlRpcValue and save data in config_map_ -bool CobBmsDriverNode::loadConfigMap(XmlRpc::XmlRpcValue &diagnostics, std::vector &topics) -{ - //for each id in list of ids - for (size_t i = 0; i < diagnostics.size(); ++i) - { - uint8_t id; - XmlRpc::XmlRpcValue config = diagnostics[i]; - - if(!config.hasMember("id")) { - ROS_ERROR_STREAM("diagnostics[" << i << "]: id is missing."); - return false; - } - if(!config.hasMember("fields")) { - ROS_ERROR_STREAM("diagnostics[" << i << "]: fields is missing."); - return false; - } - id = static_cast(static_cast(config["id"])); - - XmlRpc::XmlRpcValue fields = config["fields"]; - bool publishes = false; - - for(int32_t j=0; j(field["name"]); - - if(!field.hasMember("len")){ - ROS_ERROR_STREAM("diagnostics[" << i << "]: fields[" << j << "]: len is missing."); - return false; - } - int len = static_cast(field["len"]); - - BmsParameter::Ptr entry; - if(field.hasMember("bit_mask")){ - int bit_mask = static_cast(field["bit_mask"]); - if(bit_mask & ~((1<<(len*8))-1)){ - ROS_ERROR_STREAM("diagnostics[" << i << "]: fields[" << j << "]: bit_mask does fit not into type of length " << len); - return false; - } - entry = make_shared(bit_mask); - entry->kv.key = name; - }else{ - if(!field.hasMember("is_signed")){ - ROS_ERROR_STREAM("diagnostics[" << i << "]: fields[" << j << "]: is_signed is missing."); - return false; - } - if(field.hasMember("factor")){ - double factor = 1.0; - if(field.hasMember("factor")){ - factor = static_cast(field["factor"]); - } - - entry = make_shared(factor); - - entry->is_signed = static_cast(field["is_signed"]); - - if(field.hasMember("unit")){ - entry->kv.key = name + "[" + static_cast(field["unit"]) + "]"; - }else{ - entry->kv.key = name; - } - }else{ - if(static_cast(field["is_signed"])){ - entry = make_shared(); - }else{ - entry = make_shared(); - } - entry->is_signed = static_cast(field["is_signed"]); - entry->kv.key = name; - } - } - - if(!field.hasMember("offset")){ - ROS_ERROR_STREAM("diagnostics[" << i << "]: fields[" << j << "]: offset is missing."); - return false; - } - entry->offset = static_cast(field["offset"]); - - entry->length = len; - - std::vector::iterator topic_it = find(topics.begin(), topics.end(), name); - if(topic_it != topics.end()){ - entry->advertise(nh_priv_, name); - topics.erase(topic_it); - publishes = true; - ROS_INFO_STREAM("Created publisher for: " << name); - } - - config_map_.insert(std::make_pair(id, entry)); - - } - if(publishes) polling_list1_.push_back(id); - else polling_list2_.push_back(id); - - ROS_INFO_STREAM("Got "<< fields.size() << " BmsParameter(s) with CAN-ID: 0x" << std::hex << (unsigned int) id << std::dec); - } - return true; -} - -//helper function to evaluate poll period from given poll frequency -void CobBmsDriverNode::evaluatePollPeriodFrom(int poll_frequency_hz) -{ - //check the validity of given poll_frequency_hz - if ((poll_frequency_hz < 0) && (poll_frequency_hz > 20)) - { - ROS_WARN_STREAM("Invalid ROS parameter value: poll_frequency_hz = "<< poll_frequency_hz << ". Setting poll_frequency_hz to 20 Hz"); - poll_frequency_hz = 20; - } - //now evaluate and save poll period - poll_period_for_two_ids_in_ms_ = (1/double(poll_frequency_hz))*1000; - ROS_INFO_STREAM("Evaluated polling period: "<< poll_period_for_two_ids_in_ms_ << " ms"); -} - -//function that goes through config_map_ and fills polling_list1_ and polling_list2_. If topics are found on ROS Parameter Server, they are kept in list1 otherwise, all parameter id are divided between both lists. -void CobBmsDriverNode::optimizePollingLists() -{ - if(polling_list1_.size() == 0){ // no topics, so just distribute topics - while(polling_list1_.size() < polling_list2_.size()){ - polling_list1_.push_back(polling_list2_.back()); - polling_list2_.pop_back(); - } - }else{ - while(polling_list1_.size() > polling_list2_.size()){ - polling_list2_.push_back(polling_list1_.back()); - polling_list1_.pop_back(); - } - } - ROS_INFO_STREAM("Loaded \'"<< polling_list1_.size() << "\' CAN-ID(s) in polling_list1_ and \'"<< polling_list2_.size() <<"\' CAN-ID(s) in polling_list2_"); -} - -//function that polls BMS for given ids -void CobBmsDriverNode::pollBmsForIds(const uint16_t first_id, const uint16_t second_id) -{ - can::Frame f(can::Header(bms_id_to_poll_,false,false,false),4); - f.data[0] = first_id >> 8; //high_byte - f.data[1] = first_id & 0xff; //low_byte - f.data[2] = second_id >> 8; - f.data[3] = second_id & 0xff; - - socketcan_interface_.send(f); - - boost::this_thread::sleep_for(boost::chrono::milliseconds(poll_period_for_two_ids_in_ms_)); -} - -//cycles through polling lists and sends 2 ids at a time (one from each list) to the BMS -void CobBmsDriverNode::pollNextInLists() -{ - //restart if reached the end of polling lists - if (polling_list1_it_ == polling_list1_.end()) polling_list1_it_ = polling_list1_.begin(); - if (polling_list2_it_ == polling_list2_.end()) polling_list2_it_ = polling_list2_.begin(); - //clear stat_, so that it can be refilled with new data - stat_.clear(); - - uint16_t first_id = (polling_list1_it_ == polling_list1_.end()) ? 0 : (*polling_list1_it_ | 0x0100); - uint16_t second_id = (polling_list2_it_ == polling_list2_.end()) ? 0 : (*polling_list2_it_ | 0x0100); - - ROS_DEBUG_STREAM("polling BMS for CAN-IDs: 0x" << std::hex << (int)first_id << " and 0x" << (int) second_id << std::dec); - - pollBmsForIds(first_id,second_id); - - //increment iterators for next poll - if (!polling_list1_.empty()) ++polling_list1_it_; - if (!polling_list2_.empty()) ++polling_list2_it_; -} - -//callback function to handle all types of frames received from BMS -void CobBmsDriverNode::handleFrames(const can::Frame &f) -{ - boost::mutex::scoped_lock lock(data_mutex_); - - //id to find in config map - std::pair range = config_map_.equal_range(f.id); - - for (; range.first != range.second; ++range.first) - { - range.first->second->update(f); - } -} - -//updates the diagnostics data with the new data received from BMS -void CobBmsDriverNode::produceDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat) -{ - boost::mutex::scoped_lock lock(data_mutex_); - - can::State state = socketcan_interface_.getState(); - stat.add("error_code", state.error_code); - stat.add("can_error_code", state.internal_error); - switch (state.driver_state) - { - case can::State::closed: - stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Driver State: Closed"); - break; - - case can::State::open: - stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Driver State: Opened"); - break; - - case can::State::ready: - stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Driver State: Ready"); - break; - - default: - stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Driver State: Unknown"); - break; - } - - for (ConfigMap::iterator cm_it = config_map_.begin(); cm_it != config_map_.end(); ++cm_it) - { - stat.values.push_back(cm_it->second->kv); - } -} - -void CobBmsDriverNode::diagnosticsTimerCallback(const ros::TimerEvent& event) -{ - //update diagnostics - updater_.update(); - if(!socketcan_interface_.getState().isReady()){ - ROS_ERROR("Restarting BMS socketcan"); - socketcan_interface_.shutdown(); - socketcan_interface_.recover(); - } -} - -int main(int argc, char **argv) -{ - ros::init(argc, argv, "bms_driver_node"); - - CobBmsDriverNode cob_bms_driver_node; - - if (!cob_bms_driver_node.prepare()) return 1; - - ROS_INFO("Started polling BMS..."); - while (ros::ok()) - { - cob_bms_driver_node.pollNextInLists(); - ros::spinOnce(); - } - return 0; -} diff --git a/cob_bms_driver/src/fake_bms.py b/cob_bms_driver/src/fake_bms.py deleted file mode 100755 index 22ebad7db..000000000 --- a/cob_bms_driver/src/fake_bms.py +++ /dev/null @@ -1,91 +0,0 @@ -#!/usr/bin/env python -# -# Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - - -import rospy -from std_msgs.msg import Bool -from std_msgs.msg import Float64 -from cob_srvs.srv import SetFloat, SetFloatResponse -from cob_srvs.srv import SetInt, SetIntResponse -from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus -from diagnostic_updater import Updater - -class FakeBMS(object): - def __init__(self): - self._srv_current = rospy.Service('~set_current', SetFloat, self.current_cb) - self._srv_relative_remaining_capacity = rospy.Service('~set_relative_remaining_capacity', SetInt, self.relative_remaining_capacity_cb) - self._poll_frequency = rospy.get_param('~poll_frequency_hz', 20.0) - self._pub_voltage = rospy.Publisher('~voltage', Float64, queue_size = 1) - self._pub_current = rospy.Publisher('~current', Float64, queue_size = 1) - self._pub_remaining_capacity = rospy.Publisher('~remaining_capacity', Float64, queue_size = 1) - self._pub_full_charge_capacity = rospy.Publisher('~full_charge_capacity', Float64, queue_size = 1) - self._pub_temparature = rospy.Publisher('~temperature', Float64, queue_size = 1) - - self._updater = Updater() - self._updater.setHardwareID("bms") - self._updater.add("cob_bms_dagnostics_updater", self.produce_diagnostics) - - self._voltage = rospy.get_param('~voltage', 48.0) # V - self._current = rospy.get_param('~current', -8.0) # A - self._remaining_capacity = rospy.get_param('~remaining_capacity', 35.0) # Ah - self._full_charge_capacity = rospy.get_param('~full_charge_capacity', 35.0) # Ah - self._temperature = rospy.get_param('~temperature', 25.0) # °C - - rospy.Timer(rospy.Duration(1.0), self.publish_diagnostics) - rospy.Timer(rospy.Duration(1.0/self._poll_frequency), self.timer_cb) - rospy.Timer(rospy.Duration(1.0/self._poll_frequency), self.timer_consume_power_cb) - - def current_cb(self, req): - self._current = round(req.data,2) - res_current = SetFloatResponse(True, "Set current to {}".format(self._current)) - return res_current - - def relative_remaining_capacity_cb(self, req): - self._remaining_capacity = round(((req.data * self._full_charge_capacity)/100.0), 3) - res_capacity = SetIntResponse(True, "Set remaining capacity to {}".format(self._remaining_capacity)) - return res_capacity - - def publish_diagnostics(self, event): - self._updater.update() - - def produce_diagnostics(self, stat): - stat.summary(DiagnosticStatus.OK, "Fake Driver: Ready") - stat.add("current[A]", self._current) - stat.add("voltage[V]", self._voltage) - stat.add("temperature[Celsius]", self._temperature) - stat.add("remaining_capacity[Ah]", round(self._remaining_capacity, 3)) - stat.add("full_charge_capacity[Ah]", self._full_charge_capacity) - return stat - - def timer_cb(self, event): - self._pub_voltage.publish(self._voltage) - self._pub_current.publish(self._current) - self._pub_remaining_capacity.publish(round(self._remaining_capacity, 3)) - self._pub_full_charge_capacity.publish(self._full_charge_capacity) - self._pub_temparature.publish(self._temperature) - - def timer_consume_power_cb(self, event): - # emulate the battery usage based on the current values - self._remaining_capacity += (self._current/self._poll_frequency)/3600.0 - if self._remaining_capacity <= 0.0: - self._remaining_capacity = 0.0 - if self._remaining_capacity >= self._full_charge_capacity: - self._remaining_capacity = round(self._full_charge_capacity,3) - -if __name__ == '__main__': - rospy.init_node('fake_bms') - FakeBMS() - rospy.spin() diff --git a/cob_bms_driver/src/power_state_aggregator.py b/cob_bms_driver/src/power_state_aggregator.py deleted file mode 100755 index 30f28031e..000000000 --- a/cob_bms_driver/src/power_state_aggregator.py +++ /dev/null @@ -1,177 +0,0 @@ -#!/usr/bin/env python -# -# Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - - -import rospy -import numpy -import warnings -from std_msgs.msg import Float64 -from cob_msgs.msg import PowerState -from sensor_msgs.msg import BatteryState - -class PowerStateAggregator(): - - def __init__(self): - # get parameters - self.current_buffer_size = rospy.get_param('~current_buffer_size', 901) # approx. 60sec, topic rate 15Hz, use odd number to prevent 0.0 mean - self.pub_power_state = rospy.Publisher('power_state', PowerState, queue_size=1) - self.pub_battery_state = rospy.Publisher('battery_state', BatteryState, queue_size=1) - self.voltage = None - self.current = None - self.last_currents = [] - self.last_update = rospy.Time(0) - self.connected = False - self.charging = False - self.remaining_capacity = None - self.full_charge_capacity = None - self.temperature = None - rospy.Subscriber("voltage", Float64, self.voltage_cb) - rospy.Subscriber("current", Float64, self.current_cb) - rospy.Subscriber("remaining_capacity", Float64, self.remaining_capacity_cb) - rospy.Subscriber("full_charge_capacity", Float64, self.full_charge_capacity_cb) - rospy.Subscriber("temperature", Float64, self.temperature_cb) - - def voltage_cb(self, msg): - self.last_update = rospy.Time.now() - self.voltage = msg.data - - def current_cb(self, msg): - self.last_update = rospy.Time.now() - self.current = msg.data - - # fill current into list of past currents for filtering purposes - if len(self.last_currents) >= self.current_buffer_size: - self.last_currents.pop(0) - self.last_currents.append(msg.data) - - if msg.data > 0: - self.charging = True - else: - self.charging = False - - if msg.data > -1: # we use a limit of -1Ampere because if the battery is 100% full and the robot is still docked, there is no more current going into the battery. -1 A is biggger than the "Ruhestrom", so this should be ok until BMS is fixed and delivers a proper flag for docked or not_docked. - self.connected = True - else: - self.connected = False - - def remaining_capacity_cb(self, msg): - self.last_update = rospy.Time.now() - self.remaining_capacity = msg.data - - def full_charge_capacity_cb(self, msg): - self.last_update = rospy.Time.now() - self.full_charge_capacity = msg.data - - def temperature_cb(self, msg): - self.last_update = rospy.Time.now() - self.temperature = msg.data - - def calculate_power_consumption(self): - if not self.charging and self.voltage != None and self.current != None: - return round(self.voltage * abs(self.current), 3) - else: - return 0.0 - - def calculate_relative_remaining_capacity(self): - try: - return round(100.0*(self.remaining_capacity/self.full_charge_capacity), 3) - except ZeroDivisionError as e: - rospy.logerr("ZeroDivisionError: full_charge_capacity is 0.0: %s" % (e)) - except: - rospy.logwarn("something went wrong, cannot calculate relative remaining capacity. full_charge_capacity=%s, remaining_capacity=%s" % (self.full_charge_capacity, self.remaining_capacity)) - return 0.0 - - def calculate_time_remaining(self): - warnings.filterwarnings('error') - time_remaining_max = 10.0 # assume 10h - for cases where current approx. 0A - time_remaining = time_remaining_max - try: - if len(self.last_currents) > 0: - current = numpy.mean(self.last_currents) - - if self.full_charge_capacity != None and self.remaining_capacity != None: - if self.charging: - time_remaining = round((self.full_charge_capacity - self.remaining_capacity) / abs(current), 3) - else: - time_remaining = round(self.remaining_capacity / abs(current), 3) - else: - pass - else: - pass - except ZeroDivisionError as e: - rospy.logerr("ZeroDivisionError: current is 0.0: {}".format(e)) - except Warning as w: - rospy.logerr("Warning: {}".format(w)) - except Exception as e: - rospy.logerr("Exception: {}".format(e)) - # rospy.logdebug("calculate_time_remaining") - # rospy.logdebug("time_remaining_max: {}".format(time_remaining_max)) - # rospy.logdebug("time_remaining: {}".format(time_remaining)) - # rospy.logdebug("self.last_currents: {}".format(self.last_currents)) - # rospy.logdebug("current: {}".format(current)) - # rospy.logdebug("self.full_charge_capacity: {}".format(self.full_charge_capacity)) - # rospy.logdebug("self.remaining_capacity: {}".format(self.remaining_capacity)) - # rospy.logdebug("self.charging: {}".format(self.charging)) - return min(time_remaining, time_remaining_max) - - def publish(self): - if self.voltage != None and self.current != None and self.remaining_capacity != None and self.full_charge_capacity != None and self.temperature != None and (rospy.Time.now() - self.last_update) < rospy.Duration(1): - power_consumption = self.calculate_power_consumption() - relative_remaining_capacity = self.calculate_relative_remaining_capacity() - time_remaining = self.calculate_time_remaining() - - ps = PowerState() - ps.header.stamp = self.last_update - ps.voltage = self.voltage - ps.current = self.current - ps.power_consumption = power_consumption - ps.remaining_capacity = self.remaining_capacity - ps.relative_remaining_capacity = relative_remaining_capacity - ps.connected = self.connected - ps.charging = self.charging - ps.time_remaining = time_remaining - ps.temperature = self.temperature - - bs = BatteryState() - bs.header.stamp = self.last_update - bs.voltage = self.voltage - bs.current = self.current - bs.charge = self.remaining_capacity - bs.design_capacity = self.full_charge_capacity - bs.percentage = relative_remaining_capacity / 100.0 - bs.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_CHARGING if self.charging else BatteryState.POWER_SUPPLY_STATUS_NOT_CHARGING - bs.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_GOOD - bs.power_supply_technology = BatteryState.POWER_SUPPLY_TECHNOLOGY_UNKNOWN - bs.present = True - bs.cell_voltage = [self.voltage] - bs.location = "emulated_battery" - bs.serial_number = "emulated_battery" - - self.pub_power_state.publish(ps) - self.pub_battery_state.publish(bs) - -if __name__ == "__main__": - rospy.init_node("power_state_aggregator") - PSA = PowerStateAggregator() - rospy.loginfo("power state aggregator running") - rate = rospy.Rate(10) - while not rospy.is_shutdown(): - PSA.publish() - try: - rate.sleep() - except rospy.exceptions.ROSInterruptException as e: - pass - diff --git a/cob_driver/package.xml b/cob_driver/package.xml index 69ccdd8d5..d3999fa06 100644 --- a/cob_driver/package.xml +++ b/cob_driver/package.xml @@ -13,7 +13,6 @@ catkin - cob_bms_driver cob_light cob_mimic cob_phidgets From de6b68522026005df8dd79ddab3a4c73be0066cc Mon Sep 17 00:00:00 2001 From: fmessmer Date: Wed, 17 Apr 2024 09:43:39 +0200 Subject: [PATCH 14/14] new default branches --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index dac7dd316..c0a81e091 100644 --- a/README.md +++ b/README.md @@ -3,4 +3,4 @@ cob_driver ## GitHub Actions - Continuous Integration -CI-Status ```kinetic_dev```: [![GHA CI](https://github.com/4am-robotics/cob_driver/actions/workflows/main.yml/badge.svg?branch=kinetic_dev)](https://github.com/4am-robotics/cob_driver/actions/workflows/main.yml?query=branch%3Akinetic_dev) +CI-Status ```noetic-devel```: [![GHA CI](https://github.com/4am-robotics/cob_driver/actions/workflows/main.yml/badge.svg?branch=noetic-devel)](https://github.com/4am-robotics/cob_driver/actions/workflows/main.yml?query=branch%3Anoetic-devel)