diff --git a/.travis.rosinstall.kinetic b/.travis.rosinstall.kinetic deleted file mode 100644 index 9a03e8713..000000000 --- a/.travis.rosinstall.kinetic +++ /dev/null @@ -1,12 +0,0 @@ -- git: - uri: 'https://github.com/ipa320/cob_common.git' - local-name: cob_common - version: indigo_dev -- git: - uri: 'https://github.com/ipa320/cob_extern.git' - local-name: cob_extern - version: indigo_dev -- git: - uri: 'https://github.com/ipa320/cob_perception_common.git' - local-name: cob_perception_common - version: indigo_dev diff --git a/cob_base_drive_chain/CHANGELOG.rst b/cob_base_drive_chain/CHANGELOG.rst index bc17d0eff..55dc38024 100644 --- a/cob_base_drive_chain/CHANGELOG.rst +++ b/cob_base_drive_chain/CHANGELOG.rst @@ -2,6 +2,33 @@ Changelog for package cob_base_drive_chain ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +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 diff --git a/cob_base_drive_chain/package.xml b/cob_base_drive_chain/package.xml index a534c6714..e07352996 100644 --- a/cob_base_drive_chain/package.xml +++ b/cob_base_drive_chain/package.xml @@ -1,6 +1,6 @@ cob_base_drive_chain - 0.6.8 + 0.6.13 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 diff --git a/cob_bms_driver/CHANGELOG.rst b/cob_bms_driver/CHANGELOG.rst index 5c8d5bb1c..96fa55232 100644 --- a/cob_bms_driver/CHANGELOG.rst +++ b/cob_bms_driver/CHANGELOG.rst @@ -2,6 +2,78 @@ Changelog for package cob_bms_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +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 diff --git a/cob_bms_driver/package.xml b/cob_bms_driver/package.xml index 565740f2d..719958682 100644 --- a/cob_bms_driver/package.xml +++ b/cob_bms_driver/package.xml @@ -1,11 +1,12 @@ cob_bms_driver - 0.6.8 + 0.6.13 Driver package for interfacing the battery management system (BMS) on Care-O-bot. + Felix Messmer Florian Weisshardt mig-mc Mathias Lüdtke diff --git a/cob_bms_driver/src/cob_bms_driver_node.cpp b/cob_bms_driver/src/cob_bms_driver_node.cpp index 571263817..e440b3136 100644 --- a/cob_bms_driver/src/cob_bms_driver_node.cpp +++ b/cob_bms_driver/src/cob_bms_driver_node.cpp @@ -21,6 +21,7 @@ #include #include +#include #include diff --git a/cob_bms_driver/src/fake_bms.py b/cob_bms_driver/src/fake_bms.py index f25cdcb80..7d8114e61 100755 --- a/cob_bms_driver/src/fake_bms.py +++ b/cob_bms_driver/src/fake_bms.py @@ -20,52 +20,56 @@ from std_msgs.msg import Float64 from cob_srvs.srv import SetFloat, SetFloatResponse 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_remaining_capacity = rospy.Service('~set_remaining_capacity', SetFloat, self.remaining_capacity_cb) + self.srv_relative_remaining_capacity = rospy.Service('~set_relative_remaining_capacity', SetFloat, 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.pub_diagnostics = rospy.Publisher('/diagnostics', DiagnosticArray, queue_size=1) - self.voltage = 0.0 + self.updater = Updater() + self.updater.setHardwareID("bms") + self.updater.add("cob_bms_dagnostics_updater", self.produce_diagnostics) + + self.voltage = 48.0 self.current = -8.0 self.remaining_capacity = 35.0 self.full_charge_capacity = 35.0 # Ah - self.temperature = 0.0 + self.temperature = 25.0 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 = req.data - res_current = SetFloatResponse(True, "Set current to {}".format(req.data)) + self.current = round(req.data,2) + res_current = SetFloatResponse(True, "Set current to {}".format(self.current)) return res_current - def remaining_capacity_cb(self, req): - self.remaining_capacity = req.data - res_capacity = SetFloatResponse(True, "Set remaining capacity to {}".format(req.data)) + def relative_remaining_capacity_cb(self, req): + self.remaining_capacity = round(((req.data * self.full_charge_capacity)/100.0), 3) + res_capacity = SetFloatResponse(True, "Set relative remaining capacity to {}".format(self.remaining_capacity)) return res_capacity def publish_diagnostics(self, event): - msg = DiagnosticArray() - msg.header.stamp = rospy.get_rostime() - status = DiagnosticStatus() - status.name = rospy.get_name() - status.level = DiagnosticStatus.OK - status.message = "fake diagnostics" - status.hardware_id = rospy.get_name() - msg.status.append(status) - self.pub_diagnostics.publish(msg) + 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]", self.remaining_capacity) + stat.add("full_charge_capacity[Ah]", self.full_charge_capacity) + return stat def timer_cb(self, event): - self.voltage = 48.0 self.pub_voltage.publish(self.voltage) self.pub_current.publish(self.current) self.pub_remaining_capacity.publish(self.remaining_capacity) @@ -74,11 +78,12 @@ def timer_cb(self, event): 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 - if self.remaining_capacity <= 0: - self.remaining_capacity = 0 + self.remaining_capacity += (self.current/self.poll_frequency)/3600.0 + self.remaining_capacity = round(self.remaining_capacity,3) + if self.remaining_capacity <= 0.0: + self.remaining_capacity = 0.0 if self.remaining_capacity >= self.full_charge_capacity: - self.remaining_capacity = self.full_charge_capacity + self.remaining_capacity = round(self.full_charge_capacity,3) if __name__ == '__main__': rospy.init_node('fake_bms') diff --git a/cob_camera_sensors/CHANGELOG.rst b/cob_camera_sensors/CHANGELOG.rst index c92fe71f7..6884888cf 100644 --- a/cob_camera_sensors/CHANGELOG.rst +++ b/cob_camera_sensors/CHANGELOG.rst @@ -2,6 +2,35 @@ Changelog for package cob_camera_sensors ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +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 +* Merge branch 'indigo_dev' of github.com:ipa320/cob_driver into feature/mimic_sim +* Merge pull request `#351 `_ from mikaelarguedas/add_tinyxml_dependency + add tinyxml to package.xml +* add tinyxml to package.xml +* Merge pull request `#349 `_ from ipa320/ipa-rmb-patch-1 + Changed Maintainer +* Changed Maintainer +* Contributors: Benjamin Maidel, Felix Messmer, Mikael Arguedas, Richard Bormann, 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) ------------------ diff --git a/cob_camera_sensors/package.xml b/cob_camera_sensors/package.xml index a6b11d063..dc68f324c 100644 --- a/cob_camera_sensors/package.xml +++ b/cob_camera_sensors/package.xml @@ -1,6 +1,6 @@ cob_camera_sensors - 0.6.8 + 0.6.13 For more information read the readme.htm file located in Apache 2.0 diff --git a/cob_canopen_motor/CHANGELOG.rst b/cob_canopen_motor/CHANGELOG.rst index 7ca7f5251..0e9ce427d 100644 --- a/cob_canopen_motor/CHANGELOG.rst +++ b/cob_canopen_motor/CHANGELOG.rst @@ -2,6 +2,28 @@ Changelog for package cob_canopen_motor ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +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) ------------------ diff --git a/cob_canopen_motor/package.xml b/cob_canopen_motor/package.xml index 454974b1f..ea27915f3 100644 --- a/cob_canopen_motor/package.xml +++ b/cob_canopen_motor/package.xml @@ -1,6 +1,6 @@ cob_canopen_motor - 0.6.8 + 0.6.13 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 diff --git a/cob_driver/CHANGELOG.rst b/cob_driver/CHANGELOG.rst index c02fc67eb..641eef737 100644 --- a/cob_driver/CHANGELOG.rst +++ b/cob_driver/CHANGELOG.rst @@ -2,6 +2,33 @@ Changelog for package cob_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +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 `#353 `_ from ipa-fxm/update_maintainer + update maintainer +* update maintainer +* 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) ------------------ diff --git a/cob_driver/package.xml b/cob_driver/package.xml index 00f2ab6b4..836b36323 100644 --- a/cob_driver/package.xml +++ b/cob_driver/package.xml @@ -1,6 +1,6 @@ cob_driver - 0.6.8 + 0.6.13 The cob_driver stack includes packages that provide access to the Care-O-bot hardware through ROS messages, services and actions. E.g. for mobile base, arm, camera sensors, laser scanners, etc... Apache 2.0 @@ -8,9 +8,8 @@ http://ros.org/wiki/cob_driver + Felix Messmer Florian Weisshardt - Nadia Hammoudeh Garcia - Florian Weisshardt catkin @@ -20,7 +19,6 @@ cob_canopen_motor cob_elmo_homing cob_generic_can - cob_head_axis cob_light cob_mimic cob_phidgets diff --git a/cob_elmo_homing/CHANGELOG.rst b/cob_elmo_homing/CHANGELOG.rst index c756caadd..0ac20b822 100644 --- a/cob_elmo_homing/CHANGELOG.rst +++ b/cob_elmo_homing/CHANGELOG.rst @@ -2,6 +2,29 @@ Changelog for package cob_elmo_homing ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +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) ------------------ diff --git a/cob_elmo_homing/package.xml b/cob_elmo_homing/package.xml index 9c6755009..b417937c6 100644 --- a/cob_elmo_homing/package.xml +++ b/cob_elmo_homing/package.xml @@ -1,7 +1,7 @@ cob_elmo_homing - 0.6.8 + 0.6.13 This packagae implements the special homing procedure that is needed for old cob4/raw bases Florian Weisshardt diff --git a/cob_generic_can/CHANGELOG.rst b/cob_generic_can/CHANGELOG.rst index 804bf65b5..82da8aacf 100644 --- a/cob_generic_can/CHANGELOG.rst +++ b/cob_generic_can/CHANGELOG.rst @@ -2,6 +2,38 @@ Changelog for package cob_generic_can ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +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) ------------------ diff --git a/cob_generic_can/package.xml b/cob_generic_can/package.xml index a0f68a25b..fa8a58c52 100644 --- a/cob_generic_can/package.xml +++ b/cob_generic_can/package.xml @@ -1,6 +1,6 @@ cob_generic_can - 0.6.8 + 0.6.13 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 diff --git a/cob_head_axis/CHANGELOG.rst b/cob_head_axis/CHANGELOG.rst deleted file mode 100644 index 751c2514b..000000000 --- a/cob_head_axis/CHANGELOG.rst +++ /dev/null @@ -1,159 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package cob_head_axis -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.6.8 (2016-10-10) ------------------- - -0.6.7 (2016-04-02) ------------------- - -0.6.6 (2016-04-01) ------------------- - -0.6.5 (2015-08-31) ------------------- - -0.6.4 (2015-08-25) ------------------- -* rising no longer supported -* boost revision -* do not install headers in executable-only packages -* explicit dependency to boost -* more cleanup -* remove obsolete autogenerated mainpage.dox files -* catkin_package according to install tags -* remove trailing whitespaces -* add_dependencies EXPORTED_TARGETS -* migrate to package format 2 -* sort dependencies -* critically review dependencies -* Contributors: ipa-fxm, ipa-nhg - -0.6.3 (2015-06-17) ------------------- -* fix topic names -* use new Trigger from std_srvs -* cleanup cob_srvs -* adapt cob_head_axis to new namespaces -* Contributors: ipa-fxm, ipa-nhg - -0.6.2 (2014-12-15) ------------------- - -0.6.1 (2014-09-17) ------------------- - -0.6.0 (2014-09-09) ------------------- - -0.5.7 (2014-08-26) ------------------- -* Merge pull request `#163 `_ from ipa320/hydro_dev - updates from hydro_dev -* 0.5.6 -* update changelog -* added explicit default argument queue_size -* Cleaned up cob_driver with reduced deps to compile on indigo -* Contributors: Alexander Bubeck, Felix Messmer, Florian Weisshardt - -0.5.6 (2014-08-26) ------------------- -* Merge pull request `#163 `_ from ipa320/hydro_dev - updates from hydro_dev -* added explicit default argument queue_size -* Cleaned up cob_driver with reduced deps to compile on indigo -* Contributors: Alexander Bubeck, Felix Messmer, Florian Weisshardt - -0.5.3 (2014-03-31) ------------------- -* install tags -* cob_head_axis: tabs to spaces -* cob_head_axis: fix deprecated API in constructor -* Contributors: ipa-fxm, ipa-mig - -0.5.2 (2014-03-20) ------------------- - -0.5.1 (2014-03-20) ------------------- -* cherry-pick -* removed a lot of code related to packages not available in hydro anymore -* output long unsigned variables correctly -* upstream changes -* Merge branch 'groovy_dev' of git://github.com/ipa320/cob_driver into groovy_dev -* fixed build errors for gcc version >= 4.7 -* trying to fix quantal compilation -* merged with upstream -* Fixed diagnostic error for head_axis -* trying to get error state out of head axis -* cleaned up CMakeLists and added install directives -* further modifications for catkin, now everything is compiling and linking -* futher include and linkpath modifications -* compiling but still some linker errors -* Second catkinization push -* First catkinization, still need to update some CMakeLists.txt -* dirty hack for fixing joint direction -* fix finish bug in head axis -* fix finish bug in head axis -* added effort to joint states message -* remove deprecated link to urdf -* switched from pr2_controllers_msgs::JointTrajectoryAction to control_msgs::FollowJointTrajectory -* removed outdated file - not used anymore -* fix actionserver bug with early success -* beautyfing -* partial fix for head axis -* fix for crash if stop is executed before init -* add state topic for head_axis -* changes for fuerte compatibility -* remove deprecated tests -* delete old in iand yaml files -* added diagnotic topic for initialization states for sdh -* move to private namespace -* private nodehandle -* merge -* adde env ROBOT to test -* added roslaunch tests -* added cob3-4 configs -* removed compiler warnings -* cob_head_axis: turning to 0deg after homing -* cob_head_axis: removed homing-sleep -* added rostest -* modifications for fetch and carry -* update cob3-3 -* dummy head axis -* adaptions for cob_head_axis on cob3-3, included some new parameters instead of hard-coded settings -* update for cob3-3 -* rearranging cob_camera_sensors launch files -* update for cob3-3 -* add services and return true for recover after init -* changed test duration to 10s -* camera settings added for head -* changed head params for cob3-2 -* moved init test to cob_srvs -* wrong namespace in test file -* modified parameters -* modified parameters -* modified tests -* release update for cob3-1 -* merge -* changed device for head axis -* adjust devices for cob3-1 -* changed trigger service -* joint_state aggregator working on cob3-1, calibration script update -* cleanup in cob_driver -* Moved hard-coded lines for head_axis_homing from CanDriveHarmonica.cpp into ElmoCtrl.cpp. Removed debugger in base_drive_chain.launch and undercarriage_ctrl.launch -* Head axis working, tested on cob3-1 but adapted parameters (-files) should work on both robots -* merge -* HeadAxis working -* bugfix -* Cleaned cob_head_axis yaml-files -* merge -* removed unused parameters -* cob_head_axis: set offset via urdf and chose can-device-path via ini-File -* cob_camera_axis tested, now also is able to be shut down -* cob_head_axis working -* cob_head_axis working -* cob_head_axis: correctly working, but front and back is switched -* renamed camera_axis to head_axis and platform to base -* Contributors: Alexander Bubeck, Felix Messmer, Frederik Hegger, Richard Bormann, abubeck, cpc-pk, fmw-jk, ipa-bnm, ipa-cob3-4, ipa-cob3-5, ipa-fmw, ipa-fxm, ipa-goa, ipa-mig, ipa-uhr diff --git a/cob_head_axis/CMakeLists.txt b/cob_head_axis/CMakeLists.txt deleted file mode 100644 index 07f96c6d0..000000000 --- a/cob_head_axis/CMakeLists.txt +++ /dev/null @@ -1,24 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(cob_head_axis) - -find_package(catkin REQUIRED COMPONENTS actionlib cob_canopen_motor cob_generic_can cob_srvs cob_utilities control_msgs diagnostic_msgs roscpp sensor_msgs std_srvs urdf) - -catkin_package() - -### BUILD ### -include_directories(common/include ${catkin_INCLUDE_DIRS}) - -add_executable(${PROJECT_NAME}_node ros/src/${PROJECT_NAME}.cpp common/src/ElmoCtrl.cpp) -add_dependencies(${PROJECT_NAME}_node ${catkin_EXPORTED_TARGETS}) -target_link_libraries(${PROJECT_NAME}_node ${catkin_LIBRARIES}) - -### INSTALL ### -install(TARGETS ${PROJECT_NAME}_node - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -install(PROGRAMS ros/src/dummy_head.py - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) diff --git a/cob_head_axis/common/include/cob_head_axis/ElmoCtrl.h b/cob_head_axis/common/include/cob_head_axis/ElmoCtrl.h deleted file mode 100644 index ad2fb8ca0..000000000 --- a/cob_head_axis/common/include/cob_head_axis/ElmoCtrl.h +++ /dev/null @@ -1,255 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -#ifndef __ELMO_CTRL_H__ -#define __ELMO_CTRL_H__ -#define NTCAN_CLEAN_NAMESPACE -#include -#include -#include -#include -#include -#include - -// Headers provided by cob-packages which should be avoided/removed^M -#include -#include - - -typedef struct _CanOpenAddress -{ - int TxPDO1; - int TxPDO2; - int RxPDO2; - int TxSDO; - int RxSDO; -} CanOpenAddress; - -class ElmoCtrl; - -typedef struct -{ - ElmoCtrl * pElmoCtrl; -} ElmoThreadArgs; - - -class ElmoCtrlParams -{ - - public: - ElmoCtrlParams(){;} - - int Init(std::string CanDevice, int BaudRate, int ModuleID) - { - SetCanDevice(CanDevice); - SetBaudRate(BaudRate); - SetModuleID(ModuleID); - return 0; - } - - //Can Device - void SetCanDevice(std::string CanDevice){m_CanDevice = CanDevice;} - std::string GetCanDevice(){return m_CanDevice;} - - //BaudRate - void SetBaudRate(int BaudRate){m_BaudRate=BaudRate;} - int GetBaudRate(){return m_BaudRate;} - - //ModuleIDs - int GetModuleID(){return m_ModuleID;} - void SetModuleID(int id){m_ModuleID = id;} - - //Angular Constraints - void SetUpperLimit(double UpperLimit){m_UpperLimit = UpperLimit;} - void SetLowerLimit(double LowerLimit){m_LowerLimit = LowerLimit;} - void SetAngleOffset(double AngleOffset){m_Offset = AngleOffset;} - double GetUpperLimit(){return m_UpperLimit;} - double GetLowerLimit(){return m_LowerLimit;} - double GetAngleOffset(){return m_Offset;} - - //Velocity - void SetMaxVel(double maxVel){m_MaxVel = maxVel;} - double GetMaxVel(){return m_MaxVel;} - - //HomingDir - void SetHomingDir(int dir){m_HomingDir = dir;} - int GetHomingDir(){return m_HomingDir;} - - //HomingDigIn - void SetHomingDigIn(int dig_in){m_HomingDigIn = dig_in;} - int GetHomingDigIn(){return m_HomingDigIn;} - - //CanIniFile - void SetCanIniFile(std::string iniFile){m_CanIniFile=iniFile;} - std::string GetCanIniFile(){return m_CanIniFile;} - - //Encoder increments - void SetEncoderIncrements(int enc_incr){m_EncIncrPerRevMot = enc_incr;} - int GetEncoderIncrements(){return m_EncIncrPerRevMot;} - - //Belt & Gear ratio in one - void SetGearRatio(double gear_ratio){m_GearRatio = gear_ratio;} - double GetGearRatio(){return m_GearRatio;} - - //Motor direction - void SetMotorDirection(int motor_dir){ - if(motor_dir<0) m_MotorDirection = -1; - else m_MotorDirection = 1; - } - int GetMotorDirection(){return m_MotorDirection;} - - - - - private: - int m_ModuleID; - std::string m_CanDevice; - int m_BaudRate; - int m_HomingDir; - int m_HomingDigIn; - int m_EncIncrPerRevMot; - int m_MotorDirection; - double m_GearRatio; - double m_Offset; - double m_UpperLimit; - double m_LowerLimit; - double m_MaxVel; - std::string m_CanIniFile; - -}; - -class ElmoCtrl -{ - -public: - ElmoCtrl(); - ~ElmoCtrl(); - - bool Init(ElmoCtrlParams* params, bool home = true); - - - double MoveJointSpace (double PosRad); - - - bool Home(); - - bool RecoverAfterEmergencyStop(); - - bool Stop(); - - - void setMaxVelocity(float radpersec) - { - m_MaxVel = radpersec; - } - - /** - * Gets the position and velocity. - * @param pdAngleGearRad joint-position in radian - * @param pdVelGearRadS joint-velocity in radian per second - */ - int getGearPosVelRadS(double* pdAngleGearRad, double* pdVelGearRadS); - - /** - * Sets required position and veolocity. - * Use this function only in position mode. - */ - int setGearPosVelRadS(double dPosRad, double dVelRadS); - - /** - * Triggers evaluation of the can-buffer. - */ - int evalCanBuffer(); - - //_double getConfig(); - - double getJointVelocity(); - - - - bool SetMotionCtrlType(int type); - int GetMotionCtrlType(); - bool isError() ; - - - enum { - POS_CTRL, - VEL_CTRL - } MOTION_CTRL_TYPE; - - - CANPeakSysUSB* GetCanCtrl(){return m_CanCtrl;} - //CanESD* GetCanCtrl(){return m_CanCtrl;} - //bool m_ElmoCtrlThreadActive; - /// @brief joint mutexes - //pthread_mutex_t m_cs_elmoCtrlIO; - CanDriveHarmonica * m_Joint; - -private: - - bool sendNetStartCanOpen(CanItf* canCtrl); - - int m_MotionCtrlType; - - DriveParam * m_JointParams; - - int m_CanBaseAddress; - CanOpenAddress m_CanAddress; - - CANPeakSysUSB * m_CanCtrl; - //CanESD * m_CanCtrl; - - double m_MaxVel; - int m_HomingDir; - int m_HomingDigIn; - int m_EncIncrPerRevMot; - int m_MotorDirection; - double m_GearRatio; - - double m_UpperLimit; - double m_LowerLimit; - double m_JointOffset; - /* - Jointd* m_CurrentAngularVelocity; - /// @brief current joint angles - Jointd* m_CurrentJointAngles; - */ - /// @brief joint mutexes - pthread_mutex_t m_Mutex; - /// @brief joint velocity mutexes - //pthread_mutex_t m_AngularVel_Mutex; - /// @brief Thread IDs for PowerCube connection threads - //pthread_t m_ElmoThreadID; - /// @brief Arguments for Powercube connection Threads - //ElmoThreadArgs * m_ElmoCtrlThreadArgs; - ElmoCtrlParams * m_Params; - CanMsg m_CanMsgRec; - - - /// @brief the logfile for debugging info & errors: - -}; - - - - - - - - - -#endif //__ELMO_CTRL_H__ diff --git a/cob_head_axis/common/src/ElmoCtrl.cpp b/cob_head_axis/common/src/ElmoCtrl.cpp deleted file mode 100644 index 29d094d1c..000000000 --- a/cob_head_axis/common/src/ElmoCtrl.cpp +++ /dev/null @@ -1,436 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -#include -#include -#include -#include - -using namespace std; - -void Sleep(int msecs){usleep(1000*msecs);} - -bool ElmoCtrl::sendNetStartCanOpen(CanItf* canCtrl) { - bool ret = false; - - CanMsg msg; - - msg.m_iID = 0; - msg.m_iLen = 2; - msg.set(1,0,0,0,0,0,0,0); - ret = canCtrl->transmitMsg(msg, false); - - usleep(100000); - - return ret; -} - - -ElmoCtrl::ElmoCtrl() { - m_Joint = NULL; - m_JointParams = NULL; - m_CanCtrl = NULL; - m_CanBaseAddress = 0; - m_MotionCtrlType = POS_CTRL; - m_MaxVel = 2.0; - m_Params = NULL; -} - -ElmoCtrl::~ElmoCtrl() { - if (m_Joint) - m_Joint->shutdown(); - delete m_Joint; - if (m_JointParams) - delete m_JointParams; - if (m_CanCtrl) - delete m_CanCtrl; -} - -bool ElmoCtrl::Home() -{ - bool success = false; - if (m_Joint != NULL) { - //THIS is needed for head_axis on cob3-2! - //set input logic to 'general purpose' - m_Joint->IntprtSetInt(8, 'I', 'L', 2, 7); - usleep(20000); - - //THIS is needed for cob3-3 (it's using DIN1): - //set input logic to 'general purpose' - m_Joint->IntprtSetInt(8, 'I', 'L', 1, 6); - usleep(20000); - - m_Joint->initHoming(); - } - - //ToDo: UHR: necessary? - Sleep(10); - printf("ElmoCtrl: Home(): Homing Vel = %f\n",m_HomingDir*0.3); - m_Joint->setGearVelRadS(m_HomingDir*0.3); - //cpc-pk: removed this sleep, it's not necessary to leave home-sensor during homing - //Sleep(750); - success = m_Joint->execHoming(); - m_Joint->setGearVelRadS(0.0); - - return success; -} - - -int ElmoCtrl::evalCanBuffer() { - bool bRet; - - //pthread_mutex_lock(&(m_Mutex)); - - // as long as there is something in the can buffer -> read out next message - while(m_CanCtrl->receiveMsg(&m_CanMsgRec) == true) { - bRet = false; - // check if the message belongs to head_axis motor - bRet |= m_Joint->evalReceivedMsg(m_CanMsgRec); - - if (bRet == true) { - } else std::cout << "cob_head_axis: Unknown CAN-msg: " << m_CanMsgRec.m_iID << " " << (int)m_CanMsgRec.getAt(0) << " " << (int)m_CanMsgRec.getAt(1) << std::endl; - } - - //pthread_mutex_unlock(&(m_Mutex)); - - return 0; -} - -bool ElmoCtrl::RecoverAfterEmergencyStop() { - - bool success = false; - printf("Resetting motor ...\n"); - success = m_Joint->start(); - if (!success) { - printf("failed!\n"); - } else { - printf("successful\n"); - m_Joint->setGearVelRadS(0); - } - Sleep(1000); - return success; -} - - -bool ElmoCtrl::Init(ElmoCtrlParams * params, bool home) { //home = true by default - bool success = false; - string CanIniFile; - string CanDevice; - int baudrate = 0; - - m_Params = params; - - if (params == NULL) { - printf("ElmoCtrlParams:Error:%s:%d:, invalid parameters!\n",__FILE__,__LINE__); - success = false; - } else { - success = true; - } - - if (success) { - printf( "------------ ElmoCtrl Init ---------------\n"); - - //Allocate memory and read params e.g. gotten from ROS parameter server - m_Joint = new CanDriveHarmonica(); - m_JointParams = new DriveParam(); - m_CanBaseAddress = params->GetModuleID(); - CanIniFile = params->GetCanIniFile(); - m_MaxVel = params->GetMaxVel(); - m_HomingDir = params->GetHomingDir(); - m_HomingDigIn = params->GetHomingDigIn(); - - m_EncIncrPerRevMot = params->GetEncoderIncrements(); - m_MotorDirection = params->GetMotorDirection(); - m_GearRatio = params->GetGearRatio(); - - if (CanIniFile.length() == 0) { - printf("%s,%d:Error: Parameter 'CanIniFile' not given!\n",__FILE__,__LINE__); - success = false; - } - - CanDevice = params->GetCanDevice(); - if (CanDevice.length() == 0) { - printf("%s,%d:Error: Parameter 'Can-Device' not given!\n",__FILE__,__LINE__); - success = false; - } - - baudrate = params->GetBaudRate(); - if (baudrate == 0) { - printf("%s,%d:Error: Parameter 'Baud-Rate' not given!\n",__FILE__,__LINE__); - success = false; - } - - //Setting motor model data - if (success) { - m_JointOffset = params->GetAngleOffset(); - m_UpperLimit = params->GetUpperLimit(); - m_LowerLimit = params->GetLowerLimit(); - } - - if (success) { - printf("The following parameters were successfully read from the parameter server (given through *params): \n"); - printf("CanIniFile: %s\n",CanIniFile.c_str()); - printf("CanDieODvice: %s\n",CanDevice.c_str()); - printf("Baudrate: %d\n",baudrate); - printf("Module ID: %d\n",m_CanBaseAddress); - printf("Max Vel: %f\n",m_MaxVel); - printf("Homing Dir: %d\n",m_HomingDir); - printf("HomingDigIn: %d\n",m_HomingDigIn); - printf("Offset/Limit(min/max) %f/(%f,%f)\n",m_JointOffset,m_LowerLimit,m_UpperLimit); - } - } - - //Setting up CAN interface - - if (success) { - //m_CanCtrl = new CanESD(CanIniFile.c_str(), false); - m_CanCtrl = new CANPeakSysUSB(CanIniFile.c_str()); - if (m_CanCtrl == NULL) { - printf("%s,%d:Error: Could not open Can Device!\n",__FILE__,__LINE__); - success = false; - } - } - - if (success) { - /* WRONG CAN-identifiers - //m_CanBaseAddress = params->GetModulID(i); - m_CanAddress.TxPDO1 = 0x181 + m_CanBaseAddress -1; - m_CanAddress.TxPDO2 = 0x285 + m_CanBaseAddress -1; - m_CanAddress.RxPDO2 = 0x301 + m_CanBaseAddress -1; - m_CanAddress.TxSDO = 0x491 + m_CanBaseAddress -1; - m_CanAddress.RxSDO = 0x511 + m_CanBaseAddress -1; - */ - m_CanAddress.TxPDO1 = 0x181 + m_CanBaseAddress -1; - m_CanAddress.TxPDO2 = 0x281 + m_CanBaseAddress -1; - m_CanAddress.RxPDO2 = 0x301 + m_CanBaseAddress -1; - m_CanAddress.TxSDO = 0x581 + m_CanBaseAddress -1; - m_CanAddress.RxSDO = 0x601 + m_CanBaseAddress -1; - m_Joint->setCanItf(m_CanCtrl); - m_Joint->setCanOpenParam(m_CanAddress.TxPDO1, - m_CanAddress.TxPDO2, - m_CanAddress.RxPDO2, - m_CanAddress.TxSDO, - m_CanAddress.RxSDO ); - - printf("CanAdresses set to %d (Base), %x, %x, %x, %x, %x...\n", m_CanBaseAddress, - m_CanAddress.TxPDO1, - m_CanAddress.TxPDO2, - m_CanAddress.RxPDO2, - m_CanAddress.TxSDO, - m_CanAddress.RxSDO); - - } - - if (success) { - success = sendNetStartCanOpen(m_CanCtrl); - } - - if (success) { - //ToDo: Read from File! - /* - int iDriveIdent, - int iEncIncrPerRevMot, - double dVelMeasFrqHz, - double dBeltRatio, - double dGearRatio, - int iSign, - double dVelMaxEncIncrS, - double dAccIncrS2, - double dDecIncrS2, - int iEncOffsetIncr, - bool bIsSteer, - double dCurrToTorque, - double dCurrMax, - int iHomingDigIn - */ - -/* - m_JointParams->setParam( //parameters taken from CanCtrl.ini - 0, //int iDriveIdent, - 4096,//int iEncIncrPerRevMot, - 1,//double dVelMeasFrqHz, - 1,//double dBeltRatio, - 47.77,//double dGearRatio, - -1.0,//int iSign, - 740000,//double dVelMaxEncIncrS, - 1000000,//80000,//double dAccIncrS2, - 1000000,//80000//double dDecIncrS2), - 0, //int iEncOffsetIncr - true, // bool bIsSteer - 0, // double dCurrToTorque - 0, // double dCurrMax - m_HomingDigIn // int iHomingDigIn //cob3-2->11, cob3-1->9 - ); -*/ - - m_JointParams->setParam( //parameters taken from CanCtrl.ini - 0, //int iDriveIdent, - m_EncIncrPerRevMot,//int iEncIncrPerRevMot, - 1,//double dVelMeasFrqHz, - 1,//double dBeltRatio, - m_GearRatio,//double dGearRatio, - m_MotorDirection,//int iSign, - 74000000,//double dVelMaxEncIncrS, - 1000000,//80000,//double dAccIncrS2, - 1000000,//80000//double dDecIncrS2), - 0, //int iEncOffsetIncr - true, // bool bIsSteer - 0, // double dCurrToTorque - 0, // double dm_HomingDigInCurrMax - m_HomingDigIn // int iHomingDigIn //cob3-2->11, cob3-1->9 - ); - - - m_Joint->setDriveParam(*m_JointParams); - } - - if (success) - { - printf("Init motor ...\n"); - success = m_Joint->init(); - if (!success) - { - printf("failed!\n"); - } - else - { - printf("successful\n"); - } - } - - if (success) - success = SetMotionCtrlType(m_MotionCtrlType); - if(!success) std::cout << "Failed to SetMotionCtrlType to " << m_MotionCtrlType << std::endl; - - if (success) - { - printf("Starting motor ..\n"); - success = m_Joint->start(); - if (!success) - { - printf("failed!\n"); - } - else - { - printf("successful\n"); - m_Joint->setGearVelRadS(0); - } - } - //ToDo: UHR: necessary? - Sleep(1000); - - if (success && home) - { - std::cout << "Start homing procedure now.." << std::endl; - success = Home(); - } - //Thread init - if (success) - { - pthread_mutex_init(&m_Mutex,NULL); - } - - - return success; -} - -bool ElmoCtrl::SetMotionCtrlType(int type) -{ - m_MotionCtrlType = type; - bool success = false; - if (type == POS_CTRL) - { - success = m_Joint->shutdown(); - if (success) - success = m_Joint->setTypeMotion(CanDriveHarmonica::MOTIONTYPE_POSCTRL); - - //ToDo: necessary? - Sleep(100); - success = m_Joint->start(); - - } - else if (type == VEL_CTRL) - { - //UHR: ToDo - printf("%s%d:Error: Velocity control not implemented yet!\n",__FILE__,__LINE__); - success = false; - } - return success; -}; - - -int ElmoCtrl::GetMotionCtrlType() -{ - return m_MotionCtrlType; -} - -bool ElmoCtrl::isError() -{ - return m_Joint->isError(); -} - -int ElmoCtrl::getGearPosVelRadS( double* pdAngleGearRad, double* pdVelGearRadS) -{ - - // init default outputs - *pdAngleGearRad = 0; - *pdVelGearRadS = 0; - - m_Joint->getGearPosVelRadS(pdAngleGearRad, pdVelGearRadS); - *pdAngleGearRad = *pdAngleGearRad - m_JointOffset; - - return 0; -} - -//----------------------------------------------- - -int ElmoCtrl:: setGearPosVelRadS(double dPosRad, double dVelRadS) -{ - - if(dPosRad< m_LowerLimit) { - std::cout << "Position under LowerBound -> set up" << std::endl; - dPosRad = m_LowerLimit; - } else if(dPosRad > m_UpperLimit) { - std::cout << "Position over UpperBound -> set down" << std::endl; - dPosRad = m_UpperLimit; - } - - if(dVelRadS > m_MaxVel) - dVelRadS = m_MaxVel; - else if(dVelRadS < -m_MaxVel) - dVelRadS = -m_MaxVel; - - //COB3-2: m_Joint->setGearPosVelRadS(dPosRad + m_JointOffset, dVelRadS); - - //COB3-1: m_Joint->setGearPosVelRadS(-3.141592654 - dPosRad + m_JointOffset, dVelRadS); - printf("ElmoCtrl:setGearPosVelRadS: dPosRad %f with vel %f\n",dPosRad, dVelRadS); - m_Joint->setGearPosVelRadS(m_HomingDir * dPosRad + m_JointOffset, dVelRadS); - return 0; -} - -bool ElmoCtrl::Stop() -{ - //UHR: ToDo: what happens exactly in this method? Sudden stop? - double pos = 0.0; - double vel = 0.0; - m_Joint->getGearPosVelRadS(&pos,&vel); - m_Joint->setGearPosVelRadS(pos,0); - - return true; - //m_Joint[i]->shutdown(); - -} diff --git a/cob_head_axis/package.xml b/cob_head_axis/package.xml deleted file mode 100644 index 1b3691ca0..000000000 --- a/cob_head_axis/package.xml +++ /dev/null @@ -1,30 +0,0 @@ - - cob_head_axis - 0.6.8 - cob_head_axis - - Apache 2.0 - - http://ros.org/wiki/cob_head_axis - - - Matthias Gruhler - Ulrich Reiser - - catkin - - actionlib - cob_canopen_motor - cob_generic_can - cob_srvs - cob_utilities - control_msgs - diagnostic_msgs - roscpp - sensor_msgs - std_srvs - urdf - - rospy - - diff --git a/cob_head_axis/ros/src/cob_head_axis.cpp b/cob_head_axis/ros/src/cob_head_axis.cpp deleted file mode 100644 index 293c62d4f..000000000 --- a/cob_head_axis/ros/src/cob_head_axis.cpp +++ /dev/null @@ -1,560 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -//################## -//#### includes #### - -// standard includes -//-- - -// ROS includes -#include -#include -#include - -// ROS message includes -#include -#include -#include -#include - -// ROS service includes -#include -#include -#include - -// external includes -#include - -#include - -//#################### -//#### node class #### -class NodeClass -{ - // - public: - // create a handle for this node, initialize node - ros::NodeHandle n_; - ros::NodeHandle n_private_; - - // declaration of topics to publish - ros::Publisher topicPub_JointState_; - ros::Publisher topicPub_ControllerState_; - ros::Publisher topicPub_Diagnostic_; - - // declaration of topics to subscribe, callback is called for new messages arriving - ros::Subscriber topicSub_JointCommand_; - - // declaration of service servers - ros::ServiceServer srvServer_Init_; - ros::ServiceServer srvServer_Stop_; - ros::ServiceServer srvServer_Recover_; - ros::ServiceServer srvServer_SetOperationMode_; - ros::ServiceServer srvServer_SetDefaultVel_; - - // declaration of service clients - //-- - - // action lib server - actionlib::SimpleActionServer as_; - std::string action_name_; - control_msgs::FollowJointTrajectoryFeedback feedback_; - control_msgs::FollowJointTrajectoryResult result_; - - // global variables - ElmoCtrl * CamAxis_; - ElmoCtrlParams* CamAxisParams_; - - std::string CanDevice_; - std::string CanIniFile_; - int CanBaudrate_; - int HomingDir_; - int HomingDigIn_; - double MaxVel_; - int ModID_; - std::string operationMode_; - double LowerLimit_; - double UpperLimit_; - double Offset_; - int MotorDirection_; - int EnoderIncrementsPerRevMot_; - double GearRatio_; - - std::string JointName_; - bool isInitialized_; - bool isError_; - bool finished_; - double ActualPos_; - double ActualVel_; - double GoalPos_; - trajectory_msgs::JointTrajectory traj_; - trajectory_msgs::JointTrajectoryPoint traj_point_; - unsigned int traj_point_nr_; - - // Constructor - NodeClass(): - as_(n_, "joint_trajectory_controller/follow_joint_trajectory", boost::bind(&NodeClass::executeCB, this, _1), false), - action_name_("follow_joint_trajectory") - { - n_private_ = ros::NodeHandle("~"); - - isInitialized_ = false; - isError_ = false; - ActualPos_=0.0; - ActualVel_=0.0; - - CamAxis_ = new ElmoCtrl(); - CamAxisParams_ = new ElmoCtrlParams(); - - // implementation of topics to publish - topicPub_JointState_ = n_.advertise("joint_states", 1); - topicPub_ControllerState_ = n_.advertise("joint_trajectory_controller/state", 1); - topicPub_Diagnostic_ = n_.advertise("diagnostics", 1); - - - // implementation of topics to subscribe - - // implementation of service servers - srvServer_Init_ = n_.advertiseService("driver/init", &NodeClass::srvCallback_Init, this); - srvServer_Stop_ = n_.advertiseService("driver/stop", &NodeClass::srvCallback_Stop, this); - srvServer_Recover_ = n_.advertiseService("driver/recover", &NodeClass::srvCallback_Recover, this); - srvServer_SetOperationMode_ = n_.advertiseService("driver/set_operation_mode", &NodeClass::srvCallback_SetOperationMode, this); - srvServer_SetDefaultVel_ = n_.advertiseService("driver/set_default_vel", &NodeClass::srvCallback_SetDefaultVel, this); - - // implementation of service clients - //-- - - // read parameters from parameter server - ROS_INFO("Namespace: %s", n_private_.getNamespace().c_str()); - if(!n_private_.hasParam("EnoderIncrementsPerRevMot")) ROS_WARN("cob_head_axis: couldn't find parameter EnoderIncrementsPerRevMot, check if ALL parameters have been set correctly"); - - n_private_.param("CanDevice", CanDevice_, "PCAN"); - n_private_.param("CanBaudrate", CanBaudrate_, 500); - n_private_.param("HomingDir", HomingDir_, 1); - n_private_.param("HomingDigIn", HomingDigIn_, 11); - n_private_.param("ModId",ModID_, 17); - n_private_.param("JointName",JointName_, "head_axis_joint"); - n_private_.param("CanIniFile",CanIniFile_, "/"); - n_private_.param("operation_mode",operationMode_, "position"); - n_private_.param("MotorDirection",MotorDirection_, 1); - n_private_.param("GearRatio",GearRatio_, 62.5); - n_private_.param("EnoderIncrementsPerRevMot",EnoderIncrementsPerRevMot_, 4096); - - ROS_INFO("CanDevice=%s, CanBaudrate=%d, ModID=%d, HomingDigIn=%d",CanDevice_.c_str(),CanBaudrate_,ModID_,HomingDigIn_); - - - // load parameter server string for robot/model - std::string xml_string; - n_.getParam("/robot_description",xml_string); - if (xml_string.size()==0) - { - ROS_ERROR("Unable to load robot model from param server robot_description\n"); - exit(2); - } - - // extract limits and velocitys from urdf model - urdf::Model model; - if (!model.initString(xml_string)) - { - ROS_ERROR("Failed to parse urdf file"); - exit(2); - } - ROS_INFO("Successfully parsed urdf file"); - - // get LowerLimits out of urdf model - LowerLimit_ = model.getJoint(JointName_.c_str())->limits->lower; - //std::cout << "LowerLimits[" << JointNames[i].c_str() << "] = " << LowerLimits[i] << std::endl; - CamAxisParams_->SetLowerLimit(LowerLimit_); - - // get UpperLimits out of urdf model - UpperLimit_ = model.getJoint(JointName_.c_str())->limits->upper; - //std::cout << "LowerLimits[" << JointNames[i].c_str() << "] = " << LowerLimits[i] << std::endl; - CamAxisParams_->SetUpperLimit(UpperLimit_); - - // get Offset out of urdf model - //calibration rising no longer supported - //Offset_ = model.getJoint(JointName_.c_str())->calibration->rising.get()[0]; - Offset_ = 0.0; - //std::cout << "Offset[" << JointNames[i].c_str() << "] = " << Offsets[i] << std::endl; - CamAxisParams_->SetAngleOffset(Offset_); - - // get velocitiy out of urdf model - MaxVel_ = model.getJoint(JointName_.c_str())->limits->velocity; - ROS_INFO("Successfully read limits from urdf"); - - - //initializing and homing of camera_axis - CamAxisParams_->SetCanIniFile(CanIniFile_); - CamAxisParams_->SetHomingDir(HomingDir_); - CamAxisParams_->SetHomingDigIn(HomingDigIn_); - CamAxisParams_->SetMaxVel(MaxVel_); - CamAxisParams_->SetGearRatio(GearRatio_); - CamAxisParams_->SetMotorDirection(MotorDirection_); - CamAxisParams_->SetEncoderIncrements(EnoderIncrementsPerRevMot_); - - CamAxisParams_->Init(CanDevice_, CanBaudrate_, ModID_); - - - //finally start action_server - as_.start(); - } - - // Destructor - ~NodeClass() - { - delete CamAxis_; - } - - void executeCB(const control_msgs::FollowJointTrajectoryGoalConstPtr &goal) { - if(isInitialized_) { - ROS_INFO("Received new goal trajectory with %lu points",goal->trajectory.points.size()); - // saving goal into local variables - traj_ = goal->trajectory; - traj_point_nr_ = 0; - traj_point_ = traj_.points[traj_point_nr_]; - GoalPos_ = traj_point_.positions[0]; - finished_ = false; - - // stoping axis to prepare for new trajectory - CamAxis_->Stop(); - - // check that preempt has not been requested by the client - if (as_.isPreemptRequested()) - { - ROS_INFO("%s: Preempted %s", n_.getNamespace().c_str(), action_name_.c_str()); - // set the action state to preempted - as_.setPreempted(); - } - - usleep(2000000); // needed sleep until drive starts to change status from idle to moving - - while (not finished_) - { - if (as_.isNewGoalAvailable()) - { - ROS_WARN("%s: Aborted %s", n_.getNamespace().c_str(), action_name_.c_str()); - as_.setAborted(); - return; - } - usleep(10000); - //feedback_ = - //as_.send feedback_ - } - - // set the action state to succeed - //result_.result.data = "executing trajectory"; - ROS_INFO("%s: Succeeded %s", n_.getNamespace().c_str(), action_name_.c_str()); - // set the action state to succeeded - as_.setSucceeded(result_); - - } else { - as_.setAborted(); - ROS_WARN("Camera_axis not initialized yet!"); - } - } - - // service callback functions - // function will be called when a service is querried - bool srvCallback_Init(std_srvs::Trigger::Request &req, - std_srvs::Trigger::Response &res ) - { - if (isInitialized_ == false) { - ROS_INFO("...initializing camera axis..."); - // init axis - if (CamAxis_->Init(CamAxisParams_)) - { - CamAxis_->setGearPosVelRadS(0.0f, MaxVel_); - ROS_INFO("Initializing of camera axis successfully"); - isInitialized_ = true; - res.success = true; - res.message = "initializing camera axis successfully"; - } - else - { - ROS_ERROR("Initializing camera axis not successfully \n"); - res.success = false; - res.message = "initializing camera axis not successfully"; - } - } - else - { - ROS_WARN("...camera axis already initialized..."); - res.success = true; - res.message = "camera axis already initialized"; - } - - return true; - } - - bool srvCallback_Stop(std_srvs::Trigger::Request &req, - std_srvs::Trigger::Response &res ) - { - ROS_INFO("Stopping camera axis"); - if(isInitialized_) - { - // stopping all movements - if (CamAxis_->Stop()) { - ROS_INFO("Stopping camera axis successfully"); - res.success = true; - res.message = "camera axis stopped successfully"; - } - else { - ROS_ERROR("Stopping camera axis not successfully. error"); - res.success = false; - res.message = "stopping camera axis not successfully"; - } - } - return true; - } - - bool srvCallback_Recover(std_srvs::Trigger::Request &req, - std_srvs::Trigger::Response &res ) - { - if (isInitialized_) { - ROS_INFO("Recovering camera axis"); - - // stopping all arm movements - if (CamAxis_->RecoverAfterEmergencyStop()) { - ROS_INFO("Recovering camera axis successfully"); - res.success = true; - res.message = "camera axis successfully recovered"; - } else { - ROS_ERROR("Recovering camera axis not successfully. error"); - res.success = false; - res.message = "recovering camera axis failed"; - } - } else { - ROS_WARN("...camera axis already recovered..."); - res.success = true; - res.message = "camera axis already recovered"; - } - - return true; - } - - /*! - * \brief Executes the service callback for set_operation_mode. - * - * Changes the operation mode. - * \param req Service request - * \param res Service response - */ - bool srvCallback_SetOperationMode( cob_srvs::SetString::Request &req, - cob_srvs::SetString::Response &res ) - { - ROS_INFO("Set operation mode to [%s]", req.data.c_str()); - n_.setParam("operation_mode", req.data.c_str()); - res.success = true; // 0 = true, else = false - return true; - } - - /*! - * \brief Executes the service callback for set_default_vel. - * - * Sets the default velocity. - * \param req Service request - * \param res Service response - */ - bool srvCallback_SetDefaultVel( cob_srvs::SetFloat::Request &req, - cob_srvs::SetFloat::Response &res ) - { - ROS_INFO("Set default velocity to [%f]", req.data); - MaxVel_ = req.data; - CamAxisParams_->SetMaxVel(MaxVel_); - CamAxis_->setMaxVelocity(MaxVel_); - res.success = true; // 0 = true, else = false - return true; - } - - // other function declarations - void updateCommands() - { - if (isInitialized_ == true) - { - if (operationMode_ == "position") - { - ROS_DEBUG("moving head_axis in position mode"); - - if (fabs(ActualVel_) < 0.02) - { - //feedback_.isMoving = false; - - ROS_DEBUG("next point is %d from %lu",traj_point_nr_,traj_.points.size()); - - if (traj_point_nr_ < traj_.points.size()) - { - // if axis is not moving and not reached last point of trajectory, then send new target point - ROS_INFO("...moving to trajectory point[%d], %f",traj_point_nr_,traj_.points[traj_point_nr_].positions[0]); - traj_point_ = traj_.points[traj_point_nr_]; - CamAxis_->setGearPosVelRadS(traj_point_.positions[0], MaxVel_); - usleep(900000); - CamAxis_->m_Joint->requestPosVel(); - traj_point_nr_++; - //feedback_.isMoving = true; - //feedback_.pointNr = traj_point_nr; - //as_.publishFeedback(feedback_); - } - else if ( fabs( fabs(ActualPos_) - fabs(GoalPos_) ) < 0.5*M_PI/180.0 && !finished_ ) - { - ROS_DEBUG("...reached end of trajectory"); - finished_ = true; - } - else - { - //do nothing until GoalPos_ is reached - } - } - else - { - ROS_DEBUG("...axis still moving to point[%d]",traj_point_nr_); - } - } - else if (operationMode_ == "velocity") - { - ROS_WARN("Moving in velocity mode currently disabled"); - } - else - { - ROS_ERROR("axis neither in position nor in velocity mode. OperationMode = [%s]", operationMode_.c_str()); - } - } - else - { - ROS_DEBUG("axis not initialized"); - } - } - - void publishJointState() - { - - if (isInitialized_ == true) { - isError_ = CamAxis_->isError(); - - // create message - int DOF = 1; - - CamAxis_->evalCanBuffer(); - CamAxis_->getGearPosVelRadS(&ActualPos_,&ActualVel_); - CamAxis_->m_Joint->requestPosVel(); - - // really bad hack - ActualPos_ = HomingDir_ * ActualPos_; - ActualVel_ = HomingDir_ * ActualVel_; - - sensor_msgs::JointState msg; - msg.header.stamp = ros::Time::now(); - msg.name.resize(DOF); - msg.position.resize(DOF); - msg.velocity.resize(DOF); - msg.effort.resize(DOF); - - msg.name[0] = JointName_; - msg.position[0] = ActualPos_; - msg.velocity[0] = ActualVel_; - msg.effort[0] = 0.0; - - - //std::cout << "Joint " << msg.name[0] <<": pos="<< msg.position[0] << " vel=" << msg.velocity[0] << std::endl; - - // publish message - topicPub_JointState_.publish(msg); - - // publish controller state message - control_msgs::JointTrajectoryControllerState controllermsg; - controllermsg.header = msg.header; - controllermsg.joint_names.resize(DOF); - controllermsg.desired.positions.resize(DOF); - controllermsg.desired.velocities.resize(DOF); - controllermsg.actual.positions.resize(DOF); - controllermsg.actual.velocities.resize(DOF); - controllermsg.error.positions.resize(DOF); - controllermsg.error.velocities.resize(DOF); - - controllermsg.joint_names = msg.name; - controllermsg.desired.positions = msg.position; - controllermsg.desired.velocities = msg.velocity; - controllermsg.actual.positions = msg.position; - controllermsg.actual.velocities = msg.velocity; - // error, calculated out of desired and actual values - for (int i = 0; i`_ 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) +------------------- +* fixed function return value +* Contributors: Benjamin Maidel + +0.6.9 (2017-07-18) +------------------ +* added test script + added test script + added test script + fix + fix + fix + added additional mode to test scripts + speed up test scripts +* protect callbacks with scoped locks +* void function +* manually fix changelog +* xmas mode corrections +* added new lightmode xmas +* Contributors: Benjamin Maidel, ipa-fxm + 0.6.8 (2016-10-10) ------------------ * return true for stop mode service to avoid ugly error messages on client side diff --git a/cob_light/package.xml b/cob_light/package.xml index a3e1a5e6e..0a47056f3 100644 --- a/cob_light/package.xml +++ b/cob_light/package.xml @@ -1,6 +1,6 @@ cob_light - 0.6.8 + 0.6.13 This package contains scripts to operate the LED lights on Care-O-bot. Apache 2.0 @@ -8,7 +8,8 @@ http://ros.org/wiki/cob_light - Benjamin Maidel + Felix Messmer + Benjamin Maidel Benjamin Maidel catkin diff --git a/cob_mimic/CHANGELOG.rst b/cob_mimic/CHANGELOG.rst index c12fdb8f6..0ff95d932 100644 --- a/cob_mimic/CHANGELOG.rst +++ b/cob_mimic/CHANGELOG.rst @@ -2,6 +2,72 @@ Changelog for package cob_mimic ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.6.13 (2019-03-14) +------------------- + +0.6.12 (2018-07-21) +------------------- +* update maintainer +* Merge pull request `#375 `_ from fmessmer/bulletproof_mimic + bulletproof mimic +* bulletproof mimic +* Merge pull request `#371 `_ from fmessmer/mimic_play_nondefault_mimics + allow to play non-default mimics by specifying full filepath +* allow to play non-default mimics by specifying full filepath +* Contributors: Felix Messmer, fmessmer, ipa-fxm + +0.6.11 (2018-01-07) +------------------- +* Merge remote-tracking branch 'origin/indigo_release_candidate' into indigo_dev +* Merge pull request `#359 `_ from ipa-bnm/fix/mimic + [cob_mimic] use glx/opengl output for mimic +* use glx/opengl output for mimic, fixes mimic issue for 6th and 7th gen nuc +* Merge pull request `#354 `_ from ipa-bnm/feature/mimic + [Mimic] improvements +* Merge pull request `#353 `_ from ipa-fxm/update_maintainer + update maintainer +* Merge pull request `#356 `_ from ipa-nhg/MimicPy + HotFix: readded python node for mimic +* remove duplicated test_mimic.py install tag +* use the old driver +* readded python node for mimic +* do not start blinking timer on sleeping or falling_asleep requests +* added random mimics +* double check username +* update maintainer +* Merge pull request `#341 `_ from ipa-fxm/APACHE_license + use license apache 2.0 +* Merge pull request `#352 `_ from ipa-bnm/feature/mimic_sim + Do not run mimic in fullscreen if sim is enabled +* use license apache 2.0 +* Merge branch 'indigo_dev' of github.com:ipa320/cob_driver into feature/mimic_sim +* no fullscreen if sim enabled +* Merge pull request `#345 `_ from ipa-fxm/fix_mimic_permission + guarantee unique copy destinations +* guarantee unique copy destinations +* Contributors: Benjamin Maidel, Felix Messmer, Florian Weisshardt, Nadia Hammoudeh García, ipa-fxm, ipa-nhg, ipa-uhr-mk + +0.6.10 (2017-07-24) +------------------- +* Merge branch 'indigo_dev' into indigo_release_candidate +* added apache header +* ported mimic from python to c++ +* Contributors: Benjamin Maidel, flg-pb + +0.6.9 (2017-07-18) +------------------ +* update license +* Delete ___init_\_.py +* Update package.xml +* remove vlc.py and add it as rosdep dependency (PR to rosdistro is https://github.com/ros/rosdistro/pull/15366) +* cleanup mimic node +* - removed hardcoded sleep time between transition between emotions. +* fix for the flickering in playback, caused when an emotion is set. +* fix cpu usage of the mimic node +* manually fix changelog +* mimic support the rotation of the face +* Contributors: Felix Messmer, Florian Weisshardt, fmw-ss, ipa-cob4-5, ipa-fxm, ipa-nhg, souravran + 0.6.8 (2016-10-10) ------------------ * vlc 2.2 version use by default the wrong video output diff --git a/cob_mimic/CMakeLists.txt b/cob_mimic/CMakeLists.txt index fc8645dbf..4ef70b9ca 100644 --- a/cob_mimic/CMakeLists.txt +++ b/cob_mimic/CMakeLists.txt @@ -42,3 +42,7 @@ install(PROGRAMS src/test_mimic.py install(DIRECTORY common DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) + +install(PROGRAMS src/mimic_node.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}/src +) diff --git a/cob_mimic/package.xml b/cob_mimic/package.xml index 128d51584..2ce6cea3d 100644 --- a/cob_mimic/package.xml +++ b/cob_mimic/package.xml @@ -1,13 +1,14 @@ cob_mimic - 0.6.8 + 0.6.13 This package implements the Care-O-bot mimic Apache 2.0 http://ros.org/wiki/cob_mimic - Nadia Hammoudeh Garcia + Felix Messmer + Benjamin Maidel Nadia Hammoudeh Garcia Benjamin Maidel diff --git a/cob_mimic/src/mimic_node.cpp b/cob_mimic/src/mimic_node.cpp index 0d0fb8a74..8e2bc4390 100644 --- a/cob_mimic/src/mimic_node.cpp +++ b/cob_mimic/src/mimic_node.cpp @@ -33,13 +33,14 @@ #include #include #include +#include class Mimic { public: Mimic(): as_mimic_(nh_, ros::this_node::getName() + "/set_mimic", boost::bind(&Mimic::as_cb_mimic_, this, _1), false), - new_mimic_request_(false), dist_(2,10), sim_enabled_(false) + new_mimic_request_(false), sim_enabled_(false), real_dist_(2,10), int_dist_(0,6) { nh_ = ros::NodeHandle("~"); } @@ -59,6 +60,14 @@ class Mimic sim_enabled_ = nh_.param("sim", false); srvServer_mimic_ = nh_.advertiseService("set_mimic", &Mimic::service_cb_mimic, this); + random_mimics_.push_back("blinking"); + random_mimics_.push_back("blinking"); + random_mimics_.push_back("blinking"); + random_mimics_.push_back("blinking_left"); + random_mimics_.push_back("blinking_right"); + + int_dist_ = boost::random::uniform_int_distribution<>(0,static_cast(random_mimics_.size())-1); + char const *argv[] = { "--ignore-config", @@ -70,16 +79,19 @@ class Mimic "--playlist-enqueue", "--no-video-title-show", "--no-skip-frames", - "--no-audio" + "--no-audio", + "--vout=glx,none" }; int argc = sizeof( argv ) / sizeof( *argv ); vlc_inst_ = libvlc_new(argc, argv); + if(!vlc_inst_){ROS_ERROR("failed to create libvlc instance"); return false;} vlc_player_ = libvlc_media_player_new(vlc_inst_); - if(!sim_enabled_) - libvlc_set_fullscreen(vlc_player_, 1); + if(!vlc_player_){ROS_ERROR("failed to create vlc media player object"); return false;} + + if(!sim_enabled_){libvlc_set_fullscreen(vlc_player_, 1);} set_mimic("default", 1, 1.0, false); - blinking_timer_ = nh_.createTimer(ros::Duration(dist_(gen_)), &Mimic::blinking_cb, this, true); + blinking_timer_ = nh_.createTimer(ros::Duration(real_dist_(gen_)), &Mimic::blinking_cb, this, true); as_mimic_.start(); return true; } @@ -100,15 +112,21 @@ class Mimic boost::mutex mutex_; boost::random::mt19937 gen_; - boost::random::uniform_real_distribution<> dist_; + boost::random::uniform_real_distribution<> real_dist_; + boost::random::uniform_int_distribution<> int_dist_; + std::vector random_mimics_; bool copy_mimic_files() { char *lgn; if((lgn = getlogin()) == NULL) { - ROS_ERROR("unable to get user name"); - return false; + lgn = getenv("USER"); + if(lgn == NULL || std::string(lgn) == "") + { + ROS_ERROR("unable to get user name"); + return false; + } } std::string username(lgn); mimic_folder_ = "/tmp/mimic_" + username; @@ -149,7 +167,8 @@ class Mimic else as_mimic_.setAborted(); - blinking_timer_ = nh_.createTimer(ros::Duration(dist_(gen_)), &Mimic::blinking_cb, this, true); + if(goal->mimic != "falling_asleep" && goal->mimic != "sleeping") + blinking_timer_ = nh_.createTimer(ros::Duration(real_dist_(gen_)), &Mimic::blinking_cb, this, true); } bool service_cb_mimic(cob_mimic::SetMimic::Request &req, @@ -160,7 +179,8 @@ class Mimic res.success = set_mimic(req.mimic, req.repeat, req.speed); res.message = ""; - blinking_timer_ = nh_.createTimer(ros::Duration(dist_(gen_)), &Mimic::blinking_cb, this, true); + if(req.mimic != "falling_asleep" && req.mimic != "sleeping") + blinking_timer_ = nh_.createTimer(ros::Duration(real_dist_(gen_)), &Mimic::blinking_cb, this, true); return true; } @@ -178,9 +198,17 @@ class Mimic // check if mimic exists if ( !boost::filesystem::exists(filename) ) { - ROS_ERROR("File not found: %s", filename.c_str()); - mutex_.unlock(); - return false; + if ( !boost::filesystem::exists(mimic) ) + { + ROS_ERROR("File not found: %s", filename.c_str()); + mutex_.unlock(); + return false; + } + else + { + ROS_INFO("Playing mimic from non-default file: %s", mimic.c_str()); + filename = mimic; + } } // repeat cannot be 0 @@ -193,31 +221,43 @@ class Mimic speed = 1.0; } - libvlc_media_player_set_rate(vlc_player_, speed); + // returns -1 if an error was detected, 0 otherwise (but even then, it might not actually work depending on the underlying media protocol) + if(libvlc_media_player_set_rate(vlc_player_, speed)!=0){ROS_ERROR("failed to set movie play rate");} while(repeat > 0) { vlc_media_ = libvlc_media_new_path(vlc_inst_, filename.c_str()); + if(!vlc_media_) + { + ROS_ERROR("failed to create media for filepath %s", filename.c_str()); + mutex_.unlock(); + return false; + } + + libvlc_media_player_set_media(vlc_player_, vlc_media_); + libvlc_media_release(vlc_media_); - if (vlc_media_ != NULL) + // returns 0 if playback started (and was already started), or -1 on error. + if(libvlc_media_player_play(vlc_player_)!=0) + { + ROS_ERROR("failed to play"); + mutex_.unlock(); + return false; + } + + ros::Duration(0.1).sleep(); + while(blocking && (libvlc_media_player_is_playing(vlc_player_) == 1)) { - libvlc_media_player_set_media(vlc_player_, vlc_media_); - libvlc_media_release(vlc_media_); - libvlc_media_player_play(vlc_player_); ros::Duration(0.1).sleep(); - while(blocking && (libvlc_media_player_is_playing(vlc_player_) == 1)) + ROS_DEBUG("still playing %s", mimic.c_str()); + if(new_mimic_request_) { - ros::Duration(0.1).sleep(); - ROS_DEBUG("still playing %s", mimic.c_str()); - if(new_mimic_request_) - { - ROS_WARN("mimic %s preempted", mimic.c_str()); - mutex_.unlock(); - return false; - } + ROS_WARN("mimic %s preempted", mimic.c_str()); + mutex_.unlock(); + return false; } - repeat --; } + repeat --; } mutex_.unlock(); return true; @@ -225,8 +265,9 @@ class Mimic void blinking_cb(const ros::TimerEvent&) { - set_mimic("blinking", 1, 1.5); - blinking_timer_ = nh_.createTimer(ros::Duration(dist_(gen_)), &Mimic::blinking_cb, this, true); + int rand = int_dist_(gen_); + set_mimic(random_mimics_[rand], 1, 1.5); + blinking_timer_ = nh_.createTimer(ros::Duration(real_dist_(gen_)), &Mimic::blinking_cb, this, true); } bool copy_dir( boost::filesystem::path const & source, @@ -239,7 +280,6 @@ class Mimic if(!fs::exists(source) || !fs::is_directory(source)) { ROS_ERROR_STREAM("Source directory " << source.string() << " does not exist or is not a directory."); - ; return false; } if(fs::exists(mimic_folder)) diff --git a/cob_mimic/src/mimic_node.py b/cob_mimic/src/mimic_node.py new file mode 100755 index 000000000..b47a27d7b --- /dev/null +++ b/cob_mimic/src/mimic_node.py @@ -0,0 +1,94 @@ +#!/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 os +import vlc +import threading + +import roslib +import rospy +import actionlib + +from cob_mimic.srv import * +from cob_mimic.msg import * + +class Mimic: + def service_cb(self, req): + success = self.set_mimic(req.mimic, req.speed, req.repeat) + return SetMimicResponse(success, "") + + def action_cb(self, goal): + if self.set_mimic(goal.mimic, goal.speed, goal.repeat): + self._as.set_succeeded() + else: + self._as.set_aborted() + + def set_mimic(self, mimic, speed, repeat): + rospy.loginfo("Mimic: %s", mimic) + file_location = '/tmp/mimic/' + mimic + '.mp4' + if(not os.path.isfile(file_location)): + rospy.logerror("File not found: %s", file_location) + return False + + # repeat cannot be 0 + repeat = max (1, repeat) + + for i in range(0, repeat): + rospy.loginfo("Repeat: %s, Mimic: %s", repeat, mimic) + command = "export DISPLAY=:0 && vlc --video-wallpaper --video-filter 'rotate{angle=%d}' --vout glx --one-instance --playlist-enqueue --no-video-title-show --rate %f %s vlc://quit" % (self.rotation, speed, file_location) + os.system(command) + + return True + + def defaultMimic(self): + file_location = '/tmp/mimic/' + self.default_mimic + '.mp4' + if not os.path.isfile(file_location): + rospy.logerror("File not found: %s", file_location) + return + + while not rospy.is_shutdown(): + command = "export DISPLAY=:0 && vlc --video-wallpaper --video-filter 'rotate{angle=%d}' --vout glx --loop --one-instance --playlist-enqueue --no-video-title-show --rate %f %s vlc://quit" % (self.rotation, self.default_speed, file_location) + os.system(command) + + def main(self): + self.default_speed = 1.0 + self.default_mimic = "default" + self.rotation = rospy.get_param('~rotation', 0) + + # copy all videos to /tmp + rospy.loginfo("copying all mimic files to /tmp/mimic...") + file_location = roslib.packages.get_pkg_dir('cob_mimic') + '/common/*.mp4' + os.system("mkdir -p /tmp/mimic") + os.system("cp " + file_location + " /tmp/mimic") + rospy.loginfo("...copied all mimic files to /tmp/mimic") + + self._ss = rospy.Service('~set_mimic', SetMimic, self.service_cb) + self._as = actionlib.SimpleActionServer('~set_mimic', cob_mimic.msg.SetMimicAction, execute_cb=self.action_cb, auto_start = False) + self._as.start() + + rospy.spin() + + + +if __name__ == "__main__": + rospy.init_node('mimic') + try: + mimic = Mimic() + mimic.main() + except (rospy.ROSInterruptException, KeyboardInterrupt, SystemExit) as e: + rospy.loginfo('Exiting: ' + str(e)) + pass + diff --git a/cob_phidget_em_state/CHANGELOG.rst b/cob_phidget_em_state/CHANGELOG.rst index cbd42ca99..08a101d35 100644 --- a/cob_phidget_em_state/CHANGELOG.rst +++ b/cob_phidget_em_state/CHANGELOG.rst @@ -2,6 +2,30 @@ Changelog for package cob_phidget_em_state ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +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 diff --git a/cob_phidget_em_state/package.xml b/cob_phidget_em_state/package.xml index 954d98acc..e6689cd93 100644 --- a/cob_phidget_em_state/package.xml +++ b/cob_phidget_em_state/package.xml @@ -1,10 +1,10 @@ cob_phidget_em_state - 0.6.8 + 0.6.13 The cob_phidget_em_state package - Benjamin Maidel + Benjamin Maidel Benjamin Maidel Apache 2.0 diff --git a/cob_phidget_power_state/CHANGELOG.rst b/cob_phidget_power_state/CHANGELOG.rst index 9c76d9304..3c8cb6737 100644 --- a/cob_phidget_power_state/CHANGELOG.rst +++ b/cob_phidget_power_state/CHANGELOG.rst @@ -2,6 +2,32 @@ Changelog for package cob_phidget_power_state ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +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 diff --git a/cob_phidget_power_state/package.xml b/cob_phidget_power_state/package.xml index 51e4e95df..d66a3ffa2 100644 --- a/cob_phidget_power_state/package.xml +++ b/cob_phidget_power_state/package.xml @@ -1,10 +1,10 @@ cob_phidget_power_state - 0.6.8 + 0.6.13 The cob_phidget_power_state package - Benjamin Maidel + Benjamin Maidel Benjamin Maidel Apache 2.0 diff --git a/cob_phidgets/CHANGELOG.rst b/cob_phidgets/CHANGELOG.rst index 4d7a1d381..da41e70c8 100644 --- a/cob_phidgets/CHANGELOG.rst +++ b/cob_phidgets/CHANGELOG.rst @@ -2,6 +2,36 @@ Changelog for package cob_phidgets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.6.13 (2019-03-14) +------------------- +* Merge pull request `#386 `_ from benmaidel/fix/check_sensor_value + [cob_phidgets] srv callback check if digital sensor is already at desired state +* fix indentation +* on srv callback check if digital sensor is already at desired state +* Contributors: Benjamin Maidel, Felix Messmer, fmessmer + +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) +------------------ +* use usleep +* manually fix changelog +* Contributors: ipa-fxm + 0.6.8 (2016-10-10) ------------------ diff --git a/cob_phidgets/package.xml b/cob_phidgets/package.xml index ece93ad3f..f7fe56f9b 100644 --- a/cob_phidgets/package.xml +++ b/cob_phidgets/package.xml @@ -1,6 +1,6 @@ cob_phidgets - 0.6.8 + 0.6.13 cob_phidgets Apache 2.0 @@ -8,7 +8,7 @@ http://ros.org/wiki/cob_phidgets - Benjamin Maidel + Benjamin Maidel Florian Weisshardt catkin diff --git a/cob_phidgets/ros/src/phidgetik_ros.cpp b/cob_phidgets/ros/src/phidgetik_ros.cpp index 47300f1a7..b81e701e0 100644 --- a/cob_phidgets/ros/src/phidgetik_ros.cpp +++ b/cob_phidgets/ros/src/phidgetik_ros.cpp @@ -297,28 +297,38 @@ auto PhidgetIKROS::setDigitalOutCallback(cob_phidgets::SetDigitalSensor::Request _indexNameMapRevItr = _indexNameMapDigitalOutRev.find(req.uri); if(_indexNameMapRevItr != _indexNameMapDigitalOutRev.end()) { - ROS_INFO("Setting digital output %i to state %i", _indexNameMapDigitalOutRev[req.uri], req.state); - this->setOutputState(_indexNameMapDigitalOutRev[req.uri], req.state); - - ros::Time start = ros::Time::now(); - while((ros::Time::now().toSec() - start.toSec()) < 1.0) + if(this->getOutputState(_indexNameMapDigitalOutRev[req.uri]) == req.state) { - _mutex.lock(); - if(_outputChanged.updated == true) + ROS_INFO("Digital output %i is already at state %i", _indexNameMapDigitalOutRev[req.uri], req.state); + res.uri = req.uri; + res.state = req.state; + ret = true; + } + else + { + ROS_INFO("Setting digital output %i to state %i", _indexNameMapDigitalOutRev[req.uri], req.state); + this->setOutputState(_indexNameMapDigitalOutRev[req.uri], req.state); + + ros::Time start = ros::Time::now(); + while((ros::Time::now().toSec() - start.toSec()) < 1.0) { + _mutex.lock(); + if(_outputChanged.updated == true) + { + _mutex.unlock(); + break; + } _mutex.unlock(); - break; + + ros::Duration(0.025).sleep(); } + _mutex.lock(); + res.uri = _indexNameMapDigitalOut[_outputChanged.index]; + res.state = _outputChanged.state; + ROS_DEBUG("Sending response: updated: %u, index: %d, state: %d",_outputChanged.updated, _outputChanged.index, _outputChanged.state); + ret = (_outputChanged.updated && (_outputChanged.index == _indexNameMapDigitalOutRev[req.uri])); _mutex.unlock(); - - ros::Duration(0.025).sleep(); } - _mutex.lock(); - res.uri = _indexNameMapDigitalOut[_outputChanged.index]; - res.state = _outputChanged.state; - ROS_DEBUG("Sending response: updated: %u, index: %d, state: %d",_outputChanged.updated, _outputChanged.index, _outputChanged.state); - ret = (_outputChanged.updated && (_outputChanged.index == _indexNameMapDigitalOutRev[req.uri])); - _mutex.unlock(); } else { @@ -406,10 +416,10 @@ auto PhidgetIKROS::attachHandler() -> int auto PhidgetIKROS::detachHandler() -> int { int serial_number; - const char *device_name; + const char *device_name; - CPhidget_getDeviceName ((CPhidgetHandle)_iKitHandle, &device_name); - CPhidget_getSerialNumber((CPhidgetHandle)_iKitHandle, &serial_number); - ROS_INFO("%s Serial number %d detached!", device_name, serial_number); - return 0; + CPhidget_getDeviceName ((CPhidgetHandle)_iKitHandle, &device_name); + CPhidget_getSerialNumber((CPhidgetHandle)_iKitHandle, &serial_number); + ROS_INFO("%s Serial number %d detached!", device_name, serial_number); + return 0; } diff --git a/cob_relayboard/CHANGELOG.rst b/cob_relayboard/CHANGELOG.rst index 5d9cd7193..a36ae47cb 100644 --- a/cob_relayboard/CHANGELOG.rst +++ b/cob_relayboard/CHANGELOG.rst @@ -2,6 +2,28 @@ Changelog for package cob_relayboard ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +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) ------------------ diff --git a/cob_relayboard/package.xml b/cob_relayboard/package.xml index b4986d8c3..34d1d61a8 100644 --- a/cob_relayboard/package.xml +++ b/cob_relayboard/package.xml @@ -1,6 +1,6 @@ cob_relayboard - 0.6.8 + 0.6.13 cob_relayboard Apache 2.0 diff --git a/cob_scan_unifier/CHANGELOG.rst b/cob_scan_unifier/CHANGELOG.rst index a5d390069..996f4381f 100644 --- a/cob_scan_unifier/CHANGELOG.rst +++ b/cob_scan_unifier/CHANGELOG.rst @@ -2,6 +2,46 @@ Changelog for package cob_scan_unifier ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.6.13 (2019-03-14) +------------------- +* Merge pull request `#385 `_ from fmessmer/scan_unifier_range_initialization + initialize ranges with max_range rather than zero +* add explanatory comment +* initialize ranges with max_range rather than zero +* Contributors: Felix Messmer, fmessmer + +0.6.12 (2018-07-21) +------------------- +* update maintainer +* Merge pull request `#366 `_ from ipa-bnm/feature/scan_unifier + merge up to 4 laserscans +* merge up to 4 laserscans +* Contributors: Benjamin Maidel, Richard Bormann, fmessmer + +0.6.11 (2018-01-07) +------------------- +* Merge remote-tracking branch 'origin/indigo_release_candidate' into indigo_dev +* Merge pull request `#353 `_ from ipa-fxm/update_maintainer + update maintainer +* update maintainer +* 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) +------------------ +* remove commented line +* Added sleep in constructor, new topic parameter parsing, better error handling. +* Some small fixes +* Cleanup +* Use message_filter::Synchronizer (there is still a bug) +* manually fix changelog +* Contributors: Elias Marks, Matthias Gruhler, ipa-fxm + 0.6.8 (2016-10-10) ------------------ diff --git a/cob_scan_unifier/CMakeLists.txt b/cob_scan_unifier/CMakeLists.txt index b7dc0eb9c..7f8e61ac1 100644 --- a/cob_scan_unifier/CMakeLists.txt +++ b/cob_scan_unifier/CMakeLists.txt @@ -5,7 +5,8 @@ find_package(catkin REQUIRED COMPONENTS laser_geometry roscpp sensor_msgs - tf + tf2 + tf2_ros ) catkin_package() diff --git a/cob_scan_unifier/include/cob_scan_unifier/scan_unifier_node.h b/cob_scan_unifier/include/cob_scan_unifier/scan_unifier_node.h index 62579266f..bdaefac67 100644 --- a/cob_scan_unifier/include/cob_scan_unifier/scan_unifier_node.h +++ b/cob_scan_unifier/include/cob_scan_unifier/scan_unifier_node.h @@ -28,17 +28,19 @@ // ROS includes #include -#include -#include -#include #include +#include #include #include #include #include +//includes tf2_ros +#include + // ROS message includes #include +#include //#################### @@ -66,11 +68,25 @@ class ScanUnifierNode std::vector* > message_filter_subscribers_; - message_filters::Synchronizer >* synchronizer2_; - message_filters::Synchronizer >* synchronizer3_; - - void messageFilterCallback(const sensor_msgs::LaserScan::ConstPtr& first_scanner, const sensor_msgs::LaserScan::ConstPtr& second_scanner); - void messageFilterCallback(const sensor_msgs::LaserScan::ConstPtr& first_scanner, const sensor_msgs::LaserScan::ConstPtr& second_scanner, const sensor_msgs::LaserScan::ConstPtr& third_scanner); + message_filters::Synchronizer >* synchronizer2_; + message_filters::Synchronizer >* synchronizer3_; + message_filters::Synchronizer >* synchronizer4_; + + void messageFilterCallback(const sensor_msgs::LaserScan::ConstPtr& first_scanner, + const sensor_msgs::LaserScan::ConstPtr& second_scanner); + void messageFilterCallback(const sensor_msgs::LaserScan::ConstPtr& first_scanner, + const sensor_msgs::LaserScan::ConstPtr& second_scanner, + const sensor_msgs::LaserScan::ConstPtr& third_scanner); + void messageFilterCallback(const sensor_msgs::LaserScan::ConstPtr& first_scanner, + const sensor_msgs::LaserScan::ConstPtr& second_scanner, + const sensor_msgs::LaserScan::ConstPtr& third_scanner, + const sensor_msgs::LaserScan::ConstPtr& fourth_scanner); public: @@ -90,8 +106,9 @@ class ScanUnifierNode // declaration of ros publishers ros::Publisher topicPub_LaserUnified_; - // tf listener - tf::TransformListener listener_; + tf2_ros::Buffer *p_tfBuffer; + tf2_ros::TransformListener *p_tfListener; + uint _num_transform_errors; // laser geometry projector laser_geometry::LaserProjection projector_; diff --git a/cob_scan_unifier/package.xml b/cob_scan_unifier/package.xml index 5e0c91ce3..b12379965 100644 --- a/cob_scan_unifier/package.xml +++ b/cob_scan_unifier/package.xml @@ -1,15 +1,15 @@ cob_scan_unifier - 0.6.8 + 0.6.13 The cob_scan_unifier package holds code to unify two or more laser-scans to one unified scan-message - Benjamin Maidel + Felix Messmer + Benjamin Maidel + Florian Mirus Apache 2.0 - Florian Mirus - catkin laser_geometry diff --git a/cob_scan_unifier/src/scan_unifier_node.cpp b/cob_scan_unifier/src/scan_unifier_node.cpp index 198a5248b..4b39110c7 100644 --- a/cob_scan_unifier/src/scan_unifier_node.cpp +++ b/cob_scan_unifier/src/scan_unifier_node.cpp @@ -26,10 +26,18 @@ ScanUnifierNode::ScanUnifierNode() nh_ = ros::NodeHandle(); pnh_ = ros::NodeHandle("~"); + // Init TF-Buffer + p_tfBuffer = new tf2_ros::Buffer(); + p_tfListener = new tf2_ros::TransformListener(*p_tfBuffer, true); + // Publisher topicPub_LaserUnified_ = nh_.advertise("scan_unified", 1); getParams(); + + synchronizer2_ = NULL; + synchronizer3_ = NULL; + synchronizer4_ = NULL; // Subscribe to Laserscan topics @@ -46,7 +54,8 @@ ScanUnifierNode::ScanUnifierNode() case 2: { typedef message_filters::sync_policies::ApproximateTime SyncPolicy; - synchronizer2_ = new message_filters::Synchronizer(SyncPolicy(2), *message_filter_subscribers_.at(0), *message_filter_subscribers_.at(1)); + synchronizer2_ = new message_filters::Synchronizer(SyncPolicy(2), *message_filter_subscribers_.at(0), + *message_filter_subscribers_.at(1)); // Set the InterMessageLowerBound to double the period of the laser scans publishing ( 1/{(1/2)*f_laserscans} ). synchronizer2_->setInterMessageLowerBound(0, ros::Duration(0.167)); synchronizer2_->setInterMessageLowerBound(1, ros::Duration(0.167)); @@ -56,13 +65,29 @@ ScanUnifierNode::ScanUnifierNode() case 3: { typedef message_filters::sync_policies::ApproximateTime SyncPolicy; - synchronizer3_ = new message_filters::Synchronizer(SyncPolicy(3), *message_filter_subscribers_.at(0), *message_filter_subscribers_.at(1), *message_filter_subscribers_.at(2)); + synchronizer3_ = new message_filters::Synchronizer(SyncPolicy(3), *message_filter_subscribers_.at(0), + *message_filter_subscribers_.at(1), + *message_filter_subscribers_.at(2)); synchronizer3_->setInterMessageLowerBound(0, ros::Duration(0.167)); synchronizer3_->setInterMessageLowerBound(1, ros::Duration(0.167)); synchronizer3_->setInterMessageLowerBound(2, ros::Duration(0.167)); synchronizer3_->registerCallback(boost::bind(&ScanUnifierNode::messageFilterCallback, this, _1, _2, _3)); break; } + case 4: + { + typedef message_filters::sync_policies::ApproximateTime SyncPolicy; + synchronizer4_ = new message_filters::Synchronizer(SyncPolicy(4), *message_filter_subscribers_.at(0), + *message_filter_subscribers_.at(1), + *message_filter_subscribers_.at(2), + *message_filter_subscribers_.at(3)); + synchronizer4_->setInterMessageLowerBound(0, ros::Duration(0.167)); + synchronizer4_->setInterMessageLowerBound(1, ros::Duration(0.167)); + synchronizer4_->setInterMessageLowerBound(2, ros::Duration(0.167)); + synchronizer4_->setInterMessageLowerBound(3, ros::Duration(0.167)); + synchronizer4_->registerCallback(boost::bind(&ScanUnifierNode::messageFilterCallback, this, _1, _2, _3, _4)); + break; + } default: ROS_ERROR_STREAM(config_.number_input_scans << " topics have been set as input, but scan_unifier doesn't support this."); return; @@ -75,8 +100,12 @@ ScanUnifierNode::ScanUnifierNode() ScanUnifierNode::~ScanUnifierNode() { - delete(synchronizer2_); - delete(synchronizer3_); + if(synchronizer2_ != NULL) + delete(synchronizer2_); + if(synchronizer3_ != NULL) + delete(synchronizer3_); + if(synchronizer4_ != NULL) + delete(synchronizer4_); } /** @@ -125,7 +154,9 @@ void ScanUnifierNode::messageFilterCallback(const sensor_msgs::LaserScan::ConstP topicPub_LaserUnified_.publish(unified_scan); } -void ScanUnifierNode::messageFilterCallback(const sensor_msgs::LaserScan::ConstPtr& scan1, const sensor_msgs::LaserScan::ConstPtr& scan2, const sensor_msgs::LaserScan::ConstPtr& scan3) +void ScanUnifierNode::messageFilterCallback(const sensor_msgs::LaserScan::ConstPtr& scan1, + const sensor_msgs::LaserScan::ConstPtr& scan2, + const sensor_msgs::LaserScan::ConstPtr& scan3) { std::vector current_scans; current_scans.push_back(scan1); @@ -142,6 +173,27 @@ void ScanUnifierNode::messageFilterCallback(const sensor_msgs::LaserScan::ConstP topicPub_LaserUnified_.publish(unified_scan); } +void ScanUnifierNode::messageFilterCallback(const sensor_msgs::LaserScan::ConstPtr& scan1, + const sensor_msgs::LaserScan::ConstPtr& scan2, + const sensor_msgs::LaserScan::ConstPtr& scan3, + const sensor_msgs::LaserScan::ConstPtr& scan4) +{ + std::vector current_scans; + current_scans.push_back(scan1); + current_scans.push_back(scan2); + current_scans.push_back(scan3); + current_scans.push_back(scan4); + + sensor_msgs::LaserScan unified_scan = sensor_msgs::LaserScan(); + if (!unifyLaserScans(current_scans, unified_scan)) + { + return; + } + + ROS_DEBUG("Publishing unified scan."); + topicPub_LaserUnified_.publish(unified_scan); +} + /** * @function unifyLaserScans * @brief unifie the scan information from all laser scans in vec_laser_struct_ @@ -152,7 +204,10 @@ void ScanUnifierNode::messageFilterCallback(const sensor_msgs::LaserScan::ConstP */ bool ScanUnifierNode::unifyLaserScans(std::vector current_scans, sensor_msgs::LaserScan &unified_scan) { + std::vector vec_cloud2; std::vector vec_cloud; + geometry_msgs::TransformStamped transform; + vec_cloud2.assign(config_.number_input_scans, sensor_msgs::PointCloud2()); vec_cloud.assign(config_.number_input_scans, sensor_msgs::PointCloud()); if(!current_scans.empty()) @@ -163,20 +218,21 @@ bool ScanUnifierNode::unifyLaserScans(std::vectorheader.stamp; ROS_DEBUG_STREAM("Converting scans to point clouds at index: " << i << ", at time: " << current_scans.at(i)->header.stamp << " now: " << ros::Time::now()); try - { - if (!listener_.waitForTransform(frame_, current_scans.at(i)->header.frame_id, - current_scans.at(i)->header.stamp, ros::Duration(1.0))) - { - ROS_WARN_STREAM("Scan unifier skipped scan with " << current_scans.at(i)->header.stamp << " stamp, because of missing tf transform."); - return false; + { + transform = p_tfBuffer->lookupTransform(frame_, current_scans.at(i)->header.frame_id, current_scans.at(i)->header.stamp, ros::Duration(1.0)); + _num_transform_errors = 0; + } + catch (tf2::TransformException ex) + { + if (_num_transform_errors%100 == 0){ + ROS_ERROR("%s", ex.what()); } - - ROS_DEBUG("now project to point_cloud"); - projector_.transformLaserScanToPointCloud(frame_,*current_scans.at(i), vec_cloud.at(i), listener_); - } - catch(tf::TransformException &ex){ - ROS_ERROR("%s",ex.what()); - } + _num_transform_errors++; + return false; + } + ROS_DEBUG("now project to point_cloud"); + projector_.transformLaserScanToPointCloud(frame_,*current_scans.at(i), vec_cloud2.at(i), *p_tfBuffer); + if(!sensor_msgs::convertPointCloud2ToPointCloud(vec_cloud2.at(i),vec_cloud.at(i))) return false; } ROS_DEBUG("Creating message header"); unified_scan.header = current_scans.at(0)->header; @@ -188,8 +244,11 @@ bool ScanUnifierNode::unifyLaserScans(std::vectorscan_time; unified_scan.range_min = current_scans.at(0)->range_min; unified_scan.range_max = current_scans.at(0)->range_max; - unified_scan.ranges.resize(round((unified_scan.angle_max - unified_scan.angle_min) / unified_scan.angle_increment) + 1); - unified_scan.intensities.resize(round((unified_scan.angle_max - unified_scan.angle_min) / unified_scan.angle_increment) + 1); + //default values (ranges: range_max, intensities: 0) are used to better reflect the driver behavior + //there "phantom" data has values > range_max + //but those values are removed during projection to pointcloud + unified_scan.ranges.resize(round((unified_scan.angle_max - unified_scan.angle_min) / unified_scan.angle_increment) + 1, unified_scan.range_max); + unified_scan.intensities.resize(round((unified_scan.angle_max - unified_scan.angle_min) / unified_scan.angle_increment) + 1, 0.0); // now unify all Scans ROS_DEBUG("unify scans"); @@ -217,7 +276,7 @@ bool ScanUnifierNode::unifyLaserScans(std::vector`_ 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 diff --git a/cob_sick_lms1xx/package.xml b/cob_sick_lms1xx/package.xml index c72234323..67b045c37 100644 --- a/cob_sick_lms1xx/package.xml +++ b/cob_sick_lms1xx/package.xml @@ -1,6 +1,6 @@ cob_sick_lms1xx - 0.6.8 + 0.6.13 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. diff --git a/cob_sick_s300/CHANGELOG.rst b/cob_sick_s300/CHANGELOG.rst index e286a4587..243020fe0 100644 --- a/cob_sick_s300/CHANGELOG.rst +++ b/cob_sick_s300/CHANGELOG.rst @@ -2,6 +2,51 @@ Changelog for package cob_sick_s300 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.6.13 (2019-03-14) +------------------- +* Merge pull request `#378 `_ from mateuszcierpikowski/feature/auto_reconnect + add auto reconnect when scanner is disconnected +* add auto reconnect when scanner is disconnected +* Contributors: Felix Messmer, mateuszcierpikowski + +0.6.12 (2018-07-21) +------------------- +* update maintainer +* Merge pull request `#377 `_ from fmessmer/fix_sicks300_rate + remove faulty publish_rate mechanism +* remove faulty publish_rate mechanism +* Contributors: Felix Messmer, fmessmer, ipa-fxm + +0.6.11 (2018-01-07) +------------------- +* Merge remote-tracking branch 'origin/indigo_release_candidate' into indigo_dev +* Merge pull request `#358 `_ from ipa-jba/fix/s300-max-range + [cob_sick_s300] adjust range_max +* Merge pull request `#357 `_ from ipa-jba/fix/remove-dead-code + [cob_sick_s300] remove dead code +* remove dead code +* Merge pull request `#341 `_ from ipa-fxm/APACHE_license + use license apache 2.0 +* use license apache 2.0 +* cob_sick_s300: adjust range_max +* Contributors: Felix Messmer, ipa-fxm, ipa-mig, ipa-uhr-mk + +0.6.10 (2017-07-24) +------------------- + +0.6.9 (2017-07-18) +------------------ +* [cob_sick_s300] fix typo in Readme +* cob_sick_s300: clarify some points about the measurement range fields in the readme +* cob_sick_s300: add readme +* cob_sick_s300: remove debug print out +* cob_sick_s300: properly get offsets and add corresponding docu to source code +* cob_sick_s300: comment warning about field parameters --> this is currently buggy, old version works better +* cob_sick_s300: move telegram description to telegram parser file +* cob_sick_s300: clarify telegram calculation according to telegram listing +* manually fix changelog +* Contributors: Matthias Gruhler, ipa-fxm, ipa-mig + 0.6.8 (2016-10-10) ------------------ diff --git a/cob_sick_s300/CMakeLists.txt b/cob_sick_s300/CMakeLists.txt index 42802d746..dc6031bdd 100644 --- a/cob_sick_s300/CMakeLists.txt +++ b/cob_sick_s300/CMakeLists.txt @@ -8,19 +8,18 @@ find_package(Boost REQUIRED COMPONENTS date_time thread) catkin_package() ### BUILD ### -include_directories(common/include common/src ${Boost_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}) - -set(OODL_SickS300_SRC - ${PROJECT_SOURCE_DIR}/common/src/Errors.cpp - ${PROJECT_SOURCE_DIR}/common/src/Logger.cpp - # ${PROJECT_SOURCE_DIR}/common/src/LaserScannerData.cpp - ${PROJECT_SOURCE_DIR}/common/src/LaserScannerConfiguration.cpp - # ${PROJECT_SOURCE_DIR}/common/src/LaserScannerDataWithIntensities.cpp - ${PROJECT_SOURCE_DIR}/common/src/ScannerSickS300.cpp - ${PROJECT_SOURCE_DIR}/common/src/SerialIO.cpp +include_directories( + common/include + ${Boost_INCLUDE_DIRS} + ${catkin_INCLUDE_DIRS} +) + +add_executable(${PROJECT_NAME} + common/src/ScannerSickS300.cpp + common/src/SerialIO.cpp + ros/src/${PROJECT_NAME}.cpp ) -add_executable(${PROJECT_NAME} ros/src/${PROJECT_NAME}.cpp ${OODL_SickS300_SRC}) add_executable(cob_scan_filter ros/src/cob_scan_filter.cpp) add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) diff --git a/cob_sick_s300/common/include/cob_sick_s300/Errors.hpp b/cob_sick_s300/common/include/cob_sick_s300/Errors.hpp deleted file mode 100644 index 97150e54f..000000000 --- a/cob_sick_s300/common/include/cob_sick_s300/Errors.hpp +++ /dev/null @@ -1,62 +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 BRICS_OODL_ERRORS_H -#define BRICS_OODL_ERRORS_H - - -#include -using namespace std; - -#include -#include -#include "Logger.hpp" -namespace brics_oodl { - -/** - * \brief - * - */ -class Errors { - public: - Errors(); - - virtual ~Errors(); - - void getNextError(std::string& name, std::string& description); - - void getAllErrors(map& allErrors); - - unsigned int getAmountOfErrors(); - - void addError(std::string name, std::string description); - - void deleteAllErrors(); - - void printErrorsToConsole(); - - - private: - //map of error name and error description - map occurredErrors; - - map::iterator iter; - -}; - -} // namespace brics_oodl -#endif diff --git a/cob_sick_s300/common/include/cob_sick_s300/LaserScannerConfiguration.hpp b/cob_sick_s300/common/include/cob_sick_s300/LaserScannerConfiguration.hpp deleted file mode 100644 index ecfbf45cf..000000000 --- a/cob_sick_s300/common/include/cob_sick_s300/LaserScannerConfiguration.hpp +++ /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 BRICS_OODL_LASERSCANNERCONFIGURATION_H -#define BRICS_OODL_LASERSCANNERCONFIGURATION_H - -/** - * \file - * - * \author - * \date - */ -#include -#include -#include -#include "cob_sick_s300/Units.hpp" -namespace brics_oodl { - -enum baud_rate { - BAUD_9600, - BAUD_19200, - BAUD_38400, - BAUD_115200, - BAUD_500K, - BAUD_UNKNOWN - -}; -/** - * \brief - * - */ -class LaserScannerConfiguration { - public: - LaserScannerConfiguration(); - - virtual ~LaserScannerConfiguration(); - - LaserScannerConfiguration(const LaserScannerConfiguration & source); - - virtual LaserScannerConfiguration & operator=(const LaserScannerConfiguration & source); - - /// Vendor name. - std::string vendor; - - /// Product name. - std::string product; - - /// Firmware version. - std::string firmware; - - std::string protocol; - - std::string serialNumber; - - std::string model; - - quantity scanAngleStart; - - quantity scanAngleStop; - - quantity scanResolution; - - quantity minRangeDistance; - - quantity maxRangeDistance; - - baud_rate baud; - - std::string devicePath; - - int scannerID; - -}; - -} // namespace brics_oodl -#endif diff --git a/cob_sick_s300/common/include/cob_sick_s300/Logger.hpp b/cob_sick_s300/common/include/cob_sick_s300/Logger.hpp deleted file mode 100644 index 331e7af9d..000000000 --- a/cob_sick_s300/common/include/cob_sick_s300/Logger.hpp +++ /dev/null @@ -1,71 +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 BRICS_OODL_LOGGER_HPP -#define BRICS_OODL_LOGGER_HPP - -#include -#include -#include - -#include "boost/date_time/posix_time/posix_time.hpp" - - -namespace brics_oodl { - - enum severity_level { - trace, - debug, - info, - warning, - error, - fatal - }; - - /////////////////////////////////////////////////////////////////////////////// - /// Implementation logging to console and to a file - /////////////////////////////////////////////////////////////////////////////// - class Logger { - private: - std::stringstream out; - bool print; - severity_level level; - public: - - Logger(const std::string &funcName, const int &lineNo, const std::string &fileName, severity_level level); - ~Logger(); - - static bool toConsole; - static bool toFile; - static bool toROS; - static severity_level logginLevel; - - template - Logger & operator<<(const T &v) { - out << v; - return *this; - } - }; - - -#define LOG(level) Logger(__PRETTY_FUNCTION__, __LINE__ , __FILE__, level) - - -} // namespace youbot - -#endif /* YOUBOT_LOGGER_HPP */ - diff --git a/cob_sick_s300/common/include/cob_sick_s300/Units.hpp b/cob_sick_s300/common/include/cob_sick_s300/Units.hpp deleted file mode 100644 index 085e65b19..000000000 --- a/cob_sick_s300/common/include/cob_sick_s300/Units.hpp +++ /dev/null @@ -1,45 +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 BRICS_OODL_UNITS_HPP -#define BRICS_OODL_UNITS_HPP -#include -#include -#include -#include -#include -#include - - -using namespace boost::units; -using namespace boost::units::si; -using namespace boost::units::angle; - -//typedef boost::units::si::length meter; -using boost::units::si::meters; - - -typedef boost::units::make_scaled_unit > >::type millimeter; -typedef boost::units::make_scaled_unit > >::type centimeter; -BOOST_UNITS_STATIC_CONSTANT(centimeters, centimeter); - - -typedef boost::units::make_scaled_unit > >::type millisecond; -//BOOST_UNITS_STATIC_CONSTANT(millimeters, millimeter); - -#endif /* BRICS_OODL_UNITS_HPP */ - diff --git a/cob_sick_s300/common/src/Errors.cpp b/cob_sick_s300/common/src/Errors.cpp deleted file mode 100644 index bdeb1d3cf..000000000 --- a/cob_sick_s300/common/src/Errors.cpp +++ /dev/null @@ -1,85 +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 "cob_sick_s300/Errors.hpp" -namespace brics_oodl { - -Errors::Errors() { - // Bouml preserved body begin 000211F1 - iter = this->occurredErrors.begin(); - // Bouml preserved body end 000211F1 -} - -Errors::~Errors() { - // Bouml preserved body begin 00021271 - this->occurredErrors.clear(); - // Bouml preserved body end 00021271 -} - -void Errors::getNextError(std::string& name, std::string& description) { - // Bouml preserved body begin 00022AFC - if(iter == this->occurredErrors.end()){ - iter = this->occurredErrors.begin(); - }else{ - iter++; - } - name = iter->first; - description = iter->second; - // Bouml preserved body end 00022AFC -} - -void Errors::getAllErrors(map& allErrors) { - // Bouml preserved body begin 00022B7C - allErrors = this->occurredErrors; - // Bouml preserved body end 00022B7C -} - -unsigned int Errors::getAmountOfErrors() { - // Bouml preserved body begin 00021171 - return this->occurredErrors.size(); - // Bouml preserved body end 00021171 -} - -void Errors::addError(std::string name, std::string description) { - // Bouml preserved body begin 000212F1 - this->occurredErrors[name] = description; - // std::cout << "ERROR: " << name << " " << description << std::endl; - - - LOG(error) << name << ": " << description; - - // Bouml preserved body end 000212F1 -} - -void Errors::deleteAllErrors() { - // Bouml preserved body begin 00021371 - this->occurredErrors.clear(); - // Bouml preserved body end 00021371 -} - -void Errors::printErrorsToConsole() { - // Bouml preserved body begin 0002FA71 - map::iterator iterator;; - for(iterator = this->occurredErrors.begin();iterator != this->occurredErrors.end(); iterator++){ - std::cout << iterator->first << ": " << iterator->second << std::endl; - } - - // Bouml preserved body end 0002FA71 -} - - -} // namespace brics_oodl diff --git a/cob_sick_s300/common/src/LaserScannerConfiguration.cpp b/cob_sick_s300/common/src/LaserScannerConfiguration.cpp deleted file mode 100644 index 340542efd..000000000 --- a/cob_sick_s300/common/src/LaserScannerConfiguration.cpp +++ /dev/null @@ -1,63 +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 "cob_sick_s300/LaserScannerConfiguration.hpp" -namespace brics_oodl { - -LaserScannerConfiguration::LaserScannerConfiguration(){ - // Bouml preserved body begin 0001F47C - // Bouml preserved body end 0001F47C -} - -LaserScannerConfiguration::~LaserScannerConfiguration(){ - // Bouml preserved body begin 0001F4FC - // Bouml preserved body end 0001F4FC -} - -LaserScannerConfiguration::LaserScannerConfiguration(const LaserScannerConfiguration & source) { - // Bouml preserved body begin 000214F1 - *this = source; - // Bouml preserved body end 000214F1 -} - -LaserScannerConfiguration & LaserScannerConfiguration::operator=(const LaserScannerConfiguration & source) { - // Bouml preserved body begin 00021571 - - this->baud = source.baud; - this->devicePath = source.devicePath; - this->firmware = source.firmware; - - this->model = source.model; - - this->product = source.product; - this->protocol = source.protocol; - this->scanAngleStart = source.scanAngleStart; - this->scanAngleStop = source.scanAngleStop; - this->scanResolution = source.scanResolution; - this->maxRangeDistance = source.maxRangeDistance; - this->minRangeDistance = source.minRangeDistance; - this->serialNumber = source.serialNumber; - this->vendor = source.vendor; - this->scannerID = source.scannerID; - - return *this; - - // Bouml preserved body end 00021571 -} - - -} // namespace brics_oodl diff --git a/cob_sick_s300/common/src/Logger.cpp b/cob_sick_s300/common/src/Logger.cpp deleted file mode 100644 index 1db9de929..000000000 --- a/cob_sick_s300/common/src/Logger.cpp +++ /dev/null @@ -1,108 +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 "cob_sick_s300/Logger.hpp" - -namespace brics_oodl { - - bool Logger::toConsole = false; - bool Logger::toFile = false; - bool Logger::toROS = true; - severity_level Logger::logginLevel = trace; - - Logger::Logger(const std::string &funcName, const int &lineNo, const std::string &fileName, severity_level level) { - - this->level = level; - if (toConsole || toFile) { - if (level >= logginLevel) { - print = true; - - switch (level) { - case trace: - out << "Trace" << ": "; - break; - case debug: - out << "Debug" << ": "; - break; - case info: - out << "Info" << ": "; - break; - case warning: - out << "Warning" << ": "; - break; - case error: - out << "Error" << ": "; - break; - case fatal: - out << "Fatal" << ": "; - break; - default: - break; - } - // out << "function " << funcName << ": "; - // out << "line " << lineNo << ": "; - // out << "fileName " << fileName << ": "; - // out << "time " << boost::posix_time::microsec_clock::local_time() << ": "; - } else { - print = false; - } - } else { - print = false; - } - - } - - Logger::~Logger() { - //end of message - if (toConsole && print) { - std::cout << out.str() << std::endl; - } - - if (toFile && print) { - std::fstream filestr; - filestr.open("log.txt", std::fstream::out | std::fstream::app); - filestr << out.str() << std::endl; - filestr.close(); - } - - if (toROS) { - switch (level) { - case trace: - ROS_DEBUG("%s",out.str().c_str()); - break; - case debug: - ROS_DEBUG("%s",out.str().c_str()); - break; - case info: - ROS_INFO("%s",out.str().c_str()); - break; - case warning: - ROS_WARN("%s",out.str().c_str()); - break; - case error: - ROS_ERROR("%s",out.str().c_str()); - break; - case fatal: - ROS_FATAL("%s",out.str().c_str()); - break; - default: - break; - } - } - } - -} // namespace youbot diff --git a/cob_sick_s300/package.xml b/cob_sick_s300/package.xml index 3a8fac294..efbbeb399 100644 --- a/cob_sick_s300/package.xml +++ b/cob_sick_s300/package.xml @@ -1,6 +1,6 @@ cob_sick_s300 - 0.6.8 + 0.6.13 This package published a laser scan message out of a Sick S300 laser scanner. Apache 2.0 @@ -8,7 +8,7 @@ http://ros.org/wiki/cob_sick_s300 - Joshua Hampp + Felix Messmer Florian Weisshardt catkin diff --git a/cob_sick_s300/ros/src/cob_sick_s300.cpp b/cob_sick_s300/ros/src/cob_sick_s300.cpp index f15e066fb..96eac2a7c 100644 --- a/cob_sick_s300/ros/src/cob_sick_s300.cpp +++ b/cob_sick_s300/ros/src/cob_sick_s300.cpp @@ -67,7 +67,7 @@ class NodeClass // global variables std::string port; std::string node_name; - int baud, scan_id, publish_frequency; + int baud, scan_id; bool inverted; double scan_duration, scan_cycle_time; std::string frame_id; @@ -75,8 +75,8 @@ class NodeClass unsigned int syncedSICKStamp; bool syncedTimeReady; bool debug_; + double communication_timeout; ScannerSickS300 scanner_; - ros::Time loop_rate_; std_msgs::Bool inStandby_; // Constructor @@ -106,11 +106,11 @@ class NodeClass if(!nh.hasParam("scan_cycle_time")) ROS_WARN("Used default parameter for scan_cycle_time"); nh.param("scan_cycle_time", scan_cycle_time, 0.040); //SICK-docu says S300 scans every 40ms - if (!nh.hasParam("publish_frequency")) ROS_WARN("Used default parameter for publish_frequency"); - nh.param("publish_frequency", publish_frequency, 12); //Hz - if(nh.hasParam("debug")) nh.param("debug", debug_, false); + if(!nh.hasParam("communication_timeout")) ROS_WARN("Used default parameter for communication timeout"); + nh.param("communication_timeout", communication_timeout, 0.2); + try { //get params for each measurement @@ -178,19 +178,20 @@ class NodeClass topicPub_LaserScan = nh.advertise("scan", 1); topicPub_InStandby = nh.advertise("scan_standby", 1); topicPub_Diagnostic_ = nh.advertise("/diagnostics", 1); - - loop_rate_ = ros::Time::now(); // Hz } bool open() { return scanner_.open(port.c_str(), baud, scan_id); } - void receiveScan() { + bool receiveScan() { std::vector< double > ranges, rangeAngles, intensities; unsigned int iSickTimeStamp, iSickNow; - if(scanner_.getScan(ranges, rangeAngles, intensities, iSickTimeStamp, iSickNow, debug_)) + int result = scanner_.getScan(ranges, rangeAngles, intensities, iSickTimeStamp, iSickNow, debug_); + static boost::posix_time::ptime point_time_communiaction_ok = boost::posix_time::microsec_clock::local_time(); + + if(result) { if(scanner_.isInStandby()) { @@ -203,7 +204,21 @@ class NodeClass publishStandby(false); publishLaserScan(ranges, rangeAngles, intensities, iSickTimeStamp, iSickNow); } + + point_time_communiaction_ok = boost::posix_time::microsec_clock::local_time(); } + else + { + boost::posix_time::time_duration diff = boost::posix_time::microsec_clock::local_time() - point_time_communiaction_ok; + + if (diff.total_milliseconds() > static_cast(1000*communication_timeout)) + { + ROS_WARN("Communiaction timeout"); + return false; + } + } + + return true; } // Destructor @@ -220,10 +235,6 @@ class NodeClass // other function declarations void publishLaserScan(std::vector vdDistM, std::vector vdAngRAD, std::vector vdIntensAU, unsigned int iSickTimeStamp, unsigned int iSickNow) { - if(ros::Time::now()-loop_rate_.now()>=ros::Duration(1./publish_frequency)) - return; - loop_rate_ = ros::Time::now(); - // fill message int start_scan, stop_scan; int num_readings = vdDistM.size(); // initialize with max scan size @@ -255,7 +266,7 @@ class NodeClass laserScan.header.frame_id = frame_id; laserScan.angle_increment = vdAngRAD[start_scan + 1] - vdAngRAD[start_scan]; laserScan.range_min = 0.001; - laserScan.range_max = 30.0; + laserScan.range_max = 29.5; // though the specs state otherwise, the max range reported by the scanner is 29.96m laserScan.time_increment = (scan_duration) / (vdDistM.size()); // rescale scan @@ -332,27 +343,32 @@ int main(int argc, char** argv) NodeClass nodeClass; - bool bOpenScan = false; - while (!bOpenScan && ros::ok()) { - ROS_INFO("Opening scanner... (port:%s)", nodeClass.port.c_str()); + while (ros::ok()) + { + bool bOpenScan = false; + while (!bOpenScan && ros::ok()) { + ROS_INFO("Opening scanner... (port:%s)", nodeClass.port.c_str()); - bOpenScan = nodeClass.open(); - //bOpenScan = sickS300.open(errors, nodeClass.debug_); + bOpenScan = nodeClass.open(); - // check, if it is the first try to open scanner - if (!bOpenScan) { - ROS_ERROR("...scanner not available on port %s. Will retry every second.", nodeClass.port.c_str()); - nodeClass.publishError("...scanner not available on port"); + // check, if it is the first try to open scanner + if (!bOpenScan) { + ROS_ERROR("...scanner not available on port %s. Will retry every second.", nodeClass.port.c_str()); + nodeClass.publishError("...scanner not available on port"); + } + sleep(1); // wait for scan to get ready if successfull, or wait befor retrying } - sleep(1); // wait for scan to get ready if successfull, or wait befor retrying - } - ROS_INFO("...scanner opened successfully on port %s", nodeClass.port.c_str()); + ROS_INFO("...scanner opened successfully on port %s", nodeClass.port.c_str()); - // main loop - while (ros::ok()) { - // read scan - nodeClass.receiveScan(); - ros::spinOnce(); + // main loop + while (ros::ok()) { + // read scan + if (!nodeClass.receiveScan()) + { + break; + } + ros::spinOnce(); + } } return 0; } diff --git a/cob_sound/CHANGELOG.rst b/cob_sound/CHANGELOG.rst index 9ab3ad654..b38429d30 100644 --- a/cob_sound/CHANGELOG.rst +++ b/cob_sound/CHANGELOG.rst @@ -2,6 +2,45 @@ Changelog for package cob_sound ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.6.13 (2019-03-14) +------------------- +* Merge pull request `#382 `_ from fmessmer/install_fix_swift + fix install tags +* fix install tags +* Contributors: Felix Messmer, fmessmer + +0.6.12 (2018-07-21) +------------------- +* update maintainer +* Merge pull request `#376 `_ from fmessmer/bulletproof_sound + bulletproof sound +* bulletproof sound +* Merge pull request `#362 `_ from ipa-fmw/fix/sound_play + [cob_sound] catch if filename is empty or invalid +* catch if filename is empty +* Contributors: Felix Messmer, fmessmer, ipa-fxm, msh + +0.6.11 (2018-01-07) +------------------- +* Merge remote-tracking branch 'origin/indigo_release_candidate' into indigo_dev +* Merge pull request `#353 `_ from ipa-fxm/update_maintainer + update maintainer +* update maintainer +* 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) +------------------ +* remove dependency to sound_play +* fix sound test_client +* manually fix changelog +* Contributors: Felix Messmer, ipa-fxm + 0.6.8 (2016-10-10) ------------------ * fade volume on play and between tracks diff --git a/cob_sound/CMakeLists.txt b/cob_sound/CMakeLists.txt index 1542596e7..31143951e 100644 --- a/cob_sound/CMakeLists.txt +++ b/cob_sound/CMakeLists.txt @@ -32,6 +32,6 @@ install(TARGETS sound RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) -install(PROGRAMS ros/src/test_client.py - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}/ros/src +install(PROGRAMS ros/src/test_client.py fix_swift_for_precise.sh + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) diff --git a/cob_sound/package.xml b/cob_sound/package.xml index e3b135be2..a9847b614 100644 --- a/cob_sound/package.xml +++ b/cob_sound/package.xml @@ -1,6 +1,6 @@ cob_sound - 0.6.8 + 0.6.13 This package implements a sound play module using text2wave and aplay through python. Apache 2.0 @@ -8,7 +8,8 @@ http://ros.org/wiki/cob_sound - Nadia Hammoudeh Garcia + Felix Messmer + Benjamin Maidel Florian Weisshardt Benjamin Maidel diff --git a/cob_sound/ros/src/sound.cpp b/cob_sound/ros/src/sound.cpp index 74dd8a28f..54dbb733b 100644 --- a/cob_sound/ros/src/sound.cpp +++ b/cob_sound/ros/src/sound.cpp @@ -26,6 +26,8 @@ #include #include +#include +#include class SoundAction { @@ -61,6 +63,22 @@ class SoundAction as_play_(nh_, ros::this_node::getName() + "/play", false) { nh_ = ros::NodeHandle("~"); + } + + ~SoundAction(void) + { + libvlc_media_player_stop(vlc_player_); + libvlc_media_player_release(vlc_player_); + libvlc_release(vlc_inst_); + } + + bool init() + { + vlc_inst_ = libvlc_new(0,NULL); + if(!vlc_inst_){ROS_ERROR("failed to create libvlc instance"); return false;} + vlc_player_ = libvlc_media_player_new(vlc_inst_); + if(!vlc_player_){ROS_ERROR("failed to create vlc media player object"); return false;} + as_play_.registerGoalCallback(boost::bind(&SoundAction::as_goal_cb_play_, this)); as_play_.registerPreemptCallback(boost::bind(&SoundAction::as_preempt_cb_play_, this)); srvServer_say_ = nh_.advertiseService("say", &SoundAction::service_cb_say, this); @@ -79,18 +97,9 @@ class SoundAction mute_ = false; - vlc_inst_ = libvlc_new(0,NULL); - vlc_player_ = libvlc_media_player_new(vlc_inst_); - as_say_.start(); as_play_.start(); - } - - ~SoundAction(void) - { - libvlc_media_player_stop(vlc_player_); - libvlc_media_player_release(vlc_player_); - libvlc_release(vlc_inst_); + return true; } void as_goal_cb_play_() @@ -153,6 +162,12 @@ class SoundAction cob_srvs::SetString::Response &res ) { res.success = play(req.data, res.message); + ros::Duration(0.1).sleep(); + while((libvlc_media_player_is_playing(vlc_player_) == 1)) + { + ros::Duration(0.1).sleep(); + ROS_DEBUG("still playing %s", req.data.c_str()); + } return true; } @@ -246,84 +261,75 @@ class SoundAction { command = "echo " + data + " | text2wave | aplay -q"; } - if (system(command.c_str()) != 0) + + int ret = system(command.c_str()); + if (ret != 0) { - message = "Command say failed to play sound using mode " + mode; + message = "Command say failed to say '" + data + "' using mode " + mode + " (system return value: " + boost::lexical_cast(ret) + ")"; ROS_ERROR_STREAM(message); - // publishing diagnotic error if output fails - diagnostic_msgs::DiagnosticStatus status; - status.level = 2; - status.name = "sound"; - status.message = message; - diagnostics_.status.push_back(status); - - diagnostics_.header.stamp = ros::Time::now(); - diagnostics_pub_.publish(diagnostics_); - - diagnostics_.status.resize(0); + publish_diagnostics(diagnostic_msgs::DiagnosticStatus::ERROR, message); return false; } + + message = "Say successfull"; return true; } - bool play(std::string filename, std::string message) + bool play(std::string filename, std::string &message) { - bool ret = false; if (mute_) { - message = "Sound is set to mute. You will hear nothing."; + message = "Sound is set to mute. You will hear nothing."; ROS_WARN_STREAM(message); - return ret; + return false; + } + + if (filename.empty()) + { + message = "Cannot play because filename is empty."; + ROS_WARN_STREAM(message); + return false; + } + + if ( !boost::filesystem::exists(filename) ) + { + message = "Cannot play '" + filename +"' because file does not exist."; + ROS_WARN_STREAM(message); + return false; } - ROS_INFO("Playing: %s", filename.c_str()); vlc_media_ = libvlc_media_new_path(vlc_inst_, filename.c_str()); + if(!vlc_media_) + { + message = "failed to create media for filepath %s", filename.c_str(); + ROS_WARN_STREAM(message); + return false; + } - if (vlc_media_ != NULL) + libvlc_media_player_set_media(vlc_player_, vlc_media_); + libvlc_media_release(vlc_media_); + + if(!fade_out()) { - libvlc_media_player_set_media(vlc_player_, vlc_media_); - libvlc_media_release(vlc_media_); - fade_out(); - if(fade_in()) - { - ret = true; - message = "Play successfull"; - } + message = "failed to fade out"; + ROS_WARN_STREAM(message); + return false; } - if(ret == false) + + if(!fade_in()) { - message = "Could not play file %s" + filename; - ROS_ERROR_STREAM(message); - // publishing diagnotic error if output fails - diagnostic_msgs::DiagnosticStatus status; - status.level = 2; - status.name = "sound"; - status.message = message; - diagnostics_.status.push_back(status); - - diagnostics_.header.stamp = ros::Time::now(); - diagnostics_pub_.publish(diagnostics_); - - diagnostics_.status.resize(0); - return ret; + message = "failed to fade in"; + ROS_WARN_STREAM(message); + return false; } - return ret; + message = "Play successfull"; + return true; } void timer_cb(const ros::TimerEvent&) { - diagnostic_msgs::DiagnosticStatus status; - status.level = 0; - status.name = "sound"; - status.hardware_id = "none"; - status.message = "sound controller running"; - diagnostics_.status.push_back(status); - - diagnostics_.header.stamp = ros::Time::now(); - diagnostics_pub_.publish(diagnostics_); - - diagnostics_.status.resize(0); + publish_diagnostics(diagnostic_msgs::DiagnosticStatus::OK, "sound controller running"); } void timer_play_feedback_cb(const ros::TimerEvent&) @@ -378,6 +384,21 @@ class SoundAction pubMarker_.publish(marker); } + void publish_diagnostics(unsigned char level, std::string message) + { + diagnostic_msgs::DiagnosticStatus status; + status.level = level; + status.name = "sound"; + status.hardware_id = "sound"; + status.message = message; + diagnostics_.status.push_back(status); + + diagnostics_.header.stamp = ros::Time::now(); + diagnostics_pub_.publish(diagnostics_); + + diagnostics_.status.resize(0); + } + bool fade_in() { if(libvlc_media_player_play(vlc_player_) >= 0) @@ -396,11 +417,13 @@ class SoundAction { while(libvlc_audio_set_volume(vlc_player_,100) != 0) ros::Duration(0.05).sleep(); - } } else + { return false; + } + return true; } @@ -411,7 +434,7 @@ class SoundAction { if(fade_volume_) { - for(int i = volume - (volume%5); i >=0; i-=5) + for(int i = volume - (volume%5); i>=0; i-=5) { libvlc_audio_set_volume(vlc_player_,i); ros::Duration(fade_duration_/20.0).sleep(); @@ -420,7 +443,6 @@ class SoundAction } return true; } - }; @@ -429,8 +451,15 @@ int main(int argc, char** argv) ros::init(argc, argv, "cob_sound"); SoundAction sound; - ROS_INFO("sound node started"); - - ros::spin(); - return 0; + if(!sound.init()) + { + ROS_ERROR("sound init failed"); + return 1; + } + else + { + ROS_INFO("sound node started"); + ros::spin(); + return 0; + } } diff --git a/cob_undercarriage_ctrl/CHANGELOG.rst b/cob_undercarriage_ctrl/CHANGELOG.rst index e5151053d..d2377702e 100644 --- a/cob_undercarriage_ctrl/CHANGELOG.rst +++ b/cob_undercarriage_ctrl/CHANGELOG.rst @@ -2,6 +2,28 @@ Changelog for package cob_undercarriage_ctrl ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +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) ------------------ diff --git a/cob_undercarriage_ctrl/package.xml b/cob_undercarriage_ctrl/package.xml index dac3bf774..bed12e11c 100644 --- a/cob_undercarriage_ctrl/package.xml +++ b/cob_undercarriage_ctrl/package.xml @@ -1,6 +1,6 @@ cob_undercarriage_ctrl - 0.6.8 + 0.6.13 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 diff --git a/cob_utilities/CHANGELOG.rst b/cob_utilities/CHANGELOG.rst index 315bf3a9d..158eb7006 100644 --- a/cob_utilities/CHANGELOG.rst +++ b/cob_utilities/CHANGELOG.rst @@ -2,6 +2,28 @@ Changelog for package cob_utilities ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +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) ------------------ diff --git a/cob_utilities/package.xml b/cob_utilities/package.xml index 454ce67e0..9f3c44979 100644 --- a/cob_utilities/package.xml +++ b/cob_utilities/package.xml @@ -1,6 +1,6 @@ cob_utilities - 0.6.8 + 0.6.13 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! diff --git a/cob_voltage_control/CHANGELOG.rst b/cob_voltage_control/CHANGELOG.rst index 5071cd2f5..495c3cfa9 100644 --- a/cob_voltage_control/CHANGELOG.rst +++ b/cob_voltage_control/CHANGELOG.rst @@ -2,6 +2,29 @@ Changelog for package cob_voltage_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +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) ------------------ diff --git a/cob_voltage_control/package.xml b/cob_voltage_control/package.xml index b9d83cca8..5d4d05afa 100644 --- a/cob_voltage_control/package.xml +++ b/cob_voltage_control/package.xml @@ -1,6 +1,6 @@ cob_voltage_control - 0.6.8 + 0.6.13 Interface to IO board that manages emergency stop and battery voltage on rob@work 3 Apache 2.0 diff --git a/laser_scan_densifier/CHANGELOG.rst b/laser_scan_densifier/CHANGELOG.rst new file mode 100644 index 000000000..cd82a3196 --- /dev/null +++ b/laser_scan_densifier/CHANGELOG.rst @@ -0,0 +1,12 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package laser_scan_densifier +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.6.13 (2019-03-14) +------------------- +* Merge pull request `#383 `_ from fmessmer/laser_densifier + add laser scan densifier +* support copy and interpolation mode +* test interpolated densification +* add laser_scan_densifier +* Contributors: Felix Messmer, fmessmer diff --git a/laser_scan_densifier/CMakeLists.txt b/laser_scan_densifier/CMakeLists.txt new file mode 100644 index 000000000..7ed51e4c8 --- /dev/null +++ b/laser_scan_densifier/CMakeLists.txt @@ -0,0 +1,52 @@ +cmake_minimum_required(VERSION 2.8.3) +project(laser_scan_densifier) + +# List C++ dependencies on ros packages +set( ROS_CXX_DEPENDENCIES + roscpp + nodelet + sensor_msgs) + +# Find catkin and all required ROS components +find_package(catkin REQUIRED COMPONENTS ${ROS_CXX_DEPENDENCIES}) + +# Set include directories +include_directories(include ${catkin_INCLUDE_DIRS}) + +# Declare info that other packages need to import library generated here +catkin_package( + INCLUDE_DIRS include + LIBRARIES laser_scan_densifier + CATKIN_DEPENDS ${ROS_CXX_DEPENDENCIES} +) + +#Create library +add_library(laser_scan_densifier src/laser_scan_densifier.cpp) +target_link_libraries( laser_scan_densifier ${catkin_LIBRARIES}) +add_dependencies(laser_scan_densifier ${catkin_EXPORTED_TARGETS}) + +#Create nodelet +add_library(laser_scan_densifier_nodelet src/laser_scan_densifier_nodelet.cpp) +target_link_libraries(laser_scan_densifier_nodelet laser_scan_densifier) + +#Create node +add_executable(laser_scan_densifier_node src/laser_scan_densifier_node.cpp) +target_link_libraries( laser_scan_densifier_node laser_scan_densifier ) + +#Install library +install(TARGETS laser_scan_densifier laser_scan_densifier_nodelet + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) + +#Install library includes +install(DIRECTORY include/laser_scan_densifier/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} ) + +#Install node +install(TARGETS laser_scan_densifier_node + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) + +#Install nodelet description +install(FILES laser_scan_densifier_nodelet.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) diff --git a/laser_scan_densifier/include/laser_scan_densifier/laser_scan_densifier.h b/laser_scan_densifier/include/laser_scan_densifier/laser_scan_densifier.h new file mode 100644 index 000000000..d90858c86 --- /dev/null +++ b/laser_scan_densifier/include/laser_scan_densifier/laser_scan_densifier.h @@ -0,0 +1,80 @@ +/* + * 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. + */ +/* + * Copyright (c) 2011, Ivan Dryanovski + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the CCNY Robotics Lab nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef LASER_SCAN_DENSIFIER_LASER_SCAN_DENSIFIER_H +#define LASER_SCAN_DENSIFIER_LASER_SCAN_DENSIFIER_H + +#include +#include + +namespace scan_tools { + +class LaserScanDensifier +{ + public: + + LaserScanDensifier(ros::NodeHandle nh, ros::NodeHandle nh_private); + virtual ~LaserScanDensifier(); + + private: + + // **** ROS-related + ros::NodeHandle nh_; + ros::NodeHandle nh_private_; + ros::Subscriber scan_subscriber_; + ros::Publisher scan_publisher_; + + // **** paramaters + + int step_; + int mode_; + + // **** member functions + + void scanCallback(const sensor_msgs::LaserScanConstPtr& scan_msg); +}; + +} //namespace scan_tools + +#endif // LASER_SCAN_DENSIFIER_LASER_SCAN_DENSIFIER_H diff --git a/laser_scan_densifier/include/laser_scan_densifier/laser_scan_densifier_nodelet.h b/laser_scan_densifier/include/laser_scan_densifier/laser_scan_densifier_nodelet.h new file mode 100644 index 000000000..8875d3cf8 --- /dev/null +++ b/laser_scan_densifier/include/laser_scan_densifier/laser_scan_densifier_nodelet.h @@ -0,0 +1,66 @@ +/* + * 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. + */ +/* + * Copyright (c) 2011, Ivan Dryanovski + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the CCNY Robotics Lab nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef LASER_SCAN_DENSIFIER_LASER_SCAN_DENSIFIER_NODELET_H +#define LASER_SCAN_DENSIFIER_LASER_SCAN_DENSIFIER_NODELET_H + +#include +#include + +#include "laser_scan_densifier/laser_scan_densifier.h" + +namespace scan_tools { + +class LaserScanDensifierNodelet : public nodelet::Nodelet +{ + public: + virtual void onInit(); + + private: + boost::shared_ptr< LaserScanDensifier > laser_scan_densifier_; +}; + +} //namespace scan_tools + +#endif // LASER_SCAN_DENSIFIER_LASER_SCAN_DENSIFIER_NODELET_H diff --git a/laser_scan_densifier/laser_scan_densifier_nodelet.xml b/laser_scan_densifier/laser_scan_densifier_nodelet.xml new file mode 100644 index 000000000..bae1f99f9 --- /dev/null +++ b/laser_scan_densifier/laser_scan_densifier_nodelet.xml @@ -0,0 +1,9 @@ + + + + + Laser scan densifier nodelet publisher. + + + diff --git a/laser_scan_densifier/package.xml b/laser_scan_densifier/package.xml new file mode 100644 index 000000000..ad19d6c2b --- /dev/null +++ b/laser_scan_densifier/package.xml @@ -0,0 +1,23 @@ + + laser_scan_densifier + 0.6.13 + + The laser_scan_densifier takes in a LaserScan message and densifies it. + Node is inspired by laser_scan_sparsifier (http://wiki.ros.org/laser_scan_sparsifier). + + Felix Messmer + Felix Messmer + + BSD + + catkin + + roscpp + nodelet + sensor_msgs + + + + + + diff --git a/laser_scan_densifier/src/laser_scan_densifier.cpp b/laser_scan_densifier/src/laser_scan_densifier.cpp new file mode 100644 index 000000000..c27483916 --- /dev/null +++ b/laser_scan_densifier/src/laser_scan_densifier.cpp @@ -0,0 +1,132 @@ +/* + * 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. + */ +/* + * Copyright (c) 2011, Ivan Dryanovski, William Morris + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the CCNY Robotics Lab nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "laser_scan_densifier/laser_scan_densifier.h" + +namespace scan_tools { + +LaserScanDensifier::LaserScanDensifier(ros::NodeHandle nh, ros::NodeHandle nh_private): + nh_(nh), + nh_private_(nh_private) +{ + ROS_INFO ("Starting LaserScanDensifier"); + + // **** get paramters + + nh_private_.param("step", step_, 2); + nh_private_.param("mode", mode_, 0); + + ROS_ASSERT_MSG(step_ > 0, "step parameter is set to %, must be > 0", step_); + + switch(mode_) { + case 0: ROS_INFO("LaserScanDensifier started with mode %d: copy data points", mode_); + break; + case 1: ROS_INFO("LaserScanDensifier started with mode %d: interpolate data points", mode_); + break; + default: ROS_WARN("LaserScanDensifier started with unsupported mode %d. Defaulting to mode 0: copy data points", mode_); + mode_ = 0; + break; + } + + // **** advertise topics + + scan_publisher_ = nh_.advertise("scan_dense", 1); + + // **** subscribe to laser scan messages + + scan_subscriber_ = nh_.subscribe("scan", 1, &LaserScanDensifier::scanCallback, this); +} + +LaserScanDensifier::~LaserScanDensifier () +{ + ROS_INFO ("Destroying LaserScanDensifier"); +} + +void LaserScanDensifier::scanCallback (const sensor_msgs::LaserScanConstPtr& scan_msg) +{ + sensor_msgs::LaserScan::Ptr scan_dense; + scan_dense = boost::make_shared(); + + // copy over equal fields + + scan_dense->header = scan_msg->header; + scan_dense->range_min = scan_msg->range_min; + scan_dense->range_max = scan_msg->range_max; + scan_dense->angle_min = scan_msg->angle_min; + scan_dense->angle_max = scan_msg->angle_max; + scan_dense->angle_increment = scan_msg->angle_increment / step_; + scan_dense->time_increment = scan_msg->time_increment; + scan_dense->scan_time = scan_msg->scan_time; + + scan_dense->ranges.clear(); + scan_dense->intensities.clear(); + + for (size_t i = 0; i < scan_msg->ranges.size()-1; i++) + { + switch (mode_) { + case 0: { //copy data points + scan_dense->ranges.insert(scan_dense->ranges.end(), step_, scan_msg->ranges[i]); + scan_dense->intensities.insert(scan_dense->intensities.end(), step_, scan_msg->intensities[i]); + break; + } + case 1: { //interpolate data points + double delta_range = (scan_msg->ranges[i+1]-scan_msg->ranges[i])/step_; + double delta_intensities = (scan_msg->intensities[i+1]-scan_msg->intensities[i])/step_; + for (int k = 0; k < step_; k++) { + scan_dense->ranges.insert(scan_dense->ranges.end(), 1, scan_msg->ranges[i]+k*delta_range); + scan_dense->intensities.insert(scan_dense->intensities.end(), 1, scan_msg->intensities[i]+k*delta_intensities); + } + break; + } + } + } + // add angle_max data point + scan_dense->ranges.push_back(scan_msg->ranges.back()); + scan_dense->intensities.push_back(scan_msg->intensities.back()); + + scan_publisher_.publish(scan_dense); +} + +} //namespace scan_tools + diff --git a/laser_scan_densifier/src/laser_scan_densifier_node.cpp b/laser_scan_densifier/src/laser_scan_densifier_node.cpp new file mode 100644 index 000000000..fdff8f99b --- /dev/null +++ b/laser_scan_densifier/src/laser_scan_densifier_node.cpp @@ -0,0 +1,55 @@ +/* + * 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. + */ +/* + * Copyright (c) 2011, Ivan Dryanovski, William Morris + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the CCNY Robotics Lab nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "laser_scan_densifier/laser_scan_densifier.h" + +int main (int argc, char **argv) +{ + ros::init (argc, argv, "LaserScanDensifier"); + ros::NodeHandle nh; + ros::NodeHandle nh_private("~"); + scan_tools::LaserScanDensifier laser_scan_densifier(nh, nh_private); + ros::spin (); + return 0; +} diff --git a/laser_scan_densifier/src/laser_scan_densifier_nodelet.cpp b/laser_scan_densifier/src/laser_scan_densifier_nodelet.cpp new file mode 100644 index 000000000..df56f51fa --- /dev/null +++ b/laser_scan_densifier/src/laser_scan_densifier_nodelet.cpp @@ -0,0 +1,60 @@ +/* + * 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. + */ +/* + * Copyright (c) 2011, Ivan Dryanovski, William Morris + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the CCNY Robotics Lab nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "laser_scan_densifier/laser_scan_densifier_nodelet.h" + +typedef scan_tools::LaserScanDensifierNodelet LaserScanDensifierNodelet; + +PLUGINLIB_EXPORT_CLASS(LaserScanDensifierNodelet, nodelet::Nodelet) + +void LaserScanDensifierNodelet::onInit () +{ + NODELET_INFO("Initializing LaserScanDensifier Nodelet"); + + // TODO: Do we want the single threaded or multithreaded NH? + ros::NodeHandle nh = getMTNodeHandle(); + ros::NodeHandle nh_private = getMTPrivateNodeHandle(); + + laser_scan_densifier_ = boost::shared_ptr< LaserScanDensifier >(new LaserScanDensifier(nh, nh_private)); +}