gazebo_irlock_plugin.cpp
3.7 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
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
/*
* Copyright (C) 2012-2015 Open Source Robotics Foundation
*
* 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.
*
*/
#ifdef _WIN32
// Ensure that Winsock2.h is included before Windows.h, which can get
// pulled in by anybody (e.g., Boost).
#include <Winsock2.h>
#endif
#include "gazebo/sensors/DepthCameraSensor.hh"
#include "gazebo_irlock_plugin.h"
#include <opencv2/opencv.hpp>
#include <math.h>
#include <string>
#include <iostream>
#include <boost/algorithm/string.hpp>
#include <ignition/math.hh>
using namespace cv;
using namespace std;
using namespace gazebo;
GZ_REGISTER_SENSOR_PLUGIN(IRLockPlugin)
IRLockPlugin::IRLockPlugin() : SensorPlugin()
{
}
IRLockPlugin::~IRLockPlugin()
{
this->camera.reset();
}
void IRLockPlugin::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf)
{
if (!_sensor)
gzerr << "Invalid sensor pointer.\n";
this->world = physics::get_world(_sensor->WorldName());
this->camera = std::dynamic_pointer_cast<sensors::LogicalCameraSensor>(_sensor);
if (!this->camera) {
gzerr << "IRLockPlugin requires a CameraSensor.\n";
}
if (_sdf->HasElement("robotNamespace")) {
namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
} else {
gzwarn << "[gazebo_irlock_plugin] Please specify a robotNamespace.\n";
}
node_handle_ = transport::NodePtr(new transport::Node());
node_handle_->Init(namespace_);
const string scopedName = _sensor->ParentName();
string topicName = "~/" + scopedName + "/irlock";
boost::replace_all(topicName, "::", "/");
irlock_pub_ = node_handle_->Advertise<sensor_msgs::msgs::IRLock>(topicName, 10);
this->updateConnection = this->camera->ConnectUpdated(
std::bind(&IRLockPlugin::OnUpdated, this));
this->camera->SetActive(true);
}
void IRLockPlugin::OnUpdated()
{
// Get the current simulation time.
#if GAZEBO_MAJOR_VERSION >= 9
common::Time now = world->SimTime();
#else
common::Time now = world->GetSimTime();
#endif
gazebo::msgs::LogicalCameraImage img = this->camera->Image();
for (int idx = 0; idx < img.model_size(); idx++) {
gazebo::msgs::LogicalCameraImage_Model model = img.model(idx);
if (model.has_name() && model.name() == "irlock_beacon") {
if (model.has_pose()) {
// position of the beacon in camera frame
ignition::math::Vector3d pos;
pos.X() = model.pose().position().x();
pos.Y() = model.pose().position().y();
pos.Z() = model.pose().position().z();
// the default orientation of the IRLock sensor reports beacon in front of vehicle as -y values, beacon right of vehicle as x values
// rotate the measurement accordingly
ignition::math::Vector3d meas(-pos.Y()/pos.X(), -pos.Z()/pos.X(), 1.0);
// prepare irlock message
irlock_message.set_time_usec(now.Double() * 1e6);
irlock_message.set_signature(idx); // unused by beacon estimator
irlock_message.set_pos_x(meas.X());
irlock_message.set_pos_y(meas.Y());
irlock_message.set_size_x(0); // unused by beacon estimator
irlock_message.set_size_y(0); // unused by beacon estimator
// send message
irlock_pub_->Publish(irlock_message);
}
}
}
}
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=2 ts=2 : */