├── srv ├── SetCharge.srv ├── SetCharging.srv ├── SetLoad.srv ├── SetChargingRate.srv └── SetCoef.srv ├── LICENSE ├── src ├── battery_consumer.hh ├── ROS_debugging.h ├── battery_consumer.cpp ├── battery_discharge.hh └── battery_discharge.cpp ├── .gitignore ├── test ├── launch │ └── cp1-base-test.launch └── worlds │ ├── battery_demo.world │ └── p2-cp1.world ├── package.xml ├── README.md └── CMakeLists.txt /srv/SetCharge.srv: -------------------------------------------------------------------------------- 1 | float64 charge 2 | --- 3 | bool result -------------------------------------------------------------------------------- /srv/SetCharging.srv: -------------------------------------------------------------------------------- 1 | bool charging 2 | --- 3 | bool result -------------------------------------------------------------------------------- /srv/SetLoad.srv: -------------------------------------------------------------------------------- 1 | float64 power_load 2 | --- 3 | bool result -------------------------------------------------------------------------------- /srv/SetChargingRate.srv: -------------------------------------------------------------------------------- 1 | float64 charge_rate 2 | --- 3 | bool result -------------------------------------------------------------------------------- /srv/SetCoef.srv: -------------------------------------------------------------------------------- 1 | float64 constant_coef 2 | float64 linear_coef 3 | --- 4 | bool result -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | The MIT License (MIT) 2 | 3 | Permission is hereby granted, free of charge, to any person obtaining a copy 4 | of this software and associated documentation files (the "Software"), to deal 5 | in the Software without restriction, including without limitation the rights 6 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | copies of the Software, and to permit persons to whom the Software is 8 | furnished to do so, subject to the following conditions: 9 | 10 | The above copyright notice and this permission notice shall be included in all 11 | copies or substantial portions of the Software. 12 | 13 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 16 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 19 | SOFTWARE. -------------------------------------------------------------------------------- /src/battery_consumer.hh: -------------------------------------------------------------------------------- 1 | #ifndef BRASS_GAZEBO_BATTERY_BATTERY_CONSUMER_H 2 | #define BRASS_GAZEBO_BATTERY_BATTERY_CONSUMER_H 3 | 4 | #include "gazebo/common/Plugin.hh" 5 | #include "gazebo/common/CommonTypes.hh" 6 | #include 7 | #include "brass_gazebo_battery/SetLoad.h" 8 | #include "ros/ros.h" 9 | #include "ros/subscribe_options.h" 10 | #include "ros/callback_queue.h" 11 | 12 | #define CONSUMER_DEBUG 13 | 14 | namespace gazebo 15 | { 16 | class GAZEBO_VISIBLE BatteryConsumerPlugin : public ModelPlugin 17 | { 18 | // Constructor 19 | public: BatteryConsumerPlugin(); 20 | 21 | public: ~BatteryConsumerPlugin(); 22 | 23 | // Inherited from ModelPlugin 24 | public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); 25 | 26 | public: virtual void Init(); 27 | 28 | public: virtual void Reset(); 29 | 30 | public: bool SetConsumerPowerLoad(brass_gazebo_battery::SetLoad::Request& req, 31 | brass_gazebo_battery::SetLoad::Response& res); 32 | 33 | // Connection to the World Update events. 34 | protected: event::ConnectionPtr updateConnection; 35 | 36 | protected: physics::WorldPtr world; 37 | 38 | protected: physics::PhysicsEnginePtr physics; 39 | 40 | protected: physics::ModelPtr model; 41 | 42 | protected: physics::LinkPtr link; 43 | 44 | protected: sdf::ElementPtr sdf; 45 | 46 | // Battery 47 | private: common::BatteryPtr battery; 48 | 49 | // Consumer identifier 50 | private: int32_t consumerId; 51 | 52 | protected: double powerLoad; 53 | 54 | // This node is for ros communications 55 | protected: std::unique_ptr rosNode; 56 | 57 | protected: ros::ServiceServer set_power_load; 58 | 59 | protected: boost::mutex lock; 60 | 61 | }; 62 | 63 | } 64 | 65 | #endif //BRASS_GAZEBO_BATTERY_BATTERY_CONSUMER_H 66 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # temp folders 2 | tmp/ 3 | temp/ 4 | 5 | # mac stuff 6 | .DS_Store 7 | 8 | # PyCharm 9 | .idea/ 10 | 11 | # Byte-compiled / optimized / DLL files 12 | __pycache__/ 13 | *.py[cod] 14 | *$py.class 15 | 16 | # Byte-compiled / optimized / DLL files 17 | __pycache__/ 18 | *.py[cod] 19 | *$py.class 20 | 21 | # C extensions 22 | *.so 23 | 24 | # Distribution / packaging 25 | cmake-build-debug/ 26 | .Python 27 | env/ 28 | build/ 29 | develop-eggs/ 30 | dist/ 31 | downloads/ 32 | eggs/ 33 | .eggs/ 34 | lib/ 35 | lib64/ 36 | parts/ 37 | sdist/ 38 | var/ 39 | wheels/ 40 | *.egg-info/ 41 | .installed.cfg 42 | *.egg 43 | 44 | # PyInstaller 45 | # Usually these files are written by a python script from a template 46 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 47 | *.manifest 48 | *.spec 49 | 50 | # Installer logs 51 | pip-log.txt 52 | pip-delete-this-directory.txt 53 | 54 | # Unit test / coverage reports 55 | htmlcov/ 56 | .tox/ 57 | .coverage 58 | .coverage.* 59 | .cache 60 | nosetests.xml 61 | coverage.xml 62 | *.cover 63 | .hypothesis/ 64 | 65 | # Translations 66 | *.mo 67 | *.pot 68 | 69 | # Django stuff: 70 | *.log 71 | local_settings.py 72 | 73 | # Flask stuff: 74 | instance/ 75 | .webassets-cache 76 | 77 | # Scrapy stuff: 78 | .scrapy 79 | 80 | # Sphinx documentation 81 | docs/_build/ 82 | 83 | # PyBuilder 84 | target/ 85 | 86 | # Jupyter Notebook 87 | .ipynb_checkpoints 88 | 89 | # pyenv 90 | .python-version 91 | 92 | # celery beat schedule file 93 | celerybeat-schedule 94 | 95 | # SageMath parsed files 96 | *.sage.py 97 | 98 | # dotenv 99 | .env 100 | 101 | # virtualenv 102 | .venv 103 | venv/ 104 | ENV/ 105 | 106 | # Spyder project settings 107 | .spyderproject 108 | .spyproject 109 | 110 | # Rope project settings 111 | .ropeproject 112 | 113 | # mkdocs documentation 114 | /site 115 | 116 | # mypy 117 | .mypy_cache/ -------------------------------------------------------------------------------- /src/ROS_debugging.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by pjamshidi on 2/4/18. 3 | // 4 | 5 | #ifndef BRASS_GAZEBO_BATTERY_ROS_DEBUGGING_H 6 | #define BRASS_GAZEBO_BATTERY_ROS_DEBUGGING_H 7 | 8 | 9 | // Defining customized ROS stream color 10 | 11 | namespace pc 12 | { 13 | enum PRINT_COLOR 14 | { 15 | BLACK, 16 | RED, 17 | GREEN, 18 | YELLOW, 19 | BLUE, 20 | MAGENTA, 21 | CYAN, 22 | WHITE, 23 | ENDCOLOR 24 | }; 25 | 26 | std::ostream& operator<<(std::ostream& os, PRINT_COLOR c) 27 | { 28 | switch(c) 29 | { 30 | case BLACK : os << "\033[1;30m"; break; 31 | case RED : os << "\033[1;31m"; break; 32 | case GREEN : os << "\033[1;32m"; break; 33 | case YELLOW : os << "\033[1;33m"; break; 34 | case BLUE : os << "\033[1;34m"; break; 35 | case MAGENTA : os << "\033[1;35m"; break; 36 | case CYAN : os << "\033[1;36m"; break; 37 | case WHITE : os << "\033[1;37m"; break; 38 | case ENDCOLOR : os << "\033[0m"; break; 39 | default : os << "\033[1;37m"; 40 | } 41 | return os; 42 | } 43 | } //namespace pc 44 | 45 | #define ROS_BLACK_STREAM(x) ROS_INFO_STREAM(pc::BLACK << x << pc::ENDCOLOR) 46 | #define ROS_RED_STREAM(x) ROS_INFO_STREAM(pc::RED << x << pc::ENDCOLOR) 47 | #define ROS_GREEN_STREAM(x) ROS_INFO_STREAM(pc::GREEN << x << pc::ENDCOLOR) 48 | #define ROS_YELLOW_STREAM(x) ROS_INFO_STREAM(pc::YELLOW << x << pc::ENDCOLOR) 49 | #define ROS_BLUE_STREAM(x) ROS_INFO_STREAM(pc::BLUE << x << pc::ENDCOLOR) 50 | #define ROS_MAGENTA_STREAM(x) ROS_INFO_STREAM(pc::MAGENTA << x << pc::ENDCOLOR) 51 | #define ROS_CYAN_STREAM(x) ROS_INFO_STREAM(pc::CYAN << x << pc::ENDCOLOR) 52 | 53 | #define ROS_BLACK_STREAM_COND(c, x) ROS_INFO_STREAM_COND(c, pc::BLACK << x << pc::ENDCOLOR) 54 | #define ROS_RED_STREAM_COND(c, x) ROS_INFO_STREAM_COND(c, pc::RED << x << pc::ENDCOLOR) 55 | #define ROS_GREEN_STREAM_COND(c, x) ROS_INFO_STREAM_COND(c, pc::GREEN << x << pc::ENDCOLOR) 56 | #define ROS_YELLOW_STREAM_COND(c, x) ROS_INFO_STREAM_COND(c, pc::YELLOW << x << pc::ENDCOLOR) 57 | #define ROS_BLUE_STREAM_COND(c, x) ROS_INFO_STREAM_COND(c, pc::BLUE << x << pc::ENDCOLOR) 58 | #define ROS_MAGENTA_STREAM_COND(c, x) ROS_INFO_STREAM_COND(c, pc::MAGENTA << x << pc::ENDCOLOR) 59 | #define ROS_CYAN_STREAM_COND(c, x) ROS_INFO_STREAM_COND(c, pc::CYAN << x << pc::ENDCOLOR) 60 | 61 | 62 | 63 | #endif //BRASS_GAZEBO_BATTERY_ROS_DEBUGGING_H 64 | -------------------------------------------------------------------------------- /test/launch/cp1-base-test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | -------------------------------------------------------------------------------- /test/worlds/battery_demo.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | model://sun 6 | 7 | 8 | model://ground_plane 9 | 10 | 11 | 12 | 0 0 0 0 0 0 13 | false 14 | 15 | 0 0 0.5 0 0 0 16 | 17 | 18 | 1.8 19 | 0.0 20 | 0.0 21 | 1.8 22 | 0.0 23 | 1.8 24 | 25 | 10.0 26 | 27 | 28 | 29 | 30 | 1 1 1 31 | 32 | 33 | 34 | 35 | 36 | 0.1 37 | 0.1 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 1 1 1 46 | 47 | 48 | 49 | 50 | 12.592 51 | 52 | 53 | 54 | battery_monitor_client 55 | body 56 | linear_battery 57 | 12.694 58 | -100.1424 59 | 1.1665 60 | 1.2009 61 | 0.061523 62 | 1.9499 63 | 0.2 64 | 65 | 66 | body 67 | linear_battery 68 | 6.6 69 | 70 | 71 | 72 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | brass_gazebo_battery 4 | 0.0.0 5 | The plugin for battery discharge/charge 6 | 7 | 8 | 9 | 10 | pjamshidi 11 | 12 | 13 | 14 | 15 | 16 | MIT 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | roscpp 38 | roscpp 39 | 40 | message_generation 41 | 42 | 43 | 44 | catkin 45 | 46 | message_runtime 47 | 48 | 49 | 50 | 51 | catkin 52 | gazebo_plugins 53 | gazebo_ros 54 | roscpp 55 | gazebo_plugins 56 | gazebo_ros 57 | roscpp 58 | gazebo_plugins 59 | gazebo_ros 60 | roscpp 61 | std_msgs 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | -------------------------------------------------------------------------------- /src/battery_consumer.cpp: -------------------------------------------------------------------------------- 1 | #include "battery_consumer.hh" 2 | #include "gazebo/common/Battery.hh" 3 | #include "gazebo/physics/physics.hh" 4 | #include "ROS_debugging.h" 5 | 6 | #define BATTERY_CONSUMER_DEBUG 7 | 8 | using namespace gazebo; 9 | 10 | GZ_REGISTER_MODEL_PLUGIN(BatteryConsumerPlugin); 11 | 12 | BatteryConsumerPlugin::BatteryConsumerPlugin() : consumerId(-1) 13 | { 14 | } 15 | 16 | BatteryConsumerPlugin::~BatteryConsumerPlugin() 17 | { 18 | if (this->battery && this->consumerId !=-1) 19 | this->battery->RemoveConsumer(this->consumerId); 20 | } 21 | 22 | void BatteryConsumerPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) 23 | { 24 | // TODO: checking whether these elements exists 25 | 26 | 27 | #ifdef CONSUMER_DEBUG 28 | gzdbg << "started loading consumer \n"; 29 | #endif 30 | 31 | // check if the ros is up! 32 | if (!ros::isInitialized()){ 33 | int argc = 0; 34 | char **argv = NULL; 35 | ros::init(argc, argv, _sdf->Get("ros_node"), ros::init_options::NoSigintHandler); 36 | } 37 | 38 | this->model = _model; 39 | this->world = _model->GetWorld(); 40 | 41 | std::string linkName = _sdf->Get("link_name"); 42 | this->link = _model->GetLink(linkName); 43 | 44 | // Create battery 45 | std::string batteryName = _sdf->Get("battery_name"); 46 | this->battery = this->link->Battery(batteryName); 47 | 48 | // Add consumer and sets its power load 49 | this->powerLoad = _sdf->Get("power_load"); 50 | this->consumerId = this->battery->AddConsumer(); 51 | this->battery->SetPowerLoad(this->consumerId, powerLoad); 52 | 53 | // Create ros node and publish stuff there! 54 | this->rosNode.reset(new ros::NodeHandle(_sdf->Get("ros_node"))); 55 | 56 | this->set_power_load = this->rosNode->advertiseService(this->model->GetName() + "/set_power_load", &BatteryConsumerPlugin::SetConsumerPowerLoad, this); 57 | 58 | #ifdef CONSUMER_DEBUG 59 | gzdbg << "consumer loaded \n"; 60 | #endif 61 | 62 | ROS_GREEN_STREAM("Consumer loaded"); 63 | 64 | } 65 | 66 | void BatteryConsumerPlugin::Init() 67 | { 68 | #ifdef CONSUMER_DEBUG 69 | gzdbg << "consumer is initialized \n"; 70 | #endif 71 | ROS_GREEN_STREAM("Consumer is initialized"); 72 | } 73 | 74 | void BatteryConsumerPlugin::Reset() 75 | { 76 | #ifdef CONSUMER_DEBUG 77 | gzdbg << "consumer is reset \n"; 78 | #endif 79 | ROS_GREEN_STREAM("Consumer is reset"); 80 | } 81 | 82 | bool BatteryConsumerPlugin::SetConsumerPowerLoad(brass_gazebo_battery::SetLoad::Request &req, 83 | brass_gazebo_battery::SetLoad::Response &res) 84 | { 85 | lock.lock(); 86 | double load = this->powerLoad; 87 | this->powerLoad = req.power_load; 88 | this->battery->SetPowerLoad(this->consumerId, this->powerLoad); 89 | 90 | #ifdef BATTERY_CONSUMER_DEBUG 91 | gzdbg << "Power load of consumer has changed from:" << load << ", to:" << this->powerLoad << "\n"; 92 | #endif 93 | ROS_GREEN_STREAM("Power load of consumer has changed to: " << this->powerLoad); 94 | 95 | lock.unlock(); 96 | res.result = true; 97 | return true; 98 | } -------------------------------------------------------------------------------- /src/battery_discharge.hh: -------------------------------------------------------------------------------- 1 | #ifndef BRASS_GAZEBO_BATTERY_BATTERY_DISCHARGE_H 2 | #define BRASS_GAZEBO_BATTERY_BATTERY_DISCHARGE_H 3 | 4 | #include 5 | #include 6 | #include "gazebo/common/Plugin.hh" 7 | #include "gazebo/common/CommonTypes.hh" 8 | #include "gazebo/physics/physics.hh" 9 | #include "ros/ros.h" 10 | #include "ros/subscribe_options.h" 11 | #include "ros/callback_queue.h" 12 | #include 13 | #include "std_msgs/Bool.h" 14 | #include "brass_gazebo_battery/SetCharge.h" 15 | #include "brass_gazebo_battery/SetCharging.h" 16 | #include "brass_gazebo_battery/SetCoef.h" 17 | #include "brass_gazebo_battery/SetChargingRate.h" 18 | 19 | #define BATTERY_DEBUG 20 | #define DBG_INTERVAL 5.0 21 | 22 | namespace gazebo 23 | { 24 | /// \brief A plugin that simulate BRASS CP1 battery model: discharge and charge according to power models 25 | class GAZEBO_VISIBLE BatteryPlugin : public ModelPlugin 26 | { 27 | /// \brief Constructor 28 | public: BatteryPlugin(); 29 | 30 | public: ~BatteryPlugin(); 31 | 32 | // Inherited. 33 | public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); 34 | 35 | public: virtual void Init(); 36 | 37 | public: virtual void Reset(); 38 | 39 | private: double OnUpdateVoltage(const common::BatteryPtr &_battery); 40 | 41 | public: bool SetCharging(brass_gazebo_battery::SetCharging::Request& req, 42 | brass_gazebo_battery::SetCharging::Response& res); 43 | 44 | public: bool SetCharge(brass_gazebo_battery::SetCharge::Request& req, 45 | brass_gazebo_battery::SetCharge::Response& res); 46 | 47 | public: bool SetModelCoefficients(brass_gazebo_battery::SetCoef::Request& req, 48 | brass_gazebo_battery::SetCoef::Response& res); 49 | 50 | public: bool SetChargingRate(brass_gazebo_battery::SetChargingRate::Request& req, 51 | brass_gazebo_battery::SetChargingRate::Response& res); 52 | 53 | // Connection to the World Update events. 54 | protected: event::ConnectionPtr updateConnection; 55 | 56 | protected: physics::WorldPtr world; 57 | 58 | protected: physics::PhysicsEnginePtr physics; 59 | 60 | protected: physics::ModelPtr model; 61 | 62 | protected: physics::LinkPtr link; 63 | 64 | protected: common::BatteryPtr battery; 65 | 66 | protected: sdf::ElementPtr sdf; 67 | 68 | 69 | // E(t) = e0 + e1* Q(t)/c 70 | protected: double et; 71 | protected: double e0; 72 | protected: double e1; 73 | 74 | // Initial battery charge in Ah. 75 | protected: double q0; 76 | 77 | // Charge rate in A 78 | // More description about charge rate: http://batteriesbyfisher.com/determining-charge-time 79 | protected: double qt; 80 | 81 | // Battery capacity in Ah. 82 | protected: double c; 83 | 84 | // Battery inner resistance in Ohm 85 | protected: double r; 86 | 87 | // Current low-pass filter characteristic time in seconds. 88 | protected: double tau; 89 | 90 | // Raw battery current in A. 91 | protected: double iraw; 92 | 93 | // Smoothed battery current in A. 94 | protected: double ismooth; 95 | 96 | // Instantaneous battery charge in Ah. 97 | protected: double q; 98 | 99 | // This node is for ros communications 100 | protected: std::unique_ptr rosNode; 101 | 102 | protected: ros::Publisher charge_state; 103 | protected: ros::Publisher charge_state_mwh; 104 | protected: ros::Publisher motor_power; 105 | 106 | protected: ros::ServiceServer set_charging; 107 | protected: ros::ServiceServer set_charge; 108 | protected: ros::ServiceServer set_coefficients; 109 | protected: ros::ServiceServer set_charging_rate; 110 | 111 | protected: boost::mutex lock; 112 | 113 | protected: bool charging; 114 | 115 | protected: double sim_time_now; 116 | 117 | }; 118 | } 119 | 120 | #endif //BRASS_GAZEBO_BATTERY_BATTERY_DISCHARGE_H -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Functionality 2 | This is a Gazebo plugin that simulate an open-circuit battery model. This is a fairly extensible and reusable battery plugin for any kind of Gazebo compatible robots. We developed this plugin primarily for A DARPA sponsored project [BRASS MARS](http://www.cs.cmu.edu/~brassmars/) at CMU. 3 | 4 | This power model simulates the power consumption of a robot. The amount of power consumed by each component of a robot depends on its usage. The battery its current state of the charge after each simulation iteration determined by `dt` in the code. The battery plugin takes the power loads for each components in the robot that consume energy and current voltage value of the battery (which updates according to the open circuit voltage model) as inputs and returns a new voltage value. 5 | 6 | 7 | # Support 8 | This plugin is tested for ROS kinetic and Gazebo 7.8.1. 9 | 10 | # Build 11 | Create the build directory: 12 | ```bash 13 | mkdir ~/catkin_ws/src/brass_gazebo_battery/build 14 | cd ~/catkin_ws/src/brass_gazebo_battery/build 15 | ``` 16 | 17 | Make sure you have sourced ROS before compiling the code: 18 | ```bash 19 | source /opt/ros//setup.bash 20 | ``` 21 | 22 | Compile the code: 23 | ```bash 24 | cmake ../ 25 | make 26 | ``` 27 | 28 | Compiling will result in a shared library, `~/catkin_ws/src/brass_gazebo_battery/build/devel/lib/libbattery_discharge.so`, that can be inserted in a Gazebo simulation. 29 | 30 | Lastly, add your library path to the `GAZEBO_PLUGIN_PATH`: 31 | ```bash 32 | export GAZEBO_PLUGIN_PATH=${GAZEBO_PLUGIN_PATH}:~/catkin_ws/src/brass_gazebo_battery/build/devel/lib 33 | ``` 34 | 35 | # Build by catkin 36 | Build the plugin by going to the base of your work space and running catkin: 37 | ```bash 38 | cd ~/catkin_ws 39 | catkin_make 40 | ``` 41 | 42 | # Installing the Plugin 43 | ```bash 44 | cd ~/catkin_ws/src/brass_gazebo_battery/build 45 | cmake ../ 46 | make 47 | sudo make install 48 | ``` 49 | 50 | # Usage 51 | 52 | In the brass.world file, `libbattery_discharge.so` is mentioned as a plugin. 53 | This implied that plugin is initialized and loaded when `p2-cp1.world` is opened in Gazebo. 54 | The xml code could be linked to any model in a new `.world` file. 55 | ```xml 56 | 57 | 0 0 0 0 0 0 58 | false 59 | 60 | 61 | 12.592 62 | 63 | 64 | 65 | battery_monitor_client 66 | body 67 | linear_battery 68 | 12.694 69 | -3.1424 70 | 1.1665 71 | 1.2009 72 | 0.061523 73 | 1.9499 74 | 0.2 75 | 76 | 77 | body 78 | linear_battery 79 | 6.6 80 | 81 | 82 | ``` 83 | 84 | # Run the Plugin 85 | ```bash 86 | cd ~/catkin_ws/src/brass_gazebo_battery/ 87 | gazebo test/worlds/p2-cp1.world --verbose 88 | ``` 89 | 90 | 91 | # Exposed ROS services and topics 92 | 93 | This Gazebo plugin expose several services that can be accessed via ROS: 94 | 95 | ``` 96 | /battery_monitor_client/battery_demo_model/set_charge 97 | /battery_monitor_client/battery_demo_model/set_charge_rate 98 | /battery_monitor_client/battery_demo_model/set_charging 99 | /battery_monitor_client/battery_demo_model/set_model_coefficients 100 | /battery_monitor_client/battery_demo_model/set_power_load 101 | ``` 102 | 103 | Also, this publish information about the status of robot battery to the following topics: 104 | ``` 105 | /mobile_base/commands/charge_level 106 | /mobile_base/commands/motor_power 107 | ``` 108 | 109 | # Extending ROS Services 110 | 111 | First create the service description file `.srv` and put it in the `srv` folder. Then declare it in the `CMakeList.txt` in the 112 | `add_service_files()` section. Also, add the following to the `CMakeList.txt`: 113 | ```cmake 114 | generate_messages( 115 | DEPENDENCIES 116 | std_msgs # Or other packages containing msgs 117 | ) 118 | ``` 119 | 120 | For updating the parameters of the battery model we use ROS services, 121 | so here we explain how to add new services to the code if needed: 122 | 123 | ```bash 124 | cd ~/catkin_ws 125 | catkin_make 126 | ``` 127 | The header files associated to the service can be found here: 128 | 129 | ```bash 130 | cd ~/catkin_ws/devel/include/brass_gazebo_battery 131 | ``` 132 | The add the following header into the code that want to use the services: 133 | 134 | ```cpp 135 | #include "brass_gazebo_battery/SetLoad.h" 136 | ``` 137 | And then add the following declaration: 138 | 139 | ```cpp 140 | public: bool ServiceName(brass_gazebo_battery::SetLoad::Request& req, brass_gazebo_battery::SetLoad::Response& res); 141 | ``` 142 | The service can then be advertised as follows: 143 | 144 | ```cpp 145 | this->rosNode->advertiseService(this->model->GetName() + "/api", &Plugin::ServiceName, this); 146 | ``` 147 | 148 | # Notes about conversions 149 | For converting capacity and charge rate (in `Ah`) to power (`mwh`) which is consumed by planner the formula is `(Ah)*(V) = (Wh)`. For example, if you have a `3Ah` battery rated at `5V`, the power is `3Ah * 5V = 15wh` or `15000mwh`. 150 | For converting `Watts` to `watt-hour`, we do `watt * hour`, e.g., `6 watts / 3600 (wh)` per seconds. 151 | 152 | # Acknowledgements 153 | We used/inspired by existing theory of open circuit battery model. This battery discharge/charge plugin uses the Gazebo `Battery` class which is shipped by the default simulator. 154 | 155 | 156 | Further references: [r1](http://security.livewatch.com/forum-ref/ohms-law-calculator), [r2](http://batteriesbyfisher.com/determining-charge-time), [r3](https://electronics.stackexchange.com/questions/24160/how-to-calculate-the-time-of-charging-and-discharging-of-battery). 157 | 158 | # Maintainer 159 | 160 | If you need a new feature to be added, please contact [Pooyan Jamshidi](https://pooyanjamshidi.github.io). 161 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(brass_gazebo_battery) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | gazebo_dev 12 | gazebo_plugins 13 | gazebo_ros 14 | roscpp 15 | message_generation 16 | ) 17 | 18 | find_package(catkin REQUIRED COMPONENTS 19 | gazebo_dev 20 | gencpp 21 | message_generation 22 | gazebo_msgs 23 | roscpp 24 | rospy 25 | nodelet 26 | angles 27 | std_srvs 28 | geometry_msgs 29 | sensor_msgs 30 | nav_msgs 31 | urdf 32 | tf 33 | tf2_ros 34 | dynamic_reconfigure 35 | rosgraph_msgs 36 | trajectory_msgs 37 | image_transport 38 | rosconsole 39 | cv_bridge 40 | polled_camera 41 | diagnostic_updater 42 | camera_info_manager 43 | ) 44 | 45 | find_package(ignition-math2 REQUIRED) 46 | 47 | ## System dependencies are found with CMake's conventions 48 | # find_package(Boost REQUIRED COMPONENTS system) 49 | 50 | 51 | ## Uncomment this if the package has a setup.py. This macro ensures 52 | ## modules and global scripts declared therein get installed 53 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 54 | # catkin_python_setup() 55 | 56 | ################################################ 57 | ## Declare ROS messages, services and actions ## 58 | ################################################ 59 | 60 | ## To declare and build messages, services or actions from within this 61 | ## package, follow these steps: 62 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 63 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 64 | ## * In the file package.xml: 65 | ## * add a build_depend tag for "message_generation" 66 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 67 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 68 | ## but can be declared for certainty nonetheless: 69 | ## * add a run_depend tag for "message_runtime" 70 | ## * In this file (CMakeLists.txt): 71 | ## * add "message_generation" and every package in MSG_DEP_SET to 72 | ## find_package(catkin REQUIRED COMPONENTS ...) 73 | ## * add "message_runtime" and every package in MSG_DEP_SET to 74 | ## catkin_package(CATKIN_DEPENDS ...) 75 | ## * uncomment the add_*_files sections below as needed 76 | ## and list every .msg/.srv/.action file to be processed 77 | ## * uncomment the generate_messages entry below 78 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 79 | 80 | ## Generate messages in the 'msg' folder 81 | # add_message_files( 82 | # FILES 83 | # Message1.msg 84 | # Message2.msg 85 | # ) 86 | 87 | # Generate services in the 'srv' folder 88 | add_service_files( 89 | FILES 90 | SetCharge.srv 91 | SetChargingRate.srv 92 | SetCharging.srv 93 | SetCoef.srv 94 | SetLoad.srv 95 | ) 96 | 97 | ## Generate actions in the 'action' folder 98 | # add_action_files( 99 | # FILES 100 | # Action1.action 101 | # Action2.action 102 | # ) 103 | 104 | ## Generate added messages and services with any dependencies listed here 105 | generate_messages( 106 | DEPENDENCIES 107 | std_msgs # Or other packages containing msgs 108 | ) 109 | 110 | 111 | ################################################ 112 | ## Declare ROS dynamic reconfigure parameters ## 113 | ################################################ 114 | 115 | ## To declare and build dynamic reconfigure parameters within this 116 | ## package, follow these steps: 117 | ## * In the file package.xml: 118 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 119 | ## * In this file (CMakeLists.txt): 120 | ## * add "dynamic_reconfigure" to 121 | ## find_package(catkin REQUIRED COMPONENTS ...) 122 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 123 | ## and list every .cfg file to be processed 124 | 125 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 126 | # generate_dynamic_reconfigure_options( 127 | # cfg/DynReconf1.cfg 128 | # cfg/DynReconf2.cfg 129 | # ) 130 | 131 | ################################### 132 | ## catkin specific configuration ## 133 | ################################### 134 | ## The catkin_package macro generates cmake config files for your package 135 | ## Declare things to be passed to dependent projects 136 | ## INCLUDE_DIRS: uncomment this if your package contains header files 137 | ## LIBRARIES: libraries you create in this project that dependent projects also need 138 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 139 | ## DEPENDS: system dependencies of this project that dependent projects also need 140 | catkin_package( 141 | LIBRARIES brass_gazebo_battery 142 | CATKIN_DEPENDS gazebo_plugins gazebo_ros roscpp std_msgs 143 | ) 144 | 145 | ########### 146 | ## Build ## 147 | ########### 148 | 149 | ## Specify additional locations of header files 150 | ## Your package locations should be listed before other locations 151 | include_directories( 152 | # include 153 | ${catkin_INCLUDE_DIRS} 154 | ${IGNITION-MATH2_INCLUDE_DIRS} 155 | ) 156 | 157 | IF(CMAKE_SYSTEM_PROCESSOR MATCHES "x86_64") 158 | ADD_DEFINITIONS(-fPIC) 159 | ENDIF() 160 | 161 | add_library(battery_discharge SHARED src/battery_discharge.cpp src/ROS_debugging.h) 162 | add_dependencies(battery_discharge ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 163 | target_link_libraries(battery_discharge ${roscpp_LIBRARIES} ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} ${IGNITION-MATH_LIBRARIES}) 164 | 165 | # Important: add ${roscpp_LIBRARIES} to the target_link_library if you are using ros libraries as we use in the battery plugin 166 | add_library(battery_consumer SHARED src/battery_consumer.cpp src/ROS_debugging.h) 167 | add_dependencies(battery_consumer ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 168 | target_link_libraries(battery_consumer ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} ${IGNITION-MATH_LIBRARIES}) 169 | 170 | 171 | ## Declare a C++ library 172 | #add_library(${PROJECT_NAME} SHARED src/battery_discharge.cpp src/battery_discharge.hh src/battery_consumer.cpp src/battery_consumer.hh) 173 | 174 | ## Add cmake target dependencies of the library 175 | ## as an example, code may need to be generated before libraries 176 | ## either from message generation or dynamic reconfigure 177 | #add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 178 | 179 | ## Declare a C++ executable 180 | ## With catkin_make all packages are built within a single CMake context 181 | ## The recommended prefix ensures that target names across packages don't collide 182 | #add_executable(battery_discharge_node src/battery_discharge.cpp) 183 | 184 | ## Rename C++ executable without prefix 185 | ## The above recommended prefix causes long target names, the following renames the 186 | ## target back to the shorter version for ease of user use 187 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 188 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 189 | 190 | ## Add cmake target dependencies of the executable 191 | ## same as for the library above 192 | #add_dependencies(battery_discharge_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 193 | 194 | ## Specify libraries to link a library or executable target against 195 | #target_link_libraries(battery_discharge_node 196 | # ${catkin_LIBRARIES} 197 | #) 198 | 199 | 200 | ############# 201 | ## Install ## 202 | ############# 203 | 204 | # all install targets should use catkin DESTINATION variables 205 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 206 | 207 | ## Mark executable scripts (Python etc.) for installation 208 | ## in contrast to setup.py, you can choose the destination 209 | # install(PROGRAMS 210 | # scripts/my_python_script 211 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 212 | # ) 213 | 214 | ## Mark executables and/or libraries for installation 215 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 216 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 217 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 218 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 219 | # ) 220 | 221 | install(TARGETS 222 | battery_discharge 223 | battery_consumer 224 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 225 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 226 | ) 227 | 228 | ## Mark cpp header files for installation 229 | #install(DIRECTORY include/${PROJECT_NAME}/ 230 | #DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 231 | #FILES_MATCHING PATTERN "*.h" 232 | #PATTERN ".svn" EXCLUDE 233 | #) 234 | 235 | ## Mark other files for installation (e.g. launch and bag files, etc.) 236 | # install(FILES 237 | # # myfile1 238 | # # myfile2 239 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 240 | # ) 241 | 242 | ############# 243 | ## Testing ## 244 | ############# 245 | 246 | ## Add gtest based cpp test target and link libraries 247 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_brass_gazebo_battery.cpp) 248 | # if(TARGET ${PROJECT_NAME}-test) 249 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 250 | # endif() 251 | 252 | ## Add folders to be run by python nosetests 253 | # catkin_add_nosetests(test) 254 | -------------------------------------------------------------------------------- /src/battery_discharge.cpp: -------------------------------------------------------------------------------- 1 | #include "gazebo/common/Time.hh" 2 | #include "gazebo/common/Plugin.hh" 3 | #include "gazebo/common/Battery.hh" 4 | #include "gazebo/physics/physics.hh" 5 | #include "battery_discharge.hh" 6 | #include 7 | #include "std_msgs/Float64.h" 8 | #include "ROS_debugging.h" 9 | 10 | 11 | enum power{ 12 | OFF = 0, 13 | ON = 1 14 | }; 15 | 16 | template 17 | T max(T x, T y) 18 | { 19 | return x < y ? y : x; 20 | } 21 | 22 | using namespace gazebo; 23 | 24 | GZ_REGISTER_MODEL_PLUGIN(BatteryPlugin); 25 | 26 | BatteryPlugin::BatteryPlugin() 27 | { 28 | this->c = 0.0; 29 | this->r = 0.0; 30 | this->tau = 0.0; 31 | 32 | this->e0 = 0.0; 33 | this->e1 = 0.0; 34 | 35 | this->q0 = 0.0; 36 | this->q = 0.0; 37 | this->qt = 0.0; 38 | 39 | this->iraw = 0.0; 40 | this->ismooth = 0.0; 41 | 42 | #ifdef BATTERY_DEBUG 43 | gzdbg << "Constructed BatteryPlugin and initialized parameters. \n"; 44 | #endif 45 | 46 | ROS_INFO_STREAM("BRASS CP1 battery is constructed."); 47 | } 48 | 49 | BatteryPlugin::~BatteryPlugin() 50 | { 51 | #ifdef BATTERY_DEBUG 52 | gzdbg << "Destructing BatteryPlugin and removing the ros node. \n"; 53 | #endif 54 | this->rosNode->shutdown(); 55 | } 56 | 57 | void BatteryPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) 58 | { 59 | #ifdef BATTERY_DEBUG 60 | gzdbg << "Loading the BatteryPlugin \n"; 61 | #endif 62 | 63 | // check if the ros is up! 64 | if (!ros::isInitialized()){ 65 | ROS_INFO_STREAM("Initializing ROS..."); 66 | int argc = 0; 67 | char **argv = NULL; 68 | ros::init(argc, argv, _sdf->Get("ros_node"), ros::init_options::NoSigintHandler); 69 | } 70 | 71 | this->model = _model; 72 | this->world = _model->GetWorld(); 73 | 74 | this->sim_time_now = this->world->SimTime().Double(); 75 | 76 | // Create ros node and publish stuff there! 77 | this->rosNode.reset(new ros::NodeHandle(_sdf->Get("ros_node"))); 78 | if (this->rosNode->ok()) 79 | { 80 | ROS_GREEN_STREAM("ROS node is up"); 81 | } 82 | 83 | // Publish a topic for motor power and charge level 84 | this->motor_power = this->rosNode->advertise("/mobile_base/commands/motor_power", 1); 85 | this->charge_state = this->rosNode->advertise("/mobile_base/commands/charge_level", 1); 86 | this->charge_state_mwh = this->rosNode->advertise("/mobile_base/commands/charge_level_mwh", 1); 87 | 88 | this->set_charging = this->rosNode->advertiseService(this->model->GetName() + "/set_charging", &BatteryPlugin::SetCharging, this); 89 | this->set_charging_rate = this->rosNode->advertiseService(this->model->GetName() + "/set_charge_rate", &BatteryPlugin::SetChargingRate, this); 90 | this->set_charge = this->rosNode->advertiseService(this->model->GetName() + "/set_charge", &BatteryPlugin::SetCharge, this); 91 | this->set_coefficients = this->rosNode->advertiseService(this->model->GetName() + "/set_model_coefficients", &BatteryPlugin::SetModelCoefficients, this); 92 | 93 | std::string linkName = _sdf->Get("link_name"); 94 | this->link = this->model->GetLink(linkName); 95 | 96 | this->e0 = _sdf->Get("constant_coef"); 97 | this->e1 = _sdf->Get("linear_coef"); 98 | this->q0 = _sdf->Get("initial_charge"); 99 | this->qt = _sdf->Get("charge_rate"); 100 | this->c = _sdf->Get("capacity"); 101 | this->r = _sdf->Get("resistance"); 102 | this->tau = _sdf->Get("smooth_current_tau"); 103 | 104 | std::string batteryName = _sdf->Get("battery_name"); 105 | 106 | if (this->link->BatteryCount() > 0) { 107 | // Creates the battery 108 | this->battery = this->link->Battery(batteryName); 109 | ROS_GREEN_STREAM("Created a battery"); 110 | } 111 | else 112 | { 113 | ROS_RED_STREAM("There is no battery specification in the link"); 114 | }; 115 | 116 | 117 | // Specifying a custom update function 118 | this->battery->SetUpdateFunc(std::bind(&BatteryPlugin::OnUpdateVoltage, this, std::placeholders::_1)); 119 | 120 | this->sim_time_now = this->world->SimTime().Double(); 121 | 122 | #ifdef BATTERY_DEBUG 123 | gzdbg << "Loaded the BatteryPlugin at time:" << this->sim_time_now << "\n"; 124 | #endif 125 | 126 | ROS_GREEN_STREAM("Plugin is fully loaded."); 127 | } 128 | 129 | // This is for in initialization purposes and is called once after Load 130 | // So explanation about Gazebo Init are here: http://playerstage.sourceforge.net/doc/Gazebo-manual-0.5-html/plugin_models.html 131 | void BatteryPlugin::Init() 132 | { 133 | ROS_GREEN_STREAM("Init Battery"); 134 | this->q = this->q0; 135 | this->charging = false; 136 | } 137 | 138 | void BatteryPlugin::Reset() 139 | { 140 | this->iraw = 0.0; 141 | this->ismooth = 0.0; 142 | this->Init(); 143 | } 144 | 145 | double BatteryPlugin::OnUpdateVoltage(const common::BatteryPtr &_battery) 146 | { 147 | double dt = this->world->Physics()->GetMaxStepSize(); 148 | double totalpower = 0.0; 149 | double k = dt / this->tau; 150 | 151 | if (fabs(_battery->Voltage())<1e-3) 152 | return 0.0; 153 | 154 | for (auto powerLoad : _battery->PowerLoads()) 155 | totalpower += powerLoad.second; 156 | 157 | // current = power(Watts)/Voltage 158 | this->iraw = totalpower / _battery->Voltage(); 159 | 160 | this->ismooth = this->ismooth + k * (this->iraw - this->ismooth); 161 | 162 | if (!this->charging) 163 | { 164 | this->q = this->q - GZ_SEC_TO_HOUR(dt * this->ismooth); 165 | } 166 | else 167 | { 168 | this->q = this->q + GZ_SEC_TO_HOUR(dt * this->qt); 169 | } 170 | 171 | this->sim_time_now = this->world->SimTime().Double(); 172 | 173 | #ifdef BATTERY_DEBUG 174 | gzdbg << "Current charge:" << this->q << ", at:" << this->sim_time_now << "\n"; 175 | #endif 176 | // ROS_INFO_STREAM(this->q); 177 | 178 | this->et = this->e0 + this->e1 * (1 - this->q / this->c) - this->r * this->ismooth; 179 | 180 | #ifdef BATTERY_DEBUG 181 | gzdbg << "Current voltage:" << this->et << ", at:" << this->sim_time_now << "\n"; 182 | #endif 183 | 184 | //Turn off the motor 185 | if (this->q <= 0) 186 | { 187 | this->sim_time_now = this->world->SimTime().Double(); 188 | 189 | #ifdef BATTERY_DEBUG 190 | gzdbg << "Out of juice at:" << this->sim_time_now << "\n"; 191 | #endif 192 | 193 | this->q = 0; 194 | kobuki_msgs::MotorPower power_msg; 195 | power_msg.state = power::OFF; 196 | lock.lock(); 197 | this->motor_power.publish(power_msg); 198 | lock.unlock(); 199 | } 200 | else if (this->q >= this->c) 201 | { 202 | this->q = this->c; 203 | } 204 | 205 | std_msgs::Float64 charge_msg, charge_msg_mwh; 206 | charge_msg.data = this->q; 207 | charge_msg_mwh.data = this->q * 1000 * this-> et; 208 | 209 | lock.lock(); 210 | this->charge_state.publish(charge_msg); 211 | this->charge_state_mwh.publish(charge_msg_mwh); 212 | lock.unlock(); 213 | 214 | return et; 215 | } 216 | 217 | bool BatteryPlugin::SetCharging(brass_gazebo_battery::SetCharging::Request& req, 218 | brass_gazebo_battery::SetCharging::Response& res) 219 | { 220 | lock.lock(); 221 | this->charging = req.charging; 222 | if (this->charging) { 223 | #ifdef BATTERY_DEBUG 224 | gzdbg << "Bot is charging" << "\n"; 225 | #endif 226 | ROS_GREEN_STREAM("Bot is charging"); 227 | } 228 | else{ 229 | #ifdef BATTERY_DEBUG 230 | gzdbg << "Bot disconnected from the charging station" << "\n"; 231 | #endif 232 | ROS_GREEN_STREAM("Bot disconnected from the charging station"); 233 | } 234 | lock.unlock(); 235 | res.result = true; 236 | return true; 237 | } 238 | 239 | bool BatteryPlugin::SetChargingRate(brass_gazebo_battery::SetChargingRate::Request& req, 240 | brass_gazebo_battery::SetChargingRate::Response& res) 241 | { 242 | lock.lock(); 243 | this->qt = req.charge_rate; 244 | #ifdef BATTERY_DEBUG 245 | gzdbg << "Charging rate has been changed to: " << this->qt << "\n"; 246 | #endif 247 | ROS_GREEN_STREAM("Charging rate has been changed to: " << this->qt); 248 | lock.unlock(); 249 | res.result = true; 250 | return true; 251 | } 252 | 253 | 254 | bool BatteryPlugin::SetCharge(brass_gazebo_battery::SetCharge::Request &req, 255 | brass_gazebo_battery::SetCharge::Response &res) 256 | { 257 | lock.lock(); 258 | if (req.charge <= this->c){ 259 | this->q = req.charge; 260 | #ifdef BATTERY_DEBUG 261 | gzdbg << "Received charge:" << this->q << "\n"; 262 | #endif 263 | ROS_GREEN_STREAM("A new charge is set: " << this->q); 264 | } 265 | else 266 | { 267 | this->q = this->c; 268 | ROS_RED_STREAM("The charge cannot be higher than the capacity of the battery"); 269 | } 270 | lock.unlock(); 271 | res.result = true; 272 | return true; 273 | } 274 | 275 | bool BatteryPlugin::SetModelCoefficients(brass_gazebo_battery::SetCoef::Request &req, 276 | brass_gazebo_battery::SetCoef::Response &res) 277 | { 278 | lock.lock(); 279 | this->e0 = req.constant_coef; 280 | this->e1 = req.linear_coef; 281 | #ifdef BATTERY_DEBUG 282 | gzdbg << "Power model is changed, new coefficients (constant, linear):" << this->e0 << this->e1 << "\n"; 283 | #endif 284 | ROS_GREEN_STREAM("Power model is changed, new coefficients (constant, linear):" << this->e0 << this->e1); 285 | lock.unlock(); 286 | res.result = true; 287 | return true; 288 | } -------------------------------------------------------------------------------- /test/worlds/p2-cp1.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 1 5 | 0 0 10 0 -0 0 6 | 0.8 0.8 0.8 1 7 | 0.2 0.2 0.2 1 8 | 9 | 1000 10 | 0.9 11 | 0.01 12 | 0.001 13 | 14 | -0.5 0.1 -0.9 15 | 16 | 17 | 1 18 | 19 | 20 | 21 | 22 | 0 0 1 23 | 100 100 24 | 25 | 26 | 27 | 28 | 29 | 100 30 | 50 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 10 39 | 40 | 41 | 0 42 | 43 | 44 | 0 0 1 45 | 100 100 46 | 47 | 48 | 49 | 53 | 54 | 55 | 0 56 | 0 57 | 1 58 | 59 | 60 | 61 | 0.001 62 | 1 63 | 1000 64 | 0 0 -9.8 65 | 66 | 67 | 0.4 0.4 0.4 1 68 | 0.7 0.7 0.7 1 69 | 1 70 | 71 | 72 | EARTH_WGS84 73 | 0 74 | 0 75 | 0 76 | 0 77 | 78 | 79 | 9626 246000000 80 | 11932 241302148 81 | 1473451805 354483185 82 | 83 | 8 -5 0 0 -0 0 84 | 85 | -3.075 -9.00009 0 0 -0 0 86 | 0 0 0 0 -0 0 87 | 0 0 0 0 -0 0 88 | 0 0 0 0 -0 0 89 | 90 | 91 | 2.59996 -4.95009 0 0 -0 1.5708 92 | 0 0 0 0 -0 0 93 | 0 0 0 0 -0 0 94 | 0 0 0 0 -0 0 95 | 96 | 97 | 1.54996 -0.90009 0 0 -0 3.14159 98 | 0 0 0 0 -0 0 99 | 0 0 0 0 -0 0 100 | 0 0 0 0 -0 0 101 | 102 | 103 | 0.49996 -0.72509 0 0 -0 1.5708 104 | 0 0 0 0 -0 0 105 | 0 0 0 0 -0 0 106 | 0 0 0 0 -0 0 107 | 108 | 109 | -0.42504 -0.55009 0 0 -0 3.14159 110 | 0 0 0 0 -0 0 111 | 0 0 0 0 -0 0 112 | 0 0 0 0 -0 0 113 | 114 | 115 | -1.35004 0.37491 0 0 -0 1.5708 116 | 0 0 0 0 -0 0 117 | 0 0 0 0 -0 0 118 | 0 0 0 0 -0 0 119 | 120 | 121 | 11.7 1.29991 0 0 -0 0 122 | 0 0 0 0 -0 0 123 | 0 0 0 0 -0 0 124 | 0 0 0 0 -0 0 125 | 126 | 127 | 24.75 -5 0 0 0 -1.57079 128 | 0 0 0 0 -0 0 129 | 0 0 0 0 -0 0 130 | 0 0 0 0 -0 0 131 | 132 | 133 | 8.00005 -11.2999 0 0 -0 3.14159 134 | 0 0 0 0 -0 0 135 | 0 0 0 0 -0 0 136 | 0 0 0 0 -0 0 137 | 138 | 139 | -8.75 -10.15 0 0 -0 1.57083 140 | 0 0 0 0 -0 0 141 | 0 0 0 0 -0 0 142 | 0 0 0 0 -0 0 143 | 144 | 145 | 4.59371 -4.95009 0 0 -0 1.5708 146 | 0 0 0 0 -0 0 147 | 0 0 0 0 -0 0 148 | 0 0 0 0 -0 0 149 | 150 | 151 | 13.6038 -0.90009 0 0 -0 0 152 | 0 0 0 0 -0 0 153 | 0 0 0 0 -0 0 154 | 0 0 0 0 -0 0 155 | 156 | 157 | 22.6381 -4.92579 0 0 0 -1.5708 158 | 0 0 0 0 -0 0 159 | 0 0 0 0 -0 0 160 | 0 0 0 0 -0 0 161 | 162 | 163 | 13.6038 -9.00009 0 0 -0 3.14159 164 | 0 0 0 0 -0 0 165 | 0 0 0 0 -0 0 166 | 0 0 0 0 -0 0 167 | 168 | 169 | 170 | 0 0 0 0 -0 0 171 | 172 | 0 0 0 0 -0 0 173 | 0 0 0 0 -0 0 174 | 0 0 0 0 -0 0 175 | 0 0 0 0 -0 0 176 | 177 | 178 | 179 | 180 | -0.058045 0.021907 0 0 -0 0 181 | 182 | 183 | 184 | 185 | 11.5 0.15 2.5 186 | 187 | 188 | 0 0 1.25 0 -0 0 189 | 10 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 0 0 1.25 0 -0 0 202 | 203 | 204 | 11.5 0.15 2.5 205 | 206 | 207 | 208 | 212 | 1 1 1 1 213 | 214 | 215 | -11.075 -4.00009 0 0 -0 0 216 | 0 217 | 0 218 | 1 219 | 220 | 221 | 222 | 223 | 224 | 8.25 0.15 2.5 225 | 226 | 227 | 0 0 1.25 0 -0 0 228 | 10 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 0 0 1.25 0 -0 0 241 | 242 | 243 | 8.25 0.15 2.5 244 | 245 | 246 | 247 | 251 | 1 1 1 1 252 | 253 | 254 | -5.40004 0.049907 0 0 -0 1.5708 255 | 0 256 | 0 257 | 1 258 | 259 | 260 | 261 | 262 | 263 | 2.25 0.15 2.5 264 | 265 | 266 | 0 0 1.25 0 -0 0 267 | 10 268 | 269 | 270 | 271 | 272 | 273 | 274 | 275 | 276 | 277 | 278 | 279 | 0 0 1.25 0 -0 0 280 | 281 | 282 | 2.25 0.15 2.5 283 | 284 | 285 | 286 | 290 | 1 1 1 1 291 | 292 | 293 | -6.45004 4.09991 0 0 -0 3.14159 294 | 0 295 | 0 296 | 1 297 | 298 | 299 | 300 | 301 | 302 | 0.5 0.15 2.5 303 | 304 | 305 | 0 0 1.25 0 -0 0 306 | 10 307 | 308 | 309 | 310 | 311 | 312 | 313 | 314 | 315 | 316 | 317 | 318 | 0 0 1.25 0 -0 0 319 | 320 | 321 | 0.5 0.15 2.5 322 | 323 | 324 | 325 | 329 | 1 1 1 1 330 | 331 | 332 | -7.50004 4.27491 0 0 -0 1.5708 333 | 0 334 | 0 335 | 1 336 | 337 | 338 | 339 | 340 | 341 | 2 0.15 2.5 342 | 343 | 344 | 0 0 1.25 0 -0 0 345 | 10 346 | 347 | 348 | 349 | 350 | 351 | 352 | 353 | 354 | 355 | 356 | 357 | 0 0 1.25 0 -0 0 358 | 359 | 360 | 2 0.15 2.5 361 | 362 | 363 | 364 | 368 | 1 1 1 1 369 | 370 | 371 | -8.42504 4.44991 0 0 -0 3.14159 372 | 0 373 | 0 374 | 1 375 | 376 | 377 | 378 | 379 | 380 | 2 0.15 2.5 381 | 382 | 383 | 0 0 1.25 0 -0 0 384 | 10 385 | 386 | 387 | 388 | 389 | 390 | 391 | 392 | 393 | 394 | 395 | 396 | 0 0 1.25 0 -0 0 397 | 398 | 399 | 2 0.15 2.5 400 | 401 | 402 | 403 | 407 | 1 1 1 1 408 | 409 | 410 | -9.35004 5.37491 0 0 -0 1.5708 411 | 0 412 | 0 413 | 1 414 | 415 | 416 | 417 | 418 | 419 | 26.25 0.15 2.5 420 | 421 | 422 | 0 0 1.25 0 -0 0 423 | 10 424 | 425 | 426 | 427 | 428 | 429 | 430 | 431 | 432 | 433 | 434 | 435 | 0 0 1.25 0 -0 0 436 | 437 | 438 | 26.25 0.15 2.5 439 | 440 | 441 | 442 | 446 | 1 1 1 1 447 | 448 | 449 | 3.69996 6.29991 0 0 -0 0 450 | 0 451 | 0 452 | 1 453 | 454 | 455 | 456 | 457 | 458 | 12.75 0.15 2.5 459 | 460 | 461 | 0 0 1.25 0 -0 0 462 | 10 463 | 464 | 465 | 466 | 467 | 468 | 469 | 470 | 471 | 472 | 473 | 474 | 0 0 1.25 0 -0 0 475 | 476 | 477 | 12.75 0.15 2.5 478 | 479 | 480 | 481 | 485 | 1 1 1 1 486 | 487 | 488 | 16.75 -0 0 0 0 -1.57079 489 | 0 490 | 0 491 | 1 492 | 493 | 494 | 495 | 496 | 497 | 33.65 0.15 2.5 498 | 499 | 500 | 0 0 1.25 0 -0 0 501 | 10 502 | 503 | 504 | 505 | 506 | 507 | 508 | 509 | 510 | 511 | 512 | 513 | 0 0 1.25 0 -0 0 514 | 515 | 516 | 33.65 0.15 2.5 517 | 518 | 519 | 520 | 524 | 1 1 1 1 525 | 526 | 527 | 4.5e-05 -6.29991 0 0 -0 3.14159 528 | 0 529 | 0 530 | 1 531 | 532 | 533 | 534 | 535 | 536 | 2.44982 0.15 2.5 537 | 538 | 539 | 0 0 1.25 0 -0 0 540 | 10 541 | 542 | 543 | 544 | 545 | 546 | 547 | 548 | 549 | 550 | 551 | 552 | 0 0 1.25 0 -0 0 553 | 554 | 555 | 2.44982 0.15 2.5 556 | 557 | 558 | 559 | 563 | 1 1 1 1 564 | 565 | 566 | -16.75 -5.15 0 0 -0 1.57083 567 | 0 568 | 0 569 | 1 570 | 571 | 572 | 573 | 574 | 575 | 8.25 0.15 2.5 576 | 577 | 578 | 0 0 1.25 0 -0 0 579 | 10 580 | 581 | 582 | 583 | 584 | 585 | 586 | 587 | 588 | 589 | 590 | 591 | 0 0 1.25 0 -0 0 592 | 593 | 594 | 8.25 0.15 2.5 595 | 596 | 597 | 598 | 602 | 1 1 1 1 603 | 604 | 605 | -3.40629 0.049907 0 0 -0 1.5708 606 | 0 607 | 0 608 | 1 609 | 610 | 611 | 612 | 613 | 614 | 18.1701 0.15 2.5 615 | 616 | 617 | 0 0 1.25 0 -0 0 618 | 10 619 | 620 | 621 | 622 | 623 | 624 | 625 | 626 | 627 | 628 | 629 | 630 | 0 0 1.25 0 -0 0 631 | 632 | 633 | 18.1701 0.15 2.5 634 | 635 | 636 | 637 | 641 | 1 1 1 1 642 | 643 | 644 | 5.60378 4.09991 0 0 -0 0 645 | 0 646 | 0 647 | 1 648 | 649 | 650 | 651 | 652 | 653 | 8.25 0.15 2.5 654 | 655 | 656 | 0 0 1.25 0 -0 0 657 | 10 658 | 659 | 660 | 661 | 662 | 663 | 664 | 665 | 666 | 667 | 668 | 669 | 0 0 1.25 0 -0 0 670 | 671 | 672 | 8.25 0.15 2.5 673 | 674 | 675 | 676 | 680 | 1 1 1 1 681 | 682 | 683 | 14.6381 0.074213 0 0 0 -1.5708 684 | 0 685 | 0 686 | 1 687 | 688 | 689 | 690 | 691 | 692 | 18.1701 0.15 2.5 693 | 694 | 695 | 0 0 1.25 0 -0 0 696 | 10 697 | 698 | 699 | 700 | 701 | 702 | 703 | 704 | 705 | 706 | 707 | 708 | 0 0 1.25 0 -0 0 709 | 710 | 711 | 18.1701 0.15 2.5 712 | 713 | 714 | 715 | 719 | 1 1 1 1 720 | 721 | 722 | 5.60378 -4.00009 0 0 -0 3.14159 723 | 0 724 | 0 725 | 1 726 | 727 | 1 728 | 729 | 730 | 731 | 732 | 733 | 734 | 735 | 736 | 737 | 738 | 739 | 740 | 741 | 742 | 743 | 744 | 745 | 746 | 747 | 748 | 749 | 750 | 751 | 752 | 753 | 754 | 755 | 756 | 757 | 758 | 759 | 760 | 761 | 762 | 763 | 764 | 765 | 766 | 767 | 768 | 769 | 770 | 771 | 772 | 773 | 774 | 775 | 776 | 777 | 778 | 779 | 780 | 781 | 782 | 783 | 784 | 785 | 786 | 787 | 788 | 789 | 790 | 791 | 792 | 793 | 0.626978 -4.32423 54.2199 -0.009631 1.54152 2.24602 794 | orbit 795 | 796 | 797 | 798 | 799 | --------------------------------------------------------------------------------