Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[4881] pkg/trunk
|
From: <hsu...@us...> - 2008-10-01 17:31:25
|
Revision: 4881
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4881&view=rev
Author: hsujohnhsu
Date: 2008-10-01 17:31:05 +0000 (Wed, 01 Oct 2008)
Log Message:
-----------
* changed battery full capacity for debug.
* update simulated battery plugin.
listens to ROS topic plugged_in, and if the message (gazebo_plugin::PlugCommand.status) is "the robot is very much plugged into the wall",
then the robot begins charging. any other message content will stop charging.
Modified Paths:
--------------
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_battery.h
pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_battery.cpp
pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo/controllers_gazebo.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo/controllers_gazebo_headless.xml
Added Paths:
-----------
pkg/trunk/drivers/simulator/gazebo_plugin/msg/PlugCommand.msg
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_battery.h
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_battery.h 2008-10-01 15:01:31 UTC (rev 4880)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_battery.h 2008-10-01 17:31:05 UTC (rev 4881)
@@ -38,6 +38,7 @@
#include <robot_msgs/BatteryState.h>
#include <robot_msgs/DiagnosticMessage.h>
#include <robot_msgs/DiagnosticStatus.h>
+#include <gazebo_plugin/PlugCommand.h>
#include <ros/node.h>
namespace gazebo
@@ -115,6 +116,38 @@
/// \brief stores current simulator time
private: double current_time_;
+
+ /// \brief stores last simulator time
+ private: double last_time_;
+
+ /// \brief rate to broadcast diagnostic message
+ private: double diagnostic_rate_;
+
+ /// \brief rate to broadcast battery states message
+ private: double battery_state_rate_;
+
+ /// \brief some internal variables for keeping track of simulated battery
+ /// @todo make consumption rate vary with joint commands, motion, etc
+
+ /// \brief full capacity of battery
+ private: double full_capacity_;
+
+ /// \brief charge state;
+ private: double charge_;
+
+ /// \brief default charge rate when plugged in
+ private: double default_charge_rate_;
+
+ /// \brief power drain, if this is negative, we are charging the battery.
+ private: double consumption_rate_;
+
+ /// \brief listen to ROS to see if we are charging
+ private: void SetPlug();
+ private: gazebo_plugin::PlugCommand plug_msg_;
+
+/// @todo make DISCHAGE_RATE something else
+#define DISCHARGE_RATE 1.0
+
};
/** \} */
Added: pkg/trunk/drivers/simulator/gazebo_plugin/msg/PlugCommand.msg
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/msg/PlugCommand.msg (rev 0)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/msg/PlugCommand.msg 2008-10-01 17:31:05 UTC (rev 4881)
@@ -0,0 +1 @@
+string status
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_battery.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_battery.cpp 2008-10-01 15:01:31 UTC (rev 4880)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_battery.cpp 2008-10-01 17:31:05 UTC (rev 4881)
@@ -78,12 +78,37 @@
rosnode_->advertise<robot_msgs::BatteryState>(this->stateTopicName_,10);
this->diagnosticMessageTopicName_ = node->GetString("diagnosticMessageTopicName","diagnostic",0);
rosnode_->advertise<robot_msgs::DiagnosticMessage>(this->diagnosticMessageTopicName_,10);
+
+ /// faking the plug and unplug of robot
+ rosnode_->subscribe("plugged_in",this->plug_msg_,&GazeboBattery::SetPlug,this,10);
+
+ this->full_capacity_ = node->GetDouble("full_charge_energy",0.0,0);
+ this->default_charge_rate_ = node->GetDouble("default_charge_rate",-2.0,0);
+
+ /// @todo make below useful
+ this->diagnostic_rate_ = node->GetDouble("diagnostic_rate",1.0,0);
+ /// @todo make below useful
+ this->battery_state_rate_ = node->GetDouble("dbattery_state_rate_",1.0,0);
}
+ void GazeboBattery::SetPlug()
+ {
+ this->lock_.lock();
+ if (this->plug_msg_.status == "the robot is very much plugged into the wall")
+ this->consumption_rate_ = this->default_charge_rate_ + DISCHARGE_RATE;
+ else
+ this->consumption_rate_ = DISCHARGE_RATE;
+ this->lock_.unlock();
+ }
+
void GazeboBattery::InitChild()
{
this->current_time_ = Simulator::Instance()->GetSimTime();
+ this->last_time_ = this->current_time_;
+ /// initialize battery
+ this->charge_ = this->full_capacity_; /// our convention is joules
+ this->consumption_rate_ = DISCHARGE_RATE; /// time based decay rate in watts
}
void GazeboBattery::UpdateChild()
@@ -92,14 +117,24 @@
/**********************************************************/
/* */
+ /* update battery */
+ /* */
+ /**********************************************************/
+ this->charge_ = this->charge_ - (this->current_time_ - this->last_time_)*this->consumption_rate_;
+ if (this->charge_ < 0) this->charge_ = 0;
+ if (this->charge_ > this->full_capacity_) this->charge_ = this->full_capacity_;
+ //std::cout << " battery charge remaining: " << this->charge_ << " Joules " << std::endl;
+
+ /**********************************************************/
+ /* */
/* publish battery state */
/* */
/**********************************************************/
//this->battery_state_.header.frame_id = ; // no frame id for battery
this->battery_state_.header.stamp.sec = (unsigned long)floor(this->current_time_);
this->battery_state_.header.stamp.nsec = (unsigned long)floor( 1e9 * ( this->current_time_ - this->battery_state_.header.stamp.sec) );
- this->battery_state_.energy_remaining = 1000;
- this->battery_state_.power_consumption = 10;
+ this->battery_state_.energy_remaining = this->charge_;
+ this->battery_state_.power_consumption = this->consumption_rate_;
this->lock_.lock();
this->rosnode_->publish(this->stateTopicName_,this->battery_state_);
@@ -119,6 +154,7 @@
this->lock_.lock();
this->rosnode_->publish(this->diagnosticMessageTopicName_,diagnostic_message_);
this->lock_.unlock();
+ this->last_time_ = this->current_time_;
}
void GazeboBattery::FiniChild()
@@ -127,6 +163,7 @@
rosnode_->unadvertise(this->stateTopicName_);
rosnode_->unadvertise(this->diagnosticMessageTopicName_);
+ rosnode_->unsubscribe("plugged_in");
}
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo/controllers_gazebo.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo/controllers_gazebo.xml 2008-10-01 15:01:31 UTC (rev 4880)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo/controllers_gazebo.xml 2008-10-01 17:31:05 UTC (rev 4881)
@@ -33,7 +33,7 @@
<timeout>5</timeout>
<diagnostic_rate>1.0</diagnostic_rate>
<battery_state_rate>1.0</battery_state_rate>
- <full_charge_energy>1.0</full_charge_energy>
+ <full_charge_energy>120.0</full_charge_energy>
<diagnosticTopicName>diagnostic</diagnosticTopicName>
<stateTopicName>battery_state</stateTopicName>
<selfTestServiceName>self_test</selfTestServiceName>
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo/controllers_gazebo_headless.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo/controllers_gazebo_headless.xml 2008-10-01 15:01:31 UTC (rev 4880)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo/controllers_gazebo_headless.xml 2008-10-01 17:31:05 UTC (rev 4881)
@@ -33,7 +33,7 @@
<timeout>5</timeout>
<diagnostic_rate>1.0</diagnostic_rate>
<battery_state_rate>1.0</battery_state_rate>
- <full_charge_energy>1.0</full_charge_energy>
+ <full_charge_energy>120.0</full_charge_energy>
<diagnosticTopicName>diagnostic</diagnosticTopicName>
<stateTopicName>battery_state</stateTopicName>
<selfTestServiceName>self_test</selfTestServiceName>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
