gazebo_controller_interface.cpp
3.41 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
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
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
/*
* Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland
* Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland
* Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland
* Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland
* Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include "gazebo_controller_interface.h"
namespace gazebo {
GazeboControllerInterface::~GazeboControllerInterface() {
updateConnection_->~Connection();
}
void GazeboControllerInterface::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) {
// Store the pointer to the model.
model_ = _model;
world_ = model_->GetWorld();
namespace_.clear();
if (_sdf->HasElement("robotNamespace"))
namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
else
gzerr << "[gazebo_controller_interface] Please specify a robotNamespace.\n";
node_handle_ = transport::NodePtr(new transport::Node());
node_handle_->Init(namespace_);
getSdfParam<std::string>(_sdf, "commandMotorSpeedSubTopic", command_motor_speed_sub_topic_,
command_motor_speed_sub_topic_);
getSdfParam<std::string>(_sdf, "motorSpeedCommandPubTopic", motor_velocity_reference_pub_topic_,
motor_velocity_reference_pub_topic_);
// Listen to the update event. This event is broadcast every
// simulation iteration.
updateConnection_ = event::Events::ConnectWorldUpdateBegin(
boost::bind(&GazeboControllerInterface::OnUpdate, this, _1));
cmd_motor_sub_ = node_handle_->Subscribe<mav_msgs::msgs::CommandMotorSpeed>("~/" + model_->GetName() + command_motor_speed_sub_topic_,
&GazeboControllerInterface::CommandMotorCallback,
this);
motor_velocity_reference_pub_ = node_handle_->Advertise<mav_msgs::msgs::CommandMotorSpeed>("~/" + model_->GetName() + motor_velocity_reference_pub_topic_, 1);
}
// This gets called by the world update start event.
void GazeboControllerInterface::OnUpdate(const common::UpdateInfo& /*_info*/) {
if(!received_first_referenc_)
return;
#if GAZEBO_MAJOR_VERSION >= 9
common::Time now = world_->SimTime();
#else
common::Time now = world_->GetSimTime();
#endif
mav_msgs::msgs::CommandMotorSpeed turning_velocities_msg;
for (int i = 0; i < input_reference_.size(); i++)
turning_velocities_msg.add_motor_speed(input_reference_[i]);
// Add header timestamp etc
motor_velocity_reference_pub_->Publish(turning_velocities_msg);
}
void GazeboControllerInterface::CommandMotorCallback(CommandMotorSpeedPtr &input_reference_msg) {
input_reference_.resize(input_reference_msg->motor_speed_size());
for (int i = 0; i < input_reference_msg->motor_speed_size(); ++i) {
input_reference_[i] = input_reference_msg->motor_speed(i);
}
received_first_referenc_ = true;
}
GZ_REGISTER_MODEL_PLUGIN(GazeboControllerInterface);
}