From 938d608bfb1a41a475fdd57204a4cf53bb25ae07 Mon Sep 17 00:00:00 2001 From: fmessmer Date: Wed, 20 Mar 2024 16:49:27 +0100 Subject: [PATCH] remove cob_voltage_control --- cob_driver/package.xml | 1 - cob_voltage_control/CHANGELOG.rst | 287 --------------- cob_voltage_control/CMakeLists.txt | 32 -- .../cfg/cob_voltage_control.cfg | 11 - .../common/src/cob_voltage_control_common.cpp | 123 ------- .../model/voltage_control.ros_package | 11 - .../model/voltage_control.ros_package_diagram | 43 --- cob_voltage_control/package.xml | 32 -- .../ros/config/voltage_filter.yaml | 0 .../ros/launch/csv_processing.launch | 13 - .../launch/record_current_and_voltage.launch | 13 - .../ros/src/cob_voltage_control_ros.cpp | 225 ------------ .../ros/src/record_current_and_voltage.py | 49 --- cob_voltage_control/ros/src/savitzky.py | 119 ------ cob_voltage_control/ros/src/savitzky_golay.py | 87 ----- cob_voltage_control/ros/src/time_volt.py | 344 ------------------ 16 files changed, 1390 deletions(-) delete mode 100644 cob_voltage_control/CHANGELOG.rst delete mode 100644 cob_voltage_control/CMakeLists.txt delete mode 100755 cob_voltage_control/cfg/cob_voltage_control.cfg delete mode 100644 cob_voltage_control/common/src/cob_voltage_control_common.cpp delete mode 100644 cob_voltage_control/model/voltage_control.ros_package delete mode 100644 cob_voltage_control/model/voltage_control.ros_package_diagram delete mode 100644 cob_voltage_control/package.xml delete mode 100644 cob_voltage_control/ros/config/voltage_filter.yaml delete mode 100644 cob_voltage_control/ros/launch/csv_processing.launch delete mode 100644 cob_voltage_control/ros/launch/record_current_and_voltage.launch delete mode 100644 cob_voltage_control/ros/src/cob_voltage_control_ros.cpp delete mode 100755 cob_voltage_control/ros/src/record_current_and_voltage.py delete mode 100755 cob_voltage_control/ros/src/savitzky.py delete mode 100755 cob_voltage_control/ros/src/savitzky_golay.py delete mode 100755 cob_voltage_control/ros/src/time_volt.py diff --git a/cob_driver/package.xml b/cob_driver/package.xml index 8d779e7e2..d6a6001e2 100644 --- a/cob_driver/package.xml +++ b/cob_driver/package.xml @@ -27,7 +27,6 @@ cob_sound cob_undercarriage_ctrl cob_utilities - cob_voltage_control diff --git a/cob_voltage_control/CHANGELOG.rst b/cob_voltage_control/CHANGELOG.rst deleted file mode 100644 index 06e3c3a1e..000000000 --- a/cob_voltage_control/CHANGELOG.rst +++ /dev/null @@ -1,287 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package cob_voltage_control -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.7.16 (2024-02-20) -------------------- - -0.7.15 (2023-11-06) -------------------- - -0.7.14 (2022-11-17) -------------------- - -0.7.13 (2022-07-29) -------------------- - -0.7.12 (2022-03-15) -------------------- - -0.7.11 (2022-01-12) -------------------- - -0.7.10 (2021-12-23) -------------------- - -0.7.9 (2021-11-26) ------------------- - -0.7.8 (2021-10-19) ------------------- - -0.7.7 (2021-08-02) ------------------- - -0.7.6 (2021-05-10) ------------------- -* Merge pull request `#423 `_ from mikaelarguedas/python2-deps - ROS_PYTHON_VERSION conditional dependency for `python-tk` and `python-numpy` -* ROS_PYTHON_VERSION conditional dependency for python-tk -* Contributors: Felix Messmer, Mikael Arguedas - -0.7.5 (2021-04-06) ------------------- -* Merge pull request `#418 `_ from fmessmer/fix_catkin_lint - fix catkin_lint -* fix catkin_lint -* Contributors: Felix Messmer, fmessmer - -0.7.4 (2020-10-14) ------------------- -* Merge pull request `#417 `_ from fmessmer/test_noetic - test noetic -* ROS_PYTHON_VERSION conditional dependency for matplotlib -* Bump CMake version to avoid CMP0048 warning -* Contributors: Felix Messmer, fmessmer - -0.7.3 (2020-03-18) ------------------- - -0.7.2 (2020-03-18) ------------------- -* Merge pull request `#409 `_ from LoyVanBeek/feature/python3_compatibility - [ci_updates] pylint + Python3 compatibility -* fix pylint errors -* Merge pull request `#408 `_ from fmessmer/ci_updates - [travis] ci updates -* catkin_lint fixes -* Contributors: Felix Messmer, fmessmer - -0.7.1 (2019-11-07) ------------------- - -0.7.0 (2019-08-06) ------------------- -* Merge pull request `#396 `_ from HannesBachter/indigo_dev - 0.6.15 -* Contributors: Felix Messmer - -0.6.15 (2019-07-17) -------------------- - -0.6.14 (2019-06-07) -------------------- - -0.6.13 (2019-03-14) -------------------- - -0.6.12 (2018-07-21) -------------------- - -0.6.11 (2018-01-07) -------------------- -* Merge remote-tracking branch 'origin/indigo_release_candidate' into indigo_dev -* Merge pull request `#341 `_ from ipa-fxm/APACHE_license - use license apache 2.0 -* use license apache 2.0 -* Contributors: Felix Messmer, ipa-fxm, ipa-uhr-mk - -0.6.10 (2017-07-24) -------------------- - -0.6.9 (2017-07-18) ------------------- -* Update csv_processing.launch -* manually fix changelog -* Contributors: Felix Messmer, ipa-fxm - -0.6.8 (2016-10-10) ------------------- - -0.6.7 (2016-04-02) ------------------- - -0.6.6 (2016-04-01) ------------------- -* beautify -* Merge branch 'aggregated_power_message' into feature/raw_batterie_state - Conflicts: - cob_voltage_control/common/src/cob_voltage_control_common.cpp - cob_voltage_control/ros/src/cob_voltage_control_ros.cpp -* voltage and current measurements for raw3-3 -* use aggregated power state message -* changed name relayboard to powerboard -* removed debug outputs -* fixed topic names -* Contributors: Benjamin Maidel, ipa-bnm, ipa-fmw - -0.6.5 (2015-08-31) ------------------- - -0.6.4 (2015-08-25) ------------------- -* explicit dependency to boost -* remove trailing whitespaces -* add_dependencies EXPORTED_TARGETS -* migrate to package format 2 -* sort dependencies -* critically review dependencies -* Contributors: ipa-fxm - -0.6.3 (2015-06-17) ------------------- - -0.6.2 (2014-12-15) ------------------- -* fix install tag -* Tested on cob4-2 -* make record current and voltage generic -* Contributors: ipa-cob4-2 - -0.6.1 (2014-09-17) ------------------- -* merge conflict -* Deleted CurrentMeasurement.msg -* Re-add time_volt -* reocord current script and launch file -* fix install tags -* Current measurement -* Removed emergency model and custom Relayboard -* Merge branch 'groovy_dev' of https://github.com/ipa320/cob_driver into groovy_dev - Conflicts: - cob_relayboard/ros/src/new_method.py - cob_relayboard/ros/src/relayboard_sim.py -* Voltage filter now on the config folder -* Changes -* Adjusting path for the required files -* More thrash -* Removing some thrash -* License -* Modifications for the battery characterization on the robot -* Contributors: Thiago de Freitas, ipa-cob4-2, ipa-nhg, thiagodefreitas, thiagodefreitas@gmail.com - -0.6.0 (2014-09-09) ------------------- -* trying to fix cob_voltage_control buildfarm error -* missing dependency -* Contributors: Florian Weisshardt, ipa-fxm - -0.5.7 (2014-08-26) ------------------- -* Merge pull request `#163 `_ from ipa320/hydro_dev - updates from hydro_dev -* 0.5.6 -* update changelog -* merge -* move EmergencyStopState.msg to cob_msgs + PowerBoardState works again -* changes due to introduction of cob_msgs -* added message to submit voltage data -* Cleaned up cob_driver with reduced deps to compile on indigo -* increased receive buffer -* Merge pull request `#136 `_ from ipa-fmw/hydro_dev - change maintainer and add missing dependency -* Update package.xml -* fix -* fix -* voltage ctrl over phidgets -* voltage info from phidgets -* work in progress voltagectrl with cob_phidget - Merge branch 'groovy_dev' into feature/voltagectrl_newphidget - Conflicts: - cob_voltage_control/ros/src/cob_voltage_control_ros.cpp -* voltagectrl work in progress -* Contributors: Alexander Bubeck, Felix Messmer, Florian Weisshardt, Nadia Hammoudeh García, ipa-bnm, ipa-fxm - -0.5.6 (2014-08-26) ------------------- -* Merge pull request `#163 `_ from ipa320/hydro_dev - updates from hydro_dev -* merge -* move EmergencyStopState.msg to cob_msgs + PowerBoardState works again -* changes due to introduction of cob_msgs -* added message to submit voltage data -* Cleaned up cob_driver with reduced deps to compile on indigo -* increased receive buffer -* Merge pull request `#136 `_ from ipa-fmw/hydro_dev - change maintainer and add missing dependency -* Update package.xml -* fix -* fix -* voltage ctrl over phidgets -* voltage info from phidgets -* work in progress voltagectrl with cob_phidget - Merge branch 'groovy_dev' into feature/voltagectrl_newphidget - Conflicts: - cob_voltage_control/ros/src/cob_voltage_control_ros.cpp -* voltagectrl work in progress -* Contributors: Alexander Bubeck, Felix Messmer, Florian Weisshardt, Nadia Hammoudeh García, ipa-bnm, ipa-fxm - -0.5.3 (2014-03-31) ------------------- -* install tags -* Contributors: ipa-fxm - -0.5.2 (2014-03-20) ------------------- - -0.5.1 (2014-03-20) ------------------- -* some install tag updates -* removed phidget21.h include inside main source file due to missing header guard -* bug fix in voltage_control -* remove old rosbuild import -* merged with groovy_dev upstream -* No need for the stds, thrash from the voltage checks on robot-at-work -* Modification from RAW3-1 -* Installation stuff -* fix for battery dashboard -* V instead of mV -* use v not mV -* Set parameter nedded for battery monitor -* cleaned up CMakeLists and added install directives -* further modifications for catkin, now everything is compiling and linking -* Removed csv files from the cob_voltage_control -* Changed launch file to be related to each robot -* compiling but still some linker errors -* Second catkinization push -* First catkinization, still need to update some CMakeLists.txt -* debugging voltage filter -* fix launch file -* nasty fix for rob3-6 -* Changes the modes nomenclatures -* More organization to the voltage commit -* Organizing the voltage filter commit -* Reverting new method -* Method modification on the robot -* Reverting new method -* Merge branch 'groovy_dev' of github.com:thiagodefreitas/cob_driver into groovy_dev - Conflicts: - cob_voltage_control/ros/src/new_method.py -* Method -* Modifications to the modes -* Mods -* Launch files -* IPA PC -* Starting real time implementation -* Saved at home computer -* Mods -* More mods on the battery statistics -* Mods on the plots -* Starting the statistical analysis of the battery -* added topic for relayboard message -* changed mapping of em state topics to make sense for gui -* added parameters -* added simple voltage calculation -* changes with sensor attached -* initial version of cob_voltage_control -* Contributors: Alexander Bubeck, Frederik Hegger, Thiago de Freitas, Thiago de Freitas Oliveira Araujo, abubeck, ipa-bnm, ipa-cob3-4, ipa-cob3-6, ipa-cob3-7, ipa-fmw, robot, thiagodefreitas, thiagodefreitas@gmail.com diff --git a/cob_voltage_control/CMakeLists.txt b/cob_voltage_control/CMakeLists.txt deleted file mode 100644 index 061ff1776..000000000 --- a/cob_voltage_control/CMakeLists.txt +++ /dev/null @@ -1,32 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(cob_voltage_control) - -find_package(catkin REQUIRED COMPONENTS cob_msgs cob_phidgets dynamic_reconfigure roscpp std_msgs) - -generate_dynamic_reconfigure_options(cfg/${PROJECT_NAME}.cfg) - -catkin_package( - CATKIN_DEPENDS cob_msgs cob_phidgets dynamic_reconfigure roscpp std_msgs -) - -### BUILD ### -include_directories(common/src ${catkin_INCLUDE_DIRS}) - -add_executable(${PROJECT_NAME} ros/src/${PROJECT_NAME}_ros.cpp) -add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) - -### INSTALL ### -install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -install(DIRECTORY ros/launch/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/ros/launch -) - -catkin_install_python(PROGRAMS ros/src/record_current_and_voltage.py ros/src/savitzky.py ros/src/savitzky_golay.py ros/src/time_volt.py - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) diff --git a/cob_voltage_control/cfg/cob_voltage_control.cfg b/cob_voltage_control/cfg/cob_voltage_control.cfg deleted file mode 100755 index cffa38c96..000000000 --- a/cob_voltage_control/cfg/cob_voltage_control.cfg +++ /dev/null @@ -1,11 +0,0 @@ -#!/usr/bin/env python - -from dynamic_reconfigure.parameter_generator_catkin import * - -gen = ParameterGenerator() - -gen.add("scanner_em_input", int_t, 0, "Autogenerated parameter based on model.", 0, 0, 100) -gen.add("voltage_level_input", int_t, 0, "Autogenerated parameter based on model.", 0, 0, 100) -gen.add("global_em_input", int_t, 0, "Autogenerated parameter based on model.", 1, 0, 100) - -exit(gen.generate("cob_voltage_control", "cob_voltage_control", "cob_voltage_control")) \ No newline at end of file diff --git a/cob_voltage_control/common/src/cob_voltage_control_common.cpp b/cob_voltage_control/common/src/cob_voltage_control_common.cpp deleted file mode 100644 index 2d1a8163a..000000000 --- a/cob_voltage_control/common/src/cob_voltage_control_common.cpp +++ /dev/null @@ -1,123 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -//ROS typedefs -#include "ros/ros.h" -#include -#include -#include -#include - -class cob_voltage_control_config -{ -public: - double min_voltage; - double max_voltage; - double max_voltage_res; - int num_voltage_port; - int num_em_stop_port; - int num_scanner_em_port; -}; - -class cob_voltage_control_data -{ -// autogenerated: don't touch this class -public: - //configuration data - - //input data - int in_phidget_voltage; - int in_phidget_current; - - //output data - cob_msgs::PowerState out_pub_power_state_; - cob_msgs::EmergencyStopState out_pub_em_stop_state_; - std_msgs::Float64 out_pub_voltage_; - std_msgs::Float64 out_pub_current_; -}; - -//document how this class has to look -//never change after first generation -class cob_voltage_control_impl -{ - -public: - - //CPhidgetInterfaceKitHandle IFK; - cob_voltage_control_impl() - { - //user specific code - } - void configure() - { - } - void update(cob_voltage_control_data &data, cob_voltage_control_config config) - { - //user specific code - //Get Battery Voltage - double voltage_raw = 0; - voltage_raw = data.in_phidget_voltage; - //Get Battery Current - double current_raw = 0; - current_raw = data.in_phidget_current; - - ROS_DEBUG("voltage_raw: %f", voltage_raw); - ROS_DEBUG("current_raw: %f", current_raw); - - //Calculation of real voltage - //max_voltage = 50V ; max_counts = 999 - //from measurements: 49.1 V => 486 counts - double max_counts = 693.0; // 3v => max - double voltage = voltage_raw * config.max_voltage_res/max_counts; - data.out_pub_voltage_.data = voltage; - ROS_DEBUG("voltage %f", voltage); - - //current calculation - //500 counts => 0 A - //999 counts => 2.5 A - //000 counts => -2.5 A - double count_min = 611; - double count_max = 450; - double v_min = -3.0; - double v_max = 1.6; - - double current = v_min+(v_max - v_min)*(current_raw-count_min)/(count_max - count_min); - data.out_pub_current_.data = current; - ROS_DEBUG("current %f", current); - - bool charging; - if (current > 0){charging = true;} - else {charging = false;} - - //Linear calculation for percentage - double percentage = (voltage - config.min_voltage) * 100/(config.max_voltage - config.min_voltage); - percentage = std::min(percentage, 100.0); - - data.out_pub_power_state_.header.stamp = ros::Time::now(); - data.out_pub_power_state_.voltage = voltage; - data.out_pub_power_state_.current = current; - data.out_pub_power_state_.power_consumption = voltage * current; - data.out_pub_power_state_.relative_remaining_capacity = percentage; - data.out_pub_power_state_.charging = charging; - - } - - void exit() - { - } - -}; diff --git a/cob_voltage_control/model/voltage_control.ros_package b/cob_voltage_control/model/voltage_control.ros_package deleted file mode 100644 index 825052332..000000000 --- a/cob_voltage_control/model/voltage_control.ros_package +++ /dev/null @@ -1,11 +0,0 @@ - - - - - - - - - - - diff --git a/cob_voltage_control/model/voltage_control.ros_package_diagram b/cob_voltage_control/model/voltage_control.ros_package_diagram deleted file mode 100644 index 78b88a881..000000000 --- a/cob_voltage_control/model/voltage_control.ros_package_diagram +++ /dev/null @@ -1,43 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/cob_voltage_control/package.xml b/cob_voltage_control/package.xml deleted file mode 100644 index 63bbfe22d..000000000 --- a/cob_voltage_control/package.xml +++ /dev/null @@ -1,32 +0,0 @@ - - - - cob_voltage_control - 0.7.16 - Interface to IO board that manages emergency stop and battery voltage on rob@work 3 - - Apache 2.0 - - None - - - Alexander Bubeck - Alexander Bubeck - - catkin - - cob_msgs - cob_phidgets - dynamic_reconfigure - roscpp - std_msgs - - python-matplotlib - python3-matplotlib - python-tk - python3-tk - rospy - - diff --git a/cob_voltage_control/ros/config/voltage_filter.yaml b/cob_voltage_control/ros/config/voltage_filter.yaml deleted file mode 100644 index e69de29bb..000000000 diff --git a/cob_voltage_control/ros/launch/csv_processing.launch b/cob_voltage_control/ros/launch/csv_processing.launch deleted file mode 100644 index 1cc003eb8..000000000 --- a/cob_voltage_control/ros/launch/csv_processing.launch +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - - - - - diff --git a/cob_voltage_control/ros/launch/record_current_and_voltage.launch b/cob_voltage_control/ros/launch/record_current_and_voltage.launch deleted file mode 100644 index 7392a95dd..000000000 --- a/cob_voltage_control/ros/launch/record_current_and_voltage.launch +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - - - - - diff --git a/cob_voltage_control/ros/src/cob_voltage_control_ros.cpp b/cob_voltage_control/ros/src/cob_voltage_control_ros.cpp deleted file mode 100644 index ffff84353..000000000 --- a/cob_voltage_control/ros/src/cob_voltage_control_ros.cpp +++ /dev/null @@ -1,225 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -// ROS includes -#include - -// ROS message includes -#include -#include -#include -#include - -#include -#include -#include - -class cob_voltage_control_ros -{ - private: - int EM_stop_status_; - - public: - ros::NodeHandle n_; - - ros::Publisher topicPub_em_stop_state_; - ros::Publisher topicPub_power_state_; - - ros::Publisher topicPub_Current_; - ros::Publisher topicPub_Voltage_; - - ros::Subscriber topicSub_AnalogInputs; - ros::Subscriber topicSub_DigitalInputs; - - cob_voltage_control_data component_data_; - cob_voltage_control_config component_config_; - cob_voltage_control_impl component_implementation_; - - // - bool last_rear_em_state; - bool last_front_em_state; - - // possible states of emergency stop - enum - { - ST_EM_FREE = 0, - ST_EM_ACTIVE = 1, - ST_EM_CONFIRMED = 2 - }; - - cob_voltage_control_ros() - { - topicPub_power_state_ = n_.advertise("power_state", 1); - topicPub_em_stop_state_ = n_.advertise("em_stop_state", 1); - - topicPub_Current_ = n_.advertise("current", 10); - topicPub_Voltage_ = n_.advertise("voltage", 10); - - topicSub_AnalogInputs = n_.subscribe("input/analog_sensors", 10, &cob_voltage_control_ros::analogPhidgetSignalsCallback, this); - topicSub_DigitalInputs = n_.subscribe("input/digital_sensors", 10, &cob_voltage_control_ros::digitalPhidgetSignalsCallback, this); - - n_.param("battery_max_voltage", component_config_.max_voltage, 48.5); - n_.param("battery_min_voltage", component_config_.min_voltage, 44.0); - n_.param("robot_max_voltage", component_config_.max_voltage_res, 70.0); - n_.param("voltage_analog_port", component_config_.num_voltage_port, 1); - n_.param("em_stop_dio_port", component_config_.num_em_stop_port, 0); - n_.param("scanner_stop_dio_port", component_config_.num_scanner_em_port, 1); - - last_rear_em_state = false; - last_front_em_state = false; - - EM_stop_status_ = ST_EM_ACTIVE; - component_data_.out_pub_em_stop_state_.scanner_stop = false; - } - - void configure() - { - component_implementation_.configure(); - } - - void update() - { - component_implementation_.update(component_data_, component_config_); - topicPub_Voltage_.publish(component_data_.out_pub_voltage_); - topicPub_Current_.publish(component_data_.out_pub_current_); - topicPub_power_state_.publish(component_data_.out_pub_power_state_); - topicPub_em_stop_state_.publish(component_data_.out_pub_em_stop_state_); - } - - void analogPhidgetSignalsCallback(const cob_phidgets::AnalogSensorConstPtr &msg) - { - for(int i = 0; i < msg->uri.size(); i++) - { - if( msg->uri[i] == "voltage") - { - component_data_.in_phidget_voltage = msg->value[i]; - } - if( msg->uri[i] == "current") - { - component_data_.in_phidget_current = msg->value[i]; - } - } - } - - void digitalPhidgetSignalsCallback(const cob_phidgets::DigitalSensorConstPtr &msg) - { - bool front_em_active = false; - bool rear_em_active = false; - static bool em_caused_by_button = false; - cob_msgs::EmergencyStopState EM_msg; - bool EM_signal = false; - bool got_message = false; - - for(int i = 0; i < msg->uri.size(); i++) - { - if( msg->uri[i] == "em_stop_laser_rear") - { - rear_em_active = !((bool)msg->state[i]); - got_message = true; - } - else if( msg->uri[i] == "em_stop_laser_front") - { - front_em_active = !((bool)msg->state[i]); - got_message = true; - } - } - if(got_message) - { - if( (front_em_active && rear_em_active) && (!last_front_em_state && !last_rear_em_state)) - { - component_data_.out_pub_em_stop_state_.emergency_button_stop = true; - em_caused_by_button = true; - } - else if((!front_em_active && !rear_em_active) && (last_front_em_state && last_rear_em_state)) - { - component_data_.out_pub_em_stop_state_.emergency_button_stop = false; - em_caused_by_button = false; - } - else if((front_em_active != rear_em_active) && em_caused_by_button) - { - component_data_.out_pub_em_stop_state_.emergency_button_stop = false; - em_caused_by_button = false; - component_data_.out_pub_em_stop_state_.scanner_stop = (bool)(front_em_active | rear_em_active); - } - else - { - component_data_.out_pub_em_stop_state_.scanner_stop = (bool)(front_em_active | rear_em_active); - } - - EM_signal = component_data_.out_pub_em_stop_state_.scanner_stop | component_data_.out_pub_em_stop_state_.emergency_button_stop; - - switch (EM_stop_status_) - { - case ST_EM_FREE: - { - if (EM_signal == true) - { - ROS_INFO("Emergency stop was issued"); - EM_stop_status_ = EM_msg.EMSTOP; - } - break; - } - case ST_EM_ACTIVE: - { - if (EM_signal == false) - { - ROS_INFO("Emergency stop was confirmed"); - EM_stop_status_ = EM_msg.EMCONFIRMED; - } - break; - } - case ST_EM_CONFIRMED: - { - if (EM_signal == true) - { - ROS_INFO("Emergency stop was issued"); - EM_stop_status_ = EM_msg.EMSTOP; - } - else - { - ROS_INFO("Emergency stop released"); - EM_stop_status_ = EM_msg.EMFREE; - } - break; - } - }; - - component_data_.out_pub_em_stop_state_.emergency_state = EM_stop_status_; - - last_front_em_state = front_em_active; - last_rear_em_state = rear_em_active; - } - } -}; - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "cob_voltage_control"); - - cob_voltage_control_ros node; - node.configure(); - - ros::Rate loop_rate(20); // Hz // if cycle time == 0 do a spin() here without calling node.update() - - while(node.n_.ok()) - { - node.update(); - loop_rate.sleep(); - ros::spinOnce(); - } - return 0; -} diff --git a/cob_voltage_control/ros/src/record_current_and_voltage.py b/cob_voltage_control/ros/src/record_current_and_voltage.py deleted file mode 100755 index 582b1a377..000000000 --- a/cob_voltage_control/ros/src/record_current_and_voltage.py +++ /dev/null @@ -1,49 +0,0 @@ -#!/usr/bin/env python -# -# Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - - -import csv - -import rospy -from cob_phidgets.msg import AnalogSensor - -def callback(data): - #print "voltage=", data.value[1] # voltage sensor is on the second port of the phidgets board - #print "current=", data.value[0] # current sensor is on the first port of the phidgets board - writer.writerow( ( round((rospy.Time.now() - starttime).to_sec(),5), data.value[1], data.value[0]) ) - -def record(): - rospy.init_node('record_current_and_voltage') - global starttime - starttime = rospy.Time.now() - - global f - global writer - filename = rospy.get_param("~filename") - global port_voltage, port_current - port_voltage = rospy.get_param("~port_voltage") - port_current = rospy.get_param("~port_current") - f = open(filename, 'wt', 1) - writer = csv.writer(f) - writer.writerow(("time_from_start", "voltage", "current")) - - rospy.Subscriber("/analog_sensors", AnalogSensor, callback) - - rospy.spin() - -if __name__ == '__main__': - record() - diff --git a/cob_voltage_control/ros/src/savitzky.py b/cob_voltage_control/ros/src/savitzky.py deleted file mode 100755 index 30bacd161..000000000 --- a/cob_voltage_control/ros/src/savitzky.py +++ /dev/null @@ -1,119 +0,0 @@ -#!/usr/bin/env python -# -# Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - - -import numpy as np -from math import factorial -import pylab - -class savitzky_golay(): - r"""Smooth (and optionally differentiate) data with a Savitzky-Golay filter. - The Savitzky-Golay filter removes high frequency noise from data. - It has the advantage of preserving the original shape and - features of the signal better than other types of filtering - approaches, such as moving averages techniques. - Parameters - ---------- - y : array_like, shape (N,) - the values of the time history of the signal. - window_size : int - the length of the window. Must be an odd integer number. - order : int - the order of the polynomial used in the filtering. - Must be less then `window_size` - 1. - deriv: int - the order of the derivative to compute (default = 0 means only smoothing) - Returns - ------- - ys : ndarray, shape (N) - the smoothed signal (or it's n-th derivative). - Notes - ----- - The Savitzky-Golay is a type of low-pass filter, particularly - suited for smoothing noisy data. The main idea behind this - approach is to make for each point a least-square fit with a - polynomial of high order over a odd-sized window centered at - the point. - Examples - -------- - t = np.linspace(-4, 4, 500) - y = np.exp( -t**2 ) + np.random.normal(0, 0.05, t.shape) - ysg = savitzky_golay(y, window_size=31, order=4) - import matplotlib.pyplot as plt - plt.plot(t, y, label='Noisy signal') - plt.plot(t, np.exp(-t**2), 'k', lw=1.5, label='Original signal') - plt.plot(t, ysg, 'r', label='Filtered signal') - plt.legend() - plt.show() - References - ---------- - .. [1] A. Savitzky, M. J. E. Golay, Smoothing and Differentiation of - Data by Simplified Least Squares Procedures. Analytical - Chemistry, 1964, 36 (8), pp 1627-1639. - .. [2] Numerical Recipes 3rd Edition: The Art of Scientific Computing - W.H. Press, S.A. Teukolsky, W.T. Vetterling, B.P. Flannery - Cambridge University Press ISBN-13: 9780521880688 - """ - - def __init__(self, window_size, order, deriv=0, rate=1): - - self.window_size = window_size - self.order = order - self.deriv = deriv - self.rate = rate - - try: - self.window_size = np.abs(np.int(self.window_size)) - self.order = np.abs(np.int(self.order)) - except ValueError as msg: - raise ValueError("ValueError: window_size and order have to be of type int - {}".format(msg)) - if self.window_size % 2 != 1 or self.window_size < 1: - raise TypeError("window_size size must be a positive odd number") - if self.window_size < self.order + 2: - raise TypeError("window_size is too small for the polynomials order") - - def filter(self, y): - - self.order_range = list(range(self.order+1)) - half_window = (self.window_size -1) // 2 - # precompute coefficients - b = np.mat([[k**i for i in self.order_range] for k in range(-half_window, half_window+1)]) - m = np.linalg.pinv(b).A[self.deriv] * self.rate**self.deriv * factorial(self.deriv) - # pad the signal at the extremes with - # values taken from the signal itself - firstvals = y[0] - np.abs( y[1:half_window+1][::-1] - y[0] ) - lastvals = y[-1] + np.abs(y[-half_window-1:-1][::-1] - y[-1]) - y = np.concatenate((firstvals, y, lastvals)) - return np.convolve( m[::-1], y, mode='valid') - -if __name__ == "__main__": - - # create some sample twoD data - x = np.linspace(-3,3,100) - Z = np.exp( np.negative(x)) - - # add noise - Zn = Z + np.random.normal( 0, 0.2, Z.shape ) - - sg = savitzky_golay(window_size=29, order=4) - - l = sg.filter(Zn) - - pylab.plot(l) - - pylab.plot(Zn) - - pylab.show() diff --git a/cob_voltage_control/ros/src/savitzky_golay.py b/cob_voltage_control/ros/src/savitzky_golay.py deleted file mode 100755 index bb24694d1..000000000 --- a/cob_voltage_control/ros/src/savitzky_golay.py +++ /dev/null @@ -1,87 +0,0 @@ -#!/usr/bin/env python -# -# Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - - -from math import cos, sin -import numpy as np - -import rospy -import savitzky -from std_msgs.msg import Float64 -from cob_msgs.msg import EmergencyStopState, PowerState - -class volts_filter(): - - def __init__(self): - self.volts = 0. - self.wsize = 61 - self.filter_order = 3 - self.theta = rospy.get_param("~theta") - self.off_y = rospy.get_param("~off_y") - self.abcd = rospy.get_param("~abcd") - self.maximum_time = rospy.get_param("~maximum_time") - self.sg = savitzky.savitzky_golay(window_size=self.wsize, order=self.filter_order) - size = 2*self.wsize+1 - self.volt_filt = 48000*np.ones(size) - - rospy.Subscriber("/power_board/voltage", Float64, self.callback) - - self.pub_power = rospy.Publisher('/power_state', PowerState, queue_size=1) - self.msg_power = PowerState() - - def callback(self, data): - - self.volts = data.data - self.volts = self.volts*1000 - - if(self.volts <= 44000): - self.volts = 44000 - elif(self.volts >= 48000): - self.volts = 48000 - - self.process_voltage() - - def process_voltage(self): - - self.volt_filt = np.insert(self.volt_filt, 0, self.volts) - self.volt_filt = np.delete(self.volt_filt, -1) - - vfilt = self.sg.filter(self.volt_filt) - - old_settings = np.seterr(all='raise') - - self.t_est = np.polyval(self.abcd, vfilt[self.wsize]) - - self.t_est = vfilt[self.wsize]*sin(self.theta) + self.t_est*cos(self.theta) - self.t_est = self.t_est + self.off_y - - if(self.t_est <0): - self.t_est = 0 - - self.msg_power.header.stamp = rospy.Time.now() - self.msg_power.time_remaining.secs = self.t_est - #self.msg_power.prediction_method = '3rd_order_polynom' - #self.msg_power.relative_capacity = (self.t_est/self.maximum_time) * 100 - #self.msg_power.AC_present = 0 - - self.pub_power.publish(self.msg_power) - -if __name__ == '__main__': - rospy.init_node('volt_filt') - vf = volts_filter() - - while not rospy.is_shutdown(): - rospy.sleep(1.0) diff --git a/cob_voltage_control/ros/src/time_volt.py b/cob_voltage_control/ros/src/time_volt.py deleted file mode 100755 index fa863d231..000000000 --- a/cob_voltage_control/ros/src/time_volt.py +++ /dev/null @@ -1,344 +0,0 @@ -#!/usr/bin/env python -# -# Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - - -import pickle -import csv -import pylab -import sys -import getopt - -from math import cos, sin -import numpy as np - -import rospy -import rosparam -import yaml -import savitzky - -def main(argv): - rospy.init_node('csv_proc') - - volt_values=[] - time_values=[] - - sg = savitzky.savitzky_golay(window_size=901, order=3) - -#################### -# Parameters for the Python script -#################### - - filename = rosparam.get_param("/csv_proc/file_name") - robot_name = rosparam.get_param("/csv_proc/robot_name") - mode = rosparam.get_param("/csv_proc/mode") - yaml_file = open("voltage_filter.yaml", "w") - yl = {} - - if(mode=="update"): - abcd = rosparam.get_param("/csv_proc/abcd") - - try: - opts, args = getopt.getopt(argv,"hf:r:",["ifile=", "irobot="]) - except getopt.GetoptError: - print('time_volt.py -f -r ') - sys.exit(2) - for opt, arg in opts: - if opt == '-h': - print('time_volt.py -f -r ') - sys.exit() - elif opt in ("-f", "--ifile"): - filename = arg - elif opt in ("-r", "--irobot"): - robot_name = arg - - -#################### -# Opening the csv file and getting the values for time and voltage -#################### - - with open(filename, 'rb') as csvfile: - - csvreader = csv.reader(csvfile, delimiter=' ', quotechar='|') - for row in csvreader: - row = row[0].split(',') - volt_v = (float)(row[1]) * 1000.0 - if(volt_v < 48000 and volt_v > 44000): - time_values.append((float)(row[0])) - volt_values.append(volt_v) - - time_values[:] = [x - time_values[0] for x in time_values] - time_values = time_values[::-1] - -#################### -# Plotting graphics for the Voltage vs Time -#################### - - pylab.figure(1) - - pylab.plot(volt_values, time_values) - pylab.ylabel("Time elapsed(seconds)") - pylab.xlabel("Voltage(mV)") - pylab.title("Time x Volt,file="+ filename.replace('.csv','')) - pylab.grid(True) - - secArray = np.asarray(time_values) - voltArray = np.asarray(volt_values) - - # Polyfit for the voltage values - z1 = np.polyfit(voltArray, secArray,1) - z2 = np.polyfit(voltArray, secArray,2) - z3 = np.polyfit(voltArray, secArray,3) - - # Linear space for better visualization - xp = np.linspace(49000, 43000, 100) - - # Generating the polynomial function from the fit - p1 = np.poly1d(z1) - p2 = np.poly1d(z2) - p3 = np.poly1d(z3) - - pylab.plot(xp, p1(xp), 'r-', xp, p2(xp), 'g-', xp, p3(xp), 'm-') - - pylab.text(46000, 11900, 'p1=' + p1.__str__(), bbox=dict(facecolor='red', alpha=0.5)) - pylab.text(46000, 11600, 'p2=' + p2.__str__(), bbox=dict(facecolor='green', alpha=0.5)) - pylab.text(46000, 11200, 'p3=' + p3.__str__(), bbox=dict(facecolor='magenta', alpha=0.5)) - - pylab.savefig(filename.replace('.csv',''), format="pdf") - #pylab.show() - -#################### -# Residuals Analysis -#################### - - pylab.figure(2) - - pylab.ylabel("Residuals(s)") - pylab.xlabel("Voltage(mV)") - pylab.title("Residuals x Voltage,file="+ filename.replace('.csv','')) - - #Evaluating the polynomial through all the volt values - z1_val = np.polyval(z1, volt_values) - z2_val = np.polyval(z2, volt_values) - z3_val = np.polyval(z3, volt_values) - - # Getting the residuals from the fit functions compared to the real values - z1_res = time_values - z1_val - z2_res = time_values - z2_val - z3_res = time_values - z3_val - - pylab.plot(time_values, z1_res, time_values, z2_res, time_values, z3_res) - - pylab.grid() - - pylab.legend(('Residuals 1st order', 'Residuals 2nd order', 'Residuals 3rd order')) - - pylab.savefig(filename.replace('.csv','')+'_res', format="pdf") - -################### -# Savitzky Golay Filter Applied to the Battery Voltage -################### - - values_filt = sg.filter(voltArray) - - pylab.figure(3) - - pylab.subplot(211) - - pylab.plot(voltArray, time_values) - - pylab.grid(True) - pylab.title('Comparison between real and filtered data') - pylab.ylabel('Real Values(mV)') - - pylab.subplot(212) - - pylab.plot(values_filt, time_values) - - pylab.grid(True) - pylab.ylabel('Filtered Values(mV)') - pylab.xlabel('Time(s)') - -##### - -################### -# Filtered fits -################### - pylab.figure(4) - - pylab.plot(values_filt, time_values) - pylab.ylabel("Time elapsed(seconds)") - pylab.xlabel("Voltage(mV)") - pylab.title("Time x Volt,file="+ filename.replace('.csv','')) - pylab.grid(True) - - secArray = np.asarray(time_values) - - # Polyfit for the voltage values - z1 = np.polyfit(values_filt, secArray,1) - z2 = np.polyfit(values_filt, secArray,2) - z3 = np.polyfit(values_filt, secArray,3) - - if (mode=="initial"): - z3_l = [] - - for el in z3: - z3_l.append((float)(el)) - - abcd = z3_l - yl["abcd"] = z3_l - yl["theta"] = 0. - yl["off_y"] = 0. - - # Linear space for better visualization - xp = np.linspace(49000, 43000, 100) - - # Generating the polynomial function from the fit - p1 = np.poly1d(z1) - p2 = np.poly1d(z2) - p3 = np.poly1d(z3) - - pylab.plot(xp, p1(xp), 'r-', xp, p2(xp), 'g-', xp, p3(xp), 'm-') - - pylab.text(46000, 11900, 'p1=' + p1.__str__(), bbox=dict(facecolor='red', alpha=0.5)) - pylab.text(46000, 11600, 'p2=' + p2.__str__(), bbox=dict(facecolor='green', alpha=0.5)) - pylab.text(46000, 11200, 'p3=' + p3.__str__(), bbox=dict(facecolor='magenta', alpha=0.5)) - -#################### -# Residuals Analysis for the filtered Signal -#################### - - pylab.figure(5) - - pylab.ylabel("Residuals(s)") - pylab.xlabel("Voltage(mV)") - pylab.title("Residuals x Voltage,file="+ filename.replace('.csv','')) - - #Evaluating the polynomial through all the time values - z1_val = np.polyval(z1, values_filt) - z2_val = np.polyval(z2, values_filt) - z3_val = np.polyval(z3, values_filt) - - # Getting the residuals from the fit functions compared to the real values - z1_res = time_values - z1_val - z2_res = time_values - z2_val - z3_res = time_values - z3_val - - pylab.plot(time_values, z1_res, time_values, z2_res, time_values, z3_res) - - pylab.grid() - - pylab.legend(('Residuals 1st order', 'Residuals 2nd order', 'Residuals 3rd order')) - -#################### -# Polynomial Evaluation for the filtered signal and the function from the non-moving case -#################### - - if(mode == "update"): - pylab.figure(6) - - - poly_vals = np.polyval(abcd, values_filt) - - - pylab.plot(values_filt, poly_vals, values_filt, time_values) - - pylab.legend(('Polynomial', 'Real')) - - pylab.grid() - ##### - pylab.figure(7) - - pylab.ylabel("Time available(seconds)") - pylab.xlabel("Voltage(mV)") - pylab.title("Time x Volt,file="+ filename.replace('.csv','')) - pylab.grid(True) - - poly_vals = np.polyval(abcd, values_filt) - - ss = lambda y1, y2: ((y1-y2)**2).sum() - - #theta = -0.07 - #new_x = values_filt*cos(theta) - poly_vals*sin(theta) - #new_y = values_filt*sin(theta) + poly_vals*cos(theta) - - theta = -0.2 - theta_values = [] - cost_values = [] - off_values = [] - - while theta < 0.2: - theta +=0.01 - new_x = values_filt*cos(theta) - poly_vals*sin(theta) - new_y = values_filt*sin(theta) + poly_vals*cos(theta) - print("new_x: {}, new_y: {}".format(new_x, new_y)) - - - off_y = -6000 - - cost_values_i =[] - off_y_values_i=[] - - while off_y < 6000: - off_y +=200 - new_y_temp = new_y - new_y_temp = new_y_temp + off_y - - ss1=ss(time_values,new_y_temp) - print(ss1, off_y) - - cost_values_i.append(ss1) - off_y_values_i.append(off_y) - - #ss1=ss(time_values,new_y) - #print ss1, theta - #cost_values.append(ss1) - theta_values.append(theta) - cost_min = min(cost_values_i) - cost_min_index = cost_values_i.index(cost_min) - cost_values.append(cost_values_i[cost_min_index]) - off_values.append(off_y_values_i[cost_min_index]) - - cost_min = min(cost_values) - cost_min_index = cost_values.index(cost_min) - - theta = theta_values[cost_min_index] - off_y = off_values[cost_min_index] - - new_x = values_filt*cos(theta) - poly_vals*sin(theta) - new_y = values_filt*sin(theta) + poly_vals*cos(theta) - print("new_x: {}, new_y: {}".format(new_x, new_y)) - - new_y = new_y + off_y - - yl["abcd"] = abcd - yl["theta"] = theta - yl["off_y"] = off_y - yl["maximum_time"] = (float)(new_y[0]) - - pylab.plot(poly_vals, values_filt, time_values, values_filt, new_y, values_filt) - - pylab.legend(('Poly not moving', 'Real', 'Shifted Fit')) - - yaml.safe_dump(yl, yaml_file,default_flow_style=False) - - yaml_file.close() - - pylab.show() - - - -if __name__=="__main__": - main(sys.argv[1:])