diff --git a/cob_bms_driver/CHANGELOG.rst b/cob_bms_driver/CHANGELOG.rst deleted file mode 100644 index 04c8bde52..000000000 --- a/cob_bms_driver/CHANGELOG.rst +++ /dev/null @@ -1,266 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package cob_bms_driver -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.7.16 (2024-02-20) -------------------- - -0.7.15 (2023-11-06) -------------------- - -0.7.14 (2022-11-17) -------------------- -* Merge pull request `#437 `_ from Deleh/fix/power_consume - Fix power consumption of fake BMS -* round remaining capacity for publisher and diagnostics -* remove round of remaining capacity -* Merge pull request `#436 `_ from HannesBachter/audi_scenario - configurable battery details -* get battery details from parameter server -* adjust battery capacity and discharging values to new diff base -* Contributors: Denis Lehmann, Felix Messmer, HannesBachter, fmessmer - -0.7.13 (2022-07-29) -------------------- -* Merge pull request `#434 `_ from floweisshardt/feature/power_state_connected - add explicit power_state.connected -* remove unused subscriber -* add explicit power_state.connected -* Merge pull request `#432 `_ from fmessmer/feature/publish_battery_state - power_state_aggregator now also publishes sensor_msgs.BatteryState -* fix percentage value range - Co-authored-by: Benjamin Maidel -* update current_buffer_size -* power_state_aggregator now also publishes sensor_msgs.BatteryState -* Contributors: Felix Messmer, floweisshardt, fmessmer - -0.7.12 (2022-03-15) -------------------- - -0.7.11 (2022-01-12) -------------------- -* Merge pull request `#429 `_ from fmessmer/fix/invalid_value_warning - catch invalid value warning -* use odd number for buffer_size to prevent 0.0 mean -* catch invalid value warning -* Contributors: Felix Messmer, fmessmer - -0.7.10 (2021-12-23) -------------------- - -0.7.9 (2021-11-26) ------------------- -* Merge pull request `#427 `_ from benmaidel/fix/fake_bms_service - [BMS] fix fakebms set_relative_remaining_capacity service -* fix fakebms set_relative_remaining_capacity service -* Contributors: Benjamin Maidel, Felix Messmer - -0.7.8 (2021-10-19) ------------------- - -0.7.7 (2021-08-02) ------------------- - -0.7.6 (2021-05-10) ------------------- -* Merge pull request `#423 `_ from mikaelarguedas/python2-deps - ROS_PYTHON_VERSION conditional dependency for `python-tk` and `python-numpy` -* convter cob_bms_driver to package format 3 -* ROS_PYTHON_VERSION conditional dependency for python-numpy -* Contributors: Felix Messmer, Mikael Arguedas - -0.7.5 (2021-04-06) ------------------- -* Merge pull request `#418 `_ from fmessmer/fix_catkin_lint - fix catkin_lint -* fix catkin_lint -* Contributors: Felix Messmer, fmessmer - -0.7.4 (2020-10-14) ------------------- -* Merge pull request `#417 `_ from fmessmer/test_noetic - test noetic -* Bump CMake version to avoid CMP0048 warning -* Contributors: Felix Messmer, fmessmer - -0.7.3 (2020-03-18) ------------------- - -0.7.2 (2020-03-18) ------------------- -* Merge pull request `#408 `_ from fmessmer/ci_updates - [travis] ci updates -* catkin_lint fixes -* Contributors: Felix Messmer, fmessmer - -0.7.1 (2019-11-07) ------------------- - -0.7.0 (2019-08-06) ------------------- -* Merge pull request `#380 `_ from ipa-jba/fix/boost_shared_ptr - [Melodic] combined melodify pr -* melodify cob_bms_driver_node -* Merge pull request `#396 `_ from HannesBachter/indigo_dev - 0.6.15 -* Contributors: Felix Messmer, Jannik Abbenseth - -0.6.15 (2019-07-17) ------------ -* 0.6.14 -* update changelogs -* Merge pull request `#393 `_ from fmessmer/add_int_bms_parameter - add int bms parameter types -* get rid of c++11 compile options -* fix key name -* add int bms parameter types -* Merge pull request `#392 `_ from fmessmer/max_time_remaining - clamp time_remaining when current is zero -* clamp time_remaining when current is zero -* Contributors: Felix Messmer, fmessmer - -0.6.14 (2019-06-07) -------------------- -* Merge pull request `#393 `_ from fmessmer/add_int_bms_parameter - add int bms parameter types -* get rid of c++11 compile options -* fix key name -* add int bms parameter types -* Merge pull request `#392 `_ from fmessmer/max_time_remaining - clamp time_remaining when current is zero -* clamp time_remaining when current is zero -* Contributors: Felix Messmer, fmessmer - -0.6.13 (2019-03-14) -------------------- -* Merge pull request `#381 `_ from pholthau/boost-format - include boost/format.hpp -* include boost/format.hpp -* Contributors: Felix Messmer, Patrick Holthaus - -0.6.12 (2018-07-21) -------------------- -* update maintainer -* Merge pull request `#374 `_ from floweisshardt/feature/round_remaining_capacity - round remaining_capacity -* adjust to real driver precision -* round remaining_capacity -* Contributors: Felix Messmer, fmessmer, ipa-fmw, ipa-fxm - -0.6.11 (2018-01-07) -------------------- -* Merge remote-tracking branch 'origin/indigo_release_candidate' into indigo_dev -* Merge pull request `#364 `_ from ipa-fxm/fake_bms_diagnostics - use diagnostic updater in fake_bms -* use diagnostic updater in fake_bms -* Merge pull request `#361 `_ from ipa-fxm/set_relative_remaining_capacity - set relative remaining capacity -* set relative remaining capacity -* Merge pull request `#341 `_ from ipa-fxm/APACHE_license - use license apache 2.0 -* change maintainer -* use license apache 2.0 -* Contributors: Felix Messmer, Florian Weisshardt, ipa-fxm, ipa-uhr-mk - -0.6.10 (2017-07-24) -------------------- - -0.6.9 (2017-07-18) ------------------- -* minor change for handling exception -* made changes which only sets the current that in turn is used by power_aggregator for relative_remaining_capacity calculation -* fix typo -* added emulation of realistic current value -* minor change for publishing a realistic voltage value -* Merge pull request `#310 `_ from souravran/feature/fake_bms - added a fake bms with set_charging and set_relative_remaining_capacity services -* finalize exception handling -* fake current -* consistent naming -* publish diagnostics in fake_bms -* harmonize namespaces of fake_bms -* uses the default parameter value -* poll frequency has been set from the parameter list -* made changes as per the review. - power state elements being published at 20 Hz. - removed junk rospy log and changed division_by_zero error message. -* fake_bms publishing all power_state entities. - added exception handling in power_state_aggregator. - added package dependency and install tags. -* added a fake bms with set_charging and set_relative_remaining_capacity services -* fix typo -* fix powerstate aggregator charging flag (bms is not delivering correct flag for full battery and docked) -* use bms flag for harging -* fix identation -* use spaces for indention in BMS driver -* updated authors -* added support for bit_mask'ed booleans -* make BmsParameter an abstract base class -* BMS driver clean-up -* switch from map of vectors to multimap in BMS driver -* simplified BMS publisher creation and polling list optimization -* simplified BMS config parsing -* manually fix changelog -* Contributors: Felix Messmer, Florian Weisshardt, Mathias Lüdtke, Nadia Hammoudeh García, fmw-ss, ipa-fxm, robot - -0.6.8 (2016-10-10) ------------------- -* restart CAN on failure -* move power_state_phidget node to new package -* invert current + round values -* fix typo -* corrected namespace -* added node to calculate powerstate from phidget board -* Contributors: Benjamin Maidel, Mathias Lüdtke - -0.6.7 (2016-04-02) ------------------- -* add missing dependencies -* Contributors: ipa-fxm - -0.6.6 (2016-04-01) ------------------- -* dependency and package cleanup -* remove config and launch as it is added to cob_robots -* adjust version -* move cob_bms_driver to cob_driver -* Contributors: ipa-fxm - -0.6.5 (2015-08-31) ------------------- - -0.6.4 (2015-08-25) ------------------- - -0.6.3 (2015-06-17) ------------------- - -0.6.2 (2014-12-15) ------------------- - -0.6.1 (2014-09-17) ------------------- - -0.6.0 (2014-09-09) ------------------- - -0.5.7 (2014-08-26 09:47) ------------------------- - -0.5.6 (2014-08-26 09:42) ------------------------- - -0.5.5 (2014-08-26 08:33) ------------------------- - -0.5.4 (2014-08-25) ------------------- - -0.5.3 (2014-03-31) ------------------- - -0.5.2 (2014-03-21) ------------------- - -0.5.1 (2014-03-20 10:54) ------------------------- diff --git a/cob_bms_driver/CMakeLists.txt b/cob_bms_driver/CMakeLists.txt deleted file mode 100644 index 17529ac95..000000000 --- a/cob_bms_driver/CMakeLists.txt +++ /dev/null @@ -1,41 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(cob_bms_driver) - -find_package(catkin REQUIRED COMPONENTS - diagnostic_msgs - diagnostic_updater - roscpp - socketcan_interface - std_msgs -) - -catkin_package( - CATKIN_DEPENDS diagnostic_msgs roscpp std_msgs -) - -########### -## Build ## -########### -include_directories( - include - ${catkin_INCLUDE_DIRS} -) - -add_executable(bms_driver_node src/${PROJECT_NAME}_node.cpp) -add_dependencies(bms_driver_node ${catkin_EXPORTED_TARGETS}) -target_link_libraries(bms_driver_node ${catkin_LIBRARIES}) - -############# -## Install ## -############# -install(TARGETS bms_driver_node - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -catkin_install_python(PROGRAMS - src/fake_bms.py - src/power_state_aggregator.py - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) diff --git a/cob_bms_driver/include/cob_bms_driver/cob_bms_driver_node.h b/cob_bms_driver/include/cob_bms_driver/cob_bms_driver_node.h deleted file mode 100644 index 02f141029..000000000 --- a/cob_bms_driver/include/cob_bms_driver/cob_bms_driver_node.h +++ /dev/null @@ -1,122 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -#ifndef COB_BMS_DRIVER_NODE_H -#define COB_BMS_DRIVER_NODE_H - -#include -#include -#include -#include - -#include -#include - -struct BmsParameter -{ - unsigned int offset; - unsigned int length; - - bool is_signed; - - ros::Publisher publisher; - - diagnostic_msgs::KeyValue kv; - - virtual ~BmsParameter() {} - - virtual void update(const can::Frame &f) = 0; - virtual void advertise(ros::NodeHandle &nh, const std::string &topic) = 0; - - typedef boost::shared_ptr Ptr; -}; - -class CobBmsDriverNode -{ -private: - ros::NodeHandle nh_; - ros::NodeHandle nh_priv_; - - typedef std::multimap ConfigMap; - - //ROS parameters - ConfigMap config_map_; //holds all the information that is provided in the configuration file - int poll_period_for_two_ids_in_ms_; - std::string can_device_; - int bms_id_to_poll_; - ros::Timer updater_timer_; - - boost::mutex data_mutex_; - - //polling lists that contain CAN-ID(s) that are to be polled. Each CAN-ID corresponds to a group of BMS parameters - std::vector polling_list1_; - std::vector polling_list2_; - std::vector::iterator polling_list1_it_; - std::vector::iterator polling_list2_it_; - - //interface to send and recieve CAN frames - can::ThreadedSocketCANInterface socketcan_interface_; - - //pointer to callback function to handle CAN frames from BMS - can::CommInterface::FrameListenerConstSharedPtr frame_listener_; - - //diagnostics data received from BMS - diagnostic_updater::DiagnosticStatusWrapper stat_; - - //function to get ROS parameters from parameter server - bool getParams(); - - //function to interpret the diagnostics XmlRpcValue and save data in config_map_ - bool loadConfigMap(XmlRpc::XmlRpcValue &diagnostics, std::vector &topics); - - //helper function to evaluate poll period from given poll frequency - void evaluatePollPeriodFrom(int poll_frequency); - - //function that goes through config_map_ and fills polling_list1_ and polling_list2_ with CAN-IDs. - //If Topics are found on ROS Parameter Server, they are kept in a separate list (to be polled faster). - //Otherwise, all CAN-ID are divided between both lists. - void optimizePollingLists(); - - //function that polls BMS (bms_id_to_poll_ is used here!). - //Also, this function sleeps for time given by poll_period_for_two_ids_in_ms_ to ensure BMS is polled at the desired frequency - void pollBmsForIds(const uint16_t first_id, const uint16_t second_id); - - //callback function to handle all types of frames received from BMS - void handleFrames(const can::Frame &f); - - //updates the diagnostics data with the new data received from BMS - void produceDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat); - - //calls update function of diagnostics_updater - void diagnosticsTimerCallback(const ros::TimerEvent&); -public: - - //updater for diagnostics data - diagnostic_updater::Updater updater_; - - CobBmsDriverNode(); - ~CobBmsDriverNode(); - - //initlializes SocketCAN interface, saves data from ROS parameter server, loads polling lists and sets up diagnostic updater - bool prepare(); - - //cycles through polling lists and sends 2 ids at a time (one from each list) to the BMS - void pollNextInLists(); -}; - - -#endif //COB_BMS_DRIVER_NODE_H diff --git a/cob_bms_driver/package.xml b/cob_bms_driver/package.xml deleted file mode 100644 index 20cbf365a..000000000 --- a/cob_bms_driver/package.xml +++ /dev/null @@ -1,35 +0,0 @@ - - - - cob_bms_driver - 0.7.16 - - Driver package for interfacing the battery management system (BMS) on Care-O-bot. - - - Felix Messmer - Florian Weisshardt - - Apache 2.0 - - mig-mc - Mathias Lüdtke - - catkin - - diagnostic_msgs - diagnostic_updater - roscpp - socketcan_interface - std_msgs - - cob_msgs - cob_srvs - python-numpy - python3-numpy - rospy - sensor_msgs - - diff --git a/cob_bms_driver/src/cob_bms_driver_node.cpp b/cob_bms_driver/src/cob_bms_driver_node.cpp deleted file mode 100644 index e384dda7a..000000000 --- a/cob_bms_driver/src/cob_bms_driver_node.cpp +++ /dev/null @@ -1,473 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -#include -#include -#include -#include -#include - -#include -#include -#include - -#include - -using boost::make_shared; - -//template to be used to read CAN frames received from the BMS -template void big_endian_to_host(const void* in, void* out); -template<> void big_endian_to_host<1>(const void* in, void* out){ *(uint8_t*)out = *(uint8_t*)in;} -template<> void big_endian_to_host<2>(const void* in, void* out){ *(uint16_t*)out = be16toh(*(uint16_t*)in);} -template<> void big_endian_to_host<4>(const void* in, void* out){ *(uint32_t*)out = be32toh(*(uint32_t*)in);} -template<> void big_endian_to_host<8>(const void* in, void* out){ *(uint64_t*)out = be64toh(*(uint64_t*)in);} - -template T read_value(const can::Frame &f, uint8_t offset){ - T res; - big_endian_to_host(&f.data[offset], &res); - return res; -} -template bool readTypedValue(const can::Frame &f, const BmsParameter ¶m, T &data){ - switch (param.length) - { - case 1: - data = param.is_signed ? read_value (f, param.offset) : read_value (f, param.offset); - break; - - case 2: - data = param.is_signed ? read_value (f, param.offset) : read_value (f, param.offset); - break; - - case 4: - data = param.is_signed ? read_value (f, param.offset) : read_value (f, param.offset); - break; - - default: - ROS_WARN_STREAM("Unknown length of BmsParameter: " << param.kv.key << ". Cannot read data!"); - return false; - } - return true; -} - -template struct TypedBmsParameter : BmsParameter { - T msg_; - - void publish(){ - //if the BmsParameter is a topic, publish data to the topic - if (static_cast(publisher)) - { - publisher.publish(msg_); - } - } - virtual void advertise(ros::NodeHandle &nh, const std::string &topic){ - publisher = nh.advertise (topic, 1, true); - } -}; - -struct FloatBmsParameter : TypedBmsParameter { - double factor; - FloatBmsParameter(double factor) : factor(factor) {} - void update(const can::Frame &f){ - readTypedValue(f, *this, msg_.data); - msg_.data *= factor; - - //save data for diagnostics updater (and round to two digits for readability) - std::stringstream sstream; - sstream << std::setprecision(2) << msg_.data; - kv.value = sstream.str(); - publish(); - } -}; - -struct IntBmsParameter : TypedBmsParameter { - IntBmsParameter() {} - void update(const can::Frame &f){ - readTypedValue(f, *this, msg_.data); - - kv.value = (boost::format("%lld") % msg_.data).str(); - publish(); - } -}; - -struct UIntBmsParameter : TypedBmsParameter { - UIntBmsParameter() {} - void update(const can::Frame &f){ - readTypedValue(f, *this, msg_.data); - - kv.value = (boost::format("%llu") % msg_.data).str(); - publish(); - } -}; - -struct BooleanBmsParameter : TypedBmsParameter { - int bit_mask; - BooleanBmsParameter(int bit_mask) : bit_mask(bit_mask) { is_signed = true; } - void update(const can::Frame &f){ - int value; - readTypedValue(f, *this, value); - msg_.data = (value & bit_mask) == bit_mask; - - kv.value = msg_.data ? "True" : "False"; - publish(); - } -}; - -CobBmsDriverNode::CobBmsDriverNode() -: nh_priv_("~") -{} - -CobBmsDriverNode::~CobBmsDriverNode() -{ - socketcan_interface_.shutdown(); -} - -//initlializes SocketCAN interface, saves data from ROS parameter server, loads polling lists and sets up diagnostic updater -bool CobBmsDriverNode::prepare() -{ - //reads parameters from ROS parameter server and saves them in respective member variable: config_map_, poll_period_for_two_ids_in_ms_, can_device_, bms_id_to_poll_ - if (!getParams()) - { - ROS_ERROR("Could not prepare driver for start up"); - return false; - } - - optimizePollingLists(); - - //initalize polling lists iterators - polling_list1_it_ = polling_list1_.begin(); - polling_list2_it_ = polling_list2_.begin(); - - updater_.setHardwareID("bms"); - updater_.add("cob_bms_dagnostics_updater", this, &CobBmsDriverNode::produceDiagnostics); - - updater_timer_ = nh_.createTimer(ros::Duration(updater_.getPeriod()), &CobBmsDriverNode::diagnosticsTimerCallback, this); - - //initialize the socketcan interface - if(!socketcan_interface_.init(can_device_, false)) { - ROS_ERROR("cob_bms_driver initialization failed"); - return false; - } - - //create listener for CAN frames - frame_listener_ = socketcan_interface_.createMsgListener(can::CommInterface::FrameDelegate(this, &CobBmsDriverNode::handleFrames)); - - return true; -} - -//function to get ROS parameters from parameter server -bool CobBmsDriverNode::getParams() -{ - //local declarations - XmlRpc::XmlRpcValue diagnostics; - std::vector topics; - int poll_frequency_hz; - - if (!nh_priv_.getParam("topics", topics)) - { - ROS_INFO_STREAM("Did not find \"topics\" on parameter server"); - } - if (topics.empty()) - { - ROS_INFO("Topic list is empty. No publisher created"); - } - - if (!nh_priv_.getParam("diagnostics", diagnostics)) - { - ROS_ERROR_STREAM("Did not find \"diagnostics\" on parameter server"); - return false; - } - try { - if(!loadConfigMap(diagnostics, topics)) return false; - } - catch(XmlRpc::XmlRpcException &e){ - ROS_ERROR_STREAM("Could not parse 'diagnostics': "<< e.getMessage()); - return false; - } - - if (!topics.empty()) - { - for(std::vector::iterator it = topics.begin(); it != topics.end(); ++it){ - ROS_ERROR_STREAM("Could not find entry for topic '" << *it << "'."); - } - return false; - } - - if (!nh_priv_.getParam("can_device", can_device_)) - { - ROS_INFO_STREAM("Did not find \"can_device\" on parameter server. Using default value: can0"); - can_device_ = "can0"; - } - - if (!nh_priv_.getParam("bms_id_to_poll", bms_id_to_poll_)) - { - ROS_INFO_STREAM("Did not find \"bms_id_to_poll\" on parameter server. Using default value: 0x200"); - bms_id_to_poll_ = 0x200; - } - - if (!nh_priv_.getParam("poll_frequency_hz", poll_frequency_hz)) - { - ROS_INFO_STREAM("Did not find \"poll_frequency\" on parameter server. Using default value: 20 Hz"); - poll_frequency_hz = 20; - } - evaluatePollPeriodFrom(poll_frequency_hz); - return true; -} - -//function to interpret the diagnostics XmlRpcValue and save data in config_map_ -bool CobBmsDriverNode::loadConfigMap(XmlRpc::XmlRpcValue &diagnostics, std::vector &topics) -{ - //for each id in list of ids - for (size_t i = 0; i < diagnostics.size(); ++i) - { - uint8_t id; - XmlRpc::XmlRpcValue config = diagnostics[i]; - - if(!config.hasMember("id")) { - ROS_ERROR_STREAM("diagnostics[" << i << "]: id is missing."); - return false; - } - if(!config.hasMember("fields")) { - ROS_ERROR_STREAM("diagnostics[" << i << "]: fields is missing."); - return false; - } - id = static_cast(static_cast(config["id"])); - - XmlRpc::XmlRpcValue fields = config["fields"]; - bool publishes = false; - - for(int32_t j=0; j(field["name"]); - - if(!field.hasMember("len")){ - ROS_ERROR_STREAM("diagnostics[" << i << "]: fields[" << j << "]: len is missing."); - return false; - } - int len = static_cast(field["len"]); - - BmsParameter::Ptr entry; - if(field.hasMember("bit_mask")){ - int bit_mask = static_cast(field["bit_mask"]); - if(bit_mask & ~((1<<(len*8))-1)){ - ROS_ERROR_STREAM("diagnostics[" << i << "]: fields[" << j << "]: bit_mask does fit not into type of length " << len); - return false; - } - entry = make_shared(bit_mask); - entry->kv.key = name; - }else{ - if(!field.hasMember("is_signed")){ - ROS_ERROR_STREAM("diagnostics[" << i << "]: fields[" << j << "]: is_signed is missing."); - return false; - } - if(field.hasMember("factor")){ - double factor = 1.0; - if(field.hasMember("factor")){ - factor = static_cast(field["factor"]); - } - - entry = make_shared(factor); - - entry->is_signed = static_cast(field["is_signed"]); - - if(field.hasMember("unit")){ - entry->kv.key = name + "[" + static_cast(field["unit"]) + "]"; - }else{ - entry->kv.key = name; - } - }else{ - if(static_cast(field["is_signed"])){ - entry = make_shared(); - }else{ - entry = make_shared(); - } - entry->is_signed = static_cast(field["is_signed"]); - entry->kv.key = name; - } - } - - if(!field.hasMember("offset")){ - ROS_ERROR_STREAM("diagnostics[" << i << "]: fields[" << j << "]: offset is missing."); - return false; - } - entry->offset = static_cast(field["offset"]); - - entry->length = len; - - std::vector::iterator topic_it = find(topics.begin(), topics.end(), name); - if(topic_it != topics.end()){ - entry->advertise(nh_priv_, name); - topics.erase(topic_it); - publishes = true; - ROS_INFO_STREAM("Created publisher for: " << name); - } - - config_map_.insert(std::make_pair(id, entry)); - - } - if(publishes) polling_list1_.push_back(id); - else polling_list2_.push_back(id); - - ROS_INFO_STREAM("Got "<< fields.size() << " BmsParameter(s) with CAN-ID: 0x" << std::hex << (unsigned int) id << std::dec); - } - return true; -} - -//helper function to evaluate poll period from given poll frequency -void CobBmsDriverNode::evaluatePollPeriodFrom(int poll_frequency_hz) -{ - //check the validity of given poll_frequency_hz - if ((poll_frequency_hz < 0) && (poll_frequency_hz > 20)) - { - ROS_WARN_STREAM("Invalid ROS parameter value: poll_frequency_hz = "<< poll_frequency_hz << ". Setting poll_frequency_hz to 20 Hz"); - poll_frequency_hz = 20; - } - //now evaluate and save poll period - poll_period_for_two_ids_in_ms_ = (1/double(poll_frequency_hz))*1000; - ROS_INFO_STREAM("Evaluated polling period: "<< poll_period_for_two_ids_in_ms_ << " ms"); -} - -//function that goes through config_map_ and fills polling_list1_ and polling_list2_. If topics are found on ROS Parameter Server, they are kept in list1 otherwise, all parameter id are divided between both lists. -void CobBmsDriverNode::optimizePollingLists() -{ - if(polling_list1_.size() == 0){ // no topics, so just distribute topics - while(polling_list1_.size() < polling_list2_.size()){ - polling_list1_.push_back(polling_list2_.back()); - polling_list2_.pop_back(); - } - }else{ - while(polling_list1_.size() > polling_list2_.size()){ - polling_list2_.push_back(polling_list1_.back()); - polling_list1_.pop_back(); - } - } - ROS_INFO_STREAM("Loaded \'"<< polling_list1_.size() << "\' CAN-ID(s) in polling_list1_ and \'"<< polling_list2_.size() <<"\' CAN-ID(s) in polling_list2_"); -} - -//function that polls BMS for given ids -void CobBmsDriverNode::pollBmsForIds(const uint16_t first_id, const uint16_t second_id) -{ - can::Frame f(can::Header(bms_id_to_poll_,false,false,false),4); - f.data[0] = first_id >> 8; //high_byte - f.data[1] = first_id & 0xff; //low_byte - f.data[2] = second_id >> 8; - f.data[3] = second_id & 0xff; - - socketcan_interface_.send(f); - - boost::this_thread::sleep_for(boost::chrono::milliseconds(poll_period_for_two_ids_in_ms_)); -} - -//cycles through polling lists and sends 2 ids at a time (one from each list) to the BMS -void CobBmsDriverNode::pollNextInLists() -{ - //restart if reached the end of polling lists - if (polling_list1_it_ == polling_list1_.end()) polling_list1_it_ = polling_list1_.begin(); - if (polling_list2_it_ == polling_list2_.end()) polling_list2_it_ = polling_list2_.begin(); - //clear stat_, so that it can be refilled with new data - stat_.clear(); - - uint16_t first_id = (polling_list1_it_ == polling_list1_.end()) ? 0 : (*polling_list1_it_ | 0x0100); - uint16_t second_id = (polling_list2_it_ == polling_list2_.end()) ? 0 : (*polling_list2_it_ | 0x0100); - - ROS_DEBUG_STREAM("polling BMS for CAN-IDs: 0x" << std::hex << (int)first_id << " and 0x" << (int) second_id << std::dec); - - pollBmsForIds(first_id,second_id); - - //increment iterators for next poll - if (!polling_list1_.empty()) ++polling_list1_it_; - if (!polling_list2_.empty()) ++polling_list2_it_; -} - -//callback function to handle all types of frames received from BMS -void CobBmsDriverNode::handleFrames(const can::Frame &f) -{ - boost::mutex::scoped_lock lock(data_mutex_); - - //id to find in config map - std::pair range = config_map_.equal_range(f.id); - - for (; range.first != range.second; ++range.first) - { - range.first->second->update(f); - } -} - -//updates the diagnostics data with the new data received from BMS -void CobBmsDriverNode::produceDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat) -{ - boost::mutex::scoped_lock lock(data_mutex_); - - can::State state = socketcan_interface_.getState(); - stat.add("error_code", state.error_code); - stat.add("can_error_code", state.internal_error); - switch (state.driver_state) - { - case can::State::closed: - stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Driver State: Closed"); - break; - - case can::State::open: - stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Driver State: Opened"); - break; - - case can::State::ready: - stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Driver State: Ready"); - break; - - default: - stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Driver State: Unknown"); - break; - } - - for (ConfigMap::iterator cm_it = config_map_.begin(); cm_it != config_map_.end(); ++cm_it) - { - stat.values.push_back(cm_it->second->kv); - } -} - -void CobBmsDriverNode::diagnosticsTimerCallback(const ros::TimerEvent& event) -{ - //update diagnostics - updater_.update(); - if(!socketcan_interface_.getState().isReady()){ - ROS_ERROR("Restarting BMS socketcan"); - socketcan_interface_.shutdown(); - socketcan_interface_.recover(); - } -} - -int main(int argc, char **argv) -{ - ros::init(argc, argv, "bms_driver_node"); - - CobBmsDriverNode cob_bms_driver_node; - - if (!cob_bms_driver_node.prepare()) return 1; - - ROS_INFO("Started polling BMS..."); - while (ros::ok()) - { - cob_bms_driver_node.pollNextInLists(); - ros::spinOnce(); - } - return 0; -} diff --git a/cob_bms_driver/src/fake_bms.py b/cob_bms_driver/src/fake_bms.py deleted file mode 100755 index 22ebad7db..000000000 --- a/cob_bms_driver/src/fake_bms.py +++ /dev/null @@ -1,91 +0,0 @@ -#!/usr/bin/env python -# -# Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - - -import rospy -from std_msgs.msg import Bool -from std_msgs.msg import Float64 -from cob_srvs.srv import SetFloat, SetFloatResponse -from cob_srvs.srv import SetInt, SetIntResponse -from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus -from diagnostic_updater import Updater - -class FakeBMS(object): - def __init__(self): - self._srv_current = rospy.Service('~set_current', SetFloat, self.current_cb) - self._srv_relative_remaining_capacity = rospy.Service('~set_relative_remaining_capacity', SetInt, self.relative_remaining_capacity_cb) - self._poll_frequency = rospy.get_param('~poll_frequency_hz', 20.0) - self._pub_voltage = rospy.Publisher('~voltage', Float64, queue_size = 1) - self._pub_current = rospy.Publisher('~current', Float64, queue_size = 1) - self._pub_remaining_capacity = rospy.Publisher('~remaining_capacity', Float64, queue_size = 1) - self._pub_full_charge_capacity = rospy.Publisher('~full_charge_capacity', Float64, queue_size = 1) - self._pub_temparature = rospy.Publisher('~temperature', Float64, queue_size = 1) - - self._updater = Updater() - self._updater.setHardwareID("bms") - self._updater.add("cob_bms_dagnostics_updater", self.produce_diagnostics) - - self._voltage = rospy.get_param('~voltage', 48.0) # V - self._current = rospy.get_param('~current', -8.0) # A - self._remaining_capacity = rospy.get_param('~remaining_capacity', 35.0) # Ah - self._full_charge_capacity = rospy.get_param('~full_charge_capacity', 35.0) # Ah - self._temperature = rospy.get_param('~temperature', 25.0) # °C - - rospy.Timer(rospy.Duration(1.0), self.publish_diagnostics) - rospy.Timer(rospy.Duration(1.0/self._poll_frequency), self.timer_cb) - rospy.Timer(rospy.Duration(1.0/self._poll_frequency), self.timer_consume_power_cb) - - def current_cb(self, req): - self._current = round(req.data,2) - res_current = SetFloatResponse(True, "Set current to {}".format(self._current)) - return res_current - - def relative_remaining_capacity_cb(self, req): - self._remaining_capacity = round(((req.data * self._full_charge_capacity)/100.0), 3) - res_capacity = SetIntResponse(True, "Set remaining capacity to {}".format(self._remaining_capacity)) - return res_capacity - - def publish_diagnostics(self, event): - self._updater.update() - - def produce_diagnostics(self, stat): - stat.summary(DiagnosticStatus.OK, "Fake Driver: Ready") - stat.add("current[A]", self._current) - stat.add("voltage[V]", self._voltage) - stat.add("temperature[Celsius]", self._temperature) - stat.add("remaining_capacity[Ah]", round(self._remaining_capacity, 3)) - stat.add("full_charge_capacity[Ah]", self._full_charge_capacity) - return stat - - def timer_cb(self, event): - self._pub_voltage.publish(self._voltage) - self._pub_current.publish(self._current) - self._pub_remaining_capacity.publish(round(self._remaining_capacity, 3)) - self._pub_full_charge_capacity.publish(self._full_charge_capacity) - self._pub_temparature.publish(self._temperature) - - def timer_consume_power_cb(self, event): - # emulate the battery usage based on the current values - self._remaining_capacity += (self._current/self._poll_frequency)/3600.0 - if self._remaining_capacity <= 0.0: - self._remaining_capacity = 0.0 - if self._remaining_capacity >= self._full_charge_capacity: - self._remaining_capacity = round(self._full_charge_capacity,3) - -if __name__ == '__main__': - rospy.init_node('fake_bms') - FakeBMS() - rospy.spin() diff --git a/cob_bms_driver/src/power_state_aggregator.py b/cob_bms_driver/src/power_state_aggregator.py deleted file mode 100755 index 30f28031e..000000000 --- a/cob_bms_driver/src/power_state_aggregator.py +++ /dev/null @@ -1,177 +0,0 @@ -#!/usr/bin/env python -# -# Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - - -import rospy -import numpy -import warnings -from std_msgs.msg import Float64 -from cob_msgs.msg import PowerState -from sensor_msgs.msg import BatteryState - -class PowerStateAggregator(): - - def __init__(self): - # get parameters - self.current_buffer_size = rospy.get_param('~current_buffer_size', 901) # approx. 60sec, topic rate 15Hz, use odd number to prevent 0.0 mean - self.pub_power_state = rospy.Publisher('power_state', PowerState, queue_size=1) - self.pub_battery_state = rospy.Publisher('battery_state', BatteryState, queue_size=1) - self.voltage = None - self.current = None - self.last_currents = [] - self.last_update = rospy.Time(0) - self.connected = False - self.charging = False - self.remaining_capacity = None - self.full_charge_capacity = None - self.temperature = None - rospy.Subscriber("voltage", Float64, self.voltage_cb) - rospy.Subscriber("current", Float64, self.current_cb) - rospy.Subscriber("remaining_capacity", Float64, self.remaining_capacity_cb) - rospy.Subscriber("full_charge_capacity", Float64, self.full_charge_capacity_cb) - rospy.Subscriber("temperature", Float64, self.temperature_cb) - - def voltage_cb(self, msg): - self.last_update = rospy.Time.now() - self.voltage = msg.data - - def current_cb(self, msg): - self.last_update = rospy.Time.now() - self.current = msg.data - - # fill current into list of past currents for filtering purposes - if len(self.last_currents) >= self.current_buffer_size: - self.last_currents.pop(0) - self.last_currents.append(msg.data) - - if msg.data > 0: - self.charging = True - else: - self.charging = False - - if msg.data > -1: # we use a limit of -1Ampere because if the battery is 100% full and the robot is still docked, there is no more current going into the battery. -1 A is biggger than the "Ruhestrom", so this should be ok until BMS is fixed and delivers a proper flag for docked or not_docked. - self.connected = True - else: - self.connected = False - - def remaining_capacity_cb(self, msg): - self.last_update = rospy.Time.now() - self.remaining_capacity = msg.data - - def full_charge_capacity_cb(self, msg): - self.last_update = rospy.Time.now() - self.full_charge_capacity = msg.data - - def temperature_cb(self, msg): - self.last_update = rospy.Time.now() - self.temperature = msg.data - - def calculate_power_consumption(self): - if not self.charging and self.voltage != None and self.current != None: - return round(self.voltage * abs(self.current), 3) - else: - return 0.0 - - def calculate_relative_remaining_capacity(self): - try: - return round(100.0*(self.remaining_capacity/self.full_charge_capacity), 3) - except ZeroDivisionError as e: - rospy.logerr("ZeroDivisionError: full_charge_capacity is 0.0: %s" % (e)) - except: - rospy.logwarn("something went wrong, cannot calculate relative remaining capacity. full_charge_capacity=%s, remaining_capacity=%s" % (self.full_charge_capacity, self.remaining_capacity)) - return 0.0 - - def calculate_time_remaining(self): - warnings.filterwarnings('error') - time_remaining_max = 10.0 # assume 10h - for cases where current approx. 0A - time_remaining = time_remaining_max - try: - if len(self.last_currents) > 0: - current = numpy.mean(self.last_currents) - - if self.full_charge_capacity != None and self.remaining_capacity != None: - if self.charging: - time_remaining = round((self.full_charge_capacity - self.remaining_capacity) / abs(current), 3) - else: - time_remaining = round(self.remaining_capacity / abs(current), 3) - else: - pass - else: - pass - except ZeroDivisionError as e: - rospy.logerr("ZeroDivisionError: current is 0.0: {}".format(e)) - except Warning as w: - rospy.logerr("Warning: {}".format(w)) - except Exception as e: - rospy.logerr("Exception: {}".format(e)) - # rospy.logdebug("calculate_time_remaining") - # rospy.logdebug("time_remaining_max: {}".format(time_remaining_max)) - # rospy.logdebug("time_remaining: {}".format(time_remaining)) - # rospy.logdebug("self.last_currents: {}".format(self.last_currents)) - # rospy.logdebug("current: {}".format(current)) - # rospy.logdebug("self.full_charge_capacity: {}".format(self.full_charge_capacity)) - # rospy.logdebug("self.remaining_capacity: {}".format(self.remaining_capacity)) - # rospy.logdebug("self.charging: {}".format(self.charging)) - return min(time_remaining, time_remaining_max) - - def publish(self): - if self.voltage != None and self.current != None and self.remaining_capacity != None and self.full_charge_capacity != None and self.temperature != None and (rospy.Time.now() - self.last_update) < rospy.Duration(1): - power_consumption = self.calculate_power_consumption() - relative_remaining_capacity = self.calculate_relative_remaining_capacity() - time_remaining = self.calculate_time_remaining() - - ps = PowerState() - ps.header.stamp = self.last_update - ps.voltage = self.voltage - ps.current = self.current - ps.power_consumption = power_consumption - ps.remaining_capacity = self.remaining_capacity - ps.relative_remaining_capacity = relative_remaining_capacity - ps.connected = self.connected - ps.charging = self.charging - ps.time_remaining = time_remaining - ps.temperature = self.temperature - - bs = BatteryState() - bs.header.stamp = self.last_update - bs.voltage = self.voltage - bs.current = self.current - bs.charge = self.remaining_capacity - bs.design_capacity = self.full_charge_capacity - bs.percentage = relative_remaining_capacity / 100.0 - bs.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_CHARGING if self.charging else BatteryState.POWER_SUPPLY_STATUS_NOT_CHARGING - bs.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_GOOD - bs.power_supply_technology = BatteryState.POWER_SUPPLY_TECHNOLOGY_UNKNOWN - bs.present = True - bs.cell_voltage = [self.voltage] - bs.location = "emulated_battery" - bs.serial_number = "emulated_battery" - - self.pub_power_state.publish(ps) - self.pub_battery_state.publish(bs) - -if __name__ == "__main__": - rospy.init_node("power_state_aggregator") - PSA = PowerStateAggregator() - rospy.loginfo("power state aggregator running") - rate = rospy.Rate(10) - while not rospy.is_shutdown(): - PSA.publish() - try: - rate.sleep() - except rospy.exceptions.ROSInterruptException as e: - pass - diff --git a/cob_driver/package.xml b/cob_driver/package.xml index 69ccdd8d5..d3999fa06 100644 --- a/cob_driver/package.xml +++ b/cob_driver/package.xml @@ -13,7 +13,6 @@ catkin - cob_bms_driver cob_light cob_mimic cob_phidgets