Skip to content

Commit 0d7f070

Browse files
jlurgojlurgo
committed
Battery charger plugin
1 parent 4ffa968 commit 0d7f070

File tree

4 files changed

+214
-2
lines changed

4 files changed

+214
-2
lines changed

‎ca_description/urdf/create_base_gazebo.xacro‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -41,6 +41,10 @@
4141
<xyzOffsets>0 0 0</xyzOffsets>
4242
<rpyOffsets>0 0 0</rpyOffsets>
4343
</plugin>
44+
45+
<plugin name="battery_charger_plugin" filename="libbattery_charger_plugin.so">
46+
</plugin>
47+
4448
</gazebo>
4549

4650
<!-- Virtual Wall detector -->

‎ca_description/urdf/dock/dock.xacro‎

Lines changed: 6 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -15,9 +15,10 @@
1515
</collision>
1616
<xacro:inertial_cuboid_with_pose mass="0.15" x_length="${width}"
1717
y_length="${width/3}" z_length="${width * 2/3}" >
18+
<origin xyz="0 0 0" />
1819
</xacro:inertial_cuboid_with_pose>
1920
</link>
20-
<link name='base'>
21+
<link name='link'>
2122
<visual name='visual'>
2223
<origin xyz="0 ${width/2 - width/6} ${-(width * 2/3)/2}" />
2324
<geometry>
@@ -37,8 +38,11 @@
3738
</link>
3839
<joint name='glue' type='fixed'>
3940
<parent link="back" />
40-
<child link="base" />
41+
<child link="link" />
4142
</joint>
43+
<!-- <gazebo>
44+
<plugin name="model_pose_publisher_plugin" filename="libmodel_pose_publisher_plugin.so"/>
45+
</gazebo> -->
4246
</xacro:macro>
4347
<xacro:dock width="0.15">
4448
</xacro:dock>

‎ca_gazebo/CMakeLists.txt‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -23,6 +23,7 @@ catkin_package(
2323
LIBRARIES
2424
create_bumper_plugin
2525
model_pose_publisher_plugin
26+
battery_charger_plugin
2627
traffic_light_plugin
2728
${trajectory_actor_plugin_name}
2829
world_time_publisher
@@ -166,6 +167,10 @@ add_dependencies(model_pose_publisher_plugin ${catkin_EXPORTED_TARGETS})
166167
target_link_libraries(model_pose_publisher_plugin
167168
${catkin_LIBRARIES} ${GAZEBO_LIBRARIES})
168169

170+
add_library(battery_charger_plugin src/battery_charger_plugin.cpp)
171+
target_link_libraries(battery_charger_plugin
172+
${catkin_LIBRARIES} ${GAZEBO_LIBRARIES})
173+
169174
add_library(traffic_light_plugin src/traffic_light_plugin.cpp)
170175
add_dependencies(traffic_light_plugin ${catkin_EXPORTED_TARGETS})
171176
target_link_libraries(traffic_light_plugin
@@ -200,6 +205,7 @@ install(TARGETS
200205
create_cliff_plugin
201206
create_virtual_wall_plugin
202207
model_pose_publisher_plugin
208+
battery_charger_plugin
203209
traffic_light_plugin
204210
virtual_wall_detector
205211
world_time_publisher

‎ca_gazebo/src/battery_charger_plugin.cpp‎

Lines changed: 198 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,198 @@
1+
/*
2+
* Copyright 2020
3+
* Emiliano Borghi
4+
*
5+
* This program is free software; you can redistribute it and/or modify
6+
* it under the terms of the GNU General Public License as published by
7+
* the Free Software Foundation; either version 2 of the License, or
8+
* (at your option) any later version.
9+
*
10+
* This program is distributed in the hope that it will be useful,
11+
* but WITHOUT ANY WARRANTY; without even the implied warranty of
12+
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13+
* GNU General Public License for more details.
14+
*
15+
* You should have received a copy of the GNU General Public License
16+
* along with this program; if not, write to the Free Software
17+
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
18+
*
19+
*/
20+
/*
21+
* Desc: Model pose publisher
22+
* Author: Emiliano Borghi
23+
* Date: 11 January 2020
24+
*/
25+
26+
27+
#include <ros/ros.h>
28+
#include <tf2_ros/transform_broadcaster.h>
29+
#include <sdf/sdf.hh>
30+
31+
#include <ignition/math/Pose3.hh>
32+
#include <ignition/math/Vector3.hh>
33+
34+
#include "gazebo/gazebo.hh"
35+
#include "gazebo/common/Plugin.hh"
36+
#include "gazebo/msgs/msgs.hh"
37+
#include "gazebo/physics/physics.hh"
38+
#include "gazebo/transport/transport.hh"
39+
40+
#include "std_msgs/String.h"
41+
#include "std_msgs/Float32.h"
42+
#include "std_msgs/Int16.h"
43+
#include "ca_msgs/ChargingState.h"
44+
#include "geometry_msgs/Pose.h"
45+
#include "nav_msgs/Odometry.h"
46+
47+
#include <string>
48+
#include <cmath>
49+
50+
static const ros::Duration update_period = ros::Duration(1); // 10 ms
51+
52+
namespace gazebo
53+
{
54+
class BatteryChargerPlugin : public ModelPlugin
55+
{
56+
public:
57+
void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/)
58+
{
59+
GZ_ASSERT(_parent, "BatteryChargerPlugin _parent pointer is NULL");
60+
61+
// Make sure the ROS node for Gazebo has already been initialized
62+
if (!ros::isInitialized())
63+
{
64+
ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. "
65+
<< "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
66+
return;
67+
}
68+
ROS_INFO("Battery charger started!");
69+
// Store the pointer to the model
70+
this->model_ = _parent;
71+
ROS_INFO("model Name = %s", this->model_->GetName().c_str());
72+
this->indicator_link_ = this->model_->GetLink("link");
73+
// Listen to the update event. This event is broadcast every
74+
// simulation iteration.
75+
this->updateConnection = event::Events::ConnectWorldUpdateBegin(
76+
std::bind(&BatteryChargerPlugin::OnUpdate, this));
77+
this->prev_update_time_ = ros::Time::now();
78+
79+
this->rosnode_.reset(new ros::NodeHandle());
80+
81+
this->dock_pose_sub_ = this->rosnode_->subscribe("/dock/pose", 1, &BatteryChargerPlugin::DockPoseCb, this);
82+
83+
this->capacity_pub_ = this->rosnode_->advertise<std_msgs::Float32>("/battery/capacity", 1);
84+
this->charge_pub_ = this->rosnode_->advertise<std_msgs::Float32>("/battery/charge", 1);
85+
this->charge_ratio_pub_ = this->rosnode_->advertise<std_msgs::Float32>("/battery/charge_ratio", 1);
86+
this->current_pub_ = this->rosnode_->advertise<std_msgs::Float32>("/battery/current", 1);
87+
this->temperature_pub_ = this->rosnode_->advertise<std_msgs::Int16>("/battery/temperature", 1);
88+
this->voltage_pub_ = this->rosnode_->advertise<std_msgs::Float32>("/battery/voltage", 1);
89+
this->charging_state_pub_ = this->rosnode_->advertise<ca_msgs::ChargingState>("/battery/charging_state", 1);
90+
91+
this->charge_.data = 5;
92+
}
93+
94+
// Called by the world update start event
95+
public:
96+
void OnUpdate()
97+
{
98+
if ((ros::Time::now() - this->prev_update_time_) < update_period)
99+
{
100+
return;
101+
}
102+
ignition::math::Vector3<double> robot_pose = this->model_->WorldPose().Pos();
103+
ignition::math::Vector3<double> robot_scale = this->model_->Scale();
104+
ROS_INFO("model Scale X=%f Y=%f Z%f ",
105+
robot_scale.X(), robot_scale.Y(), robot_scale.Z());
106+
ROS_INFO("model Pose X=%f Y=%f Z%f ",
107+
robot_pose.X(), robot_pose.Y(), robot_pose.Z());
108+
ROS_INFO("dock Pose X=%f Y=%f Z%f ",
109+
this->dock_pose_.x, this->dock_pose_.y, this->dock_pose_.z);
110+
float distance = sqrt(
111+
pow(this->dock_pose_.x - robot_pose.X(), 2) +
112+
pow(this->dock_pose_.y - robot_pose.Y(), 2)
113+
);
114+
ROS_INFO("Distance to dock = %f", distance);
115+
116+
std_msgs::Float32 capacity;
117+
capacity.data = 6;
118+
this->capacity_pub_.publish(capacity);
119+
120+
std_msgs::Float32 current;
121+
ca_msgs::ChargingState charging_state;
122+
123+
if(distance<0.1) {
124+
//charging
125+
this->charge_.data += 0.05;
126+
current.data = 2;
127+
charging_state.state = 2; //Full charge
128+
if(this->charge_.data > capacity.data) {
129+
this->charge_.data = capacity.data;
130+
current.data = 0;
131+
charging_state.state = ca_msgs::ChargingState::CHARGE_TRICKLE; //Trickle charge
132+
}
133+
} else {
134+
//discharging
135+
this->charge_.data -= 0.05;
136+
current.data = -2;
137+
charging_state.state = 0; //none
138+
if(this->charge_.data < 0) {
139+
this->charge_.data = 0;
140+
current.data = 0;
141+
}
142+
}
143+
144+
this->current_pub_.publish(current);
145+
this->charging_state_pub_.publish(charging_state);
146+
this->charge_pub_.publish(this->charge_);
147+
148+
std_msgs::Float32 charge_ratio;
149+
charge_ratio.data = 1;
150+
this->charge_ratio_pub_.publish(charge_ratio);
151+
152+
std_msgs::Int16 temperature;
153+
temperature.data = 40;
154+
this->capacity_pub_.publish(temperature);
155+
156+
std_msgs::Float32 voltage;
157+
voltage.data = 12.1;
158+
this->capacity_pub_.publish(voltage);
159+
160+
float scale = this->charge_.data/6;
161+
if(scale < 0.2) scale = 0.2;
162+
robot_scale.X(scale);
163+
robot_scale.Y(scale);
164+
robot_scale.Z(scale);
165+
this->model_->SetScale(robot_scale);
166+
// Update time
167+
this->prev_update_time_ = ros::Time::now();
168+
}
169+
170+
void DockPoseCb(const geometry_msgs::Pose & msg)
171+
{
172+
ignition::math::Vector3<double> robot_pose = this->model_->WorldPose().Pos();
173+
this->dock_pose_ = msg.position;
174+
}
175+
176+
private:
177+
geometry_msgs::Point dock_pose_;
178+
std::shared_ptr<ros::NodeHandle> rosnode_;
179+
ros::Publisher capacity_pub_;
180+
ros::Publisher charge_pub_;
181+
ros::Publisher charge_ratio_pub_;
182+
ros::Publisher current_pub_;
183+
ros::Publisher temperature_pub_;
184+
ros::Publisher voltage_pub_;
185+
ros::Publisher charging_state_pub_;
186+
ros::Subscriber dock_pose_sub_;
187+
physics::ModelPtr model_;
188+
physics::LinkPtr indicator_link_;
189+
ros::Time prev_update_time_;
190+
std_msgs::Float32 charge_;
191+
// Pointer to the update event connection
192+
event::ConnectionPtr updateConnection;
193+
};
194+
195+
// Register this plugin with the simulator
196+
GZ_REGISTER_MODEL_PLUGIN(BatteryChargerPlugin)
197+
198+
} // namespace gazebo

0 commit comments

Comments
 (0)